Ask Your Question
0

Kalman Filter Question

asked 2015-09-30 10:50:54 -0500

Nbb gravatar image

updated 2015-09-30 12:25:10 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

this question is also interesting

sturkmen gravatar imagesturkmen ( 2015-09-30 11:35:10 -0500 )edit

1 answer

Sort by ยป oldest newest most voted
1

answered 2015-09-30 11:39:12 -0500

LBerger gravatar image

updated 2015-09-30 14:49:59 -0500

It's an answer but that's not my program. Please don't vote. I have used many WEB samples and it gives

#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;

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;

    KF.transitionMatrix = (Mat_<float>(6, 6) << 1,0,1,0,0.5,0, 0,1,0,1,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));

        measurement(0) = mouse_info.x;
        measurement(1) = mouse_info.y;
        Point measPt(measurement(0),measurement(1));
        cout << norm(predictPt - measPt) << "\t";
        {
              Point2f predictPt(prediction.at<float>(0),prediction.at<float>(1));
              Point2f measPt(measurement(0),measurement(1));
              cout << norm(predictPt - measPt) << "\n";
       }


        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);


        img = Scalar::all(0);
        drawCross( statePt, Scalar(255,255,255), 5 );
        drawCross( measPt, Scalar(0,0,255), 5 );

        for (int i = 0; i < mousev.size()-1; i++) 
        {
            line(img, mousev[i], mousev[i+1], Scalar(255,255,0), 1);
        }
        for (int i = 0; i < kalmanv.size()-1; i++) 
        {
            line(img, kalmanv[i], kalmanv[i+1], Scalar(0,255,0), 1);
        }



        imshow( "mouse kalman", img );
        code = (char)waitKey(10);

        if( code > 0 )
            break;
        }

    return 0;
}
edit flag offensive delete link more

Comments

It's funny too with (no speed and no acceleration)

    KF.transitionMatrix = (Mat_<float>(6, 6) << 1,0,0,0,0.,0, 0,1,0,0,0,0.0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0);
LBerger gravatar imageLBerger ( 2015-09-30 11:50:38 -0500 )edit

Hello,

Thanks for the reply. But what should I change such that the prediction overshoots and kalman filter fails ? For the particle filter, all I would have to do is set the velocity (dt) to be really high. I have edited the code above yet it only overshoots the moment my cursor stops. The prediction is always lagging behind my cursor.

What I would like to see is, when I am dragging my cursor from left to right, the prediction will overshoot and not stay to the left (lag behind) of my cursor.

In the edited code, only the mouse and predicted position is displayed on the window.

Again, thanks for the help

Nbb gravatar imageNbb ( 2015-09-30 12:24:14 -0500 )edit

Based on my understanding of kalman filter for tracking, I am supposed to predict the position of the object in the next frame before I sample the region. If my prediction is always lagging behind the object, it will be sampling the wrong region and hence how does the kalman filter correct/update itself ?

I hope my partial understanding of the Kalman filter is not wrong.. I have good understanding of the particle filter and am trying to understand the kalman filter. It is very easy to have the particle filter's prediction overshoot.. Just set the velocity really high. How do I obtain a similar behaviour with the kalman filter ? Or is that not how the kalman filter works ? ie. it will never overshoot in its prediction ?

Nbb gravatar imageNbb ( 2015-09-30 12:28:25 -0500 )edit

How do you check error distance between estimate and mouse position?

LBerger gravatar imageLBerger ( 2015-09-30 14:50:50 -0500 )edit
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2015-09-30 10:50:54 -0500

Seen: 827 times

Last updated: Sep 30 '15