卡尔曼滤波

卡尔曼滤波 一、实验内容 一个系统模型为 )()1( ,10,22kwxk 同时有下列条件： （1） 初始条件已知且有 。T0,)( （2） 是一个标量零均值白高斯序列，且自相关函数已知为)(kw 。jkjE 另外，我们有下列观测模型，即 )1()()1( ,0,222 kvxky 且有下列条件： （3） 和 是独立的零均值白高斯序列，且有)1(kv)(2 ,210,2)(,2kkvjEkvjEjjk （4） 对于所有的 j 和 k， 与观测噪声过程 和 是不相关的，)(w)1)(v 即 ,210,2,0,)(,0)( 21 kjkvjEvjE 我们希望得到由观测矢量 ，即 估计状态矢)ky Tyy)(),1( 量 的卡尔曼滤波器的公式表示形式，并求解以下问Txkx(),()(21 题： （a） 求出卡尔曼增益矩阵，并得出最优估计 和观测矢量)1(kx 之间的递归关系。)1(),.21(kyy （b） 通过一个标量框图（不是矢量框图）表示出状态矢量 中元素)1(kx 和 估计值的计算过程。)(1kx)(2k （c） 用模拟数据确定状态矢量 的估计值 并画出当)(kx,10.,)(kx k0，1，10 时 和 的图。)(1kx)(2 （d） 通常，状态矢量的真实值是得不到得。但为了用作图来说明问题，表 P8.1 和 P8.2 给出来状态矢量元素得值。对于 k0，1，10，在同一幅图 中画出真实值和在（c）中确定的 的估计值。对 重复这样过程。)(1x)(2kx 当 k 从 1 变到 10 时，对每一个元素 i1，2，计算并画出各自的误差图， 即 。)()kxii （e） 当 k 从 1 变到 10 时，通过用卡尔曼滤波器的状态误差协方差矩阵画出 和 ，而 ，)(2E)(2 )()(11kxk 。222kxk （f） 讨论一下（d）中你计算的误差与（e）中方差之间的关系。 表 P 8 . 1 题 8 . 1 到 题 8 . 3 中 的 观 测 值 时间下标 k 观测值 )(ky)(2ky观测值 1 2 3 4 5 6 7 8 9 1 0 3 . 2 9 6 9 1 9 6 9 3 . 3 8 7 3 6 5 1 5 7 . 0 2 8 3 0 6 4 1 9 . 7 1 2 1 2 5 2 1 1 1 . 4 2 0 1 8 3 1 5 1 5 . 9 7 8 7 0 5 8 3 2 2 . 0 6 9 3 4 2 8 5 2 8 . 3 0 2 1 2 7 8 1 3 0 . 4 4 6 8 3 8 3 1 3 8 . 7 5 8 7 5 5 9 5 2 . 1 0 1 3 4 2 9 4 0 . 4 7 5 4 0 7 9 7 3 . 1 7 6 8 8 8 9 8 2 . 4 9 8 1 1 1 4 0 2 . 9 1 9 9 2 4 2 4 6 . 1 7 3 0 7 6 1 6 5 . 4 2 5 1 9 2 7 4 3 . 0 5 3 6 5 7 4 1 5 . 9 8 0 5 1 1 4 1 4 . 5 1 0 1 6 3 6 1 表 P 8 . 2 题 8 . 1 到 题 8 . 3 中 的 由 模 拟 得 到 的 实 际 状 态 值 时间下标k 实际状态值 )(1kx实际状态值 )(1kx 0 1 2 3 4 5 6 7 8 9 1 0 0 . 0 0 0 0 0 0 0 0 0 0 1 . 6 5 4 2 8 7 1 4 3 . 5 0 3 0 0 7 0 2 5 . 9 9 7 8 5 2 9 2 4 9 . 1 5 0 4 0 7 4 0 1 2 . 5 0 8 7 3 9 1 0 1 6 . 9 2 1 9 2 5 9 4 2 1 . 3 4 4 8 3 3 5 2 2 5 . 8 9 3 3 5 1 4 4 3 1 . 5 4 1 3 5 3 3 0 3 6 . 9 3 6 0 5 6 7 0 0 . 0 0 0 0 0 0 0 0 0 1 . 6 5 4 2 8 7 1 4 1 . 8 4 8 7 1 9 8 8 2 . 4 7 5 5 2 2 2 2 3 . 1 7 1 8 7 8 1 6 3 . 3 5 8 3 3 1 7 0 4 . 4 1 3 1 8 6 8 4 4 . 4 2 2 9 0 7 5 8 4 . 5 4 8 5 1 7 9 2 5 . 6 4 8 0 0 1 8 6 5 . 3 9 4 4 7 0 3 4 0 二、实验原理 1、卡尔曼滤波简介 卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题，它根据前 一个估计值和最近一个观察数据来估计信号的当前值。它是用状态方程和递推 方法进行估计的，而它的解是以估计值（常常是状态变量的估计值）的形式给 出其信号模型是从状态方程和量测方程得到的。 卡尔曼过滤中信号和噪声是用状态方程和测量方程来表示的。因此设计卡 尔曼滤波器要求已知状态方程和测量方程。它不需要知道全部过去的数据，采 用递推的方法计算，它既可以用于平稳和不平稳的随机过程，同时也可以应用 解决非时变和时变系统，因而它比维纳过滤有更广泛的应用。 2、卡尔曼滤波的递推公式 (1) )(11 kkkk xACyHxA (2)RP (3)11kkkQ (4)CI)( 3、递推过程的实现 如果初始状态 的统计特性 及 已知，并0x0xEvar0 令 又 var)( 0000 xxxP 将 代入式（3）可求得 ，将 代入式（2）可求得 ，将此 代入式0P11 1H1 （1）可求得在最小均方误差条件下的 ，同时将 代入式（4）又可求得 ； x1P P 由 又可求 ，由 又可求得 ，由 又可求得 ，同时由 与 又可求222H2 2x2 得 ；以此类推，这种递推计算方法用计算机计算十分方便。2P 三、MATLAB 程序 %卡尔曼滤波实验程序 clc; y1=3.29691969,3.38736515,7.02830641,9.71212521,11.42018315,15.9787058 3,22.06934285,28.30212781,30.44683831,38.75875595; %观测值 y1(k) y2=2.10134294,0.47540797,3.17688898,2.49811140,2.91992424,6.17307616,5.4 2519274,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 %通过用卡尔曼滤波器的状态误差协方差矩阵画出 E1(k/k) 2和 E2(k/k)2 i=1:10; subplot(2,1,1) plot(i,pk(1,i),r) h= legend(由状态误差协方差矩阵得到的 E1(k/k)2) set(h,Interpreter,none) subplot(2,1,2) plot(i,pk(2,i),r) h= legend(由状态误差协方差矩阵得到的 E2(k/k)2) set(h,Interpreter,none) 四、实验结果分析 （a）卡尔曼增益矩阵： k kTT1HPC(PCR) 估计值与观测值之间的递归关系为： kk-1 k-