Version vom 27. Februar 2015, 15:02 Uhr

Idea

You have...

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

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

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

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