Das Kalman Filter einfach erklärt [Teil 2]

Nachdem wir im Teil 1 den Kern des Kalman Filters geklärt haben, widmen wir uns nun dem komplizierteren Teil. Die im Teil 1 genannte Vorgehensweise mit dem multiplizieren bzw. addieren der Mittelwerte und Varianzen funktioniert so nur im eindimensionalen Fall. \(\)

Das heißt, wenn der Zustand, den man messen möchte, mit nur einer Variablen vollständig beschrieben werden kann. Das Beispiel, welches eingangs genannt wurde, die Position eines Fahrzeugs im Tunnel zu bestimmen, kann aber nicht mehr mit einer Variablen vollständig beschrieben werden. Zwar interessiert nur die Position, aber diese ist genau genommen ja schon 2-Dimensional in der Ebene (\(x\) & \(y\)). Außerdem kann nur die Geschwindigkeit (\(\dot x\) & \(\dot y\)) gemessen werden, nicht die Position direkt. Dies führt zu einem 4D-Kalman-Filter, mit folgenden Zustandsvariablen:

$$x=\begin{bmatrix}
x \\
y \\
\dot x \\
\dot y
\end{bmatrix}=\begin{matrix}\text{Position X} \\ \text{Position Y} \\ \text{Geschwindigkeit in X} \\ \text{Geschwindigkeit in Y}
\end{matrix}$$

Herr Kalman hatte sich nun überlegt, wie man es schafft, trotz verrauschter Messung einzelner Sensoren, eine optimale Schätzung aller Zustände zu berechnen.

Multidimensionales Kalman-Filter

Ich möchte die Vorgehensweise wieder am Beispiel eines Fahrzeugs mit Navigationsgerät, welches in einen Tunnel einfährt, erläutern. Die letzte bekannte Position ist vor dem Verlieren des GPS Signals. Danach steht (bei fest eingebauten Navis) nur noch die Geschwindigkeitsinformation vom Fahrzeug (Raddrehzahlen & Gierrate) als normalverteilte verrauschte Messgröße zur Verfügung. Daraus lässt sich eine Geschwindigkeit in \(x\) und \(y\) berechnen.

Anfangsbedingungen / Initialisierung

Systemzustand \(x\)

Zu Beginn muss man mit einem Ausgangszustand initialisieren. Im 1 Dimensionalen Fall war das \(\mu_0\), jetzt im mehrdimensionalen ein Vektor.

$$x_0=\begin{bmatrix}
x_0 \\
y_0 \\
\dot x_0 \\
\dot y_0
\end{bmatrix}$$

Ist nichts bekannt, können hier einfach 0 eingetragen werden. Sind schon einige Randbedingungen bekannt, können diese dem Filter mitgeteilt werden. Über die Wahl der nachfolgenden Kovarianzmatrix \(P\) steuert man, wie schnell das Filter auf die korrekten (gemessenen) Werte konvergiert.

Kovarianzmatrix \(P\)

Es muss eine Unsicherheit für den Ausgangszustand \(x_0\) angegeben werden. Im 1D Fall war das \(\sigma_0\), jetzt eine Matrix, welche für alle Zustände einzeln eine Unsicherheit zu Beginn definiert. Hier als Beispiel mit \(\sigma_0=10\) für alle 4 Zustände.

$$P=\begin{bmatrix}
10 & 0 & 0 & 0 \\
0 & 10 & 0 & 0 \\
0 & 0 & 10 & 0 \\
0 & 0 & 0 & 10
\end{bmatrix}$$

Diese Matrix wird wohl am Häufigsten während der Filterdurchläufe geändert. Sie wird sowohl im Predict als auch Correct Schritt geändert. Ist man sich über die Zustände zu Beginn ganz sicher, kann man hier niedrige Werte einsetzen, weiß man zu Beginn nicht genau, wie die Werte des Zustandsvektors sind, sollte die Kovarianzmatrix \(P\) mit sehr großen Werten (1 Million o.ä.) initialisiert werden, um dem Filter die Möglichkeit zu geben, relativ schnell zu konvergieren (die richtigen Werte auf Grund der Messungen zu finden).

Dynamikmatrix \(A\)

