目錄(?)[-]
背景
模型
基本動態系統模型
卡爾曼濾波器
預測 predict      Mat prediction KFpredict
更新 updata     KFcorrectmeasurement
不變量Invariant

代碼
class Kalman
samplekalmancpp
鼠標跟蹤詳解
初始化狀態
預測predict讀出預測的位置
更新updata

主要資料

8075911

背景：

卡爾曼濾波是一種高效率的遞歸濾波器(自迴歸濾波器), 它能夠從一系列的不完全及包含噪聲的測量中，估計動態系統的狀態。卡爾曼濾波的一個典型實例是從一組有限的，包含噪聲的，對物體位置的觀察序列（可能有偏差）預測出物體的位置的座標及速度。

這種濾波方法以它的發明者魯道夫.E.卡爾曼（Rudolph E. Kalman）命名，但是根據文獻可知實際上Peter Swerling在更早之前就提出了一種類似的算法。

斯坦利.施密特(Stanley Schmidt)首次實現了卡爾曼濾波器。卡爾曼在NASA埃姆斯研究中心訪問時，發現他的方法對於解決阿波羅計劃的軌道預測很有用，後來阿波羅飛船的導航電腦便使用了這種濾波器。關於這種濾波器的論文由Swerling (1958)、Kalman (1960)與 Kalman and Bucy (1961)發表。

目前,卡爾曼濾波已經有很多不同的實現.卡爾曼最初提出的形式現在一般稱爲簡單卡爾曼濾波器。除此以外，還有施密特擴展濾波器、信息濾波器以及很多Bierman, Thornton 開發的平方根濾波器的變種。也許最常見的卡爾曼濾波器是鎖相環，它在收音機、計算機和幾乎任何視頻或通訊設備中廣泛存在。

模型：

基本動態系統模型:

卡爾曼濾波建立在線性代數和隱馬爾可夫模型(hidden Markov model)上。其基本動態系統可以用一個馬爾可夫鏈表示，該馬爾可夫鏈建立在一個被高斯噪聲(即正態分佈的噪聲)干擾的線性算子上的。系統的狀態可以用一個元素爲實數的向量表示。隨着離散時間的每一個增加,這個線性算子就會作用在當前狀態上，產生一個新的狀態,並也會帶入一些噪聲,同時系統的一些已知的控制器的控制信息也會被加入。同時，另一個受噪聲干擾的線性算子產生出這些隱含狀態的可見輸出。

模型圖：

爲了從一系列有噪聲的觀察數據中用卡爾曼濾波器估計出被觀察過程的內部狀態，我們必須把這個過程在卡爾曼濾波的框架下建立模型。也就是說對於每一步k，定義矩陣

例如：KalmanFilter KF(stateNum, measureNum, 0);

F_k: KF.transitionMatrix

H_k: KF.measurementMatrix

Q_k: KF.processNoiseCov

R_k: KF.measurementNoiseCov

P_k: KF.errorCovPost

有時也需要定義B_k: KF.ontrolMatrix

如下。

爾曼濾波模型假設k時刻的真實狀態是從(k − 1)時刻的狀態演化而來，符合下式：

$\textbf{x}_{k} = \textbf{F}_{k} \textbf{x}_{k-1} + \textbf{B}_{k}\textbf{u}_{k} + \textbf{w}_{k}$

其中

F_k 是作用在x_k−1上的狀態變換模型（/矩陣/矢量）。
B_k 是作用在控制器向量u_k上的輸入－控制模型。
w_k 是過程噪聲，並假定其符合均值爲零，協方差矩陣爲Q_k的多元正態分佈。

$\textbf{w}_{k} \sim N(0, \textbf{Q}_k)$

時刻k，對真實狀態 x_k的一個測量z_k滿足下式:

$\textbf{z}_{k} = \textbf{H}_{k} \textbf{x}_{k} + \textbf{v}_{k}$

其中H_k是觀測模型,它把真實狀態空間映射成觀測空間，v_k 是觀測噪聲，其均值爲零，協方差矩陣爲R_k,且服從正態分佈。

$\textbf{v}_{k} \sim N(0, \textbf{R}_k)$

初始狀態以及每一時刻的噪聲{x₀, w₁, ..., w_k, v₁ ...v_k} 都認爲是互相獨立的.

卡爾曼濾波器：

