opencv Kalman filter multiple object tracking error

Question

I have been trying to work on multiple object tracking using Kalman Filter. Here is my code,

for (int i =0; i<vGlobal.size(); i++) // Vector of objects of interest
    {
        cv::Point pTemp = cv::Point(vGlobal[i].iX, vGlobal[i].iY);
        cv::KalmanFilter kTempKF(4,2,0);
        kTempKF.statePre.at<floatt>(0) = pTemp.x;
        kTempKF.statePre.at<float>(1) = pTemp.y;
        kTempKF.statePre.at<float>(2) = 0;
        kTempKF.statePre.at<float>(3) = 0;

        kTempKF.transitionMatrix = *(cv::Mat_<float>(4,4)<< 1,0,1,0,  0,1,0,1,  0,0,1,0,  0,0,0,1);
        cv::setIdentity(kTempKF.measurementMatrix);
        cv::setIdentity(kTempKF.processNoiseCov, cv::Scalar::all(1e-4));            
        cv::setIdentity(kTempKF.measurementNoiseCov, cv::Scalar::all(10));
        cv::setIdentity(kTempKF.errorCovPost, cv::Scalar::all(.1));
        vKalmanFilters.push_back(kTempKF);  
    }

I am using vector of Kalman filters to track each of my objects. I have done initialization of the filters as above. Now I try to work on predition and measurement as follows in the below code.

 for (int i=0; i<vKalmanFilters.size();i++)
    {
        cv::Mat mPrediction = vKalmanFilters[i].predict(); 

        cv::Point pPredict(mPrediction.at<float>(0), mPrediction.at<float>(1));
        mMeasurement(0) = vGlobal[i].iX;
        mMeasurement(1) = vGlobal[i].iY;


        cv::Mat mEstimated;


            mEstimated = vKalmanFilters[i].correct(mPrediction); // Run time Error occurs here


     }

When I try to run this program, I get a runtime error in the correct(Prediction)

  OpenCV Error: Assertion failed (C.type() == type && (((flags&GEMM_3_T) == 0 && C.rows == d_size.height && C.cols == d_size.width) || ((flags&GEMM_3_T) != 0 && C.rows == d_size.width && C.cols == d_size.height))) in gemm, file /build/buildd/opencv-2.4.8+dfsg1/modules/core/src/matmul.cpp, line 741
 terminate called after throwing an instance of 'cv::Exception'
 what():  /build/buildd/opencv-2.4.8+dfsg1/modules/core/src/matmul.cpp:741: error: (-215) C.type() == type && (((flags&GEMM_3_T) == 0 && C.rows == d_size.height && C.cols == d_size.width) || ((flags&GEMM_3_T) != 0 && C.rows == d_size.width && C.cols == d_size.height)) in function gemm

I am still a beginner in Kalman Filter. The error occurs at the point of prediction. Is my approach totally wrong ? Please someone explain where I am going wrong.

Answer 1

You should use mMeasurement (2x1 matrix) instead of mPrediction (4x1 matrix) in the correction step:

mEstimated = vKalmanFilters[i].correct(mMeasurement);

Given that you did:

cv::KalmanFilter kTempKF(4,2,0);
// 4 dynamic params <-- your state, e.g. [x y dx dy]
// 2 measurements params <-- your mMeasurement [x y]