# Kalman Filter

## Video tutorials

### Theory

9:29min introduction to Kalman filter theory, part I

13:50min introduction to Kalman filter theory, part II

Excellent introduction by Cyrill Stachniss slides for this talk

### Application examples

11:49min example of Kalman filter object tracking, part II

7:44min example of Kalman filter object tracking, part II

## What is a Kalman Filter?

It is a nice tool to get better estimates for the state of a system than just taking the single measurement we can do for the system's state.

It consists out of the following steps for each time step in which we give an estimate for the system state:

1. a priori estimate: predict the next system state based on the last state estimate + the assumed state transition equation
2. measure the next system state (measurements are noisy! - measurement and noise is described by an observation equation)
3. a posteriori estimate: correct the predicted estimate for the system state from step 1 based on the weighted difference between the predicted & estimated state

So there is a predict-measure-correct structure in the Kalman Filter algorithm / equations.

The central idea in the filter is the weighting of the difference between the predicted & estimated state. Therefore we do not only try to estimate the system state but also try to estimate the (co)variance of our prediction errors.

This allows us to trust the measurements more, if our prediction is bad - we did a bad guess for the state transition equation.

On the other hand we trust the predictions more, if our measurements are bad - we did a good guess for the state transition equation.

## Demo

This demo uses OpenCV's Kalman filter class and is very similar to the standard OpenCV demo for the Kalman filter, but I rearranged the code.

It shows how the Kalman-filtering of a noisy angle measurements of a rotating point helps to improve the estimate of the angle.

• white circle: true state (angle)
• red cross: noisy measurement of this true state
• green circle: corrected state estimate from the Kalman-Filter

If the Kalman-filtering really helps, the green circle should be nearer to the white circle than the red crosses.

Code:

