Unlogic: Die Seite wurde neu angelegt: „=Idea= For the Extended Kalman Filter (EKF) you have... # A model of a robots state at every time (e.g. actual heading, actual heading speed). # A certain n…“

2015-08-22T19:24:33Z

Die Seite wurde neu angelegt: „=Idea= For the Extended Kalman Filter (EKF) you have... # A model of a robots state at every time (e.g. actual heading, actual heading speed). # A certain n…“

Neue Seite

=Idea=

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

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

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

# Predict: we predict the next robot's state by help of the old robot's state and a certaincy for each sensor.
# 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

Kalman ru - Versionsgeschichte

Unlogic: Die Seite wurde neu angelegt: „=Idea= For the Extended Kalman Filter (EKF) you have... # A model of a robots state at every time (e.g. actual heading, actual heading speed). # A certain n…“