Version vom 26. Februar 2015, 15:01 Uhr

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 neuen Kurs kann man vorhersagen als den alten Kurs plus der Drehwinkelgeschwindigkeit mal der vergangenen Zeit:

theta = theta_k+1 + omega * dt

theta_k+1: neuer Kurs
theta_k: alter Kurs
omega: Drehwinkelgeschwindigkeit
dt: vergangene Zeit

Die Drehänderungsrate ist also modelliert als:

 omega = omega_k+1

Dies ist natürlich nur ein Modell. Die wahre Drehwinkelgeschwindigkeit wird sich oftmals ändern. Dies wird aber beim Kalman-Filter berücksichtigt. Diese beiden Zustands-Gleichungen können nun in Zustands-Raum-Form gebracht werden, welches in erster Linie eine Matrix-Darstellung der beiden Gleichungen ist:

x_k = A * x_k+1

mit

x = [ theta
      omega ]

A = [ 1    dt 
      0    1  ]

Wenn man die Matrizen ausmultipliziert, erhält man wieder die beiden Gleichungen (probiert es aus!). Der Kalman-Filter benutzt noch weitere Matrizen:

y_k = C * yk+1 

mit

y = [ theta 
      omega ]

C = [ 1   0
      0   1 ]

Dabei wandelt C die Meßwerte in die Zustandsvariablen um. Der Kalman-Filter beschreibt nicht nur den Zustand des Systems, sondern auch die Kovarianz Matrix P, welche beschreibt wie gut die Zustandsvariablen und ihre Meßwerte zusammen passen.

Desweiteren benutzt Kalman eine Meßfehler Matrix R womit man dem Filter mitteilen kann wie stark der Meßfehler für jedes Signal geschätzt wird.

Zu guter letzt gibt es eine Prozessfehler Matrix Q welche den Fehler des Gesamtsystems modelliert (ungenaue Ansteuerung z.B).

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