卡爾曼濾波是一種遞歸的估計，即只要獲知上一時刻狀態的估計值以及當前狀態的觀測值就可以計算出當前狀態的估計值，因此不需要記錄觀測或者估計的歷史信息。卡爾曼濾波器與大多數濾波器不同之處，在於它是一種純粹的時域濾波器，它不需要像低通濾波器等頻域濾波器那樣，需要在頻域設計再轉換到時域實現。

卡爾曼濾波器的狀態由以下兩個變量表示：

$\hat{\textbf{x}}_{k|k}$ ，在時刻k 的狀態的估計；
$\textbf{P}_{k|k}$ ，誤差相關矩陣，度量估計值的精確程度。

卡爾曼濾波器的操作包括兩個階段：預測與更新。在預測階段，濾波器使用上一狀態的估計，做出對當前狀態的估計。在更新階段，濾波器利用對當前狀態的觀測值優化在預測階段獲得的預測值，以獲得一個更精確的新估計值。

預測 predict: Mat prediction = KF.predict();

$\hat{\textbf{x}}_{k|k-1} = \textbf{F}_{k}\hat{\textbf{x}}_{k-1|k-1} + \textbf{B}_{k} \textbf{u}_{k}$ (預測狀態)

$\textbf{P}_{k|k-1} = \textbf{F}_{k} \textbf{P}_{k-1|k-1} \textbf{F}_{k}^{T} + \textbf{Q}_{k}$ (預測估計協方差矩陣)

可參考：http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

更新 updata: KF.correct(measurement);

$\tilde{\textbf{y}}_{k} = \textbf{z}_{k} - \textbf{H}_{k}\hat{\textbf{x}}_{k|k-1}$ (測量餘量，measurement residual)

$\textbf{S}_{k} = \textbf{H}_{k}\textbf{P}_{k|k-1}\textbf{H}_{k}^{T} + \textbf{R}_{k}$ (測量餘量協方差)

$\textbf{K}_{k} = \textbf{P}_{k|k-1}\textbf{H}_{k}^{T}\textbf{S}_{k}^{-1}$ (最優卡爾曼增益)

$\hat{\textbf{x}}_{k|k} = \hat{\textbf{x}}_{k|k-1} + \textbf{K}_{k}\tilde{\textbf{y}}_{k}$ (更新的狀態估計)

$\textbf{P}_{k|k} = (I - \textbf{K}_{k} \textbf{H}_{k}) \textbf{P}_{k|k-1}$ (更新的協方差估計)

使用上述公式計算 $\textbf{P}_{k|k}$ 僅在最優卡爾曼增益的時候有效。使用其他增益的話，公式要複雜一些，請參見推導。

不變量(Invariant)

如果模型準確，而且 $\hat{\textbf{x}}_{0|0}$ 與 $\textbf{P}_{0|0}$ 的值準確的反映了最初狀態的分佈，那麼以下不變量就保持不變：所有估計的誤差均值爲零

$\textrm{E}[\textbf{x}_k - \hat{\textbf{x}}_{k|k}] = \textrm{E}[\textbf{x}_k - \hat{\textbf{x}}_{k|k-1}] = 0$
$\textrm{E}[\tilde{\textbf{y}}_k] = 0$

且協方差矩陣準確的反映了估計的協方差:

$\textbf{P}_{k|k} = \textrm{cov}(\textbf{x}_k - \hat{\textbf{x}}_{k|k})$
$\textbf{P}_{k|k-1} = \textrm{cov}(\textbf{x}_k - \hat{\textbf{x}}_{k|k-1})$
$\textbf{S}_{k} = \textrm{cov}(\tilde{\textbf{y}}_k)$

請注意，其中 $\textrm{E}[\textbf{a}]$ 表示 ${a}$ 的期望值, $\textrm{cov}(\textbf{a}) = \textrm{E}[\textbf{a}\textbf{a}^T]$ 。

代碼：

class Kalman：

[cpp]view
plaincopyprint?

class CV_EXPORTS_W KalmanFilter  

{  

public:  

    //! the default constructor  

    CV_WRAP KalmanFilter();  

    //! the full constructor taking the dimensionality of the state, of the measurement and of the control vector  

    CV_WRAP KalmanFilter(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);  

    //! re-initializes Kalman filter. The previous content is destroyed.  

    void init(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);  

    //! computes predicted state  

    CV_WRAP const Mat& predict(const Mat& control=Mat());  

    //! updates the predicted state from the measurement  

    CV_WRAP const Mat& correct(const Mat& measurement);  

    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)  

    // temporary matrices  

    Mat temp1;  

    Mat temp2;  

    Mat temp3;  

    Mat temp4;  

    Mat temp5;  

};

