Estimation de Kalman OpenCV sans mesure

Question

Hé là :) J'ai commencé à travailler avec le Kalmanfilter dans OpenCv. J'ai rencontré 2 problèmes.

Le premier est, que j'ai utilisé l'exemple de http://www.morethantechnical.com/2011/06/17/simple-kalman-filter-for-tracking-using-opencv-2-2-w-code/. Dans la vidéo, il semble que le bruit soit réduit par le Kalmanfilter, mais si je cours mon code le bruit n'est pas réduit, donc si je bouge ma souris la prédiction se comporte presque exactement comme le mouvement de la souris sans réduire le bruit. Est-ce dû à mon taux d'échantillonnage élevé ou les valeurs de measurementMatrix, processNoiseCov, measurementNoiseCov et errorCovPost sont-elles mauvaises? J'espère que vous pouvez comprendre ce que je veux dire.

Mon autre problème est que si j'appuie sur "n" je veux désactiver les nouvelles mesures et je veux que Kalmanfilter devine encore la nouvelle position de la souris. Dans http://code.opencv.org/issues/1380 Mircea Popa a déclaré:

"mais mettre la matrice de mesure à 0 avant l'appel kalman.correct() (c'est le devoir du programmeur) .Pour ce faire, le facteur de correction (le gain) devient 0, et l'état du filtre est mis à jour en fonction de ce qui a été prévu. "

Donc j'ai essayé de faire comme ceci: mesure = Mat :: zeros (2,1, CV_32F) mais alors juste la position de la prédiction avait été à la position (0,0) donc pas ce à quoi je m'attendais. Y a-t-il quelque chose que j'ai mal compris? La mesure n'est-elle pas la "matrice de mesure" dont parle Mircea Popa? Ou y a-t-il une autre façon de laisser KalmanFilter prédire la nouvelle position sans mesure? Pour clarifier ce que j'attendrais de Kalmanfilter: S'il n'y a pas de mesure, le mouvement de la position estimée devrait être uniforme et la direction du mouvement devrait être sur une ligne droite qui est déterminée par les deux dernières positions.

Voici mon code:

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

using namespace cv; 
using namespace std; 



int m_X = 0; 
int m_Y = 0; 
bool cursorSet = false; 
bool noDetection = false; 
Mat_<float> lastPosition(2,1); 


void drawCross(Mat &img, Point &center, Scalar color, int d) { 
    line(img, Point(center.x - d, center.y - d), Point(center.x + d, center.y + d), color, 2, CV_AA, 0); 
    line(img, Point(center.x + d, center.y - d), Point(center.x - d, center.y + d), color, 2, CV_AA, 0); 
}; 

void CallBackFunc(int event, int x, int y, int flags, void* userdata) { 
    m_X = x; 
    m_Y = y; 
    cursorSet = true; 
    cout << "Mouse move over the window - position (" << x << ", " << y << ")" << endl; 
}; 

void GetCursorPos(Point &mousepos){ 
    mousepos = Point(m_X, m_Y); 
}; 

int main() { 

    //create window and set callback for mouse movement 
    namedWindow("window", 1); 
    setMouseCallback("window", CallBackFunc, NULL); 

    // Image to show mouse tracking 
    Mat img(600, 800, CV_8UC3); 
    vector<Point> mousev,kalmanv; 
    mousev.clear(); 
    kalmanv.clear(); 

    //wait until mouse has an initial position inside the window 
    while(!cursorSet) { 
     imshow("window", img); 
     waitKey(10); 
    }; 



    //create kalman Filter 
    KalmanFilter KF(6, 2, 0); 

    //position of the mouse will be observed 
    Point mousePos; 
    GetCursorPos(mousePos); 

    // intialization of KF... 
    KF.transitionMatrix = *(Mat_<float>(6, 6) << 1,0,1,0,.5,0,  // x + v_x + 1/2 a_x 
               0,1,0,1,0,0.5,  // y + v_Y + 1/2 a_y 
               0,0,1,0,1,0,  //  v_x +  a_x 
               0,0,0,1,0,1,  //  v_y +  a_y  
               0,0,0,0,1,0,  //     a_x 
               0,0,0,0,0,1);  //     a_y 
    Mat_<float> measurement(2,1); measurement.setTo(Scalar(0)); 

    //initialize the pre state 
    KF.statePost.at<float>(0) = mousePos.x; 
    KF.statePost.at<float>(1) = mousePos.y; 
    KF.statePost.at<float>(2) = 0; 
    KF.statePost.at<float>(3) = 0; 
    KF.statePost.at<float>(4) = 0; 
    KF.statePost.at<float>(5) = 0; 


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


    while(1) { 
     img = Scalar::all(0); 
     // First predict, to update the internal statePre variable 
     Mat prediction = KF.predict(); 
     Point predictPt(prediction.at<float>(0),prediction.at<float>(1)); 

     // Get mouse point 
     if (!noDetection) { 
      GetCursorPos(mousePos); 
      measurement(0) = mousePos.x; 
      measurement(1) = mousePos.y; 
     }else { 
      measurement(0) = prediction.at<float>(0); 
      measurement(1) = prediction.at<float>(1); 
     } 

     // The update phase3 
     Mat estimated = KF.correct(measurement); 

     Point statePt(estimated.at<float>(0),estimated.at<float>(1)); 
     Point measPt(measurement(0),measurement(1)); 


     // draw cross for actual mouse position and kalman guess 
     mousev.push_back(measPt); 
     kalmanv.push_back(statePt); 
     drawCross(img, statePt, Scalar(255,255,255), 5); 
     drawCross(img, measPt, Scalar(0,0,255), 5); 

     // draw lines of movement 
     for (int i = 0; i < mousev.size()-1; i++) 
      line(img, mousev[i], mousev[i+1], Scalar(0,0,255), 1); 

     for (int i = 0; i < kalmanv.size()-1; i++) 
      line(img, kalmanv[i], kalmanv[i+1], Scalar(255,255,255), 1); 

     imshow("window", img);  
     char key = waitKey(10); 
     if (key == 'c') { 
      mousev.clear(); 
      kalmanv.clear(); // press c to clear screen 
     }if (key == 'n') { 
      noDetection = true; //press n to simulate that no measurement is made 
     }if (key == 'd') { 
      noDetection = false;//press d to allow measurements again 
     }else if(key == 'x') { 
      break;    // press x to exit program 
     }; 
    } 

    return 0; 
} 

Answer 1

Parlant d'une PoV plus généralisée ... si vous souhaitez avoir un filtre de Kalman qui adoucit le bruit de mesure, il faut compter davantage sur le modèle de processus et moins sur la mise à jour des mesures. Donc, dans votre cas, peaufiner le "measurementMatrix, processNoiseCov, measurementNoiseCov" peut vous conduire à une sortie plus douce.