卡尔曼滤波实验及matlab实现.pdf

实验一卡尔曼滤波一、实验目的1、了解卡尔曼滤波的准则和信号模型，以及卡尔曼滤波的应用 2、熟练掌握卡尔曼滤波的递推过程，提高对信号进行处理的能力 3、分析讨论实际状态值和估计值的误差二、实验原理1、卡尔曼滤波简介 卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题，它根据前一 个估计值和最近一个观察数据来估计信号的当前值它是用状态方程和递推方法 进行估计的， 而它的解是以估计值 （常常是状态变量的估计值）的形式给出其信 号模型是从状态方程和量测方程得到的 卡尔曼过滤中信号和噪声是用状态方程和测量方程来表示的因此设计卡尔 曼滤波器要求已知状态方程和测量方程它不需要知道全部过去的数据， 采用递 推的方法计算， 它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非 时变和时变系统，因而它比维纳过滤有更广泛的应用 2、卡尔曼滤波的递推公式)(11kkkkkkkkxACyHxAx⋯⋯⋯(1) 1)(kkkkkkkRCPCCPH⋯⋯⋯(2)11kkkkkQAPAP⋯⋯⋯(3)kkkkPCHIP)(⋯⋯⋯(4)3、递推过程的实现如果初始状态0x的统计特性][0xE及]var[0x已知，并令000][xEx又]var[]))([(000000xxxxxEP将0P代入式（ 3）可求得1P，将1P代入式（ 2）可求得1H，将此1H代入式（1）可求得在最小均方误差条件下的1x，同时将1P代入式（ 4）又可求得1P；由1P又可求2P，由2P又可求得2H，由2H又可求得2x，同时由2H与2P又可求得2P⋯⋯；以此类推，这种递推计算方法用计算机计算十分方便。

三、实验器材1、计算机一台 2、MATLAB 软件一套四、实验内容一个系统模型为)()()1(, 1, 0),()()()1(22211kwkxkxkkwkxkxkx同时有下列条件：（1） 初始条件已知且有Tx]0, 0[)0(2）)(kw是 一 个 标 量 零 均 值 白 高 斯 序 列 ， 且 自 相 关 函 数 已 知 为jkkwjwE)]()([另外，我们有下列观测模型，即) 1()1() 1(, 1,0),1() 1()1(222111 kvkxkykkvkxky且有下列条件：（3）) 1(1kv和) 1(2kv是独立的零均值白高斯序列，且有, 2, 1, 0,2)]()([,)]()([2211kkvjvEkvjvEjkjk（4） 对于所有的 j 和 k，)(kw与观测噪声过程)1(1kv和) 1(2kv是不相关的，即, 2, 1, 0,,2, 1,0,0)]()([,0)]()([21kjkvjwEkvjwE我们希望得到由观测矢量)1(ky，即Tkykyky)]1(),1([)1(21估计状态矢量Tkxkxkx)]1(),1([) 1(21的卡尔曼滤波器的公式表示形式，并求解以下问题：（a） 求 出 卡 尔 曼 增 益 矩 阵 ， 并 得 出 最 优 估 计)1(kx和 观 测 矢 量)1(),...,2(),1 (kyyy之间的递归关系。

b） 通过一个标量框图（不是矢量框图）表示出状态矢量)1(kx中元素) 1(1kx和)1(2kx估计值的计算过程c） 用模拟数据确定状态矢量)(kx的估计值,10,...,1 ,0),(kkkx并画出当k＝0，1，⋯， 10时)(1kkx和)(2kkx的图d） 通常，状态矢量的真实值是得不到得 但为了用作图来说明问题， 表 P8.1 和 P8.2 给出来状态矢量元素得值对于k＝0，1，⋯， 10，在同一幅图中画出真实值和在（ c）中确定的)(1kx的估计值对)(2kx重复这样过程当 k 从 1 变到 10 时，对每一个元素 i ＝1，2，计算并画出各自的误差图，即)()(kkxkxiie） 当 k 从 1 变到 10 时，通过用卡尔曼滤波器的状态误差协方差矩阵画出)]([21kkE和)]([22kkE，而)()()(111kkxkxkk，)()()(222kkxkxkkf ） 讨论一下（ d）中你计算的误差与（ e）中方差之间的关系表P8.1 题8.1到题 8.3中的观测值时间下标 k观测值)(1ky)(2ky观测值1 2 3 4 5 6 7 8 9 103.29691969 3.38736515 7.02830641 9.71212521 11.42018315 15.97870583 22.06934285 28.30212781 30.44683831 38.758755952.10134294 0.47540797 3.17688898 2.49811140 2.91992424 6.17307616 5.42519274 3.05365741 5.98051141 4.51016361表P8.2 题8.1到题8.3中的由模拟得到的实际状态值时间下标k实际状态值)(1kx实际状态值)(1kx0 1 2 3 4 5 6 7 8 9 1 00.0000000000 1.65428714 3.50300702 5.997852924 9.15040740 12.50873910 16.92192594 21.34483352 25.89335144 31.54135330 36.936056700.000000000 1.65428714 1.84871988 2.47552222 3.17187816 3.35833170 4.41318684 4.42290758 4.54851792 5.64800186 5.394470340五、实验结果分析（a）卡尔曼增益矩阵：kkkTT1HPC(CPCR)’’估计值与观测值之间的递归关系为：kk-1k-1kkkkkXAXH(YCAX)（b）状态矢量估计值的计算框图：（c）)(1kkx和)(2kkx的图：+ 1kH+ 1Z1kA1kC1ky1?kx1?kx1'?kx（d）真实值与估计值的比较图：各自的误差图：（e）通过用卡尔曼滤波器的状态误差协方差矩阵画出的)]([2 1kkE和)]([2 2kkE：（f ）分析： （e）中的方差是（ d）中的误差平方后取均值，是均方误差。

误 差直接由真实值减去估计值， 有正有负， 而均方误差没有这个缺陷， 更能综合的 表示滤波的效果附程序： % 卡尔曼滤波实验程序 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; %均方误差阵赋初值 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(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.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','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); 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') figure % 通 过 用 卡 尔 曼 滤 波 器 的 状 态 误 差 协 方 差 矩 阵 画 出 E[ε1(k/k)^2] 和 E[ε2(k/k)^2] i=1:10; subplot(2,1,1) plot(i,pk(1,i),'r') h= legend(' 由状态误差协方差矩阵得到的E[ε1(k/k)^2]' ) set(h,'Interpreter','none') subplot(2,1,2) plot(i,pk(2,i),'r') h= legend('由状态误差协方差矩阵得到的E[ε2(k/k)^2]') set(h,'Interpreter','none') 。