Kalman Filter(卡爾曼濾波)的個人筆記以及程序實現

簡單的介紹一下卡爾曼濾波器的關鍵的5個公式。

引入一個離散控制過程的系統。該系統可用一個線性隨機微分方程來描述：

　　X(k)=A X(k-1)+B U(k)+W(k)

　　再加上系統的測量值：

　　Z(k)=H X(k)+V(k)

上兩式子中，X(k)是k時刻的系統狀態，U(k)是k時刻對系統的控制量。A和B是系統參數，對於多模型系統，他們為矩陣。Z(k)是k時刻的測量值，H是測量系統的參數，對於多測量系統，H為矩陣。W(k)和V(k)分別表示過程和測量的噪聲。他們被假設成高斯白噪聲，他們的協方差（covariance ）分別是Q，R（這里我們假設他們不隨系統狀態變化而變化）

首先利用系統的過程模型來預測系統下一狀態，設在k時刻的系統狀態為x(k),則可以根據系統模型，由上一狀態預測出現在狀態：

x(k|k-1) = A x(k-1|k-1) + B u(k).....(1)

其中x(k|k-1)是上一時刻的狀態對現在時刻狀態的預測，x(k-1|k-1)是上一時刻狀態的最優結果， u(k)為現在時刻狀態的控制量。

系統的狀態已經更新，現在需要更新系統的誤差估計協方差矩陣，用p(k|k-1)表示誤差估計協方差矩陣：

p(k|k-1) = A p(k-1|k-1)A' + Q........(2)

其中p(k|k-1)是在k時刻由上一狀態對此狀態的預測， p(k-1|k-1)是x(k-1|k-1)對應的誤差估計協方差矩陣，A' 是A 的轉置。Q 表示系統過程噪聲的covariance。

現在我們得到了預測結果，然后我們根據得到的現在狀態的測量值進行更新（修正）（預測得到的）現在的狀態 x(k|k):

x(k|k) = x(k|k-1) + Kg(k) (Z(k)-Hx(k|k-1))....(3)

(3)式中 Kg(k)未知，則需要對其就行求解，就引出(4)式：

 Kg(k) = p(k|k-1) H' /(H p(k|k-1)H' + R)...(4)

到現在，我們以及得出的k 時刻的系統狀態的最優值x(k|k),為了讓卡爾曼濾波器不斷地進行下去，我們需要更新 x(k|k) 對應的p(k|k) (應用於式（2）)：

p(k|k) = (I-Kg(k) H) p(k|k-1).....(5)

 

卡爾曼濾波實現的小程序：opencv+ C++

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <cmath>  
#include <vector>
#include <stdio.h>
#include <iostream>

using namespace cv;
using namespace std;
const int winHeight=600;  
const int winWidth=800;

Point move_mouse = 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)
    {  
        move_mouse=Point(x,y);  
    } 
}


int main()
{
    
    ////1.kalman filter setup

    const int stateNum=4;  
    const int measureNum=2;

    KalmanFilter kalman(stateNum,measureNum,0);
    Mat processNoise(stateNum,1,CV_32F);
    Mat measureNoise(stateNum,1,CV_32F);
    Mat measurement (measureNum,1,CV_32F);
    float A[stateNum][stateNum] ={//transition matrix  
        1,0,1,0,  
        0,1,0,1,  
        0,0,1,0,  
        0,0,0,1  
    };  

    memcpy( kalman.transitionMatrix.data,A,sizeof(A));
    cout<<kalman.transitionMatrix<<endl;
    setIdentity(kalman.measurementMatrix);
    setIdentity(kalman.processNoiseCov,Scalar::all(1e-5));
    setIdentity(kalman.measurementNoiseCov,Scalar::all(1e-1));
    setIdentity(kalman.errorCovPost,Scalar::all(1));

    //initialize post state of kalman filter at random 
    randn(kalman.statePost,Scalar::all(0), Scalar::all(0.1));
    CvFont font;  
    cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1);  

   
    namedWindow("kalman",1);
    setMouseCallback("kalman",mouseEvent);
    
    Mat img(500, 500, CV_8UC3);
    while (1)
    {
        //2.kalman prediction
        Mat prediction = kalman.predict();
        Point predict_pt = Point(prediction.at<float>(0),prediction.at<float>(1));
 
       //3.update measurement 

        measurement.at<float>(0) = (float)move_mouse.x;
        measurement.at<float>(1) = (float)move_mouse.y;

        //4.update
        kalman.correct(measurement);


        //5.draw

        img = Scalar::all(0);
        circle(img,predict_pt,4,CV_RGB(0,255,0),2);//predicted point with green 
        circle(img,move_mouse,4,CV_RGB(255,0,0),2);//current position with red
     
    imshow("kalman", img);  
        int key=cvWaitKey(3);  
        if (key==27)
        {
            break;    
        }
    }
    system("pause");
    return 0;
}

程序說明：

本程序由http://blog.sina.com.cn/s/blog_9db9f81901015wuo.html 稍作修改適應新版本的opencv。

程序主要由以下幾個部分：

1.實例化一個 KalmanFilter 的對象kalman，聲明並初始化矩陣processNoise，measureNoise， measurement ;

注意：要設置的變量主要是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 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)

2.預測KalmanPredict，只需要調用函數 predict() ，然后讀出自己需要的值

3.根據采集到的數據更新觀測數據

4.用觀測數據來更新預測狀態值。

 

 

參考資料：http://blog.sina.com.cn/s/blog_4c2b25550100b8yq.html

http://blog.csdn.net/yangtrees/article/details/8075911#comments 學習OpenCV——Kalman濾波