Kalman

Idee

Man hat

1. eine Beschreibung des Roboter-Zustands zu jedem beliebigen Zeitpunkt (z.B. Position, Kurs)
2. eine bestimmte Anzahl Sensoren. Jeder Sensor hat einen bestimmten Meßfehler (%).

Der Kalman fusioniert die Meßwerte bei jedem neuen Meßwert durch ständiges Durchlaufen von zwei Phasen:

1. Vorhersage: wir sagen neuen Roboter-Zustand mit Hilfe des alten Roboter-Zustand und der Meßwerte voraus
2. Korrektur:

Scilab

Beschreibung des Algorithmus mit Hilfe des kostenloen Mathematik-Paketes "Scilab":

% Initialisierung
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