Der Kern des Filters ist allerdings nachfolgende Definition, welche man selbst nur mit großen Verständnis des physikalischen Zusammenhangs selbst aufstellen sollte. Für viele reale Probleme ist das nicht ganz einfach. Für unser einfaches Beispiel (Bewegung in der Ebene) ergibt sich die Physik dahinter aus der gleichmäßigen Bewegung. Für die Position ergibt sich \(x_{t+1}=\dot x_t \cdot t + x_t\) und für die Geschwindigkeit \(\dot x_{t+1}=\dot x_t\). Für den oben dargestellten Zustandsvektor ist die Dynamik in Matrizenschreibweise nachfolgende:

$$A=\begin{bmatrix}
1 & 0 & dt & 0 \\
0 & 1 & 0 & dt \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}$$

Diese sagt aus, “wohin” sich der Zustandsvektor von einem Berechnungsschritt zum nächsten innerhalb \(dt\) bewegt. Dieses Dynamikmodell wird auch “Constant Velocity” Modell genannt, weil es davon ausgeht, dass während eines Berechnungsschritts des Filters die Geschwindigkeit konstant bleibt.

$$\begin{bmatrix}
x \\
y \\
\dot x \\
\dot y
\end{bmatrix}_{t+1}=\begin{bmatrix}
1 & 0 & dt & 0 \\
0 & 1 & 0 & dt \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}\cdot \begin{bmatrix}
x \\
y \\
\dot x \\
\dot y
\end{bmatrix}_t$$

Als Beispiel nur mal die 1. Zeile ausgeschrieben, welche die Position nach einem Berechnungsschritt mit der Zeitdauer \(dt\) berechnet.

$$x_{t+1}=x_t+dt\cdot \dot x_t$$

Dies spiegelt einfach physikalische Zusammenhänge für die gleichförmige Bewegung wider. Eine höhere Form wäre das Constant Acceleration Modell, welches ein 6D Filter wäre und noch die Beschleunigungen imZustandsvektor beinhaltet. Hier kann man prinzipiell auch andere Dynamiken vorgeben (z.B. ein holonomes Fahrzeug).

Prozessrauschkovarianzmatrix \(Q\)

Da die Bewegung des Fahrzeugs ebenfalls gestört (im Sinne von überlagertem, normalverteiltem Rauschen) sein kann, wird dies über die Prozess-Rausch-Kovarianzmatrix \(Q\) eingebracht. Diese Matrix teilt dem Filter mit, wie der Systemzustand von einem zum nächsten Schritt “springen” kann. Stellen wir uns das Fahrzeug vor, welches fährt. Dieses kann von einem Windstoß oder Straßenunebenheiten gestört werden, was eine Krafteinwirkung zur Folge hat. Eine Geschwindigkeitsänderung durch den Fahrer ist ebenfalls eine Beschleunigung, welche auf das Fahrzeug wirkt. Wirkt nun eine Beschleunigung auf den Systemzustand \(x\) ein, so ist die physikalische Abhängigkeit dafür

$$x = \frac{1}{2}dt^2 \cdot \ddot x$$

$$\dot x = dt \cdot \ddot x$$

Die Matrix \(Q\) ist eine Kovarianzmatrix, welche folgende Elemente enthält:

$$Q = \begin{bmatrix}\sigma_{x}^2 & \sigma_{xy} & \sigma_{x \dot x} & \sigma_{x \dot y} \\ \sigma_{yx} & \sigma_{y}^2 & \sigma_{y \dot x} & \sigma_{y \dot y} \\ \sigma_{\dot x x} & \sigma_{\dot x y} & \sigma_{\dot x}^2 & \sigma_{\dot x \dot y} \\ \sigma_{\dot y x} & \sigma_{\dot y y} & \sigma_{\dot y \dot x} & \sigma_{\dot y}^2 \end{bmatrix}$$

Sie lässt sich am einfachsten berechnen, in dem man den Vektor \(G\) aufstellt und diesen nachfolgend mit der angenommenen Standardabweichung für die angreifende Beschleunigung multipliziert, z.B. \(\sigma_a=8m/s^2\).

