《卡尔曼滤波的MATLAB实现演示教学》由会员分享，可在线阅读，更多相关《卡尔曼滤波的MATLAB实现演示教学（6页珍藏版）》请在金锄头文库上搜索。
1、卡尔曼滤波的MATLAB现一、实验内容一个系统模型为xi(k 1) = xi(k) X2(k) w(k), k = 0,1, x2 (k 1) = x2 (k) w(k)同时有下列条件：(1) 初始条件已知且有x(0) =0, 0T。(2) w(k)是一个标量零均值白高斯序歹0 ,且自相关函数已知为Ew(j)w(k)=另外，我们有下列观测模型，即y1(k 1) = x1(k 1) v1 (k 1), k = 0,1, y2(k 1) = x2(k 1) v2 (k 1)且有下列条件：(3) v(k+1)和V2(k+1)是独立的零均值白高斯序列，且有Ev1(j)v1(k) =、jk , Ev2(
2、j)v2(k) =2、jk ,k = 0,1, 2,(4) 对丁所有的j和k, w(k)与观测噪声过程v(k十1)和v2(k+1)是不相关的， 即Ew(j)v1(k) = 0, Ew(j)v2(k) =0, j =0,1,2, ,k=Q1, 2,我们希望得到由观测欠量 y(k+1)，即y(k+1) =y(k+1), y2(k+1)T估计状态欠量x(k +1) =x(k +1),x2(k +1)T的卡尔曼滤波器的公式表示形式，并求解以下问 题：(a) 求出卡尔曼增益矩阵，并得出最优估计x(k +1)和观测欠量y(1), y(2),., y(k +1)之间的递归关系。(b) 通过一个标量框图(不是
3、欠量框图)表示出状态欠量x(k+1)中元素x(k +1)和x2(k +1)估计值的计算过程。(c) 用模拟数据确定状态欠量x(k)的估计值x(k/k), k=0,1,.,10,并画出当k=0, 1,，10 时 xi(k,k)和 X2(k/k)的图。(d) 通常，状态欠量的真实值是得不到得。但为了用作图来说明问题，表P8.1 和P8.2给出来状态欠量元素得值。对丁 k = 0, 1,，10,在同一幅图 中画出真实值和在(c)中确定的X1(k)的估计值。对X2(k)重复这样过程。当k从1变到10时，对每一个元素i = 1, 2,计算并画出各自的误差图， 即 xjk) x(k/k)。(e) 当k从1
4、变到10时，通过用卡尔曼滤波器的状态误差协方差矩阵画出Ek(k/k) 和 E&22(k/k), 而 &1(k/k) =x(k)X1(k,/k),s2(k/k) =x2(k) X2(k/k)。(f) 讨论一下(d)中你计算的误差与(e)中方差之间的关系。表P8.1题8.1到题8.3中的观测值时间下标k观测值y1(k)观测值y2(k)13.296919692.1013429423.387365150.4754079737.028306413.1768889849.712125212.49811140511.420183152.91992424615.978705836.17307616722.06
5、9342855.42519274828.302127813.05365741930.446838315.980511411 038.758755954.51016361表P8.2 题8.1到题8.3中的由模拟得到的实际状态值时间下标k实际状态值x1(k)实际状态值x1(k)00.00000000000.00000000011.654287141.6542871423.503007021.8487198835.9978529242.4755222249.150407403.17187816512.508739103.35833170616.921925944.41318684721.344833
6、524.42290758825.893351444.54851792931.541353305.648001861 036.936056705.394470340实验原理1、卡尔曼滤波简介卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题， 它根据前一 个估计值和最近一个观察数据来估计信号的当前值。它是用状态方程和递推方法 进行估计的，而它的解是以估计值（常常是状态变量的估计值）的形式给出其信 号模型是从状态方程和量测方程得到的。卡尔曼过滤中信号和噪声是用状态方程和测量方程来表示的。 因此设计卡尔 曼滤波器要求已知状态方程和测量方程。 它不需要知道全部过去的数据，采用递 推的方法计算，它既
7、可以用丁平稳和不平稳的随机过程， 同时也可以应用解决非 时变和时变系统，因而它比维纳过滤有更广泛的应用。2、卡尔曼滤波的递推公式AA.A.xk =Akxk，Hk（yk - Ck Ak xk J ）Hk =PQk(CkPkCkRJPk = AkP QPk =（I -HkCQPk3、递推过程的实现如果初始状态Xo的统计特性EXo及varx。已知，并A令x。= Ex。=。AA乂P。= E（x。-x）（x。-x。） = varx。将P。代入式（3）可求得Pl ,将Pl代入式（2）可求得Hl，将此Hl代入式（1）可求得在最小均方误差条件下的Xi,同时将P1代入式（4） 乂可求得Pi;A由R 乂可求P，由
8、P乂可求得H 2 ,由H 2 乂可求得X2 ,同时由H 2与P乂可求得P2；以此类推，这种递推计算方法用计算机计算十分方便。三、MATLABS序%卢尔曼滤波实验程序clc;y1=3.29691969,3.38736515,7.Q283G641,9.71212521,11.42Q18315,15.9787Q583,22.。6934285,28.3。212781,3。.44683831,38.75875595; %观测值 y1(k)y2=2.10134294,0.47540797,3.17688898,2.49811140,2.91992424,6.17307616,5.%均方误差阵赋初值%转移矩
9、阵%系统噪声矩阵%量测矩阵%测量噪声矩阵%状态矩阵赋初值42519274,3.05365741,5.98051141,4.51016361;%观测值 y2(k)%滤波方程3%滤波方程2%观测值%滤波方程1p0=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:10Pk=Ak*p*Ak+Qk;Hk=Pk*Ck*inv(Ck*Pk*Ck+Rk);yk=y1(k);y2(k);xk=Ak*xk+Hk*(yk-Ck*Ak*xk);%记录估计值%滤波方程4%记录状态误差协方差矩阵x1(k
10、)=xk(1);x2(k)=xk (2);p=(eye (2)-Hk*Ck)*Pk; pk(:,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,1.65428714,3.50300702,5.997852924,9.150407
11、40,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.42290758,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
12、),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)for i=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,interpre
13、ter,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)A2f E 2(k/k)A2i=1:10;subplot(2,1,1)plot(i,pk(1,i),r)h= legend(由状态误差协方差矩阵得到的E 1(k/k)A2)set(h,Interpreter,none)subplot(2,1,2)plot(i,pk(2,i),r)h= legend(由状态误差协方差矩阵得到的E 2(k/k)A2)set(h,Interpreter,none)四、实验结果分析(a) 卡尔曼增益矩阵：Hk=RxcTx(CKPk*C+R疽估计值与观测值之间的递归关系为：Xk=AkxXk-1 + HL(Yk-CkXAkxXk-1)(b) 状态欠量估计值的计算框图：(e)通过用卡尔曼滤波器的状态误差协方差矩阵画出的E饥2(k/k)和E防2（必）:(f)分析：(e)中的方差是(d)中的误差平方后取均值，是均方误差。误 差直接由真实值减去估计值，有正有负，而均方误差没有这个缺陷，更能综合的 表示滤波的效果。
