Previsione del filtro kalman Opencv senza nuova osservazione

Question

Desidero utilizzare l'implementazione del filtro Opencv Kalman per alcuni punti di disturbo. Quindi ho provato a codificare un semplice test per questo.

Diciamo che ho un'osservazione (un punto). Ogni frame che sto ricevendo nuove osservazioni, chiamo Kalman predicono e Kalman ha ragione. Lo stato in arrivo dopo il filtro opencv Kalman è corretto "seguendo il punto", è ok.

Quindi diciamo che ho un'osservazione mancante, voglio comunque che il filtro Kalman sia aggiornato e preveda il nuovo stato. Qui il mio codice non funziona: se chiamo kalman.predict() il valore non è più aggiornato.

Ecco il mio codice:

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

Penso che questo codice è abbastanza illustrativo di ciò che non capisco. Ho provato a seguire some theory e alcuni practical example ma non ancora unserstand come ottenere una nuova previsione della posizione futura ..

Chiunque può aiutarmi a capire cosa sto facendo male?

Answer 1

Non ho impostato la matrice di transizione e misurazione.

Ho trovato valori di spazio di stato standard per queste matrici in questo excellent MATLAB documentation page.

Answer 2

Dopo ogni previsione, è necessario copiare lo stato previsto (statePre) nello stato corretto (statePost). Questo dovrebbe essere fatto anche per la covarianza di stato (errorCovPre -> errorCovPost). Ciò impedisce al filtro di rimanere bloccato in uno stato quando non vengono eseguite correzioni. Il motivo è che predict() utilizza i valori di stato memorizzati in statePost, che non cambiano se non vengono chiamate correzioni.

La vostra funzione kalmanPredict sarà quindi la seguente:

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

Per quelle persone che hanno ancora problemi ad utilizzare il filtraggio Kalman OpenCV

Il codice di cui sopra postato funziona bene dopo la piccola modifica. Invece di impostare la matrice di transizione su Identità, è possibile impostare come segue.

modifica

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

uscita