Kalman Filter always predicts 0
I am trying to track a detected object and the code I am using as follows:
// if there is a detection
if (detectedObjectRectanglesPreviousFrame.size() != 0)
{
cout << "#: " << detectedObjectRectanglesPreviousFrame.size() << endl;
//-----------------------------------------------------------
KalmanFilter KF(4, 2, 0);
Mat_<float> state(4, 1); /* (x, y, Vx, Vy) */
Mat processNoise(4, 1, CV_32F);
Mat_<float> measurement(2, 1);
measurement.setTo(Scalar(0));
//-----------------------------------------------------------
for (int i = 0; i < detectedObjectRectanglesPreviousFrame.size(); i++)
{
// Get the center coordinates of the detected object
int trackedX = detectedObjectRectanglesPreviousFrame[i].x + detectedObjectRectanglesPreviousFrame[i].width / 2;
int trackedY = detectedObjectRectanglesPreviousFrame[i].y + detectedObjectRectanglesPreviousFrame[i].height / 2;
cout << "P" << Point(trackedX, trackedY) << endl;
// if this is the first detection, initialize kalman filter
if (firstDetection == true)
{
//---------------------------------------------------------------------------------------------
KF.statePre.at<float>(0) = trackedX;
KF.statePre.at<float>(1) = trackedY;
KF.statePre.at<float>(2) = 0;
KF.statePre.at<float>(3) = 0;
KF.transitionMatrix = *(Mat_<float>(4, 4) << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
//KF.transitionMatrix = *(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
setIdentity(KF.transitionMatrix);
setIdentity(KF.measurementMatrix);
setIdentity(KF.processNoiseCov, Scalar::all(1e-4));
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-4));
setIdentity(KF.errorCovPost, Scalar::all(.1));
points_vector.clear();
kalman_vector.clear();
//---------------------------------------------------------------------------------------------
cout << "First time-> Initialize: " << trackedX << "," << trackedY << endl;
firstDetection = false;
}
// if not, start tracking
else
{
//---------------------------------------------------------------------------------------------
Mat prediction = KF.predict();
Point predictPt(prediction.at<float>(0), prediction.at<float>(1));
KF.statePre.copyTo(KF.statePost);
KF.errorCovPre.copyTo(KF.errorCovPost);
measurement(0) = trackedX;
measurement(1) = trackedY;
Point measPt(measurement(0), measurement(1));
points_vector.push_back(measPt);
Mat estimated = KF.correct(measurement);
Point statePt(estimated.at<float>(0), estimated.at<float>(1));
kalman_vector.push_back(statePt);
for (int i = 0; i < kalman_vector.size() - 1; i++)
{
line(sourceFrame, kalman_vector[i], kalman_vector[i + 1], Scalar(0, 255, 0), 1);
}
for (int j = 0; j < kalman_vector.size(); j++)
{
cout << "Kalman Vector Element: " << kalman_vector[j] << endl;
}
//---------------------------------------------------------------------------------------------
}
}
}
And unfortunately, the output I am getting is :
#: 1
P[261, 259]
First time-> Initialize: 261,259
#: 1
P[260, 259]
Kalman Vector Element: [0, 0]
#: 1
P[258, 260]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
#: 1
P[254, 263]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
#: 1
P[255, 263]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
#: 1
P[254, 262]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
#: 1
P[252, 262]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
#: 1
P[252, 263]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
Kalman Vector Element: [0, 0]
As you can ...
imho, you should not mess with KF.statePre, KF.statePost, or KF.errorCovPre, KF.errorCovPost at all.
Yes, you should just add new measurements for the tracker or remove the bad tracker and add a new correct one ;)