KF算法学习（三）：opencv中的KF源码分析

0.前言

       终于到了opencv这一章，有关KF的理论推导和matlab例程见点击打开链接。opencv中提供了封装好的KF类实现滤波流程，这里以opencv3.1.0版本中的源码为例进行详细分析

1.KF流程

      还是先简要回顾下KF的流程，如下图所示。

       整体来说KF分为两大步：step2-step3为预测阶段，计算得到新的预测值；step4-step6为校正阶段，使用观测值校正预测值得到估计值。这里提前把它们分为两个阶段，是对应着后面opencv中KF类的两大成员函数predict和correct

2.opencv中的KF类

       首先抠出类的声明部分，位于opencv-3.1.0\modules\video\include\opencv2\video\tracking.hpp中，注释已经十分详细了。

class CV_EXPORTS_W KalmanFilter{public:    /** @brief The constructors.    @note In C API when CvKalman\* kalmanFilter structure is not needed anymore, it should be released    with cvReleaseKalman(&kalmanFilter)     */    CV_WRAP KalmanFilter();    /** @overload    @param dynamParams Dimensionality of the state.    @param measureParams Dimensionality of the measurement.    @param controlParams Dimensionality of the control vector.    @param type Type of the created matrices that should be CV_32F or CV_64F.    */    CV_WRAP KalmanFilter( int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F );    /** @brief Re-initializes Kalman filter. The previous content is destroyed.    @param dynamParams Dimensionality of the state.    @param measureParams Dimensionality of the measurement.    @param controlParams Dimensionality of the control vector.    @param type Type of the created matrices that should be CV_32F or CV_64F.     */    void init( int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F );    /** @brief Computes a predicted state.    @param control The optional input control     */    CV_WRAP const Mat& predict( const Mat& control = Mat() );    /** @brief Updates the predicted state from the measurement.    @param measurement The measured system parameters     */    CV_WRAP const Mat& correct( const Mat& measurement );    CV_PROP_RW Mat statePre;           //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)    CV_PROP_RW Mat statePost;          //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))    CV_PROP_RW Mat transitionMatrix;   //!< state transition matrix (A)    CV_PROP_RW Mat controlMatrix;      //!< control matrix (B) (not used if there is no control)    CV_PROP_RW Mat measurementMatrix;  //!< measurement matrix (H)    CV_PROP_RW Mat processNoiseCov;    //!< process noise covariance matrix (Q)    CV_PROP_RW Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)    CV_PROP_RW Mat errorCovPre;        //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/    CV_PROP_RW Mat gain;               //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)    CV_PROP_RW Mat errorCovPost;       //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)    // temporary matrices    Mat temp1;    Mat temp2;    Mat temp3;    Mat temp4;    Mat temp5;};

       接着来看KF类的实现部分，为方便加深理解自己加了一些中文注释。

namespace cv{KalmanFilter::KalmanFilter() {}/* dynamParams(DP)：状态向量中的元素种类数 * measureParams(MP)：观测的状态向量中的元素个数 * controlParams(CP)：控制矩阵的列数*/KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams, int type){    init(dynamParams, measureParams, controlParams, type);}void KalmanFilter::init(int DP, int MP, int CP, int type){    CV_Assert( DP > 0 && MP > 0 );    CV_Assert( type == CV_32F || type == CV_64F );    CP = std::max(CP, 0);    statePre = Mat::zeros(DP, 1, type);// DP是状态向量中的元素种类数，即状态矩阵的行数    statePost = Mat::zeros(DP, 1, type);    transitionMatrix = Mat::eye(DP, DP, type);// 状态转移矩阵A的大小为DP X DP    processNoiseCov = Mat::eye(DP, DP, type);// 系统噪声协方差矩阵的大小为DP X DP    measurementMatrix = Mat::zeros(MP, DP, type);// MP为观测的状态向量中的元素个数(MP <= DP),观测矩阵的大小为MP X DP    measurementNoiseCov = Mat::eye(MP, MP, type);// 观测噪声协方差矩阵大小为MP X MP    errorCovPre = Mat::zeros(DP, DP, type); // 预测值和真实值之间的误差协方差矩阵大小为DP X DP    errorCovPost = Mat::zeros(DP, DP, type);// 估计值和真实值之间的误差协方差矩阵大小为DP X DP    gain = Mat::zeros(DP, MP, type); // 卡尔曼增益矩阵    if( CP > 0 )        controlMatrix = Mat::zeros(DP, CP, type);// 控制矩阵    else        controlMatrix.release();    temp1.create(DP, DP, type);    temp2.create(MP, DP, type);    temp3.create(MP, MP, type);    temp4.create(MP, DP, type);    temp5.create(MP, 1, type);}const Mat& KalmanFilter::predict(const Mat& control){    // update the state: x'(k) = A*x(k)    statePre = transitionMatrix*statePost;    if( !control.empty() )        // x'(k) = x'(k) + B*u(k)        statePre += controlMatrix*control;    // update error covariance matrices: temp1 = A*P(k)    temp1 = transitionMatrix*errorCovPost;    // P'(k) = temp1*At + Q// errorCovPre = 1*temp1*transitionMatrix_t+1*processNoiseCov    gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);    // handle the case when there will be measurement before the next predict.    statePre.copyTo(statePost);    errorCovPre.copyTo(errorCovPost);    return statePre;}const Mat& KalmanFilter::correct(const Mat& measurement){    // temp2 = H*P'(k)    temp2 = measurementMatrix * errorCovPre;    // temp3 = temp2*Ht + R    gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T);    // temp4 = inv(temp3)*temp2 = Kt(k)    // 求解 temp3 * temp4 = temp2的问题，即temp4 = inv(temp3)*temp2// 但是此时temp4不是卡尔曼增益K，而是它的转置solve(temp3, temp2, temp4, DECOMP_SVD);    // K(k)// 转置过来得到真正的K    gain = temp4.t();    // temp5 = z(k) - H*x'(k)    temp5 = measurement - measurementMatrix*statePre;    // x(k) = x'(k) + K(k)*temp5    statePost = statePre + gain*temp5;    // P(k) = P'(k) - K(k)*temp2    errorCovPost = errorCovPre - gain*temp2;    return statePost;}}