```/*
Tracking of rotating point using a Kalman filter.
Rotation speed is constant.
Both state and measurements vectors are 1D (a point angle),
Measurement is the real point angle + gaussian noise.
Pressing any key (except ESC) will reset the tracking with a different speed.
Pressing ESC will stop the program.
*/

#ifdef _CH_
#pragma package <opencv>
#endif

#define CV_NO_BACKWARD_COMPATIBILITY

#ifndef _EiC
#include "cv.h"
#include "highgui.h"
#include <math.h>
#endif

#define calc_point(angle)                                      \
cvPoint( cvRound(img->width/2 + img->width/3*cos(angle)),  \
cvRound(img->height/2 - img->width/3*sin(angle)))

#define draw_cross( center, color, d )                                 \
cvLine( img, cvPoint( center.x - d, center.y - d ),                \
cvPoint( center.x + d, center.y + d ), color, 1, CV_AA, 0); \
cvLine( img, cvPoint( center.x + d, center.y - d ),                \
cvPoint( center.x - d, center.y + d ), color, 1, CV_AA, 0 )

int main(int argc, char** argv)
{
CvFont font;
double hScale=1.0;
double vScale=1.0;
int    lineWidth=1;
cvInitFont(&font, CV_FONT_HERSHEY_PLAIN, hScale,vScale,0,lineWidth);
char txt[100];

IplImage* img = cvCreateImage( cvSize(500,500), 8, 3 );

char code = -1;

cvNamedWindow( "Kalman", 1 );

// Create a Kalman filter object
int DimMeasurementVector = 2;               // number of measurement vector dimensions
int DimStateVector       = 1 ;              // number of state vector dimensions
CvKalman* kalman = cvCreateKalman( DimMeasurementVector, DimStateVector);

// Prepare data structures for
//  - state vector
//  - process noise
//  - measurement vector
CvMat* state         = cvCreateMat( 2, 1, CV_32FC1 ); /* (phi, delta_phi) */
CvMat* process_noise = cvCreateMat( 2, 1, CV_32FC1 );
CvMat* measurement   = cvCreateMat( 1, 1, CV_32FC1 );
cvZero( measurement );

// Prepare state transition matrix
const float A[] = { 1, 1, 0, 1 };
CvRNG rng = cvRNG(-1);
cvRandArr( &rng, state, CV_RAND_NORMAL, cvRealScalar(0), cvRealScalar(0.1) );
// Initialize state transition matrix
memcpy( kalman->transition_matrix->data.fl, A, sizeof(A));
// We will simulate some state process noise
cvSetIdentity( kalman->process_noise_cov, cvRealScalar(1e-5) );

// Measurement process is identity ...
cvSetIdentity( kalman->measurement_matrix, cvRealScalar(1) );
// ... but we also simulate some measurement noise
cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(1e-1) );

cvSetIdentity( kalman->error_cov_post, cvRealScalar(1));

cvRandArr( &rng, kalman->state_post, CV_RAND_NORMAL, cvRealScalar(0), cvRealScalar(0.1) );

int t = 0;
for(;;)
{
// Get the current state: the angle ...
float state_angle = state->data.fl[0];
// ... and compute the corresponding 2D point
CvPoint state_pt = calc_point(state_angle);

//////////////////////////////////////////////////
// 1. Predict step: predict next state, i.e. angle
//////////////////////////////////////////////////
const CvMat* prediction = cvKalmanPredict( kalman );
// ... and retrieve the predicted angle ...
float predict_angle = prediction->data.fl[0];
// ... and compute the corresponding 2D point
CvPoint predict_pt = calc_point(predict_angle);

/////////////////////////////////////////////////////////////////////////
// 2. Measure step: measure the angle (with measurement noise, remember!)
/////////////////////////////////////////////////////////////////////////
// Get measurement of the angle according to the observation/measurement equation ...
float measurement_angle;
CvPoint measurement_pt;
cvRandArr( &rng, measurement, CV_RAND_NORMAL, cvRealScalar(0),
cvRealScalar(sqrt(kalman->measurement_noise_cov->data.fl[0])) );
cvMatMulAdd( kalman->measurement_matrix, state, measurement, measurement ); //	[measurement=measurement_matrix*state+measurement]
// ... and retrieve that measure angle ...
measurement_angle = measurement->data.fl[0];
// ... and compute the corresponding 2D point
measurement_pt = calc_point(measurement_angle);

///////////////////////////////////////////////////////////////
// 3. Correct step:
//    adapt estimated state based on difference between
//    predicted state & measure state (measurement innovation)
///////////////////////////////////////////////////////////////
const CvMat* correctedEstimate = cvKalmanCorrect( kalman, measurement );
float correctedEstimate_angle = correctedEstimate->data.fl[0];
CvPoint correctedEstimate_pt = calc_point( correctedEstimate_angle );

///////////////////////////////////////////////////
// 4. Draw states:
//    - real      state of the system: white circle
//    - predicted state of the system: green cross
//    - measured  state of the system: red cross
//    - corrected state estimate     : green circle
///////////////////////////////////////////////////
cvZero( img ); // clear image

int r = 5;
cvCircle( img, state_pt,             r, CV_RGB(255,255,255) );
cvCircle( img, correctedEstimate_pt, r, CV_RGB(0,255,0) );

//draw_cross( predict_pt,     CV_RGB(0,255,0), 3 );
draw_cross( measurement_pt, CV_RGB(255,0,0), 3 );

sprintf_s(txt, "time: %d", t );
cvPutText( img, txt, cvPoint(20,20), &font, cvScalar(255,255,255) );

char filename[100];
sprintf_s(filename, "W:\\tmp\\frame%04d.png", t);
cvSaveImage( filename, img );

///////////////////////
// 5.Compute next state
///////////////////////
cvRandArr( &rng, process_noise, CV_RAND_NORMAL, cvRealScalar(0),
cvRealScalar(sqrt(kalman->process_noise_cov->data.fl[0])));
cvMatMulAdd( kalman->transition_matrix, state, process_noise, state );
t++;

cvShowImage( "Kalman", img );
code = (char) cvWaitKey( 100 );

if( code > 0 )
break;
}

cvDestroyWindow("Kalman");

return 0;
}

#ifdef _EiC
main(1, "kalman.c");
#endif```