Kalman filtering the OpenCV

The definition of a class .OpenCV

Class definitions KalmanFilter

class CV_EXPORTS_W KalmanFilter  
{  
public:      
    CV_WRAP KalmanFilter();                                                                           //构造默认KalmanFilter对象  
    CV_WRAP KalmanFilter(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);  //完整构造KalmanFilter对象方法  
    void init(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);              //初始化KalmanFilter对象，会替换原来的KF对象  
    
    CV_WRAP const Mat& predict(const Mat& control=Mat());           //计算预测的状态值      
    CV_WRAP const Mat& correct(const Mat& measurement);             //根据测量值更新状态值  
  
    Mat statePre;            //预测值 (x'(k)): x(k)=A*x(k-1)+B*u(k)  
    Mat statePost;           //状态值 (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))  
    Mat transitionMatrix;    //状态转移矩阵 (A)  
    Mat controlMatrix;       //控制矩阵 B   
    Mat measurementMatrix;   //测量矩阵 H  
    Mat processNoiseCov;     //系统误差 Q  
    Mat measurementNoiseCov; //测量误差 R  
    Mat errorCovPre;         //最小均方误差 (P'(k)): P'(k)=A*P(k-1)*At + Q)  
    Mat gain;                //卡尔曼增益   (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)  
    Mat errorCovPost;        //修正的最小均方误差 (P(k)): P(k)=(I-K(k)*H)*P'(k)  
  
    // 临时矩阵  
    Mat temp1;  
    Mat temp2;  
    Mat temp3;  
    Mat temp4;  
    Mat temp5;  
};  

Implementation correlation function, i.e. opencv / modules / video / src / kalman.cpp

#include "precomp.hpp"

namespace cv
{

KalmanFilter::KalmanFilter() {}
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);         //预测值  x(k)=A*x(k-1)+B*u(k)  
    statePost = Mat::zeros(DP, 1, type);        //修正的状态值  x(k)=x'(k)+K(k)*(z(k)-H*x'(k))  
    transitionMatrix = Mat::eye(DP, DP, type);  //状态转移矩阵

    processNoiseCov = Mat::eye(DP, DP, type);     //系统误差Q
    measurementMatrix = Mat::zeros(MP, DP, type); //测量矩阵
    measurementNoiseCov = Mat::eye(MP, MP, type); //测量误差

    errorCovPre = Mat::zeros(DP, DP, type);    //最小均方误差  (P'(k)): P'(k)=A*P(k-1)*At + Q) 
    errorCovPost = Mat::zeros(DP, DP, type);   //修正的最小均方误差 (P(k)): P(k)=(I-K(k)*H)*P'(k)  
    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)
{
    CV_INSTRUMENT_REGION();

    // 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
    gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);//GEMM_2_T表示对第2个参数转置。

    // 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)
{
    CV_INSTRUMENT_REGION();

    // 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)
    solve(temp3, temp2, temp4, DECOMP_SVD);//solve函数，用来解线性方程 temp3*temp4=temp2

    // 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;
}

}

II. Caveats

Kalman filter out the relevant formula is not posted, the above functions can be updated with the prediction formula for those shining, following several key areas described below.

(1) gemm () function

gemm () is a generalized matrix multiplication

void gemm(const GpuMat& src1, constGpuMat& src2, double alpha, const GpuMat& src3, double beta,GpuMat& dst, int flags=0, Stream& stream=Stream::Null())

corresponding:

   dst = alpha*src1*src2 +beta* src3

One thing to note is that the program which the last parameter is given GEMM_2_T second parameter representation of transposition .

(2) solve () function

bool solve(InputArray src1, InputArray src2, OutputArray dst, int flags=DECOMP_LU)

Solutions for solving linear equations A * X = B, left src1 linear system (corresponding to the above A), the right src2 linear system (corresponding to the above B), dst outputted (equivalent to be solved X), flag for the methods used.

(3) Why can solve () function to solve the Kalman gain

Significance Kalman gain K so that the minimum a posteriori estimate error covariance, after K into expression posteriori estimate error covariance,

by derivation, K can be calculated optimal value. General expression:

using the SOLVE () function is based on the above red line corresponds directly solving linear equations.

Specific reference may be derived:
https://wenku.baidu.com/view/a5a6068619e8b8f67c1cb98b.html

(4) A typical example - track the mouse position

    #include "opencv2/video/tracking.hpp"  
    #include "opencv2/highgui/highgui.hpp"  
    #include <stdio.h>  
    using namespace cv;  
    using namespace std;  
      
    const int winHeight=600;  
    const int winWidth=800;  
      
      
    Point mousePosition= Point(winWidth>>1,winHeight>>1);  
      
    //mouse event callback  
    void mouseEvent(int event, int x, int y, int flags, void *param )  
    {  
        if (event==CV_EVENT_MOUSEMOVE) {  
            mousePosition = Point(x,y);  
        }  
    }  
      
    int main (void)  
    {  
        RNG rng;  
        //1.kalman filter setup  
        const int stateNum=4;                                      //状态值4×1向量(x,y,△x,△y)  
        const int measureNum=2;                                    //测量值2×1向量(x,y)    
        KalmanFilter KF(stateNum, measureNum, 0);     
      
        KF.transitionMatrix = *(Mat_<float>(4, 4) <<1,0,1,0,0,1,0,1,0,0,1,0,0,0,0,1);  //转移矩阵A  
        setIdentity(KF.measurementMatrix);                                             //测量矩阵H  
        setIdentity(KF.processNoiseCov, Scalar::all(1e-5));                            //系统噪声方差矩阵Q  
        setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));                        //测量噪声方差矩阵R  
        setIdentity(KF.errorCovPost, Scalar::all(1));                                  //后验错误估计协方差矩阵P  
        rng.fill(KF.statePost,RNG::UNIFORM,0,winHeight>winWidth?winWidth:winHeight);   //初始状态值x(0)  
        Mat measurement = Mat::zeros(measureNum, 1, CV_32F);                           //初始测量值x'(0)，因为后面要更新这个值，所以必须先定义  
          
        namedWindow("kalman");  
        setMouseCallback("kalman",mouseEvent);  
              
        Mat image(winHeight,winWidth,CV_8UC3,Scalar(0));  
      
        while (1)  
        {  
            //2.kalman prediction  
            Mat prediction = KF.predict();  
            Point predict_pt = Point(prediction.at<float>(0),prediction.at<float>(1) );   //预测值(x',y')  
      
            //3.update measurement  
            measurement.at<float>(0) = (float)mousePosition.x;  
            measurement.at<float>(1) = (float)mousePosition.y;          
      
            //4.update  
            KF.correct(measurement);  
      
            //draw   
            image.setTo(Scalar(255,255,255,0));  
            circle(image,predict_pt,5,Scalar(0,255,0),3);    //predicted point with green  
            circle(image,mousePosition,5,Scalar(255,0,0),3); //current position with red          
              
            char buf[256];  
            sprintf_s(buf,256,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y);  
            putText(image,buf,Point(10,30),CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,Scalar(0,0,0),1,8);  
            sprintf_s(buf,256,"current position :(%3d,%3d)",mousePosition.x,mousePosition.y);  
            putText(image,buf,cvPoint(10,60),CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,Scalar(0,0,0),1,8);  
              
            imshow("kalman", image);  
            int key=waitKey(3);  
            if (key==27){//esc     
                break;     
            }         
        }  
    }  

Other examples can refer to:
https://blog.csdn.net/haima1998/article/details/80641628