Version vom 27. Februar 2015, 13:32 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-Gleichung ausmultipliziert, erhält man wieder die beiden Zustands-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]; % Zustandsübergangsmatrix - gibt an wie wir vom aktuellen zum nächsten Zustand kommen
C=[1 0; 0 1]; % Matrix bildet Mess-Zustand auf System-Zustand ab
P=[1000 0; 0 1000]; % Start-Kovarianz für anfängliche Unsicherheit 
Q=[0.01 0; 0 0.01]; % Prozess-Fehler; Zum Anpassen/Spielen
R=[0.01 0; 0 0.01]; % Messfehler; Zum Anpassen/Spielen

% Durchlauf das folgende für jede neue Messung

% VORHERSAGE
% wir sagen den nächsten System-Zustand vorher basierend of dem Wissen (Modell) unseres Systems
x = A*x;
% Wir passen auch die Unsicherheit an. Wenn wir den Systemzustand ohne Messungen vorhersagen, steigt die Unsicherheit
P = A*P*A' + Q; % Unsicherheit auch mit Zustandsübergang anpassen. Prozess-Fehler addieren

% KORREKTUR
% Mit den Messungen korrigieren wir die Zustands-Schätzung
z=[gpsHdg; gyroHdgRate]; % Dies ist die Mess-Matrix
% Zunächst Kalman-Verstärkung herausfinden; wie stark vertrauen wir der Schätzung im Vergleich zu den Messungen
K = P*C'*inv(C*P*C' + R)
% Dann den Fehler zwischen Vorhersage und Messungen herausfinden (die "Innovation")
% z-C*x
% und korrigiere die Schätzung -- aber nur ein kleines bisschen pro Zeiteinheit,
% bestimmt durch die Kalman-Verstärkung
x = x + K*(z-C*x)
% Genauso, korrigiere (genauer: erniedrige) Unsicherheit da wir nach jeder Messung ein kleines bisschen
% mehr Sicher sein können über unsere Schätzung
P = (I-K*C)*P