sample\kalman.cpp

1個1維點的運動跟蹤，雖然這個點是在2維平面中運動，但由於它是在一個圓弧上運動，只有一個自由度，角度，所以還是1維的。還是一個勻速運動，建立勻速運動模型，設定狀態變量x = [ x1, x2 ] = [ 角度，角速度 ]

[cpp]view
plaincopyprint?

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

}  

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) );//隨機生成一個矩陣，期望是0，標準差爲0.1;  

        KF.transitionMatrix = *(Mat_<float>(2, 2) << 1, 1, 0, 1);//元素導入矩陣，按行;  

        //setIdentity: 縮放的單位對角矩陣;  

        //!< measurement matrix (H) 觀測模型  

        setIdentity(KF.measurementMatrix);  

        //!< process noise covariance matrix (Q)  

        // wk 是過程噪聲，並假定其符合均值爲零，協方差矩陣爲Qk(Q)的多元正態分佈;  

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

        //!< measurement noise covariance matrix (R)  

        //vk 是觀測噪聲，其均值爲零，協方差矩陣爲Rk,且服從正態分佈;  

        setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));  

        //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/  A代表F: transitionMatrix  

        //預測估計協方差矩陣;  

        setIdentity(KF.errorCovPost, Scalar::all(1));  

        //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))  

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

            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;  

}  

<span style="font-size:14px;color:#ff0000;"><img alt="" src="https://img-my.csdn.net/uploads/201210/16/1350365491_1888.JPG"></span>

鼠標跟蹤（詳解）：

1.初始化狀態：

onst int stateNum=4;//狀態數，包括（x，y，dx，dy）座標及速度（每次移動的距離）

const int measureNum=2;//觀測量，能看到的是座標值，當然也可以自己計算速度，但沒必要

轉移矩陣或者說增益矩陣的值好像有點莫名其妙

[cpp]view
plaincopyprint?

float A[stateNum][stateNum] ={//transition matrix  

        1,0,1,0,  

        0,1,0,1,  

        0,0,1,0,  

        0,0,0,1  

    };

看下圖就清楚了

X1=X+dx，依次類推
所以這個矩陣還是很容易卻確定的，可以根據自己的實際情況定製轉移矩陣

同樣的方法，三維座標的轉移矩陣可以如下

[cpp]view
plaincopyprint?

float A[stateNum][stateNum] ={//transition matrix  

        1,0,0,1,0,0,  

        0,1,0,0,1,0,  

        0,0,1,0,0,1,  

        0,0,0,1,0,0,  

        0,0,0,0,1,0,  

        0,0,0,0,0,1  

    };

當然並不一定得是1和0

KalmanFilter KF(stateNum, measureNum, 0);

//KalmanFilter::KalmanFilter(intdynamParams, intmeasureParams, int controlParams=0, inttype=CV_32F)

Parameters:	dynamParams – Dimensionality of the state. measureParams – Dimensionality of the measurement. controlParams – Dimensionality of the control vector. type – Type of the created matrices that should be`CV_32F` or`CV_64F`.

Mat state (stateNum, 1, CV_32FC1); // state(x,y,detaX,detaY)

Mat processNoise(stateNum, 1, CV_32F); // processNoise(x,y,detaX,detaY)

Mat measurement = Mat::zeros(measureNum, 1, CV_32F); //measurement(x,y)

需定義的參數矩陣：

F_k: KF.transitionMatrix

[cpp]view
plaincopyprint?

KF.transitionMatrix = *(Mat_<float>(2, 2) << 1, 0, 1, 0......);  

H_k: KF.measurementMatrix:

[cpp]view
plaincopyprint?

setIdentity(KF.measurementMatrix);  

Q_k: KF.processNoiseCov

[cpp]view
plaincopyprint?

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

R_k: KF.measurementNoiseCov

[cpp]view
plaincopyprint?

setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));  

P_k: KF.errorCovPost

[cpp]view
plaincopyprint?

setIdentity(KF.errorCovPost, Scalar::all(1));  

注意：其中: KF.transitionMatrix ：

                                         (1,0,1,0,
                                          0,1,0,1,
                                       0,0,1,0,
                                         0,0,0,1 );

2.預測predict，讀出預測的位置

3.更新updata

3.1.更新觀測矩陣：updata/generate measurement

對於鼠標跟蹤：直接使用鼠標的實際位置更新，真實位置即爲觀測位置
對於自動更新：
[cpp] view plain copy print ?
1. randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0)));
3. // generate measurement
4. measurement += KF.measurementMatrix*state;

