卡尔曼滤波实验报告

实验报告姓名：学号：日期：（以下内容用五号字书写，本页空白不够可续页）

'1

'1

(1)(1/)(1)(1)(1)(1)(1/1)(1)(1)(2)

K k P k k H k S k K k P k k H k R k --⎧+=+++⎪⎨+=++++⎪⎩

（4） 则状态更新方程为：

ˆˆ(1/1)(1/)(1)(1)X

k k X k k K k V k ++=++++ 协方差更新方程为：

(1/1)(1/)(1)(1)(1/)P k k P k k K k H k P k k ++=+-+++ Matlab 程序如下：

clear clc close all N=200; w=randn(2,N); x=zeros(2,N); x(:,1)=[1;1]; a=[ 0;0 ]; 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 ]; 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 ）应注意状态转移矩阵与测量矩阵的选取，不然不容易观察滤波结果。

三、实验分析

2000

4000

6000

800010000

12000

14000

16000

05001000150020002500300035004000X1

X 2

Kalman Filter

题目 ##

姓名：学号：日期：（以下内容用五号字书写，本页空白不够可续页）