学习opencv中KalmanFilter应用

为什么要卡曼滤波？

简单来说，因为噪声影响，测量不准确，应用卡曼滤波来尽量去除噪声影响。

应用场景：

当预估的模型可知，如恒定、线性或多项式等。
       有固定的模型后，才会有状态转移方程，kalmanfilter应用才方便。

opencv中kalmanfilter保留的接口很简单，只有三个：

init， predict及correct。

init仅仅是初始化卡曼滤波的维数；

predict就能得到估计的结果；

correct要将测量结果作为输入，得到最优化的估计，并为下次predict做准备。

除此之外，kalmanfilter类中还有10个比较复杂的参数，它们与kalmanfilter的原理密切相关。

5个矩阵是基本参数：

    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)

5个矩阵是kalmanfilter公式计算的结果

    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 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)
   
在使用kalmanfilter之前，需要初始化5个基本参数。 重点关注

transitionMatrix

processNoiseCov：过程噪声

measurementNoiseCov测量噪声

而

measurementMatrix： 直接观察测量，一般为1

controlMatrix：没有时，为0

errorCovPost 需要初始化，当其为1时，比为0时，收敛快的多。由公式可以看出。

statePost初始化尽量接近下一个状态值， 如果收敛很快，这个不是太重要。

kalmanfilter示例可以参考，opencv samples下的kalman.cpp。

白色：真实位置

红色：测量位置

绿色：估计位置

青色：最优化位置