Simple Kalman filter for tracking using OpenCV 2.2

Simple Kalman filter for tracking using OpenCV 2.2

I wanted to put up a quick note on how to use Kalman Filters in OpenCV 2.2 with the C++ API, because all I could find online was using the old C API. Plus the kalman.cpp example that ships with OpenCV is kind of crappy and really doesn’t explain how to use the Kalman Filter.
I’m no expert on Kalman filters though, this is just a quick hack I got going as a test for a project. It worked, so I’m posting the results.

The Filter

So I wanted to do a 2D tracker that is more immune to noise. For that I set up a Kalman filter with 4 dynamic parameters and 2 measurement parameters (no control), where my measurement is: 2D location of object, and dynamic is: 2D location and 2D velocity. Pretty simple, and it makes the transition matrix also simple.

 

KalmanFilter KF(4, 2, 0);

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

Mat_<float> measurement(2,1); measurement.setTo(Scalar(0));

// init...

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;

setIdentity(KF.measurementMatrix);

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

setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));

setIdentity(KF.errorCovPost, Scalar::all(.1));

Cool, moving on to the dynamic part.
So I set up a mouse callback to get the mouse position every “frame” (a 100ms wait), and feed that into the filter:

// First predict, to update the internal statePre variable

Mat prediction = KF.predict();

Point predictPt(prediction.at<float>(0),prediction.at<float>(1));

// Get mouse point

measurement(0) = mouse_info.x;

measurement(1) = mouse_info.y;

Point measPt(measurement(0),measurement(1));

// The "correct" phase that is going to use the predicted value and our measurement

Mat estimated = KF.correct(measurement);

Point statePt(estimated.at<float>(0),estimated.at<float>(1));

All the rest is garnish (see the code)..
The important bit is to see that Predict() happens before Correct(). This is according to the excellent Kalman filter tutorial I found. Look carefully at Figure 1-2!! It will sort you out. Also take a look at OpenCV’s internal impl of Kalman, see that it follows these steps closely. Especially Mat& KalmanFilter::predict(const Mat& control) and Mat& KalmanFilter::correct(const Mat& measurement).
Another good place I found that helped me formulate the parameters for the filter is this place. Again, take everything with a grain of salt, because Kalman Filters are very versatile you just need to know how to formulate them right.

Result

Using velocity:

 

Not using velocity:

Code

As usual, grab the code off the SVN:

svn co http://morethantechnical.googlecode.com/svn/trunk/mouse_kalman/main.cpp