Kalman滤波包含两个步骤:
(1)用k-1时刻的最优估计预测k时刻的状态变量:
由上式可知,新的最优估计是根据上一最优估计预测得到的,并加上已知外部控制量的修正。
而新的不确定性由上一不确定性预测得到,并加上外部环境的干扰。
(2)对k时刻的状态进行观测,观测的状态量是Zk,协方差是Rk。用观测量对预测量进行修正,从而得到k时刻的最优状态估计。
其中,矩阵K叫做卡尔曼增益。Hk是指预测值和测量值可能不在一个单位和尺度上,用Hk这个矩阵将其转换到一个单位和尺度上。
由(7)(18)(19)公式可见:卡尔曼滤波是一个递归过程,只需要知道初始时刻的最优值,和它所对应的协方差以及测量值,就可以进行卡尔曼估计了。
附:下面是公式的推导过程,可以不看:
(推导过程总结自http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/)
公式(7)很好理解,一般根据物理公式等等都很容易建模。那么(18)(19)是怎么来的呢?
我们是从预测值和观测值来得到最优估计值的,我们的前提是两个都是有噪声的高斯分布,最优值要么靠近预测值,要么靠近观测值。我们现在就是要在这两个高斯分布区域中找一个最可能。那么这个最可能就是两个分布的重叠部分了。重叠部分怎么来?——将这两个高斯分布相乘就可以了。
由此可以得到:
继而得到:
写成矩阵形式:
带入我们的预测值和测量值:
得到:
进一步简化便得到(18)(19):