Kalman Filter with acceleration

c++ opencv kalman-filter
ekmungi picture ekmungi · Aug 9, 2013 · Viewed 9.4k times · Source

I am trying to implement a Kalman filter based mouse tracking (as a test first) using a velocity-acceleration model.

I want to try this out this simple model, my state transition equations are:

X(k) = [x(k), y(k)]'   (Position)
V(k) = [vx(k), vy(k)]' (Velocity)
X(k) = X(k-1) + dt*V(k-1) + 0.5*dt*dt*a(k-1)
V(k) = V(k-1) + t*a(k-1)
a(k) = a(k-1)

Using this I have basically wrote down the following piece of code:

#include <iostream>
#include <vector>
#include <cstdio>

#include <opencv2/core/core.hpp>
#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;

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


void printmat(const cv::Mat &__mat, std::string __str)
{
    std::cout << "--------" << __str << "----------\n";
    for (int i=0 ; i<__mat.rows ; ++i)
    {
        for (int j=0 ; j<__mat.cols ; ++j)
            std::cout << __mat.at<double>(i,j) << "  ";
        std::cout << std::endl;
    }
    std::cout << "-------------------------------------\n";
}


int main (int argc, char * const argv[])
{
    int nStates = 5, nMeasurements = 2, nInputs = 1;
    Mat img(500, 900, CV_8UC3);
    KalmanFilter KF(nStates, nMeasurements, nInputs, CV_64F);
    Mat state(nStates, 1, CV_64F); /* (x, y, Vx, Vy, a) */
    Mat measurement(nMeasurements,1,CV_64F); measurement.setTo(Scalar(0));
    Mat prevMeasurement(nMeasurements,1,CV_64F); prevMeasurement.setTo(Scalar(0));
    int key = -1, dt=100, T=1000;
    float /*a=100, acclErrMag = 0.05,*/ measurementErrVar = 100, noiseVal=0.001, covNoiseVal=0.9e-4;

    namedWindow("Mouse-Kalman");
    setMouseCallback("Mouse-Kalman", on_mouse, 0);

    //while ( (char)(key=cv::waitKey(100)) != 'q' )
    {
        /// A
        KF.transitionMatrix.at<double>(0,0) = 1;
        KF.transitionMatrix.at<double>(0,1) = 0;
        KF.transitionMatrix.at<double>(0,2) = (dt/T);
        KF.transitionMatrix.at<double>(0,3) = 0;
        KF.transitionMatrix.at<double>(0,4) = 0.5*(dt/T)*(dt/T);

        KF.transitionMatrix.at<double>(1,0) = 0;
        KF.transitionMatrix.at<double>(1,1) = 1;
        KF.transitionMatrix.at<double>(1,2) = 0;
        KF.transitionMatrix.at<double>(1,3) = (dt/T);
        KF.transitionMatrix.at<double>(1,4) = 0.5*(dt/T)*(dt/T);

        KF.transitionMatrix.at<double>(2,0) = 0;
        KF.transitionMatrix.at<double>(2,1) = 0;
        KF.transitionMatrix.at<double>(2,2) = 1;
        KF.transitionMatrix.at<double>(2,3) = 0;
        KF.transitionMatrix.at<double>(2,4) = (dt/T);

        KF.transitionMatrix.at<double>(3,0) = 0;
        KF.transitionMatrix.at<double>(3,1) = 0;
        KF.transitionMatrix.at<double>(3,2) = 0;
        KF.transitionMatrix.at<double>(3,3) = 1;
        KF.transitionMatrix.at<double>(3,4) = (dt/T);

        KF.transitionMatrix.at<double>(4,0) = 0;
        KF.transitionMatrix.at<double>(4,1) = 0;
        KF.transitionMatrix.at<double>(4,2) = 0;
        KF.transitionMatrix.at<double>(4,3) = 0;
        KF.transitionMatrix.at<double>(4,4) = 1;


        /// Initial estimate of state variables
        KF.statePost = cv::Mat::zeros(nStates, 1,CV_64F);
        KF.statePost.at<double>(0) = mouse_info.x;
        KF.statePost.at<double>(1) = mouse_info.y;
        KF.statePost.at<double>(2) = 0;
        KF.statePost.at<double>(3) = 0;
        KF.statePost.at<double>(4) = 0;

        KF.statePre = KF.statePost;

        /// Ex or Q
        setIdentity(KF.processNoiseCov, Scalar::all(noiseVal));


        /// Initial covariance estimate Sigma_bar(t) or P'(k)
        setIdentity(KF.errorCovPre, Scalar::all(1000));

        /// Sigma(t) or P(k)
        setIdentity(KF.errorCovPost, Scalar::all(1000));

        /// B
        KF.controlMatrix = cv::Mat(nStates, nInputs,CV_64F);
        KF.controlMatrix.at<double>(0,0) = 0;
        KF.controlMatrix.at<double>(1,0) = 0;
        KF.controlMatrix.at<double>(2,0) = 0;
        KF.controlMatrix.at<double>(3,0) = 0;
        KF.controlMatrix.at<double>(4,0) = 1;

        /// H
        KF.measurementMatrix = cv::Mat::eye(nMeasurements, nStates, CV_64F);

        /// Ez or R
        setIdentity(KF.measurementNoiseCov, Scalar::all(measurementErrVar*measurementErrVar));

        printmat(KF.controlMatrix, "KF.controlMatrix");
        printmat(KF.transitionMatrix, "KF.transitionMatrix");
        printmat(KF.statePre,"KF.statePre");
        printmat(KF.processNoiseCov, "KF.processNoiseCov");
        printmat(KF.measurementMatrix, "KF.measurementMatrix");
        printmat(KF.measurementNoiseCov, "KF.measurementNoiseCov");
        printmat(KF.errorCovPost,"KF.errorCovPost");
        printmat(KF.errorCovPre,"KF.errorCovPre");
        printmat(KF.statePost,"KF.statePost");

        while (mouse_info.x < 0 || mouse_info.y < 0)
        {
            imshow("Mouse-Kalman", img);
            waitKey(30);
            continue;
        }

        while ( (char)key != 's' )
        {
            /// MAKE A MEASUREMENT
            measurement.at<double>(0) = mouse_info.x;
            measurement.at<double>(1) = mouse_info.y;

            /// MEASUREMENT UPDATE
            Mat estimated = KF.correct(measurement);

            /// STATE UPDATE
            Mat prediction = KF.predict();



            cv::Mat u(nInputs,1,CV_64F);
            u.at<double>(0,0) = 0.0 * sqrt(pow((prevMeasurement.at<double>(0) - measurement.at<double>(0)),2)
                                        + pow((prevMeasurement.at<double>(1) - measurement.at<double>(1)),2));

            /// STORE ALL DATA
            Point predictPt(prediction.at<double>(0),prediction.at<double>(1));
            Point estimatedPt(estimated.at<double>(0),estimated.at<double>(1));
            Point measuredPt(measurement.at<double>(0),measurement.at<double>(1));

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

            /// DRAW ALL ON IMAGE
            img = Scalar::all(0);
            drawCross( predictPt, Scalar(255,255,255), 9 );     //WHITE
            drawCross( estimatedPt, Scalar(0,0,255), 6 );       //RED
            drawCross( measuredPt, Scalar(0,255,0), 3 );        //GREEN


            line( img, estimatedPt, measuredPt, Scalar(100,255,255), 3, CV_AA, 0 );
            line( img, estimatedPt, predictPt, Scalar(0,255,255), 3, CV_AA, 0 );

            prevMeasurement = measurement;

            imshow( "Mouse-Kalman", img );
            key=cv::waitKey(10);
        }
    }
    return 0;
}

