# ofxcv and kalman filter

thought i should share this small example on how to use the opencv kalman filter.

“The Kalman filter is an algorithm which operates recursively on streams of noisy input data to produce a statistically optimal estimate of the underlying system state.” wiki

this example only predicts the rotation angle of a point on a circle. (i.e 1 dimensional data)
the next step would be to make an example that actually predicts a point (i.e. 2/3 dimensional data)

``````
#include "testApp.h"

#include "cv.h"

using namespace cv;
using namespace ofxCv;

CvKalman* kalman;
CvMat* x_k;
CvMat* z_k;
CvMat* w_k;
const CvMat* y_k;
CvRandState rng;

unsigned long myTimer;

CvPoint phi2xy(const CvMat* mat){

//angle to point on screen
return cvPoint( cvRound(ofGetWidth()/2+ofGetWidth()/3*cos(mat->data.fl)), cvRound(ofGetHeight()/2-ofGetWidth()/3*sin(mat->data.fl)));

}

void testApp::setup() {

ofSetBackgroundAuto(false);

cvRandInit(&rng,0,1,-1,CV_RAND_UNI);

kalman = cvCreateKalman(2, 1, 0);

//state is (phi, delta_phi) - angle and angular velocity
//init with random guess
x_k = cvCreateMat(2, 1, CV_32FC1);
cvRandSetRange(&rng, 0 ,0.1,0);
rng.disttype = CV_RAND_NORMAL;
cvRand(&rng, x_k);

//process noise
w_k = cvCreateMat(2, 1, CV_32FC1);

//measurments, only one parameter for angle
z_k = cvCreateMat(1, 1, CV_32FC1);
cvZero(z_k);

//transition matrix "F" describes relationship between model parameter at step k and at step k+1
//this is the "dynamics" in our model
const float F[] = {1,1,0,1};
memcpy(kalman->transition_matrix->data.fl, F, sizeof(F));

//init other kalman filter parameters
cvSetIdentity(kalman->measurement_matrix, cvRealScalar(1));
cvSetIdentity(kalman->process_noise_cov, cvRealScalar(1e-5));
cvSetIdentity(kalman->measurement_noise_cov, cvRealScalar(1e-1));
cvSetIdentity(kalman->error_cov_post, cvRealScalar(1));

//choose random initial state
cvRand(&rng, kalman->state_post);

}

void testApp::update() {

}

void testApp::draw() {

if(ofGetElapsedTimeMillis() - myTimer > 150){

ofBackground(100);
myTimer = ofGetElapsedTimeMillis();

//predict point position
y_k = cvKalmanPredict(kalman,0);

//generate measurment (z_k)
cvRandSetRange(&rng,0,sqrt(kalman->measurement_noise_cov->data.fl),0);

cvRand(&rng,z_k);

//plot points (eg convert to planar coordinates and draw)
ofSetColor(255, 255, 0);
ofCircle(toOf(phi2xy(z_k)),5); //observed state
ofSetColor(255, 255, 255);
ofCircle(toOf(phi2xy(y_k)),5); //"predicted" state
ofSetColor(255, 0, 0);
ofCircle(toOf(phi2xy(x_k)),5); //real state

//adjust klaman filter state
cvKalmanCorrect(kalman, z_k);

//apply the transition matrix "F" (e.g. step time forward)
//also apply the "process" noise w_k
cvRandSetRange(&rng,0,sqrt(kalman->process_noise_cov->data.fl),0);

cvRand(&rng,w_k);
}

}

``````

Nice, I’ve been playing around with this as well, using Kalman filtering to track position and velocity:

``````
// First predict, to update the internal statePre variable
cv::Mat prediction = kalman.predict();
cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));

// put the ROI into the measurement
measurement(0) = selection.getX1();
measurement(1) = selection.getY1();
measurement(2) = velocity.x;
measurement(3) = velocity.y;

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

// The "correct" phase that is going to use the predicted value and our measurement
cv::Mat estimated = kalman.correct(measurement);
cv::Point statePt(estimated.at<float>(0),estimated.at<float>(1));

``````

I don’t quite understand what the intialization for the filter does though, particularly here:

``````
cvSetIdentity(kalman->measurement_matrix, cvRealScalar(1));
cvSetIdentity(kalman->process_noise_cov, cvRealScalar(1e-5));
cvSetIdentity(kalman->measurement_noise_cov, cvRealScalar(1e-1));
cvSetIdentity(kalman->error_cov_post, cvRealScalar(1));

``````

I have the same lines that pasted in from the kalman filter example in oCV2.3 and I don’t really get what’s going on in them, do you?

just found this very helpful link that explains how to adjust the kalman filter to get smoother data points.

http://stackoverflow.com/questions/3745348/opencv-kalman-filter

kalman->process_noise_cov is the ‘process noise covariance matrix’ and it is often referred in the Kalman literature as Q. The result will be smoother with lower values.

kalman->measurement_noise_cov is the ‘measurement noise covariance matrix’ and it is often referred in the Kalman literature as R. The result will be smoother with higher values.

The relation between those two matrices defines the amount and shape of filtering you are performing.

If the value of Q is high, it will mean that the signal you are measuring is varies quickly and you need the filter to be adaptable. If it is small, then big variations will be attributed to noise in the measure.

If the value of R is high (comparing to Q), it will indicate that the measuring is noisy so it will be filtered more.

this really smoothed out my noisy tracking points.