卡尔曼滤波课件.ppt

状态矢量估计值的计算框图：和的图真实值与估计值的比较图各自的误差图通过用卡尔曼滤波器的状态误差协方差矩阵画出的和：分析：此图中的方差是上图中的误差平方后取均值，是均方误差。误差直接由真实值减去估计值，有正有负，而均方误差没有这个缺陷，更能综合的表示滤波的效果。程序%卡尔曼滤波实验程序clc;y1=[3379111522283038;%观测值y1(k)y2=[2032265354;%观测值y2(k)p0=[1,0;0,1];p=p0;%均方误差阵赋初值Ak=[1,1;0,1];%转移矩阵Qk=[1,0;0,1];%系统噪声矩阵Ck=[1,0;0,1];%量测矩阵Rk=[1,0;0,2];%测量噪声矩阵x0=[0,0];xk=x0;%状态矩阵赋初值fork=1:10Pk=Ak*p*Ak+Qk;%滤波方程3Hk=Pk*Ck*inv(Ck*Pk*Ck+Rk);%滤波方程2yk=[y1(k);y2(k)];%观测值xk=Ak*xk+Hk*(yk-Ck*Ak*xk);%滤波方程1x1(k)=xk(1);x2(k)=xk(2);%记录估计值p=(eye(2)-Hk*Ck)*Pk;%滤波方程4pk(:,k)=[p(1,1),p(2,2)];%记录状态误差协方差矩阵endfigure%画图表示状态矢量的估计值subplot(2,1,1)i=1:10;plot(i,x1(i),k)h=legend(x1(k)的估计值)set(h,interpreter,none)subplot(2,1,2)i=1:10;plot(i,x2(i),k)h=legend(x2(k)的估计值)set(h,interpreter,none)?X1=[0,135.997852924,9121621253136;%由模拟得到的实际状态值X1(k)X2=[0,1123344455.394470340];%由模拟得到的实际状态值X2(k)figure%在同一幅图中画出状态矢量的估计值与真实值subplot(2,1,1)i=1:10;plot(i,x1(i),k,i,X1(i+1),b)h=legend(x1(k)的估计值,x1(k)的真实值)set(h,interpreter,none)subplot(2,1,2)i=1:10;plot(i,x2(i),k,i,X2(i+1),b)h=legend(x2(k)的估计值,x2(k)的真实值)set(h,interpreter,none)fori=1:10%计算x(k)的误差e1(i)=X1(i+1)-x1(i);e2(i)=X2(i+1)-x2(i);endfigure%画出误差图subplot(2,1,1)i=1:10;plot(i,e1(i),r)h=legend(x1(k)的误差)set(h,interpreter,none)subplot(2,1,2)i=1:10;plot(i,e2(i),r)h=legend(x2(k)的误差)set(h,interpreter,none)?figure%通过用卡尔曼滤波器的状态误差协方差矩阵画出E[ε1(k/k)^2]和E[ε2(k/k)^2]i=1:10;subplot(2,1,1)p