Here is the output of the code: http://www.youtube.com/watch?v=9_xd4HSz8_g

As you can see that the tracking very very slow. I don't understand what is wrong with the model and why the estimation is so slow. I don't expect there should be any control input.

Can anyone explain this?

Answer

ekmungi picture ekmungi · Aug 11, 2013

I have modified my code and I am posting it for those who want to tweak it to play around for more. The main problem was the choice of covariances. 

#include <iostream>
#include <vector>
#include <cstdio>

#include <opencv2/core/core.hpp>
#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;

vector<Point> mousev,kalmanv;
int trackbarProcessNoiseCov = 10, trackbarMeasurementNoiseCov = 10, trackbarStateEstimationErrorCov = 10;

float processNoiseCov=10, measurementNoiseCov = 1000, stateEstimationErrorCov = 50;
int trackbarProcessNoiseCovMax=10000, trackbarMeasurementNoiseCovMax = 10000,
      trackbarStateEstimationErrorCovMax = 5000;

float processNoiseCovMin=0, measurementNoiseCovMin = 0,
      stateEstimationErrorCovMin = 0;
float processNoiseCovMax=100, measurementNoiseCovMax = 5000,
      stateEstimationErrorCovMax = 5000;

int nStates = 5, nMeasurements = 2, nInputs = 1;
KalmanFilter KF(nStates, nMeasurements, nInputs, CV_64F);

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

