最近在做目标跟踪相关的工作,看了很多资料,零零总总的,过段时间自己都有些小混乱了…(不知道是不是自己应该吃点核桃补脑了呢),在这里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的预测结果。