$$Q = G\cdot G^T \cdot \sigma_a^2$$

mit \(G = \begin{bmatrix}0.5 \Delta t^2 & 0.5 \Delta t^2 & \Delta t & \Delta t\end{bmatrix}^T\).

Steuermatrix \(B\) und Steuergröße \(u\)

Auch ein Einwirken externer Steuergrößen (z.B. Lenken, Bremsen, etc.) ist über die Steuermatrix \(B\) möglich, soll hier aber zur einfacheren Erläuterung  weg gelassen werden.

Messmatrix \(H\)

Dem Filter muss ebenfalls mitgeteilt werden, was gemessen wird und in welchem Verhältnis es zum Zustandsvektor steht. Im Beispiel des Fahrzeugs, was in einen Tunnel einfährt, nur die Geschwindigkeit, nicht mehr die Position! Die Werte sind direkt mit dem Faktor 1 messbar (d.h. es wird direkt die Geschwindigkeit in der richtigen Einheit gemessen), weshalb in \(H\) nur 1.0 an entsprechende Stelle gesetzt wird.

$$H=\begin{bmatrix}
0 & 0 & 1 & 0\\
0 & 0 & 0 & 1
\end{bmatrix}$$

Messen die Sensoren in einer anderen Einheit oder die Größe über Umwege, müssen die Zusammenhänge in der Messmatrix \(H\) formelmäßig abgebildet werden.

Messrauschkovarianzmatrix \(R\)

Wie auch schon im eindimensionalen Fall die Varianz \(\sigma_0\), muss hier ebenfalls eine Messunsicherheit angegeben werden.

$$R=\begin{bmatrix}
10 & 0\\
0 & 10
\end{bmatrix}$$

Diese Messunsicherheit gibt an, wie sehr man den Messwerten der Sensoren vertraut. Da wir nur \(\dot x\) und \(\dot y\) messen, ist dies eine 2×2 Matrix. Ist der Sensor sehr genau, so sollten hier kleine Werte eingesetzt werden. Ist der Sensor relativ ungenau, so sollten hier große Werte eingesetzt werden.

Einheitsmatrix \(I\)

Zu guter letzt ist noch eine Einheitsmatrix notwendig.

$$I=
\begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}$$

Filterschritt Vorhersagen/Predict

Dieser Teil des Kalman-Filter wagt nun eine Vorhersage des Systemzustands in die Zukunft. Außerdem kann mit ihm unter bestimmten Voraussetzungen (Beobachtbarkeit) auch ein Zustand berechnet werden, welcher nicht gemessen werden kann! Das ist erstaunlich, aber für unseren Fall genau das, was wir benötigen. Die Position des Fahrzeugs können wir ja nicht messen, denn das GPS des Navigationsgeräts hat in einem Tunnel keinen Empfang. Durch die Initialisierung des Zustandsvektors mit einer Position und der Messung der Geschwindigkeit, kann aber mit der Dynamik \(A\) dennoch eine optimale Vorhersage über die Position getroffen werden.

$$x_{t+1}=A \cdot x_t$$

Die Kovarianz \(P\) muss ebenfalls neu berechnet werden. Die Unsicherheit über den Zustand des Systems wird im Predict Schritt größer, wie man im 1D Fall einfach gesehen hat. Im mehrdimensionalen Fall kommt die Messunsicherheit \(Q\) additiv hinzu, die Unsicherheit \(P\) wird also immer größer.

$$P=A \cdot P \cdot A’ + Q$$

Das war’s schon. Wir haben in die Zukunft (\(dt\)) gerechnet. Das Kalman-Filter hat eine Aussage über den erwarteten Systemzustand in der Zukunft getroffen.

Jetzt vergeht genau \(dt\) Zeit und das Filter ist im Messen/Korrigieren und überprüft, ob die Vorhersage des Systemzustands gut zu den neuen Messwerten passt. Wenn ja, wird die Kovarianz \(P\) vom Filter geringer gewählt (es ist sich sicherer), wenn nicht, größer (irgendwas stimmt nicht, das Filter wird unsicherer).

Filterschritt Messen/Korrigieren

