Opencv卡爾曼濾波器預測沒有新的觀測

Question

我想使用Opencv卡爾曼濾波器實現平滑的一些噪聲點。所以我試着爲它編寫一個簡單的測試。

比方說，我有一個觀察（一點）。我正在接收新觀測的每個幀，我稱卡爾曼預測和卡爾曼正確。 opencv卡爾曼濾波器正確後的狀態是「跟隨點」，沒關係。然後讓我們說我有一個缺失的觀察，我想要卡爾曼濾波器更新和預測新的狀態。在這裏，我的代碼失敗了：如果我調用kalman.predict（），則值不再更新。

這裏是我的代碼：

#include <iostream> 
#include <vector> 
#include <sys/time.h> 

#include <opencv2/highgui/highgui.hpp> 
#include <opencv2/video/tracking.hpp> 

using namespace cv; 
using namespace std; 

//------------------------------------------------ convenience method for 
//             using kalman filter with 
//             Point objects 
cv::KalmanFilter KF; 
cv::Mat_<float> measurement(2,1); 
Mat_<float> state(4, 1); // (x, y, Vx, Vy) 

void initKalman(float x, float y) 
{ 
    // Instantate Kalman Filter with 
    // 4 dynamic parameters and 2 measurement parameters, 
    // where my measurement is: 2D location of object, 
    // and dynamic is: 2D location and 2D velocity. 
    KF.init(4, 2, 0); 

    measurement = Mat_<float>::zeros(2,1); 
    measurement.at<float>(0, 0) = x; 
    measurement.at<float>(0, 0) = y; 


    KF.statePre.setTo(0); 
    KF.statePre.at<float>(0, 0) = x; 
    KF.statePre.at<float>(1, 0) = y; 

    KF.statePost.setTo(0); 
    KF.statePost.at<float>(0, 0) = x; 
    KF.statePost.at<float>(1, 0) = y; 

    setIdentity(KF.transitionMatrix); 
    setIdentity(KF.measurementMatrix); 
    setIdentity(KF.processNoiseCov, Scalar::all(.005)); //adjust this for faster convergence - but higher noise 
    setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1)); 
    setIdentity(KF.errorCovPost, Scalar::all(.1)); 
} 

Point kalmanPredict() 
{ 
    Mat prediction = KF.predict(); 
    Point predictPt(prediction.at<float>(0),prediction.at<float>(1)); 
    return predictPt; 
} 

Point kalmanCorrect(float x, float y) 
{ 
    measurement(0) = x; 
    measurement(1) = y; 
    Mat estimated = KF.correct(measurement); 
    Point statePt(estimated.at<float>(0),estimated.at<float>(1)); 
    return statePt; 
} 

//------------------------------------------------ main 

int main (int argc, char * const argv[]) 
{ 
    Point s, p; 

    initKalman(0, 0); 

    p = kalmanPredict(); 
    cout << "kalman prediction: " << p.x << " " << p.y << endl; 
    /* 
    * output is: kalman prediction: 0 0 
    * 
    * note 1: 
    *   ok, the initial value, not yet new observations 
    */ 

    s = kalmanCorrect(10, 10); 
    cout << "kalman corrected state: " << s.x << " " << s.y << endl; 
    /* 
    * output is: kalman corrected state: 5 5 
    * 
    * note 2: 
    *   ok, kalman filter is smoothing the noisy observation and 
    *   slowly "following the point" 
    *   .. how faster the kalman filter follow the point is 
    *   processNoiseCov parameter 
    */ 

    p = kalmanPredict(); 
    cout << "kalman prediction: " << p.x << " " << p.y << endl; 
    /* 
    * output is: kalman prediction: 5 5 
    * 
    * note 3: 
    *   mhmmm, same as the last correction, probabilly there are so few data that 
    *   the filter is not predicting anything.. 
    */ 

    s = kalmanCorrect(20, 20); 
    cout << "kalman corrected state: " << s.x << " " << s.y << endl; 
    /* 
    * output is: kalman corrected state: 10 10 
    * 
    * note 3: 
    *   ok, same as note 2 
    */ 

    p = kalmanPredict(); 
    cout << "kalman prediction: " << p.x << " " << p.y << endl; 
    s = kalmanCorrect(30, 30); 
    cout << "kalman corrected state: " << s.x << " " << s.y << endl; 
    /* 
    * output is: kalman prediction: 10 10 
    *   kalman corrected state: 16 16 
    * 
    * note 4: 
    *   ok, same as note 2 and 3 
    */  


    /* 
    * now let's say I don't received observation for few frames, 
    * I want anyway to update the kalman filter to predict 
    * the future states of my system 
    * 
    */ 
    for(int i=0; i<5; i++) { 
     p = kalmanPredict(); 
     cout << "kalman prediction: " << p.x << " " << p.y << endl; 
    } 
    /* 
    * output is: kalman prediction: 16 16 
    * kalman prediction: 16 16 
    * kalman prediction: 16 16 
    * kalman prediction: 16 16 
    * kalman prediction: 16 16 
    * 
    * !!! kalman filter is still on 16, 16.. 
    *  no future prediction here.. 
    *  I'm exprecting the point to go further.. 
    *  why??? 
    * 
    */  

    return 0; 
} 

我認爲這個代碼是非常說明什麼，我不明白。我試圖按照some theory和一些practical example，但不還是unserstand如何讓未來位置的新的預測..

任何人都可以幫助我明白我做錯了嗎？

Answer 1

我沒有設置過渡和測量矩陣。

我已經在這個excellent MATLAB documentation page中找到了這些矩陣的標準狀態空間值。

Answer 2

在每次預測之後，您應該將預測狀態（statePre）複製到校正狀態（statePost）中。這也應該用於狀態協方差（errorCovPre - > errorCovPost）。這可以防止過濾器在沒有執行更正時陷入狀態。原因是predict（）使用statePost中存儲的狀態值，如果不調用更正，這些狀態值不會更改。然後

你kalmanPredict功能如下：

Point kalmanPredict() 
{ 
    Mat prediction = KF.predict(); 
    Point predictPt(prediction.at<float>(0),prediction.at<float>(1)); 

    KF.statePre.copyTo(KF.statePost); 
    KF.errorCovPre.copyTo(KF.errorCovPost); 

    return predictPt; 
} 

Answer 3

爲這些人誰仍然在使用OpenCV的卡爾曼濾波

上面貼的代碼後，小改款效果很好的問題。您可以設置如下，而不是將轉換矩陣設置爲Identity。

修改

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

輸出