kalman濾波--運動跟蹤

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

這裏設計的噪聲是高斯隨機噪聲，則量測方程爲：

z = Cx + v

看了代碼，對應上以上各項：

狀態估計值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用觀測值校正狀態變量。再更新狀態變量做下一次估計。

下面是OpenCV自帶samples\cpp\kalman.cpp中的源碼：

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

static void help()
{
    printf( "\nExamle 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);
    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);

        setIdentity(KF.measurementMatrix);
        setIdentity(KF.processNoiseCov, Scalar::all(1e-5));
        setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
        setIdentity(KF.errorCovPost, Scalar::all(1));

        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(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, CV_AA, 0); \
                line( img, Point( center.x + d, center.y - d ),                \
                             Point( center.x - d, center.y + d ), color, 1, CV_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, CV_AA, 0 );
            line( img, statePt, predictPt, Scalar(0,255,255), 3, CV_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;
}

這裏給出一個二維點跟蹤的示例：

//#include "stdafx.h"
#include <cv.h>
#include <cxcore.h>
#include <highgui.h>
#include <cmath>
#include <vector>
#include <iostream>
using namespace std;
const int winHeight=600;
const int winWidth=800;
CvPoint mousePosition=cvPoint(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=cvPoint(x,y);
       }
}
int main (void)
{
       //1.kalman filter setup
       const int stateNum=4;
       const int measureNum=2;
       CvKalman* kalman = cvCreateKalman( stateNum, measureNum, 0 );//state(x,y,detaX,detaY)
       CvMat* process_noise = cvCreateMat( stateNum, 1, CV_32FC1 );
       CvMat* measurement = cvCreateMat( measureNum, 1, CV_32FC1 );//measurement(x,y)
       CvRNG rng = cvRNG(-1);
       float A[stateNum][stateNum] ={//transition matrix
              1,0,1,0,
              0,1,0,1,
              0,0,1,0,
              0,0,0,1
       };
       memcpy( kalman->transition_matrix->data.fl,A,sizeof(A));
       cvSetIdentity(kalman->measurement_matrix,cvRealScalar(1) );
       cvSetIdentity(kalman->process_noise_cov,cvRealScalar(1e-5));
       cvSetIdentity(kalman->measurement_noise_cov,cvRealScalar(1e-1));
       cvSetIdentity(kalman->error_cov_post,cvRealScalar(1));
       //initialize post state of kalman filter at random
       cvRandArr(&rng,kalman->state_post,CV_RAND_UNI,cvRealScalar(0),cvRealScalar(winHeight>winWidth?winWidth:winHeight));
       CvFont font;
       cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1);
       cvNamedWindow("kalman");
       cvSetMouseCallback("kalman",mouseEvent);
       IplImage* img=cvCreateImage(cvSize(winWidth,winHeight),8,3);
       while (1){
              //2.kalman prediction
              const CvMat* prediction=cvKalmanPredict(kalman,0);
              CvPoint predict_pt=cvPoint((int)prediction->data.fl[0],(int)prediction->data.fl[1]);
              //3.update measurement
              measurement->data.fl[0]=(float)mousePosition.x;
              measurement->data.fl[1]=(float)mousePosition.y;
              //4.update
              cvKalmanCorrect( kalman, measurement );          
              //draw
              cvSet(img,cvScalar(255,255,255,0));
              cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green
              cvCircle(img,mousePosition,5,CV_RGB(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);
              cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0));
              sprintf_s(buf,256,"current position :(%3d,%3d)",mousePosition.x,mousePosition.y);
              cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0));
             
              cvShowImage("kalman", img);
              int key=cvWaitKey(3);
              if (key==27){//esc  
                     break;  
              }
       }     
       cvReleaseImage(&img);
       cvReleaseKalman(&kalman);
       return 0;
}