卡尔曼滤波的MATLAB实现.docVIP

卡尔曼滤波的MATLAB实现 一、实验内容 一个系统模型为 同时有下列条件： 初始条件已知且有。 是一个标量零均值白高斯序列，且自相关函数已知为。 另外，我们有下列观测模型，即 且有下列条件： 和是独立的零均值白高斯序列，且有 对于所有的j和k，与观测噪声过程和是不相关的，即 我们希望得到由观测矢量，即估计状态矢量的卡尔曼滤波器的公式表示形式，并求解以下问题： 求出卡尔曼增益矩阵，并得出最优估计和观测矢量之间的递归关系。 通过一个标量框图（不是矢量框图）表示出状态矢量中元素和估计值的计算过程。 用模拟数据确定状态矢量的估计值并画出当k＝0，1，…，10时和的图。 通常，状态矢量的真实值是得不到得。但为了用作图来说明问题，表P8.1和P8.2给出来状态矢量元素得值。对于k＝0，1，…，10，在同一幅图中画出真实值和在（c）中确定的的估计值。对重复这样过程。当k从1变到10时，对每一个元素i＝1，2，计算并画出各自的误差图，即。 当k从1变到10时，通过用卡尔曼滤波器的状态误差协方差矩阵画出和，而，。 讨论一下（d）中你计算的误差与（e）中方差之间的关系。 二、实验原理 1、卡尔曼滤波简介 卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题，它根据前一个估计值和最近一个观察数据来估计信号的当前值。它是用状态方程和递推方法进行估计的，而它的解是以估计值（常常是状态变量的估计值）的形式给出其信号模型是从状态方程和量测方程得到的。 卡尔曼过滤中信号和噪声是用状态方程和测量方程来表示的。因此设计卡尔曼滤波器要求已知状态方程和测量方程。它不需要知道全部过去的数据，采用递推的方法计算，它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统，因而它比维纳过滤有更广泛的应用。 2、卡尔曼滤波的递推公式 ………(1) ………(2) ………(3) ………(4) 3、递推过程的实现 如果初始状态的统计特性及已知，并 令 又 将代入式（3）可求得，将代入式（2）可求得，将此代入式（1）可求得在最小均方误差条件下的，同时将代入式（4）又可求得；由又可求，由又可求得，由又可求得，同时由与又可求得……；以此类推，这种递推计算方法用计算机计算十分方便。 三、MATLAB程序 %卡尔曼滤波实验程序 clc; y1=[3379111522283038; %观测值y1(k) y2=[2032265354; %观测值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,inter