2022年2022年卡尔曼滤波实验报告 .pdf
实验：卡尔曼滤波实现实验报告姓名：学号：日期：（以下内容用五号字书写，本页空白不够可续页）一、实验理论及程序卡尔曼滤波是一种递推的在最小均方误差下求得的一步预测估计，估计结果对真实值是收敛的，具体可分为滤波、平滑和预测三种。对一离散时间线性动态系统，若其状态方程满足：X(k+1)=(k)X(k)+(k)u(k)+G(k)V(k)其中 X(k)为 n 维向量，表示 k 时刻的状态向量，(k)为 k 时刻 n*n 阶的状态转移矩阵，u(k)为已知 p 维输入或控制信号，(k)为 n*p 阶输入矩阵，G(k)为 n*n维过程噪声分布矩阵，V(k)为 n维过程噪声，为高斯白噪声，且满足EV(k)=0,EV(k)V(k)T=Q(k)dkj,其中 dkj为狄利克雷函数。测量方程满足：Z(k)=H(k)X(k)+W(k)其中 Z(k)为 m 为向量，表示 k 时刻的测量向量，H(k)为 m*n 阶测量矩阵，W(k)是 m 维测量噪声，为高斯白噪声，且EW(k)=0,EW(k)W(k)T=R(k)dkj。设 k 时刻的滤波值为：?(/)()/,(),1,2,.,kkX kkE X kZZZ jjk，对应的协方差矩阵为：?(/)()(/)()(/)/TkP k kEX kX kkX kX kkZ滤波过程可表示如下：（1）求出状态的一步预测和协方差的一步预测，分别为：?(1/)=()(/)()()X kkk X kkk U k(1/)=()(/)()()()()P kkk P kkkG k Q k G k（2）求出量测的预测值与量测预测协方差：?(1/)(1)/=(1)(1/)kZ kkE Z kZH kX kk(1)=(1)(1/)(1)(1)S kH kP kk HkR k（3）新增加量测Z(k+1),计算信息与增益：?(1)(1)(1/)V kZ kZ kk名师资料总结-精品资料欢迎下载-名师精心整理-第 1 页，共 7 页 -11(1)(1/)(1)(1)(1)(1)(1/1)(1)(1)(2)K kP kk HkSkK kP kkHkRk（4）则状态更新方程为：?(1/1)(1/)(1)(1)X kkX kkK kV k协方差更新方程为：(1/1)(1/)(1)(1)(1/)P kkP kkK kH kP kkMatlab 程序如下：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);%测量估计名师资料总结-精品资料欢迎下载-名师精心整理-第 2 页，共 7 页 -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）应注意状态转移矩阵与测量矩阵的选取，不然不容易观察滤波结果。三、实验分析020004000600080001000012000140001600005001000150020002500300035004000X1X2Kalman Filterkalman 滤波估计值测量值真实值名师资料总结-精品资料欢迎下载-名师精心整理-第 3 页，共 7 页 -上图中蓝色星号代表状态真实值，绿色圆点代表测量值，红色圆圈代表卡尔曼滤波结果，卡尔曼滤波初值选择的偏差很大，但从实验结果可以看出随后的滤波值可以很好地跟踪了真实值。四、实验结论线性卡尔曼滤波即便在初值选择偏差很大的情况下也可以及时准确地跟踪线性系统的状态真实值。指导教师评语名师资料总结-精品资料欢迎下载-名师精心整理-第 4 页，共 7 页 -指导老师签名：年月日名师资料总结-精品资料欢迎下载-名师精心整理-第 5 页，共 7 页 -学号姓名成绩名师资料总结-精品资料欢迎下载-名师精心整理-第 6 页，共 7 页 -题目#姓名：学号：日期：（以下内容用五号字书写，本页空白不够可续页）一、摘要二、引言三、研究内容及分析四、结论名师资料总结-精品资料欢迎下载-名师精心整理-第 7 页，共 7 页 -
