带高斯白噪声的Kalman滤波Matlab代码（带注释）

%卡尔曼滤波 clear clc N=200; q=1; w=CreateGauss(0,q,1,N) %系统预测的随机白噪声 Q=cov(w);%协方差 T=1;%采样间隔 A=[1,T;0,1];%状态转移矩阵 B=[(T^2)/2;T];%过程噪声分布矩阵 H=[1,0];%测量值的测量矩阵 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% X1=zeros(2,200);%200个滤波值的数组 X0=[9;11]; X1(:,1)=X0;%初始化状态 X=zeros(2,200);%预测值矩阵 X(:,1)=X0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Z0=ones(1,200);%测量值矩阵 Z0(1)=100; for n=2:200; Z0(n)=Z0(n-1);%真实的轨迹 end r=4;%测量值噪声的方差 V=CreateGauss(0,r,1,N); %测量值的随机白噪声 Z=Z0+V;%添加白噪声 R=cov(V);%协方差 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% P=[q,q/T;q/T,2*q/(T^2)];%预测值的协方差 P1=[r,r/T;r/T,2*r/(T^2)];%滤波值的协方差 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for k=2:N; X(:,k)=A*X1(:,k-1);%预测值X(k|k-1)=A*X(k-1|k-1) P=A*P1*A' + B*Q*B';%预测值的协方差P(K|k-1)=A*P(k-1|k-1)A'+Q Kg=P*H'/(H*P*H'+R);%Kg(k)=p(k|k-1)*H'/(H*P(k|k-1)H'+R) X1(:,k)=X(:,k)+Kg*(Z(:,k)-H*X(:,k));%Y滤波值X(k|k)=X(k|k-1)+Kg(k)*(Z(k)-H*X(k|k-1))%滤波值 P1=(eye(2,2)-Kg*H)*P;%滤波值的cov,P(k|k)=(E-Kg*H)*P(k|k-1) end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% t=0:1:200-1; x1=X1(1,:); x=X(1,:); plot(t,Z0,'-r');%真实轨迹 hold all; plot(t,Z,'-.g');%带噪声的测量轨迹 hold all; plot(t,x,'-.m');%预测轨迹 hold all; plot(t,x1,'-b');%滤波轨迹 hold all; legend('真实轨迹','带噪声的轨迹','预测轨迹','滤波轨迹','location','northwest'); axis([0 200 0 200]); xlabel('时间'); ylabel('位置'); title('kalman滤波'); grid on