Kalman Filter not giving right results

Question

I am a beginner in Kalman filter tracking and I am following the tutorial (http://opencvexamples.blogspot.com/2014/01/kalman-filter-implementation-tracking.html) to implement multiple objects tracking. I have a structure object and I have a kalman filter inside it as follows.

 struct sAsparagus
    {
        int iId;
        int iFrameId;
        int iWidth;
        int iHeight;
        int iX;
        int iY;
        int iZ;
        cv::KalmanFilter KF;
    };

Then, I am trying to initialize the values obtained from the blob detection as follows.

 for (CvBlobs::const_iterator it = blobs.begin(); it !=blobs.end();++it)
    {
        sAsparagus sAsp;
        sAsp.iFrameId = iCounter;
        sAsp.iWidth = (it->second->maxx - it->second->minx);
        sAsp.iHeight = (it->second->maxy - it->second->miny);
        sAsp.iX = it->second->centroid.x;
        sAsp.iY = it->second->centroid.y;
        sAsp.KF = cv::KalmanFilter(4, 2, 0);
        sAsp.KF.transitionMatrix = *(cv::Mat_<float>(4,4)<<1,0,1,0,   0,1,0,1,   0,0,1,0,   0,0,0,1);
        sAsp.KF.statePre.at<float>(0) = sAsp.iX;
        sAsp.KF.statePre.at<float>(1) = sAsp.iY;
        sAsp.KF.statePre.at<float>(2) = 0;
        sAsp.KF.statePre.at<float>(3) = 0;
        setIdentity(sAsp.KF.measurementMatrix);
        setIdentity(sAsp.KF.processNoiseCov,  cv::Scalar::all(1e-2));
        setIdentity(sAsp.KF.measurementNoiseCov, cv::Scalar::all(10));
        setIdentity(sAsp.KF.errorCovPost, cv::Scalar::all(.1));
        vGlobal.push_back(sAsp);
    }

Then, I tried to use the predict and correct functions as follows.

 for (int i =0; i<vGlobal.size(); i++)
    {
        cv::Mat_<float> measurement(2,1); measurement.setTo(cv::Scalar(0));
        cv::Mat prediction = vGlobal[i].KF.predict();
        cv::Point pPredict(prediction.at<float>(0), prediction.at<float>(1));

        measurement(0) = vGlobal[i].iX;
        measurement(1) = vGlobal[i].iY;

        cv::Mat mEstimated = vGlobal[i].KF.correct(measurement);

        std::cout<<"Prediction values: "<<pPredict.x<<", "<<pPredict.y<<std::endl;
        cv::Point pEstimated(mEstimated.at<float>(0), mEstimated.at<float>(1));
        std::cout<<"Measurement values: "<<measurement(0)<<", "<<measurement(1)<<std::endl;
        std::cout<<"Estimated values: "<<pEstimated.x<<", "<<pEstimated.y<<std::endl;
 }

But I am not getting the right results. The sample output for the above program is

  Prediction values: 0, 0
  Measurement values: 368, 511
  Estimated values: 7, 10

I think these results are not right. I need a value similar to measurement values. Where I am going wrong ?

Answer 1

You should set statePost, not statePre

sAsp.KF.statePost.at<float>(0) = sAsp.iX;
sAsp.KF.statePost.at<float>(1) = sAsp.iY;
sAsp.KF.statePost.at<float>(2) = 0;
sAsp.KF.statePost.at<float>(3) = 0;

Without control matrix, predict() does this:

statePre = TransitionMatrix * statePost