在上一篇文章中手把手推导了一遍卡尔曼增益,不熟悉的小伙伴可以看
这里再回顾一下重点。
问题重述
假设真实系统为
其中 。
我们对系统状态的估计(数据融合)为
其中卡曼尔增益为
我们可以看到卡尔曼增益中的估计协方差矩阵 还是未知的,因此我们需要把它表示出来。
估计协方差矩阵推导
根据定义
带入真实系统模型和状态估计的模型
再带入估计协方差矩阵的表达式
把转置放进去
把括号打开
由于四项之间是线性相加的,可以把期望的运算放进去,等于每一项的期望。
注意到 ,而 是作用到 上的,所以
和 是相互独立的。因为相互独立,所以相乘的期望等于期望的相乘
因此估计的协方差矩阵剩下两项
有了这个表达式后,我们就可以用卡尔曼滤波器来估计状态了!
卡尔曼滤波器两步法
目前为止卡尔曼增益中各项都是已知的了,因此可以用于估计了,卡尔曼滤波分为两个步骤:预测和校正。
Step1. 预测
(1)利用系统模型对状态变量进行递推估计
(2)对协方差矩阵进行估计
Step2. 校正
(3)计算卡尔曼增益
(4)数据融合即对状态的估计
(5)更新协方差矩阵用于下一次的预测
至此,卡尔曼滤波(估计器)就推导完毕了,给定初值 ,它就能递推估计了。记住核心思想无非是:数据融合、预测校正。
公式(5)的推导
在上一篇文章中我们推导得到协方差矩阵的表达式为
合并同类项
带入卡尔曼增益
评论(0)
您还未登录,请登录后发表或查看评论