卡尔曼滤波的MATLAB实现

卡尔曼滤波的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(1kv和)1(2kv是独立的零均值白高斯序列，且有,2,1,0,2)]()([,)]()([2211kkvjvEkvjvEjkjk（4）对于所有的j和k，)(kw与观测噪声过程)1(1kv和)1(2kv是不相关的，即,2,1,0,,2,1,0,0)]()([,0)]()([21kjkvjwEkvjwE我们希望得到由观测矢量)1(ky，即Tkykyky)]1(),1([)1(21估计状态矢量Tkxkxkx)]1(),1([)1(21的卡尔曼滤波器的公式表示形式，并求解以下问题：（a）求出卡尔曼增益矩阵，并得出最优估计)1(kx和观测矢量)1(),...,2(),1(kyyy之间的递归关系。（b）通过一个标量框图（不是矢量框图）表示出状态矢量)1(kx中元素)1(1kx和)1(2kx估计值的计算过程。（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，计算并画出各自的误差图，即)()(kkxkxii。（e）当k从1变到10时，通过用卡尔曼滤波器的状态误差协方差矩阵画出)]([21kkE和)]([22kkE，而)()()(111kkxkxkk，)()()(222kkxkxkk。（f）讨论一下（d）中你计算的误差与（e）中方差之间的关系。表P8.1题8.1到题8.3中的观测值时间下标k观测值)(1ky)(2ky观测值123456789103.296919693.387365157.028306419.7121252111.4201831515.9787058322.0693428528.3021278130.4468383138.758755952.101342940.475407973.176888982.498111402.919924246.173076165.425192743.053657415.980511414.51016361表P8.2题8.1到题8.3中的由模拟得到的实际状态值时间下标k实际状态值)(1kx实际状态值)(1kx0123456789100.00000000001.654287143.503007025.9978529249.1504074012.5087391016.9219259421.3448335225.8933514431.5413533036.936056700.0000000001.654287141.848719882.475522223.171878163.358331704.413186844.422907584.548517925.648001865.394470340二、实验原理1、卡尔曼滤波简介卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题，它根据前一个估计值和最近一个观察数据来估计信号的当前值。它是用状态方程和递推方法进行估计的，而它的解是以估计值（常常是状态变量的估计值）的形式给出其信号模型是从状态方程和量测方程得到的。卡尔曼过滤中信号和噪声是用状态方程和测量方程来表示的。因此设计卡尔曼滤波器要求已知状态方程和测量方程。它不需要知道全部过去的数据，采用递推的方法计算，它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统，因而它比维纳过滤有更广泛的应用。2、卡尔曼滤波的递推公式)(11kkkkkkkkxACyHxAx………(1)1)(kkkkkkkRCPCCPH………(2)11kkkkkQAPAP………(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……；以此类推，这种递推计算方法用计算机计算十分方便。三、MATLAB程序%卡尔曼滤波实验程序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;%状态矩阵赋初值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,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.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')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)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')四、实验结果分析（a）卡尔曼增益矩阵：kkkTT1HPC(CPCR)’’估计值与观测值之间的递归关系为：kk-1k-1kkkkkXAXH(YCAX)（b）状态矢量估计值的计算框图：（c）)(1kkx和)(2kkx的图：+1kH+1Z1kA1kC1ky1ˆkx1ˆkx1'ˆkx（d）真实值与估计值的比较图：各自的误差图：（e）通过用卡尔曼滤波器的状态误差协方差矩阵画出的)]([21kkE和)]([22kkE：（f）分析：（e）中的方差是（d）中的误差平方后取均值，是均方误差。误差直接由真实值减去估计值，有正有负，而均方误差没有这个缺陷，更能综合的表示滤波的效果。