Die nun folgenden mathematischen Berechnungen sind nichts, was man zwingend herleiten können muss. Das hat sich der Rudolf E. Kalman in ein paar ruhigen Minuten überlegt und es sieht verrückt aus, aber funktioniert.

Von den Sensoren kommen aktuelle Messwerte \(Z\), mit welchen eine Innovation \(w\) des Zustandsvektors \(x\) mit der Messmatrix \(H\) berechnet wird.

$$w=Z-(H \cdot x)$$

Anschließend wird geschaut, mit welcher Varianz (welche im mehrdimensionalen Fall Kovarianzmatrix heißt) weiter gerechnet werden kann. Dafür wird die Unsicherheit \(P\) sowie die Messmatrix \(H\) und die Messunsicherheit \(R\) benötigt.

$$S=(H \cdot P \cdot H’+R)$$

Damit wird das so genannte Kalman-Gain bestimmt. Es sagt aus, ob den Messwerten oder der Systemdynamik mehr vertraut werden soll.

$$K=\frac{P \cdot H’}{S}$$

Das Kalman Gain wird geringer, wenn die Messwerte dem vorhergesagten Systemzustand entsprechen. Sollten die Messwerte völlig anderes aussagen, so werden die Elemente der Matrix K größer.

Mit diesen Informationen wird jetzt der Systemzustand geupdated.

$$x=x+(K\cdot w)$$

Und auch eine neue Kovarianz für den kommenden Predict Schritt bestimmt.

$$P=(I-(K \cdot H)) \cdot P$$

Nun geht es wieder in den Schritt Prediction. Grafisch sieht das so aus:

Dieses Filter läuft dauerhaft, so lange Messwerte herein kommen. Es kann auch ‘Open Loop’ betrieben werden, also nur der Prediction Schritt wird ausgeführt, wenn keine Messwerte zur Verfügung stehen. Dann wird die Unsicherheit \(P\) allerdings immer größer.

Matlab Implementierung eines mehrdimensionalen Kalman Filters

% Paul Balzer | Motorblog
% CC-BY-SA2.0 Lizenz
clear all; clc
 
%% Messungen generieren
% Da wir jetzt keine Messwerte haben,
% generieren wir uns einfach selbst
% ein paar verrauschte Werte.
it=100;     % Anzahl Messwerte
realv=10;   % wahre Geschwindigkeit

                   % x'                y'
measurements = [realv+1.*randn(1,it); zeros(1,it)];
 
dt = 1;     % Zeitschritt
 
%% Initialisieren
%    x  y  x' y'
x = [0; 0; 10; 0];      % Initial State (Location and velocity)
P = [10, 0, 0, 0;...
    0, 10, 0, 0;...
    0, 0, 10, 0;...
    0, 0, 0, 10];       % Initial Uncertainty
A = [1, 0, dt, 0;...
    0, 1, 0, dt;...
    0, 0, 1, 0;...
    0, 0, 0, 1];        % Transition Matrix
H = [0, 0, 1, 0;...
    0, 0, 0, 1];        % Measurement function
R = [10, 0;...
    0, 10];             % measurement noise covariance
Q = [1/4*dt^4, 1/4*dt^4, 1/2*dt^3, 1/2*dt^3;...
    1/4*dt^4, 1/4*dt^4, 1/2*dt^3, 1/2*dt^3;...
    1/2*dt^3, 1/2*dt^3, dt^2, dt^2;...
    1/2*dt^3, 1/2*dt^3, dt^2, dt^2]; % Process Noise Covariance
I = eye(4);             % Identity matrix

