Opencv kalman filter prediction without new observtion

Question

I wan to use Opencv Kalman filter implementation for smooth some noise points. So I've tried to code a simple test for it.

Let's say I have an observation (a point). Each frame I'm receiving new observation, I call Kalman predict and Kalman correct. The state coming after opencv Kalman filter correct is "following the point", it is ok.

Then let's say I have a missing observation, I want anyway the Kalman filter to be updated and predict the new state. Here my code is failing: if I call kalman.predict() the value is no more updated.

Here is my code:

#include <iostream>
#include <vector>
#include <sys/time.h>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>

using namespace cv;
using namespace std;

//------------------------------------------------ convenience method for 
//                                                 using kalman filter with 
//                                                 Point objects
cv::KalmanFilter KF;
cv::Mat_<float> measurement(2,1); 
Mat_<float> state(4, 1); // (x, y, Vx, Vy)

void initKalman(float x, float y)
{
    // Instantate Kalman Filter with
    // 4 dynamic parameters and 2 measurement parameters,
    // where my measurement is: 2D location of object,
    // and dynamic is: 2D location and 2D velocity.
    KF.init(4, 2, 0);

    measurement = Mat_<float>::zeros(2,1);
    measurement.at<float>(0, 0) = x;
    measurement.at<float>(0, 0) = y;


    KF.statePre.setTo(0);
    KF.statePre.at<float>(0, 0) = x;
    KF.statePre.at<float>(1, 0) = y;

    KF.statePost.setTo(0);
    KF.statePost.at<float>(0, 0) = x;
    KF.statePost.at<float>(1, 0) = y; 

    setIdentity(KF.transitionMatrix);
    setIdentity(KF.measurementMatrix);
    setIdentity(KF.processNoiseCov, Scalar::all(.005)); //adjust this for faster convergence - but higher noise
    setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
    setIdentity(KF.errorCovPost, Scalar::all(.1));
}

Point kalmanPredict() 
{
    Mat prediction = KF.predict();
    Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
    return predictPt;
}

Point kalmanCorrect(float x, float y)
{
    measurement(0) = x;
    measurement(1) = y;
    Mat estimated = KF.correct(measurement);
    Point statePt(estimated.at<float>(0),estimated.at<float>(1));
    return statePt;
}

//------------------------------------------------ main

int main (int argc, char * const argv[]) 
{
    Point s, p;

    initKalman(0, 0);

    p = kalmanPredict();
    cout << "kalman prediction: " << p.x << " " << p.y << endl;
    /* 
     * output is: kalman prediction: 0 0
     *
     * note 1:
     *         ok, the initial value, not yet new observations
     */

    s = kalmanCorrect(10, 10);
    cout << "kalman corrected state: " << s.x << " " << s.y << endl;
    /* 
     * output is: kalman corrected state: 5 5
     *
     * note 2:
     *         ok, kalman filter is smoothing the noisy observation and 
     *         slowly "following the point"
     *         .. how faster the kalman filter follow the point is 
     *            processNoiseCov parameter
     */

    p = kalmanPredict();
    cout << "kalman prediction: " << p.x << " " << p.y << endl;
    /* 
     * output is: kalman prediction: 5 5
     *
     * note 3:
     *         mhmmm, same as the last correction, probabilly there are so few data that
     *         the filter is not predicting anything..
     */

    s = kalmanCorrect(20, 20);
    cout << "kalman corrected state: " << s.x << " " << s.y << endl;
    /* 
     * output is: kalman corrected state: 10 10
     *
     * note 3:
     *         ok, same as note 2
     */

    p = kalmanPredict();
    cout << "kalman prediction: " << p.x << " " << p.y << endl;
    s = kalmanCorrect(30, 30);
    cout << "kalman corrected state: " << s.x << " " << s.y << endl;
    /* 
     * output is: kalman prediction: 10 10
     *            kalman corrected state: 16 16
     *
     * note 4:
     *         ok, same as note 2 and 3
     */    


    /*
     * now let's say I don't received observation for few frames,
     * I want anyway to update the kalman filter to predict 
     * the future states of my system
     *
     */
    for(int i=0; i<5; i++) {
        p = kalmanPredict();
        cout << "kalman prediction: " << p.x << " " << p.y << endl;
    }
    /* 
     * output is: kalman prediction: 16 16
     * kalman prediction: 16 16
     * kalman prediction: 16 16
     * kalman prediction: 16 16
     * kalman prediction: 16 16
     *
     * !!! kalman filter is still on 16, 16..
     *     no future prediction here..
     *     I'm exprecting the point to go further..
     *     why???
     *
     */    

    return 0;
}

I think that this code is quite illustrative of what I don't understand. I've tried to follow some theory and some practical example but don't still unserstand how to get new prediction of the future position..

Anyone can help me in understand what I'm doing wrong?

Answer 1

For those people who still have problem in using OpenCV Kalman filtering

The above posted code works well after small modification. Instead of setting transition matrix to Identity, you can set as follows.

Modification

KF.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0,   0,1,0,1,  0,0,1,0,  0,0,0,1);  

Output

Answer 2

I didn't set the transition and measurement matrix.

I have found standard state-space values for those matrix in this excellent MATLAB documentation page.

Answer 3

After every prediction, you should copy the predicted state (statePre) into the corrected state (statePost). This should also be done for the state covariance (errorCovPre -> errorCovPost). This prevents the filter from getting stuck in a state when no corrections are executed. The reason is that predict() makes use of the state values stored in statePost, that do not change if no corrections are called.

Your kalmanPredict function will then be as follows:

Point kalmanPredict() 
{
    Mat prediction = KF.predict();
    Point predictPt(prediction.at<float>(0),prediction.at<float>(1));

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

    return predictPt;
}