Kalman filter - Null predicted point(s)

Question

I am trying to apply a Kalman Filter in C++ with OpenCV in order to filter some tracks. The first step to make it work for me was to predict the points with the filter from a vector of Points2f.

My code is the following one :

cv::KalmanFilter kalmanFilter(4,2,0, CV_32F); 
kalmanFilter.transitionMatrix = transitionMat;
for(int i = 0 ; i < oldTrackeables.size() ; i++)
    for(int j = 0 ; j < oldTrackeables[i].getTrack().size() ; j++)
              {
                  cv::Size msmtSize(2,1);
                  cv::Mat measurementMat(msmtSize, CV_32F);
                  measurementMat.setTo(cv::Scalar(0));
                  measurementMat.at<float>(0) = oldTrackeables[i].getTrack()[j].x;
                  measurementMat.at<float>(1) = oldTrackeables[i].getTrack()[j].y;

                  //Initialisation of the Kalman filter
                  kalmanFilter.statePre.at<float>(0) = (float) oldTrackeables[i].getTrack()[j].x;
                  kalmanFilter.statePre.at<float>(1) = (float) oldTrackeables[i].getTrack()[j].y;
                  kalmanFilter.statePre.at<float>(2) = (float) 2;
                  kalmanFilter.statePre.at<float>(3) = (float) 3;


                 cv::setIdentity(kalmanFilter.measurementMatrix);
                 cv::setIdentity(kalmanFilter.processNoiseCov, cv::Scalar::all(1e-4));
                 cv::setIdentity(kalmanFilter.measurementNoiseCov, cv::Scalar::all(.1));
                 cv::setIdentity(kalmanFilter.errorCovPost, cv::Scalar::all(.1));

                 //Prediction
                 cv::Mat prediction = kalmanFilter.predict();

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

                 cv::Point predictPt(prediction.at<float>(0), prediction.at<float>(1));
                 cv::Point Mc = oldTrackeables[i].getMassCenter();          

                 cv::circle(kalmat, predictPt, 16, cv::Scalar(0,255,0), 3, 2, 1);


                 std::cout<<"prediction : x = " << predictPt.x << " - y = " << predictPt.y <<std::endl;
                 std::cout<<"position captée : x = " << oldTrackeables[i].getTrack()[j].x << " - y = " << oldTrackeables[i].getTrack()[j].y << std::endl;
                 std::cout<<"size of frame : rows = " << frame.rows << " - width = " << frame.cols <<std::endl;
                 std::cout<<"size of kalmat : rows = " << kalmat.rows << " - width = " << kalmat.cols <<std::endl;
                 cv::imshow("kalmat", kalmat);

Where oldTrackeables[i].getTrack()[j] are just some Points2f from a vector.

The tracking is correct, but the Kalman filter does not give "correct" values for the prediction - For example, the program displays : prediction : x = 0 - y = 0 - position captée : x = 138.29 - y = 161.078 (position of the original point).

I've really been looking a lot for answers and trying many different ways to do it but I can't find anything that really helps me... The closer one I found was this one : http://answers.opencv.org/question/24865/why-kalman-filter-keeps-returning-the-same-prediction/ But it did not help me solve my problem...

If any of you has an element of answer of could help me understand the problem, I'd be very grateful. Thank you.

Answer 1

First of all I would have moved all the init stuff outside the loop otherwise you will override the internal states in the filter. Also change the statePre to statPost

  //Initialisation of the Kalman filter
  kalmanFilter.statePost.at<float>(0) = (float) 0;   
  kalmanFilter.statePost.at<float>(1) = (float) 0;
  kalmanFilter.statePost.at<float>(2) = (float) 2;
  kalmanFilter.statePost.at<float>(3) = (float) 3;

  cv::setIdentity(kalmanFilter.measurementMatrix);
  cv::setIdentity(kalmanFilter.processNoiseCov, cv::Scalar::all(1e-4));
  cv::setIdentity(kalmanFilter.measurementNoiseCov,cv::Scalar::all(.1));
  cv::setIdentity(kalmanFilter.errorCovPost, cv::Scalar::all(.1));

The part:

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

should be removed since this is done internally in the predict phase.

Finally as @Mozfox says, the correct phase is not present in the loop code you provided. Add:

kalmanFilter.predict(measurementMat);