卡尔曼滤波实验报告..doc

实验：卡尔曼滤波实现 实验报告 姓名：dkj,其中dkj为狄利克雷函数dkj。 设k时刻的滤波值为： ， 对应的协方差矩阵为： 滤波过程可表示如下： 求出状态的一步预测和协方差的一步预测，分别为： 求出量测的预测值与量测预测协方差： 新增加量测Z(k+1),计算信息与增益： 则状态更新方程为： 协方差更新方程为： Matlab程序如下： clear clc close all N=200; w=randn(2,N); x=zeros(2,N); x(:,1)=[1;1]; a=[1.1 0;0 1.02]; for k=2:N; x(:,k)=a*x(:,k-1)+w(k-1); end V=randn(2,N); Rvv=cov(V); Rww=cov(w); c=[1 0;0 0.9]; Y=c*x+V; p=zeros(2,2*N); s=zeros(2,N); b=zeros(2,2*N); x1=zeros(2,N); z1=x1; zp1=b; p(:,1:2)=[0 0;0 0]; s(:,1)=[4;8]; for t=2:N; x1(:,t-1)=a*s(:,t-1)+w(:,t-1);%X(k/k-1) p1(:,t*2-1:t*2)=a*p(:,t*2-3:t*2-2)*a+Rww;%X(k|k-1)的协方差 z1(:,t-1)=c*x1(:,t-1);%测量估计 zp1(:,t*2-1:t*2)=c*p1(:,t*2-1:t*2)*c+Rvv;%测量估计协方差 b(:,t*2-1:t*2)=p1(:,t*2-1:t*2)*c/zp1(:,t*2-1:t*2);%Kg 卡尔曼增益 s(:,t)=x1(:,t-1)+b(:,t*2-1:t*2)*(Y(:,t)-z1(:,t-1));%当前状态的最优结果 p(:,t*2-1:t*2)=p1(:,t*2-1:t*2)-b(:,t*2-1:t*2)*c*p1(:,t*2-1:t*2);%更新当前状态估计值的协方差 end t=1:N; plot(s(1,:),s(2,:),ro,Y(1,:),Y(2,:),g.,x(1,:),x(2,:),b*); xlabel(time); ylabel(Amplitude); title(Kalman Filter) legend(kalman滤波估计值,测量值,真实值,4) grid on 二、实验过程 （1） 分析卡尔曼滤波算法原理，总结卡尔曼滤波算法流程 （2） 在第一步的基础上编写matlab程序 （3） 设计状态方程及测量方程，确定初值进行滤波。 实验过程中遇到的问题：(a)矩阵的维数没有对应好，matlab报错，在搞清楚各个变量的物理意义并进行公示推算后终于调整好了所有的矩阵及向量维数；（b）维数调整好后程序可以运行，但结果很不稳定，滤波值也没能很好地跟踪真实值，期初以为是状态转移矩阵与测量矩阵选取的问题，后来发现是在计算协方差是少求了转置；（c）应注意状态转移矩阵与测量矩阵的选取，不然不容易观察滤波结果。 三、实验分析 上图中蓝色星号代表状态真实值，绿色圆点代表测量值，红色圆圈代表卡尔曼滤波结果，卡尔曼滤波初值选择的偏差很大，但从实验结果可以看出随后的滤波值可以很好地跟踪了真实值。 四、实验结论 线性卡尔曼滤波即便在初值选择偏差很大的情况下也可以及时准确地跟踪线性系统的状态真实值。 指导教师评语 指导老师签名： 年 月 日 学号 姓名 成绩 题目 ## 姓名：………………………………… 二、引言 三、研究内容及分析 四、结论