Kalman Filter always predicts 0

asked 2015-05-15 03:57:30 -0500

karmaDet gravatar image

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 ... (more)

edit retag flag offensive close merge delete

Comments

1

imho, you should not mess with KF.statePre, KF.statePost, or KF.errorCovPre, KF.errorCovPost at all.

berak gravatar imageberak ( 2015-05-15 08:02:43 -0500 )edit

Yes, you should just add new measurements for the tracker or remove the bad tracker and add a new correct one ;)

thdrksdfthmn gravatar imagethdrksdfthmn ( 2015-05-15 10:14:48 -0500 )edit