《卡尔曼滤波》由会员分享，可在线阅读，更多相关《卡尔曼滤波（7页珍藏版）》请在金锄头文库上搜索。
1、用预测值和观测值更新估计值z ( k ) -z ( k )十K ( k )( y ( k ) y ( k )计算更新后的协方差矩阵p ( k ) - (I K( k)C T( k)P -( k)z ( k + 1 ) - z ( k )P ( k + 1 ) - P ( k ) 卡尔曼滤波是一种在线性最小方差估计的基础上的最优线性递推滤波方法，具有计 算量小、存储量低，实时性高的优点。 其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻的估计值和现时刻 的观测值来更新对状态变量的估计，求出现在时刻的估计值，容易在计算机中实现。实例卡尔曼滤波的MATLAB实现一、实验内容一个系统模型为x
2、(k +1) - x (k) + x (k) + w(k), k 0,1,1 1 2x (k +1) x (k) + w(k)22同时有下列条件：(1) 初始条件已知且有x(0) 0, 0T。(2) w(k) 是一个标量零均值白高斯序列，且自相关函数已知为E w( j )w(k) 。jk另外，我们有下列观测模型，即y (k +1) = x (k +1) + v (k +1), k = 0,1,1 1 1y (k +1) = x (k +1) + v (k +1)2 2 2且有下列条件：(3) v (k +1)和v (k +1)是独立的零均值白高斯序列，且有12Ev (j)v (k) = 5 ,
3、 Ev (j)v (k) = 28 , k = 0,1, 2,11jk22jk(4) 对于所有的j和k, w(k)与观测噪声过程v (k +1)和v (k +1)是不相关的，12即E w( j )v (k) = 0 , E w( j )v (k) = 0, j = 0,1, 2,，k = 0,1, 2, 12我们希望得到由观测矢量y(k +1)，即y(k +1) = y (k +1), y (k + 1)t估计状态矢 12量x(k +1) = x (k +1),x (k + 1)t的卡尔曼滤波器的公式表示形式，并求解以下问 12题：(a) 求出卡尔曼增益矩阵，并得出最优估计x(k +1)和观测
4、矢量y (1), y (2),., y (k +1)之间的递归关系。(b) 通过一个标量框图(不是矢量框图)表示出状态矢量x(k +1)中元素x (k +1)和x (k +1)估计值的计算过程。12(c) 用模拟数据确定状态矢量x(k)的估计值x(k k), k = 0,1,.,10,并画出当 匸0， 1，,10 时x(kfk)和仝?(k. k)的图。(d) 通常，状态矢量的真实值是得不到得。但为了用作图来说明问题，表P8.1 和P8.2给出来状态矢量元素得值。对于k = 0，1，10，在同一幅图中画出真实值和在(c)中确定的x (k)的估计值。对x (k)重复这样过程。 12当k从1变到10
5、时，对每一个元素i = 1, 2,计算并画出各自的误差图，即 x (k) - x (k k)。ii(e) 当 k 从 1 变到 10 时，通过用卡尔曼滤波器的状态误差协方差矩阵画出Es 2(k/k)和 Es 2(k/k)， 而 (k/k) = x (k) 一 x Jk/k)，s (k. k) = x (k) 一 x? (k. k)。(f) 讨论一下(d)中你计算的误差与(e)中方差之间的关系。三、MATLAB程序%卡尔曼滤波实验程序clc;y1=3.29691969,3.38736515,7.02830641,9.71212521,11.42018315,15.97870583,22.0693
6、4285,28.30212781,30.44683831,38.75875595; %观测值 y1(k) y2=2.10134294,0.47540797,3.17688898,2.49811140,2.91992424,6.17307616, 5.42519274,3.05365741,5.98051141,4.51016361;%观测值 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;%状态矩阵赋初值for k=1:
7、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=lege nd(x1(k )的估计值)set(h,interpreter,non
8、e)subplot(2,1,2)i=1:10;plot(i,x2(i),k)h=lege nd(x2(k )的估计值)set(h,interpreter,none)X1=0,1.65428714,3.50300702,5.997852924,9.15040740,12.50873910,16.92192594,21.34483352,25.89335144,31.54135330,36.93605670; %由模拟得到的 实际状态值 X1(k)X2=0,1.65428714,1.84871988,2.47552222,3.17187816,3.35833170,4.41318684,4.422
9、90758,4.54851792,5.64800186,5.394470340;%由模拟得到的实际状态值 X2(k)figure%在同一幅图中画出状态矢量的估计值与真实值subplot(2,1,1)i=1:10;plot(i,x1(i),k,i,X1(i+1),b)h=lege nd(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=lege nd(x2(k )的估计值,x2(k )的真实值) set(h,interpreter,none)for i=1:
10、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=lege nd(x1(k)的误差)set(h,interpreter,none) subplot(2,1,2)i=1:10;plot(i,e2(i),r)h=lege nd(x2(k)的误差)set(h,interpreter,none)figure%通过用卡尔曼滤波器的状态误差协方差矩阵画出El(k/kF2和 Es2(k/k)A2+i=1:10;subplot(2,1,1)plot(
11、i,pk(1,i),r)h= legend(由状态误差协方差矩阵得到的El(k/k)A2) set(h,Interpreter,none)subplot(2,1,2)plot(i,pk(2,i),r)h= legend(由状态误差协方差矩阵得到的Ez2(k/k)A2) set(h,Interpreter,none)四、实验结果分析(a) 卡尔曼增益矩阵：H 二P Ctx(CxP Ct + R)-ik k k估计值与观测值之间的递归关系为：Xk = Ak x Xk-i + Hk x (Yk -ck x Ak x紀)(b) 状态矢量估计值的计算框图：k+1(c ) X (k；k)和 x2 (kf k)的图:(d)真实值与估计值的比较图:403025201510邮的估计餡 疋同的号实值刈同的估计值 耳1啊的現宾16各自的误差图：(e )通过用卡尔曼滤波器的状态误差协方差矩阵画出的Ej(k；k)和E&2 2(kk )：（f）分析：（e）中的方差是（d）中的误差平方后取均值，是均方误差。误 差直接由真实值减去估计值，有正有负，而均方误差没有这个缺陷，更能综合的 表示滤波的效果。
