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[0])), cvRound(ofGetHeight()/2-ofGetWidth()/3*sin(mat->data.fl[0])));  
  
}  
  
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]),0);  
		  
		cvRand(&rng,z_k);  
		  
		cvMatMulAdd(kalman->measurement_matrix,x_k,z_k,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]),0);  
		  
		cvRand(&rng,w_k);  
		cvMatMulAdd(kalman->transition_matrix,x_k,w_k,x_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.