Kalman

Idee

Man hat

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

Der Kalman fusioniert die Sensoren für neue Meßwerte durch ständiges Durchlaufen von zwei Phasen:

1. Vorhersage: wir sagen neuen Roboter-Zustand mit Hilfe des alten Roboter-Zustand und einem Vertrauensfaktor für jeden Sensor voraus.
2. Korrektur: wir korrigieren den Vertrauensfaktor anhand der neuen Meßwerte für jeden Sensor. Passt die Vorhersage zu den Meßwerten, erhöhen wir den Vertrauensfaktor für diesen Sensor, andernfalls erniedrigen wir ihn.

Beispiel

Wir haben einen Kurs (theta) und eine Drehwinkelgeschwindigkeit (omega). Den Zusammenhang kann man beschreiben als:

<addhtml> <html> http://latex.codecogs.com/gif.latex?\Theta_{k+1}%20=%20\Theta%20+%20\omega%20dt </html> </addhtml>

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