Kalman经典生活实例研究(有图有代码).doc

先写出 kalman filter的模型： 式中是系统的维状态向量，是系统的维观测序列，是维系统过程噪声序列，是维观测噪声序列，是系统的维状态转移矩阵，是维噪声输入矩阵，是维观测矩阵，且。 则状态一步预测： 状态估计： 滤波增益矩阵： 一步预测误差方差阵： 估计误差方差阵： 其中，滤波增益矩阵可以进一步改写为： 估计误差方差阵可以改写为： 例子：滤波 设运动物体沿直线运动，时刻的位移、速度、加速度和加加速度分别为，只对运动物体的位置做观测，观测值为(注意，这里观测向量和噪声向量均是一维的)，若，观测的采样周期为，求对的估计。 解：由于 其中为加加速度，对运动体得跟踪者来说，加加速度是随机量，此处取为白噪声。 取状态向量 则状态方程为： 式中 观测方程为： 式中 发现，这是一个定常的滤波问题。应用Kalman滤波方程有： 现假设，，，编程序，看滤波效果: 由于没有观测值数据，所以手动添加它：考虑到观测值是位移，所以添加形如 的观测值，其中的与噪声向量方差一致，便于比较，因为理论就是这个方差的噪声，生成的数据作为观测数据。实验结果如下: 实验代码如下： ******************** clc close all clear all g=9.8; t=1:0.02:5; Z=1+(1/2)*g*(t.^2);%2úéú1?2aêy?Y r=1; l=random(norm,0,sqrt(r),1,length(Z)); Z=Z+l;%1?2aêy?Y?ó??éù save data1 Z ******************** clc close all clear all load data1 %?μí3μ?2?êy X=zeros(3,1);K=zeros(3,1);P=zeros(3,3); X=[0.1;1;1];%3??μ %P=[1,1,1;1,1,1;1,1,1];%3??μ P=[5,0,0; 0,50,0; 0,0,500];%3??μ r=1; q=1; FAI=[1,0.02,0.0002; 0,1,0.02; 0,0,1]; gama=[0;0;1]; H=[1,0,0]; N=length(Z) for i=2:N Xi=FAI*X; Pi=FAI*P*FAI+q*gama*gama; K=Pi*H*(H*Pi*H+r)^(-1); X=Xi+K*(Z(i)-H*Xi); P=(eye(3)-K*H)*Pi; s(i)=X(1,1); v(i)=X(2,1); a(i)=X(3,1); ps(i)=P(1,1); pv(i)=P(2,2); pa(i)=P(3,3); end s(1)=0.1;v(1)=1;a(1)=1; ps(1)=5;pv(1)=50;pa(1)=500; error=(s-Z)./Z %?ó2?°ù·?±è MSE=MSE(error) k=1:N; n=k*0.02; figure(1) subplot(211); plot(n,Z,r,n,s,:.); xlabel(t/s); ylabel(z(t),x(t)); title(???ò?1?2a?μ?°??2¨?μ); legend(??ò?1?2az(t),??ò???2¨?μx(t),2); grid on subplot(212); plot(n,ps); xlabel(t/s); ylabel(ps); title(??ò?D-·?2?) grid on figure(2) plot(n,error); xlabel(t/s); ylabel(e); title(?à???ó2?°ù·?±è) grid on figure(3) subplot(211); plot(n,v); xlabel(t/s); ylabel(v(t)); title(?ù?è??2¨?μ); grid on subplot(212); plot(n,pv); xlabel(t/s); ylabel(pv); title(?ù?èD-·?2?) figure(4) subplot(211); plot(n,a); xlabel(t/s); ylabel(a(t)); title(?ó?ù?è??2¨?μ); grid on subplot(212); plot(n,pa); xlabel(t/s); ylabel(pa); title(?ó?ù?èD-·?2?)