%% Kalman Filter Steps
%
for n=1:length(measurements)
    % Prediction
    x=A*x;                  % Prädizierter Zustand aus Bisherigem und System
    P=A*P*A'+Q;             % Prädizieren der Kovarianz
 
    % Correction
    Z=measurements(:,n);
    y=Z-(H*x);              % Innovation aus Messwertdifferenz
    S=(H*P*H'+R);           % Innovationskovarianz
    K=P*H'*inv(S);          % Filter-Matrix (Kalman-Gain)
 
    x=x+(K*y);              % aktualisieren des Systemzustands
    P=(I-(K*H))*P;          % aktualisieren der Kovarianz
end

Das Filter bei der Arbeit

Nehmen wir nun also an, dass wir mit \(\dot x=10m/s\) in einen Tunnel fahren und die letzte bekannte Position \(x=0, y=0\) ist. Dann ermittelt das Kalman Filter die Position des Fahrzeugs, obwohl diese gar nicht gemessen werden kann. In der nachfolgenden Abbildung mal für 6 Filterdurchläufe:

Man sieht, wie sich das Filter über die aktuelle Position immer unsicherer wird (Kurve links oben wird flacher), über die gefahrene Geschwindigkeit allerdings immer sicherer (Kurve rechts oben wird immer schmaler). Die Geschwindigkeitsmessung ist aber auch immer sehr exakt 10m/s, wodurch die Innovation sehr klein wird, das Filter also nicht viel korrigieren muss.

Da die Fahrzeuggeschwindigkeit über den CAN-Bus ca. alle 20ms zur Verfügung steht, entsprechen 6 Iterationen nur reichlich 0.1s. Das Filter konvergiert, je nach Wahl der Anfangsbedingungen, relativ schnell. Beispielsweise ist nach 100 Iterationen (entspricht 2s auf dem Fahrzeug) die Varianz schon sehr gering, das Filter ist sich also ziemlich sicher, dass es richtig liegt.

Python Implementierung des Kalman Filters

Falls Matlab nicht zur Verfügung steht, hier eine Beispielimplementierung in Python.

import numpy as np
for n in range(len(measurements[0])):

    # Time Update (Prediction)
    # ========================
    # Project the state ahead
    x = A*x

    # Project the error covariance ahead
    P = A*P*A.T + Q

    # Measurement Update (Correction)
    # ===============================
    # Compute the Kalman Gain
    S = H*P*H.T + R
    K = (P*H.T) * np.linalg.pinv(S)

    # Update the estimate via z
    Z = measurements[:,n].reshape(2,1)
    y = Z - (H*x) # Innovation or Residual
    x = x + (K*y)

    # Update the error covariance
    P = (I - (K*H))*P

Der Python Code aus dem Video ist auf balzer82.github.io/Kalman zu finden.

 

Filterdesign: Wie wähle ich Q und R?

Insgesamt ist egal wie groß die Zahlenwerte in \(Q\) und \(R\) sind, vielmehr ist entscheidend in welchem Verhältnis sie stehen. Wird \(R\) zehnfach größer gewählt und \(Q\) ebenfalls, so wird das im Filter kaum Auswirkungen haben. Das Verhältnis der Werte ist entscheidend. Die korrekte Wahl von \(Q\) und \(R\) sind direkt für die Filterperformance verantworlich und bilden die grundlegende Frage des Filterdesigns:

Diese Entweder/Oder Fragestellung kann nur anwendungsspezifisch entschieden werden. In einigen Fällen möchte man einfach schlecht messende Sensoren für einen relativ konstanten Prozess filtern, im anderen Fall will man mehrere Sensoren fusionieren und die Dynamik soll erhalten bleiben. Demnach sind die Matrizen zu wählen. Alternativ kann natürlich auch das Filter Adaptiv ausgelegt werden, d.h. \(Q\) oder/und \(R\) können im laufenden Betrieb geändert werden.

Fazit

Das Kalman-Filter ist relativ schnell und einfach zu implementieren und liefert für normalverteilte verrauschte Sensorwerte unter gewissen Voraussetzungen (nicht näher erläutert) eine optimale Schätzung des Zustands. Herr Kalman war von seinem Algorithmus so überzeugt, dass er auch einen befreundeten Ingenieur bei der NASA begeistern konnte. Und so half dieses Filter erstmals im Apollo Guidance Computer bei den Mondlandungen.

Ach übrigens: Für kommerzielle Anfragen gibt es auch die Möglichkeit mich zum Thema zu konsultieren.   Hire me!

Errata

02/2014: In vorheriger Version war ein Fehler: Im Matlab Code waren die Update/Predict Schritte vertauscht.
03/2014: Die Erläuterung wurde noch mal überarbeitet. Python Implementierung eingefügt
07/2016: Travolta Filter Design Tradeoff hinzugefügt