卡尔曼滤波实验及matlab实现

实验一 卡尔曼滤波

一、 实验目的

1、了解卡尔曼滤波的准则和信号模型，以及卡尔曼滤波的应用。

2、熟练掌握卡尔曼滤波的递推过程，提高对信号进行处理的能力。

3、分析讨论实际状态值和估计值的误差。

二、实验原理

1、卡尔曼滤波简介

卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题，它根据前一个估计值和最近一个观察数据来估计信号的当前值。它是用状态方程和递推方法进行估计的，而它的解是以估计值（常常是状态变量的估计值）的形式给出其信号模型是从状态方程和量测方程得到的。

卡尔曼过滤中信号和噪声是用状态方程和测量方程来表示的。因此设计卡尔曼滤波器要求已知状态方程和测量方程。它不需要知道全部过去的数据，采用递推的方法计算，它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统，因而它比维纳过滤有更广泛的应用。

2、卡尔曼滤波的递推公式

)

(11∧

-∧

-∧

-+=k k k k k k k k x A C y H x A x (1)

1)(-+''=k k k k k k k R C P C C P H τ

τ (2)

1

1--+='k k k k k Q A P A P τ (3)

k k k k P C H I P '

-=)( (4)

3、递推过程的实现 如果初始状态

x 的统计特性

]

[0x E 及]

var[0x 已知，并

令 0

00][μ==∧

x E x

又 ]

var[]))([(000000x x x x x E P =--=∧

∧

τ

将

P 代入式（3）可求得1P '，将1P '代入式（2）可求得1H ，将此1H 代入式

（1）可求得在最小均方误差条件下的∧

1x ，同时将1P '代入式（4）又可求得1P ；由1P 又可求2P '，由2P '又可求得2H ，由2H 又可求得∧

2x ，同时由2H 与2P '又可求得2P ……；以此类推，这种递推计算方法用计算机计算十分方便。

三、实验器材

1、计算机一台

2、MATLAB 软件一套

四、实验内容

一个系统模型为 )

()()1(,1,0),()()()1(22211k w k x k x k k w k x k x k x +=+=++=+

同时有下列条件：

（1） 初始条件已知且有T x ]0,0[)0(=。

（2） )(k w 是一个标量零均值白高斯序列，且自相关函数已知为

jk k w j w E δ=)]()([。

另外，我们有下列观测模型，即 )

1()1()1(,1,0),1()1()1(222111+++=+=+++=+k v k x k y k k v k x k y

且有下列条件：

（3） )1(1+k v 和)1(2+k v 是独立的零均值白高斯序列，且有 ,2,1,0,2)]()([,

)]()([2211===k k v j v E k v j v E jk jk δδ

（4） 对于所有的j 和k ，)(k w 与观测噪声过程)1(1+k v 和)1(2+k v 是不相关的，

即

,2,1,0,,2,1,0,0)]()([,

0)]()([21====k j k v j w E k v j w E

我们希望得到由观测矢量)1(+k y ，即T k y k y k y )]1(),1([)1(21++=+估计状态矢量T k x k x k x )]1(),1([)1(21++=+的卡尔曼滤波器的公式表示形式，并求解以下问题：

（a ） 求出卡尔曼增益矩阵，并得出最优估计)1(+k x 和观测矢量

)1(),...,2(),1(+k y y y 之间的递归关系。

（b ） 通过一个标量框图（不是矢量框图）表示出状态矢量)1(+k x 中元素

)1(1+k x 和)1(2+k x 估计值的计算过程。

（c ） 用模拟数据确定状态矢量)(k x 的估计值,10,...,

1,0),(=∧

k k k x 并画出当k ＝

0，1，…，10时)(1k k x ∧和)(2k k x ∧

的图。

（d ） 通常，状态矢量的真实值是得不到得。但为了用作图来说明问题，表和给

出来状态矢量元素得值。对于k ＝0，1，…，10，在同一幅图中画出真实

值和在（c ）中确定的)(1k x 的估计值。对)(2k x 重复这样过程。当k 从1变到10时，对每一个元素i ＝1，2，计算并画出各自的误差图，即

)()(k k x k x i i ∧

-。

（e ） 当k 从1变到10时，通过用卡尔曼滤波器的状态误差协方差矩阵画出

)]

([2

1k k E ε和

)]

([2

2k k E ε，而

)

()()(111k k x k x k k ∧

-=ε，

)()()(222k k x k x k k ∧

-=ε。

（f ） 讨论一下（d ）中你计算的误差与（e ）中方差之间的关系。

表P8.1 题8.1到题8.3中的观测值

时间下标k 观测值)(1k y )(2k y 观测值1 2345678910

3.296919693.387365157.028306419.7121252111.4201831515.9787058322.0693428528.3021278130.4468383138.75875595

2.101342940.47540797

3.176888982.498111402.919924246.173076165.425192743.053657415.98051141

4.51016361

表P8.2 题8.1到题8.3中的由模拟得到的实际状态值时间下标k

实际状态值)

(1k x 实际状态值)

(1k x 0123456789100.00000000001.654287143.503007025.9978529249.1504074012.5087391016.9219259421.3448335225.8933514431.5413533036.936056700.0000000001.654287141.848719882.475522223.171878163.358331704.413186844.422907584.548517925.648001865.394470340