void on_trackbarProcessNoiseCov( int, void* )
{
    processNoiseCov = processNoiseCovMin +
        (trackbarProcessNoiseCov * (processNoiseCovMax-processNoiseCovMin)/trackbarProcessNoiseCovMax);
    setIdentity(KF.processNoiseCov, Scalar::all(processNoiseCov));
    std::cout << "\nProcess Noise Cov:     " << processNoiseCov;
    std::cout << "\nMeasurement Noise Cov: " << measurementNoiseCov << std::endl;
}

void on_trackbarMeasurementNoiseCov( int, void* )
{
    measurementNoiseCov = measurementNoiseCovMin +
(trackbarMeasurementNoiseCov * (measurementNoiseCovMax-measurementNoiseCovMin)/trackbarMeasurementNoiseCovMax);
    setIdentity(KF.measurementNoiseCov, Scalar::all(measurementNoiseCov));
    std::cout << "\nProcess Noise Cov:     " << processNoiseCov;
    std::cout << "\nMeasurement Noise Cov: " << measurementNoiseCov << std::endl;
}

int main (int argc, char * const argv[])
{
    Mat img(500, 1000, CV_8UC3);
    Mat state(nStates, 1, CV_64F);/* (x, y, Vx, Vy, a) */
    Mat measurementNoise(nMeasurements, 1, CV_64F), processNoise(nStates, 1, CV_64F);
    Mat measurement(nMeasurements,1,CV_64F); measurement.setTo(Scalar(0.0));
    Mat noisyMeasurement(nMeasurements,1,CV_64F); noisyMeasurement.setTo(Scalar(0.0));
    Mat prevMeasurement(nMeasurements,1,CV_64F); prevMeasurement.setTo(Scalar(0.0));
    Mat prevMeasurement2(nMeasurements,1,CV_64F); prevMeasurement2.setTo(Scalar(0.0));
    int key = -1, dt=50, T=1000;


    namedWindow("Mouse-Kalman");
    setMouseCallback("Mouse-Kalman", on_mouse, 0);
    createTrackbar( "Process Noise Cov", "Mouse-Kalman", &trackbarProcessNoiseCov,
            trackbarProcessNoiseCovMax, on_trackbarProcessNoiseCov );
    createTrackbar( "Measurement Noise Cov", "Mouse-Kalman", &trackbarMeasurementNoiseCov,
            trackbarMeasurementNoiseCovMax, on_trackbarMeasurementNoiseCov );

    on_trackbarProcessNoiseCov( trackbarProcessNoiseCov, 0 );
    on_trackbarMeasurementNoiseCov( trackbarMeasurementNoiseCov, 0 );

    //while ( (char)(key=cv::waitKey(100)) != 'q' )
    {
    /// A (TRANSITION MATRIX INCLUDING VELOCITY AND ACCELERATION MODEL)
    KF.transitionMatrix.at<double>(0,0) = 1;
    KF.transitionMatrix.at<double>(0,1) = 0;
    KF.transitionMatrix.at<double>(0,2) = (dt/T);
    KF.transitionMatrix.at<double>(0,3) = 0;
    KF.transitionMatrix.at<double>(0,4) = 0.5*(dt/T)*(dt/T);

    KF.transitionMatrix.at<double>(1,0) = 0;
    KF.transitionMatrix.at<double>(1,1) = 1;
    KF.transitionMatrix.at<double>(1,2) = 0;
    KF.transitionMatrix.at<double>(1,3) = (dt/T);
    KF.transitionMatrix.at<double>(1,4) = 0.5*(dt/T)*(dt/T);

    KF.transitionMatrix.at<double>(2,0) = 0;
    KF.transitionMatrix.at<double>(2,1) = 0;
    KF.transitionMatrix.at<double>(2,2) = 1;
    KF.transitionMatrix.at<double>(2,3) = 0;
    KF.transitionMatrix.at<double>(2,4) = (dt/T);

    KF.transitionMatrix.at<double>(3,0) = 0;
    KF.transitionMatrix.at<double>(3,1) = 0;
    KF.transitionMatrix.at<double>(3,2) = 0;
    KF.transitionMatrix.at<double>(3,3) = 1;
    KF.transitionMatrix.at<double>(3,4) = (dt/T);

    KF.transitionMatrix.at<double>(4,0) = 0;
    KF.transitionMatrix.at<double>(4,1) = 0;
    KF.transitionMatrix.at<double>(4,2) = 0;
    KF.transitionMatrix.at<double>(4,3) = 0;
    KF.transitionMatrix.at<double>(4,4) = 1;


    /// Initial estimate of state variables
    KF.statePost = cv::Mat::zeros(nStates, 1,CV_64F);
    KF.statePost.at<double>(0) = mouse_info.x;
    KF.statePost.at<double>(1) = mouse_info.y;
    KF.statePost.at<double>(2) = 0.1;
    KF.statePost.at<double>(3) = 0.1;
    KF.statePost.at<double>(4) = 0.1;

    KF.statePre = KF.statePost;
    state = KF.statePost;

    /// Ex or Q (PROCESS NOISE COVARIANCE)
    setIdentity(KF.processNoiseCov, Scalar::all(processNoiseCov));


    /// Initial covariance estimate Sigma_bar(t) or P'(k)
    setIdentity(KF.errorCovPre, Scalar::all(stateEstimationErrorCov));

    /// Sigma(t) or P(k) (STATE ESTIMATION ERROR COVARIANCE)
    setIdentity(KF.errorCovPost, Scalar::all(stateEstimationErrorCov));

    /// B (CONTROL MATRIX)
    KF.controlMatrix = cv::Mat(nStates, nInputs,CV_64F);
    KF.controlMatrix.at<double>(0,0) = /*0.5*(dt/T)*(dt/T);//*/0;
    KF.controlMatrix.at<double>(1,0) = /*0.5*(dt/T)*(dt/T);//*/0;
    KF.controlMatrix.at<double>(2,0) = 0;
    KF.controlMatrix.at<double>(3,0) = 0;
    KF.controlMatrix.at<double>(4,0) = 1;

    /// H (MEASUREMENT MATRIX)
    KF.measurementMatrix = cv::Mat::eye(nMeasurements, nStates, CV_64F);

    /// Ez or R (MEASUREMENT NOISE COVARIANCE)
    setIdentity(KF.measurementNoiseCov, Scalar::all(measurementNoiseCov));


    while (mouse_info.x < 0 || mouse_info.y < 0)
    {
        imshow("Mouse-Kalman", img);
        waitKey(30);
        continue;
    }

    prevMeasurement.at<double>(0,0) = 0;
    prevMeasurement.at<double>(1,0) = 0;
    prevMeasurement2 = prevMeasurement;

    while ( (char)key != 's' )
    {
        /// STATE UPDATE
        Mat prediction = KF.predict();

        /// MAKE A MEASUREMENT
        measurement.at<double>(0) = mouse_info.x;
        measurement.at<double>(1) = mouse_info.y;

        /// MEASUREMENT NOISE SIMULATION
        randn( measurementNoise, Scalar(0),
          Scalar::all(sqrtf(measurementNoiseCov)));
        noisyMeasurement = measurement + measurementNoise;

        /// MEASUREMENT UPDATE
        Mat estimated = KF.correct(noisyMeasurement);

        cv::Mat u(nInputs,1,CV_64F);
        u.at<double>(0,0) = 0.0 * sqrtf(pow((prevMeasurement.at<double>(0) - measurement.at<double>(0)),2)
                    + pow((prevMeasurement.at<double>(1) - measurement.at<double>(1)),2));

        /// STORE ALL DATA
        Point noisyPt(noisyMeasurement.at<double>(0),noisyMeasurement.at<double>(1));
        Point estimatedPt(estimated.at<double>(0),estimated.at<double>(1));
        Point measuredPt(measurement.at<double>(0),measurement.at<double>(1));


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

        /// DRAW ALL ON IMAGE
        img = Scalar::all(0);
        drawCross( noisyPt, Scalar(255,255,255), 9 );     //WHITE
        drawCross( estimatedPt, Scalar(0,0,255), 6 );       //RED
        drawCross( measuredPt, Scalar(0,255,0), 3 );        //GREEN


        line( img, estimatedPt, measuredPt, Scalar(100,255,255), 3, CV_AA, 0 );
        line( img, estimatedPt, noisyPt, Scalar(0,255,255), 3, CV_AA, 0 );

        imshow( "Mouse-Kalman", img );
        key=cv::waitKey(dt);
        prevMeasurement = measurement;
    }
    }

    return 0;
}

Related questions

blob tracking with kalman filter in opencv

Well, I am trying to create a small example of blob tracking using the kalman filter. I am using openCV in order to accomplish this task, however it does not seem to work as it supposed to, since when I …

Read More
Tracking multiple moving objects with KalmanFilter in OpenCV/C++ - How to assign tracks individually to detected objects

I am working on realtime movement detection and object tracking in OpenCV/C++ and am currently stuck at the tracking part. Matlab-Example of what I want to do: http://www.mathworks.de/de/help/vision/examples/motion-based-multiple-object-tracking.html (I'm troubled …

Read More
OpenCV Kalman filter

I have three gyroscope values, pitch, roll and yaw. I would like to add Kalman filter to get more accurate values. I found the opencv library, which implements a Kalman filter, but I can't understand it how is it really …

Read More