卡尔曼滤波实验及matlab实现

1、实验一 卡尔曼滤波一、 实验目的1、了解卡尔曼滤波的准则和信号模型，以及卡尔曼滤波的应用。 2、熟练掌握卡尔曼滤波的递推过程，提高对信号进行处理的能力。 3、分析讨论实际状态值和估计值的误差。二、实验原理1、卡尔曼滤波简介卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题，它根据前一个估计值和最近一个观察数据来估计信号的当前值。它是用状态方程和递推方法进行估计的，而它的解是以估计值（常常是状态变量的估计值）的形式给出其信号模型是从状态方程和量测方程得到的。卡尔曼过滤中信号和噪声是用状态方程和测量方程来表示的。因此设计卡尔曼滤波器要求已知状态方程和测量方程。它不需要知道全部过去的数据，采用递

2、推的方法计算，它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统，因而它比维纳过滤有更广泛的应用。2、卡尔曼滤波的递推公式x k =A k x k -1+H k (y k -C k A k x k -1(1H k =P k C k (C k P k C k +R k -1(2 P k =A k P k -1A k +Q k -1(3P k =(I -H k C k P k (4 3、递推过程的实现如果初始状态x 0的统计特性E x 0及varx 0已知，并 令 又x 0=E x 0=0P 0=E (x 0-x 0(x 0-x 0 =varx 0将P 0代入式（3）可求得P

3、1，将P 1代入式（2）可求得H 1，将此H 1代入式1；（1）可求得在最小均方误差条件下的x 1，同时将P 1代入式（4）又可求得P2，由P 2又可求得H 2，由H 2又可求得x 2，同时由H 2与P 2又可求1又可求P 由P得P 2；以此类推，这种递推计算方法用计算机计算十分方便。三、实验器材1、计算机一台2、MATLAB 软件一套四、实验内容一个系统模型为 同时有下列条件：（1） 初始条件已知且有x (0 =0, 0T 。（2） w (k 是一个标量零均值白高斯序列，且自相关函数已知为x 1(k +1 =x 1(k +x 2(k +w (k , k =0, 1, x 2(k +1 =x

4、2(k +w (k E w (j w (k =jk 。另外，我们有下列观测模型，即 且有下列条件：（3） v 1(k +1 和v 2(k +1 是独立的零均值白高斯序列，且有 E v 1(j v 1(k =jk ,y 1(k +1 =x 1(k +1 +v 1(k +1, k =0, 1, y 2(k +1 =x 2(k +1 +v 2(k +1E v 2(j v 2(k =2jk , k =0, 1, 2,（4） 对于所有的j 和k ，w (k 与观测噪声过程v 1(k +1 和v 2(k +1 是不相关的，即E w (j v 1(k =0,E w (j v 2(k =0, j =0, 1,

5、 2, , k =0, 1, 2,我们希望得到由观测矢量y (k +1 ，即y (k +1 =y 1(k +1, y 2(k +1T 估计状态矢量x (k +1 =x 1(k +1, x 2(k +1T 的卡尔曼滤波器的公式表示形式，并求解以下问题：（a ） 求出卡尔曼增益矩阵，并得出最优估计x (k +1 和观测矢量y (1, y (2,., y (k +1 之间的递归关系。（b ） 通过一个标量框图（不是矢量框图）表示出状态矢量x (k +1 中元素x 1(k +1 和x 2(k +1 估计值的计算过程。10, 并画出当k （c ） 用模拟数据确定状态矢量x (k 的估计值x (k k ,

6、 k =0, 1,.,0，1，10时x 1(k k 和x 2(k k 的图。（d ） 通常，状态矢量的真实值是得不到得。但为了用作图来说明问题，表P8.1和P8.2给出来状态矢量元素得值。对于k 0，1，10，在同一幅图中画出真实值和在（c ）中确定的x 1(k 的估计值。对x 2(k 重复这样过程。当k 从1变到10时，对每一个元素i 1，2，计算并画出各自的误差图，即x i (k -x i (k k 。（e ） 当k 从1变到10时，通过用卡尔曼滤波器的状态误差协方差矩阵画出E 1(k k 2和E 2(k k 2，而1(k k =x 1(k -x 1(k k ，2(k k =x 2(k -

7、x 2(k k 。（f ） 讨论一下（d ）中你计算的误差与（e ）中方差之间的关系。表P8.1 题8.1到题8.3中的观测值时间下标k 1 2345678910观测值y 1(k 3.296919693.387365157.028306419.7121252111.4201831515.9787058322.0693428528.3021278130.4468383138.75875595观测值y 2(k 2.101342940.475407973.176888982.498111402.919924246.173076165.425192743.053657415.980511414.510

8、16361表P8.2 题8.1到题8.3中的由模拟得到的实际状态值时间下标k实际状态值x 1(k 实际状态值x 1(k 0123456789100.00000000001.654287143.503007025.9978529249.1504074012.5087391016.9219259421.3448335225.8933514431.5413533036.936056700.0000000001.654287141.848719882.475522223.171878163.358331704.413186844.422907584.548517925.648001865.394470

9、340五、实验结果分析C T (CP k C T +R -1 （a ）卡尔曼增益矩阵：H k =P k 估计值与观测值之间的递归关系为：X k =A k X k-1+H k (Yk -C k A k X k-1（b ）状态矢量估计值的计算框图：k +1 x（c ）x 1(k k 和x 2(k k 的图： （d ）真实值与估计值的比较图： 各自的误差图： （e ）通过用卡尔曼滤波器的状态误差协方差矩阵画出的E 1(k k 和2E 2(k k ：2（f）分析： （e）中的方差是（d）中的误差平方后取均值，是均方误差。误 差直接由真实值减去估计值，有正有负，而均方误差没有这个缺陷，更能综合的 表示滤

10、波的效果。 附程序： %卡尔曼滤波实验程序 clc; y1=3.29691969,3.38736515,7.02830641,9.71212521,11.42018315,15.97870583 ,22.06934285,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;

11、%均方误差阵赋初值 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:10 Pk=Ak*p*Ak+Qk; %滤波方程 3 Hk=Pk*Ck*inv(Ck*Pk*Ck+Rk; %滤波方程 2 yk=y1(k;y2(k; %观测值 xk=Ak*xk+Hk*(yk-Ck*Ak*xk; %滤波方程 1 x1(k=xk(1; x2(k=xk(2; %记录估计值 p=(eye(2-Hk*Ck*Pk; %滤波方程 4 pk(:,k=p(1,1,p

12、(2,2; %记录状态误差协方差矩阵 end figure %画图表示状态矢量的估计值 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.15040740,12.50873910,16.9219 2594,21.34483352,25.89335144,31.

13、54135330,36.93605670; %由模拟得到的 实际状态值 X1(k X2=0,1.65428714,1.84871988,2.47552222,3.17187816,3.35833170,4.4131868 4,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,b h=legend(x1(k的估计值,x1(k的真实值 set(h,interpreter,n

14、one 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; end figure %画出误差图 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 fi