Kalman ru

Idea

For the Extended Kalman Filter (EKF) you have...

1. A model of a robots state at every time (e.g. actual heading, actual heading speed).
2. A certain number of sensors. Each sensor has a certain measurement error (%).

Kalman fusions all sensors measurements by iterating over and over two phases:

1. Predict: we predict the next robot's state by help of the old robot's state and a certaincy for each sensor.
2. Correct: we correct the certaincy based on new measurements for each sensor. Does the prediction fit to the sensor measurement, we increase certaincy for that sensor, otherwise we decrease certaincy.

Example

We have a heading (theta) and a heading speed rate (omega). The new heading can be predicted by old heading plus heading rate and delta time:

theta = theta_k+1 + omega * dt

theta_k+1: new heading
theta_k: old heading
omega: heading speed rate
dt: delta time

The heading speed rate is modelled after this:

 omega = omega_k+1

This is a model only. The real heading rate will change. The Kalman considers this. Those both state equations can now be turned into state space form which is basically a matrix form of the two equations:

x_k = A * x_k+1

with

x = [ theta
      omega ]

A = [ 1    dt 
      0    1  ]

If you muliply the matrix equation, you get again both state equations. Kalman filter also uses additional matrizes:

y_k = C * yk+1 

with

y = [ theta 
      omega ]

C = [ 1   0
      0   1 ]

Matrix C transfers measurements into state variables. The Kalman filter also uses Covariance Matrix P which describes how well state variables and measurements fit.

Furthermore, Kalman uses a measurement error matrix R where you can estimate the measurement error for each signal.

Finally, there's a process error matrix Q which models the complete system error (due to noise in servos, motors etc).

Scilab

Kalman algorithm implemented with free math software "Scilab":

% initialize
A=[1 dt; 0 1]; % state transition matrix; represents how we get from prior state to next state
C=[1 0; 0 1]; % the matrix that maps measurement to system state
P=[1000 0; 0 1000]; % sets the initial covariance to indicate initial uncertainty
Q=[0.01 0; 0 0.01]; % process noise; I just picked some numbers; you can play with these
R=[0.01 0; 0 0.01]; % measurement noise; I just picked some numbers here too

% loop the following every time you get a new measurement

% PREDICT
% we predict the next system state based on our knowledge (model) of the system
x = A*x;
% We also have to adjust certainty. If we're predicting system state
% without meausrements, certainty reduces
P = A*P*A' + Q; % adjust certainty with the state transition, too. Add process noise

% CORRECT
% With measurements, we correct the state estimate
z=[gpsHdg; gyroHdgRate]; % this is the measurement matrix
% First, find the Kalman Gain; how much we trust the estimate vs measurements
K = P*C'*inv(C*P*C' + R)
% Then we find out the error between prediction and measurement (the Innovation)
% z-C*x
% and correct the estimate -- but only a little at a time,
% as determined by the Kalman Gain
x = x + K*(z-C*x)
% Likewise, we correct (actually increase) certainty, because any time
% we have a measurement we can be a little more certain about our estimate
P = (I-K*C)*P