最优估计作业4.docVIP

第五讲作业 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的线性 最小方差估计。也就是说，对于所求目标 满足： 量测估计方差和状态量测协方差分别为： 状态估计误差为： 量测预测误差为: 系统增益 其中，状态预测的协方差矩阵为： 由于状态噪声和观测噪声协方差矩阵已知，状态转移矩阵已知，因此系统增益矩阵可以进行离线计算。 2.2 Kalman滤波算法流程与算法描述 为了提高Kalman滤波器的速度，我们采用了离线计算系统增