OpenCV2学习笔记（十九）：Kalman滤波算法

在视频跟踪处理中，预测目标运动轨迹是一项基本任务。目标运动状态估计的目的有三个：一是对目标过去的状态进行平滑；二是对目标现在的运动状态进行滤波；三是对目标未来的运动状态进行预测。物体的运动状态一般包括目标位置、速度、加速度等。著名的Kalman滤波技术就是其中一种，这是一种线性系统估计技术。

OpenCV中自带了kalman滤波的代码和例程，可参照kalman.cpp，它存在于类KalmanFilter中。kalman滤波算法的调用比较方便，主要的难点是了解多个参数和矩阵计算公式。一个总体的思路是，需要了解前一时刻的状态估计值x和当前的观测值y，然后建立状态方程和观测方程。经过一些运算后即可预测下一步的状态。

一、离散时间线性动态系统的状态方程

Kalman滤波利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。一个线性系统是采用状态方程、观测方程及其初始条件来描述。线性离散时间系统的一般状态方程可描述为：

　　　　　　　
其中， 是状态转移矩阵， 是过程噪声增益矩阵。是k时刻目标的状态向量， 是过程噪声，它是具有均值为零、方差矩阵为Q(k)的高斯噪声向量，即：

　　　　　　　　
二、传感器的观测方程

传感器的通用观测方程为：

　　　
这里， 是传感器在 时刻的观测向量，观测噪声 是具有零均值和正定协方差矩阵R(k)的高斯分布测量噪声向量，即：

　　　　
三、初始状态的描述
初始状态 是高斯的，具有均值 和协方差 ，即：

　　　　　　
以上的描述比较抽象，因此记录一个例子加以说明：

例：目标沿x轴作匀速直线运动，过程噪声为速度噪声，试写出目标的状态方程。

解：由题意知，目标的状态为：

用T表示时间间隔，ux表速度噪声，则有：

写成矩阵形式为：

令：

则有：

其中：

为均值等于0，方差为q的高斯噪声。

在OpenCV中自带的例程里面描述了一个一维的运动跟踪，该点在一个圆弧上运动，只有一个自由度即角度。因此只需建立匀速运动模型即可。

例程的路径：C:\opencv\sources\samples\cpp\kalman.cpp

在代码中各变量的对应情况如下：
状态估计值X对应：state
当前观测值Z对应：measurement

KalmanFilter类内成员变量transitionMatrix即为状态转移方程中的矩阵A
KalmanFilter类内成员变量measurementMatrix即为量测方程中矩阵C

     Mat statePre;          //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)  
    Mat statePost;          //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))  
    Mat transitionMatrix;   //!< state transition matrix (A)  
    Mat controlMatrix;      //!< control matrix (B) (not used if there is no control)  
    Mat measurementMatrix;  //!< measurement matrix (H)  
    Mat processNoiseCov;    //!< process noise covariance matrix (Q)  
    Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)  
    Mat errorCovPre;        //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/  
    Mat gain;               //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)  
    Mat errorCovPost;       //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)  

以下是OpenCV/modules/video/src/Kalman.cpp的源代码，后续需继续分析这些代码：

/*M///
//
//  IMPORTANT: READ BEFORE DOWNLOADING,