       其中用到一些opencv对矩阵的数学操作，记录如下：

（1）gemm()实现几个矩阵的加权乘法后相加（还可以选择对某个输入矩阵进行转置）

（2）solve()解决AX=B的问题，等价于求X=inv(A)*B

（3）randn()用于生成指定均值和方差的高斯分布结果

（4）setIdentity()用于初始化矩阵元素

3.opencv中使用KF

      结合KF的流程，opencv中使用KF类的大致流程如下：

      step1.初始化KF类对象

      step2.KF.predict()得到新的预测值

      step3.KF.correct()得到新的估计值

      之后重复step2-step3即可，下面以opencv自带的一个卡尔曼滤波例子进行分析，位于opencv-3.1.0\samples\cpp\kalman.cpp。例子中我们建立一个绕某一圆心做匀速圆周运动的小球，但是实际中它会受到系统噪声影响从而其角度和角速度有所变化，我们通过带有噪声的观测值（真实值+观测噪声）和匀速运动模型的预测值为输入使用KF得到估计值，具体代码如下。

#include "opencv2/video/tracking.hpp"#include "opencv2/highgui/highgui.hpp"#include <stdio.h>using namespace cv;// 根据圆心和夹角计算点的二维坐标static inline Point calcPoint(Point2f center, double R, double angle){    return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R;// 图像坐标系中y轴向下}static void help(){    printf( "\nExample of c calls to OpenCV's Kalman filter.\n""   Tracking of rotating point.\n""   Rotation speed is constant.\n""   Both state and measurements vectors are 1D (a point angle),\n""   Measurement is the real point angle + gaussian noise.\n""   The real and the estimated points are connected with yellow line segment,\n""   the real and the measured points are connected with red line segment.\n""   (if Kalman filter works correctly,\n""    the yellow segment should be shorter than the red one).\n"            "\n""   Pressing any key (except ESC) will reset the tracking with a different speed.\n""   Pressing ESC will stop the program.\n"            );}int main(int, char**){    help();    Mat img(500, 500, CV_8UC3);    KalmanFilter KF(2, 1, 0);// 系统状态矩阵大小为2X1， 观测矩阵大小为1X2，卡尔曼增益矩阵大小为2X1    Mat state(2, 1, CV_32F); /* (phi, delta_phi)，系统状态为[角度，角速度] */    Mat processNoise(2, 1, CV_32F);// 系统状态噪声矩阵    Mat measurement = Mat::zeros(1, 1, CV_32F);// 观测矩阵，这里只观测角度    char code = (char)-1;    for(;;)    {        randn( state, Scalar::all(0), Scalar::all(0.1) );// 初始化系统状态真实值        KF.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 0, 1);// 匀速运动模型中的状态转移矩阵A// 初始化矩阵        setIdentity(KF.measurementMatrix); // A        setIdentity(KF.processNoiseCov, Scalar::all(1e-5));// Q        setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));// R        setIdentity(KF.errorCovPost, Scalar::all(1));// P        randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));// 估计值        for(;;)        {            Point2f center(img.cols*0.5f, img.rows*0.5f);            float R = img.cols/3.f;            double stateAngle = state.at<float>(0);            Point statePt = calcPoint(center, R, stateAngle);// 真实位置            Mat prediction = KF.predict();            double predictAngle = prediction.at<float>(0);            Point predictPt = calcPoint(center, R, predictAngle);// 预测位置            randn( measurement, Scalar::all(0), Scalar::all(sqrt(KF.measurementNoiseCov.at<float>(0)));            // generate measurement            measurement += KF.measurementMatrix*state;// 观测位置 = 真实位置+观测位置噪声            double measAngle = measurement.at<float>(0);            Point measPt = calcPoint(center, R, measAngle);// 观测位置坐标            // plot points            #define drawCross( center, color, d )                                        \                line( img, Point( center.x - d, center.y - d ),                          \                             Point( center.x + d, center.y + d ), color, 1, LINE_AA, 0); \                line( img, Point( center.x + d, center.y - d ),                          \                             Point( center.x - d, center.y + d ), color, 1, LINE_AA, 0 )// 实时更新三个位置            img = Scalar::all(0);            drawCross( statePt, Scalar(255,255,255), 3 );            drawCross( measPt, Scalar(0,0,255), 3 );            drawCross( predictPt, Scalar(0,255,0), 3 );            line( img, statePt, measPt, Scalar(0,0,255), 3, LINE_AA, 0 );            line( img, statePt, predictPt, Scalar(0,255,255), 3, LINE_AA, 0 );            if(theRNG().uniform(0,4) != 0)                KF.correct(measurement);// 使用观测值更新估计值            randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));            state = KF.transitionMatrix*state + processNoise;            imshow( "Kalman", img );            code = (char)waitKey(100);            if( code > 0 )                break;        }        if( code == 27 || code == 'q' || code == 'Q' )            break;    }    return 0;}

       唯一感到奇怪的是，例程中比较的是预测值和真实值的偏差，没有比较估计值（KF.correct()的返回值）和真实值的偏差，尚不清楚这样做的目的。程序运行效果如下图，其中黄线代表预测值和真实值的偏差，红线代表测量值和真实值的偏差。