OpenCV中的卡爾曼濾波方法

一.OpenCV中的類定義

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

相關函數的實現方法，即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;
}

}

二.需要說明的地方

卡爾曼濾波的相關公式就不貼出來了，上面的更新與預測函數可以對照着那些公式，下面對幾個關鍵的地方進行說明。

（1）gemm（）函數

gemm( )是矩陣的廣義乘法

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

對應着：

   dst = alpha*src1*src2 +beta* src3

需要注意的一點是，程序裏面給出了最後一個參數是GEMM_2_T表示對第2個參數轉置。

（2）solve（）函數

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

用來解線性方程 A*X=B，src1 線性系統的左側（相當於上面的A），src2 線性系統的右側（相當於上面的B），dst 輸出的解決方案（相當於要求解的X）,flag爲使用的方法。

（3）爲什麼可以用solve（）函數求解卡爾曼增益

卡爾曼增益K的意義是使後驗估計誤差協方差最小，將K帶入後驗估計誤差協方差的表達式，

通過求導，可以計算出最優的K值。一般的表達式：

採用SOLVE（）函數的依據就是上面的紅線部分，相當於直接進行線性方程的求解。

具體推導可以參考：
https://wenku.baidu.com/view/a5a6068619e8b8f67c1cb98b.html

（4）一個典型的例子-- 跟蹤鼠標位置

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

其他例子可以參考：
https://blog.csdn.net/haima1998/article/details/80641628