第五讲作业

1. 问题描述：

考虑时不变系统：

?10.50.125??0.125?X??X???k?1k?1,kkk?k?,???0.5? 010.5其中?????????Zk?HkXk?vk??1??00??1??量测矩阵H = [1 0 0]，系统噪声方差阵Q = [100]，量测噪声方差阵R = [200]，初始状态估计

= [100 0 0],

= diag{45, 500, 1}。要求编制仿真程序验证

Kalman滤波，要求输出：

1. 以友好的人机交互方式，选择过程噪声为均匀分布或高斯分布 2. 允许滤波终止时间在20-100步之间任意设置 3. 分别输出状态各维的预测方差、估计方差曲线

4. 经过一次仿真，分别输出状态各维的预测误差、估计误差曲线 5. 经过100次仿真，统计出状态各维的预测与估计的样本方差曲线

2. Kalman滤波器原理与算法描述

2.1 Kalman滤波器原理

对于问题1描述的带有量测噪声以及状态噪声的离散时不变系统，设计滤波器满足估计误差

根据正交原理，量测zk+1已知，xk+1在zk+1的正交投影等价于已知b时a的线性 最小方差估计。也就是说，对于所求目标

满足：

量测估计方差和状态量测协方差分别为：

1

状态估计误差

为：

量测预测误差为:

系统增益

其中，状态预测的协方差矩阵为：

由于状态噪声和观测噪声协方差矩阵已知，状态转移矩阵已知，因此系统增益矩阵可以进行离线计算。

2.2 Kalman滤波算法流程与算法描述

为了提高Kalman滤波器的速度，我们采用了离线计算系统增益Kk的方法，这样使得在实时计算滤波过程中的计算负担很小。离线部分算法如下表所示：

Algorithm 1 Kalman滤波增益Kk的离线递推计算

跟据上面的离线计算，我们可以得到每一步的滤波增益矩阵Kk，这样就可以快速的实现Kalman滤波的实时计算。在线部分计算如下：

Algorithm 2 离散系统Kalman滤波估计

2

这样，就实时计算除了K时刻的状态估计值。

3．人机交互界面

根据题目1要求，使用Matlab GUI工具箱，设计人机交互界面，其中使用单选框组“Uniformly Noise”和“Gaussian Noise”选择符合均匀分布或者高斯分布的噪声，使用文本框设置终止时间，按钮“1 Simu”，“Kalman Filter”， “100 Simu”分别控制进行1次仿真，对一次仿真状态进行Kalman滤波估计，进行100次仿真并进行Kalman滤波实验。右侧4图分别记录数据状态维度1，状态维度2，状态维度3和量测的实际值和估计值，其中横坐标表示时间，纵坐标表示数值，实线代表真实值，虚线代表估计值；左侧两图分别记录了1次仿真的预测误差、估计误差曲线和100次仿真的预测误差和估计误差方差的曲线。其中三条实线表示对三个维度的预测误差，三条虚线表示对三个维度的估计误差，总体界面如下图所示：

图 2: 人机交互界面的实现

高斯分布过程噪声下的实验结果 3