Kalman滤波实现目标跟踪

      Kalman滤波理论主要应用在现实世界中个，并不是理想环境。主要是来跟踪的某一个变量的值，跟踪的依据是首先根据系统的运动方程来对该值做预测，比如说我们知道一个物体的运动速度，那么下面时刻它的位置按照道理是可以预测出来的，不过该预测肯定有误差，只能作为跟踪的依据。另一个依据是可以用测量手段来测量那个变量的值，当然该测量也是有误差的，也只能作为依据，不过这2个依据的权重比例不同。最后kalman滤波就是利用这两个依据进行一些列迭代进行目标跟踪的。

     在这个理论框架中，有2个公式一定要懂，即：

　　

　　

     第一个方程为系统的运动方程，第二个方程为系统的观测方程，学过自控原理中的现代控制理论的同学应该对这2个公式很熟悉。具体的相关理论本文就不做介绍了。

       kalman滤波大家都很熟悉，其基本思想就是先不考虑输入信号和观测噪声的影响，得到状态变量和输出信号的估计值，再用输出信号的估计误差加权后校正状态变量的估计值，使状态变量估计误差的均方差最小。具体它的原理和实现，我想也不用我在这里费口舌，但这个理论基础必须的有，必须得知道想用kalman滤波做跟踪，必须得先建立运动模型和观察模型，不是想用就能用的。如果不能建立运动模型，也就意味着你所要面对的问题不能用kalman滤波解决。

      我结合一下OpenCV自带的kalman.cpp这个例程来介绍一下如何在OpenCV中使用kalman滤波吧，OpenCV已经把Kalman滤波封装到一个类KalmanFilter中了。使用起来非常方便，但那繁多的各种矩阵还是容易让人摸不着头脑。这里要知道的一点是，想要用kalman滤波，要知道前一时刻的状态估计值x，当前的观测值y，还得建立状态方程和量测方程。有了这些就可以运用kalman滤波了。

       OpenCV自带了例程里面是对一个1维点的运动跟踪，虽然这个点是在2维平面中运动，但由于它是在一个圆弧上运动，只有一个自由度，角度，所以还是1维的。还是一个匀速运动，建立匀速运动模型，设定状态变量x = [ x1, x2 ] = [ 角度，角速度 ]，则运动模型为

                                   x1（k+1） = x1（k）+x2（k）*T

                                   x2（k+1）= x2（k）

则状态转移方程为

                                   x* = Ax + w

                                   A = [1    1;

                                          0    1]

这里设计的噪声是高斯随机噪声，则量测方程为：

                                   z = Cx + v

                                   C = [1  0]

下面来看看使用Kalman编程的主要步骤：

　　步骤一  ：

     Kalman这个类需要初始化下面变量：      

　　转移矩阵，测量矩阵，控制向量(没有的话，就是0)，过程噪声协方差矩阵，测量噪声协方差矩阵，后验错误协方差矩阵，前一状态校正后的值，当前观察值。 

　　步骤二：

　　调用kalman这个类的predict方法得到状态的预测值矩阵，预测状态的计算公式如下：

　　             predicted state (x'(k)): x'(k)=A*x(k-1)+B*u(k)

　　其中x(k-1)为前一状态的校正值，第一个循环中在初始化过程中已经给定了，后面的循环中Kalman这个类内部会计算。A,B,u(k),也都是给定了的值。这样进过计算就得到了系统状态的预测值x'(k)了。 

　　步骤三：

　　调用kalman这个类的correct方法得到加入观察值校正后的状态变量值矩阵，其公式为：

　　             corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))

　　其中x'(k)为步骤二算出的结果，z(k)为当前测量值，是我们外部测量后输入的向量。H为Kalman类初始化给定的测量矩阵。K(k)为Kalman增益，其计算公式为：

　　             Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)

　　计算该增益所依赖的变量要么初始化中给定，要么在kalman理论中通过其它公式可以计算。

　　经过步骤三后，我们又重新获得了这一时刻的校正值，后面就不断循环步骤二和步骤三即可完成Kalman滤波过程。

看了代码，对应上以上各项：

         状态估计值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)  

我想就不用我再翻译了吧。相信有了以上的注释，大家都能找到它们的对应项。

使用的时候，除了初始化我刚刚初始化过的transitionMatrix和measurementMatrix外，还需要初始化processNoiseCov，measurementNoiseCov和errorCovPost。

把它们初始化好之后，接下来的动作就很简单了，分两步走，第一步调用成员函数predict得到当前状态变量的估计值，第二步调用成员函数correct用观测值校正状态变量。再更新状态变量做下一次估计。听着好简单啊，代码就不上传坑爹了，在opencv2.3.1\samples\cpp\kalman.cpp中其义自见。