3.2.更新KF：KF.correct(measurement);

分別顯示前一狀態的位置：Point statePt = Point( (int)KF.statePost.at<float>(0), (int)KF.statePost.at<float>(1));;

預測位置：Point predictPt = Point( (int)prediction.at<float>(0), (int)prediction.at<float>(1));

觀測位置(真實位置)：mousePosition（由setMouseCallback("Kalman", mouseEvent);得到，遞歸方式通過measurement計算得到）

[cpp]view
plaincopyprint?

#include <opencv/cv.h>  

#include <opencv/highgui.h>  

#include <iostream>  

using namespace cv;  

using namespace std;  

const int winWidth = 800;  

const int winHeight = 600;  

Point mousePosition = Point(winWidth>>1, winHeight>>1);  

//mouse call back  

void mouseEvent(int event, int x, int y, int flags, void *param)  

{  

    if(event==CV_EVENT_MOUSEMOVE)  

    {  

        mousePosition=Point(x,y);  

    }  

}  

int main()  

{  

    //1.kalman filter setup     

    const int stateNum=4;    

    const int measureNum=2;    

    KalmanFilter KF(stateNum, measureNum, 0);  

    Mat state (stateNum, 1, CV_32FC1); //state(x,y,detaX,detaY)  

    Mat processNoise(stateNum, 1, CV_32F);  

    Mat measurement = Mat::zeros(measureNum, 1, CV_32F);    //measurement(x,y)  

        randn( state, Scalar::all(0), Scalar::all(0.1) ); //隨機生成一個矩陣，期望是0，標準差爲0.1;  

        KF.transitionMatrix = *(Mat_<float>(4, 4) <<   

            1,0,1,0,   

            0,1,0,1,   

            0,0,1,0,   

            0,0,0,1 );//元素導入矩陣，按行;  

        //setIdentity: 縮放的單位對角矩陣;  

        //!< measurement matrix (H) 觀測模型  

        setIdentity(KF.measurementMatrix);  

        //!< process noise covariance matrix (Q)  

        // wk 是過程噪聲，並假定其符合均值爲零，協方差矩陣爲Qk(Q)的多元正態分佈;  

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

        //!< measurement noise covariance matrix (R)  

        //vk 是觀測噪聲，其均值爲零，協方差矩陣爲Rk,且服從正態分佈;  

        setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));  

        //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/  A代表F: transitionMatrix  

        //預測估計協方差矩陣;  

        setIdentity(KF.errorCovPost, Scalar::all(1));  

        //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))  

        //initialize post state of kalman filter at random   

        randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));  

        Mat showImg(winWidth, winHeight,CV_8UC3);  

        for(;;)  

        {  

            setMouseCallback("Kalman", mouseEvent);  

            showImg.setTo(0);  

            Point statePt = Point( (int)KF.statePost.at<float>(0), (int)KF.statePost.at<float>(1));  

            //2.kalman prediction     

            Mat prediction = KF.predict();  

            Point predictPt = Point( (int)prediction.at<float>(0), (int)prediction.at<float>(1));  

            //3.update measurement  

            measurement.at<float>(0)= (float)mousePosition.x;  

            measurement.at<float>(1) = (float)mousePosition.y;  

            //4.update  

            KF.correct(measurement);  

            //randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));  

            //state = KF.transitionMatrix*state + processNoise;  

            //draw  

            circle(showImg, statePt, 5, CV_RGB(255,0,0),1);//former point  

            circle(showImg, predictPt, 5, CV_RGB(0,255,0),1);//predict point  

            circle(showImg, mousePosition, 5, CV_RGB(0,0,255),1);//ture point  

//          CvFont font;//字體  

//          cvInitFont(&font, CV_FONT_HERSHEY_SCRIPT_COMPLEX, 0.5f, 0.5f, 0, 1, 8);  

            putText(showImg, "Red: Former Point", cvPoint(10,30), FONT_HERSHEY_SIMPLEX, 1 ,Scalar :: all(255));  

            putText(showImg, "Green: Predict Point", cvPoint(10,60), FONT_HERSHEY_SIMPLEX, 1 ,Scalar :: all(255));  

            putText(showImg, "Blue: Ture Point", cvPoint(10,90), FONT_HERSHEY_SIMPLEX, 1 ,Scalar :: all(255));  

            imshow( "Kalman", showImg );  

            int key = waitKey(3);  

            if (key == 27)  

            {  

                break;  

            }  

        }  

}