Kalman Filter Question

edit

Hello forum,

I am trying to understand the theory behind kalman filter and I have a question to ask. I am following the tutorial shown here. http://opencvexamples.blogspot.com/20...

The predicted position of the cursor is always "lagging behind" no matter how high the velocity of the transition matrix is at. I even tried setting it to 100. What values should I change such that the filter predicts the position of my cursor way ahead ? In other words, I would like the prediction to not lag behind, just like the particle filter for tracking.

#include <iostream>
#include <vector>

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

using namespace cv;
using namespace std;


struct mouse_info_struct { int x, y; };
struct mouse_info_struct mouse_info = { -1, -1 }, last_mouse;


// plot points
#define drawCross( center, color, d )                                 \
line( img, Point( center.x - d, center.y - d ),                \
Point( center.x + d, center.y + d ), color, 2, CV_AA, 0); \
line( img, Point( center.x + d, center.y - d ),                \
Point( center.x - d, center.y + d ), color, 2, CV_AA, 0 )


vector<Point> mousev, kalmanv, predictv;

void on_mouse(int event, int x, int y, int flags, void* param) {
    {
        last_mouse = mouse_info;
        mouse_info.x = x;
        mouse_info.y = y;

        cout << "got mouse " << x << "," << y << endl;
    }
}


int main(int argc, char * const argv[])
{
    Mat img(500, 500, CV_8UC3);
    KalmanFilter KF(6, 2, 0);
    Mat_<float> state(6, 1);
    Mat processNoise(6, 1, CV_32F);
    Mat_<float> measurement(2, 1); measurement.setTo(Scalar(0));
    char code = (char)-1;

    namedWindow("mouse kalman");
    setMouseCallback("mouse kalman", on_mouse, 0);

    for (;;)
    {
        if (mouse_info.x < 0 || mouse_info.y < 0)
        {
            imshow("mouse kalman", img);
            waitKey(30);
            continue;
        }
        else
            break;
    }
    KF.statePre.at<float>(0) = mouse_info.x;
    KF.statePre.at<float>(1) = mouse_info.y;
    KF.statePre.at<float>(2) = 0;
    KF.statePre.at<float>(3) = 0;
    KF.statePre.at<float>(4) = 0;
    KF.statePre.at<float>(5) = 0;

    int dt_x = 0;
    int dt_y = 0;

    KF.transitionMatrix = (Mat_<float>(6, 6) << 1, 0, dt_x, 0, 0.5, 0, 
                                                0, 1, 0, dt_y, 0, 0.5, 
                                                0, 0, 1, 0, 1, 0,
                                                0, 0, 0, 1, 0, 1, 
                                                0, 0, 0, 0, 1, 0, 
                                                0, 0, 0, 0, 0, 1);


    KF.measurementMatrix = (Mat_<float>(2, 6) << 1, 0, 1, 0, 0.5, 0, 0, 1, 0, 1, 0, 0.5);

    setIdentity(KF.measurementMatrix);
    setIdentity(KF.processNoiseCov, Scalar::all(1e-4));
    setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
    setIdentity(KF.errorCovPost, Scalar::all(.1));

    mousev.clear();
    kalmanv.clear();
    while (1 == 1)
    {

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

        if (predictv.size() > 1)
            for (int i = 0; i < predictv.size() - 1; i++)
        {
            line(img, predictv[i], predictv[i + 1], Scalar(255, 255, 255), 2);
        }

        measurement(0) = mouse_info.x;
        measurement(1) = mouse_info.y;

        Point measPt(measurement(0), measurement(1));
        mousev.push_back(measPt);

        Mat estimated = KF.correct(measurement);
        Point statePt(estimated.at<float>(0), estimated.at<float>(1));
        kalmanv.push_back(statePt);

        if (kalmanv.size() > 1)
        {
            dt_x = kalmanv.end()[-1].x - kalmanv.end()[-2].x;
            dt_y = kalmanv.end()[-1 ...