Kalman filter的个人理解

最近在做目标跟踪相关的工作，看了很多资料，零零总总的，过段时间自己都有些小混乱了…（不知道是不是自己应该吃点核桃补脑了呢），在这里Mark些资料，当脑容量不足时候，就暂且卸掉一些，扩充容量后，再继续装载……

Kalman Filter，wiki上对它的介绍很详细，http://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2，对应中文的介绍~

卡尔曼滤波建立在线性代数和隐马尔科夫模型（Hidden Markov Model）上，其基本动态系统可以用一个马尔科夫链（建立在一个被高斯噪声干扰的线性算子上）表示。

对每一步k，定义矩阵A，H，Q，R，有时候也需要定义B，卡尔曼滤波模型假设k时刻的真实状态时从k-1时刻状态演化而来，卡尔曼滤波有经典的五个公式，下面一一来对每个公式进行介绍：

1. 预测模型和测量模型

预测模型为：

xk = A·xk-1+B·Uk +wk；Ak预测转移矩阵，Bk输入控制模型，Uk控制向量，wk预测过程噪声~N（0，Q）

测量模型为：

zk = H·xk +vk；H测量矩阵，vk测量过程噪声~N（0，R）

以上两个模型是进行Kalman Filter的基础，缺少任何一个，则无法构成完整的预测测量系统，而后……自然也就没有而后了……只能另寻他法喽。

2.后验误差协方差模型

Pk = A·Pk-1·A+Q， 因为这里不方便敲击公式，故A的转置无法打出来，第一项相乘的三个因子分别为A，Pk-1，A的转置。

1&2为预测过程，而后利用Kalman Filter经典的另三个公式进行更新过程。

3. Kalman Filter增益K的计算

Kk =  Pk·H/(H·Pk·H + R)，其中除了Pk前H外的另两个H均表示H的转置。

获得的Kk可以理解为对预测值和测量值之间的权重分配过程，哪个值的置信度更高。

4. 进行xk校正更新

xk = xk +Kk·(zk - H·xk)

Kalman Filter 求解的X值是最优自回归的解，根据Kalman Gain对Xk的值进行校正更新。

5. 级更新后验误差协方差Pk

Pk = ( 1- Kk·H)·Pk，为k+1提供协方差值，以此循环下去。

Note: 以上Pk的更新式为仅在使用最优卡尔曼增益时候才得到此化简式，由于化简式的计算简单，故在计算过程中其使用频率最高。

如果算数精度总是很低导致数值稳定性出现问题，或者特意使用非最优卡尔曼增益，以上这个化简式就不再适用。

个人总结：

Kalman Filter适用于能获得x预测值和观测值，但各自都有一定偏差的情况下，通过Kalman Filter完成自回归过程，以此得到最小误差的x值。

后期会再结合OpenCV中cvKalman的相关结构和函数对其实现进行Mark

********************************************************************************************************************************************************

后续：对Kalman Filter的五个公式理解后，即可进入到实现过程

1.初始化

对OpenCV中Kalman中的如下变量进行初始化操作：

A 预测转移矩阵；

uk控制向量，B输入-控制向量；（如果存在uk，则需初始化B，否则B.release）

H测量矩阵；

wk过程噪声协方差矩阵；

vk观测噪声协方差矩阵；

Pk后验错误协方差矩阵；

xk-1前一状态校正后的值；

zk当前观察值；

DP状态向量维数；

MP测量向量维数；

CP控制向量维数（如果存在uk）

2,预测predict

xk = A·xk-1+B·Uk 

3,校正Correct

xk = xk +Kk·(zk - H·xk)

通过以上几个步骤，即可获得Kalman Filter的预测结果。