Das Kalman-Filter einfach erklärt [Teil 1]

Nehmen wir an man fährt mit seinem PKW Navi in einen Tunnel. Das GPS Signal ist weg. Trotzdem möchte man vielleicht angezeigt bekommen, dass man die Ausfahrt im Tunnel nehmen soll. Woher soll das Navi wissen, wo man gerade ist? Richtig: Es fusioniert die Fahrzeugsensoren und berechnet damit die Position so gut es geht.

Nun denkt man sich: OK, wenn ich zuletzt am Tunneleingang war und mit 50km/h fahre, dann kann das Navi ja exakt ausrechnen, wo (x=Position) man sich 1 Minute (t=Zeit) später befindet:

Soweit die perfekte Welt. Doch das Ausrechnen übernimmt ein Mikrocontroller und dieser ist auf Sensoren angewiesen. Sowohl die Sensoren haben zufällige Messabweichungen, die Übertragungsstrecke hat Störungen als auch die Auflösung von CAN-Bus oder Analog-Digitalwandler lassen viele Ungenauigkeiten in die einfach Angabe “Geschwindigkeit” kommen. Beispielsweise sieht ein Geschwindigkeitssignal so aus:

Im Mittel stimmt die gemessene Geschwindigkeit schon, allerdings gibt es gewisses “Rauschen”. Berechnet man ein Histogramm der ermittelten Geschwindigkeiten, so sieht man, dass die ermittelten Werte ungefähr einer Normalverteilung unterliegen.

Es gibt also einen, und wirklich nur einen, Maximalwert (unimodal) und eine Streubreite (Varianz). Ist dies der Fall, so kann man mit einem Trick die Berechnung trotzdem sehr gut durchführen.

Idee des Kalman-Filter in 1 Dimension

Um nicht gleich mit Kanonen auf Spatzen zu schießen, möchte ich die Idee des Kalman-Filter (nach Rudolf Emil Kalman) zunächst nur mit einer Dimension (hier: Entfernung) erläutern. Die nachfolgende Beschreibung ist dem Udacity Kurs CS373  von Prof. Sebastian Thrun entlehnt.

Bekanntes Rauschen hilft

Um die Berechnung trotz Messrauschen optimal durchführen zu können, muss das “wie stark” bekannt sein. Dieses “wie stark” wird mit der Varianz der Normalverteilung ausgedrückt. Diese bestimmt man 1x für den Sensor und nutzt danach nur diese “Unsicherheit” für die Berechnung.

Es wird im Folgenden nun nicht mehr mit absoluten Werten gerechnet, sondern mit Mittelwerten (\(\mu\)) und Varianzen (\(\sigma^2\)) der Normalverteilung. Der Mittelwert der Normalverteilung ist der Wert, den man berechnen möchte, die Varianz gibt an, wie sicher man sich dabei ist. Umso schmaler die Normalverteilung (geringe Varianz), desto sicherer ist man sich mit der Messung. Ein Sensor, welcher 100% exakt misst, hat also eine Varianz von \(\sigma^2=0\) (den gibt es aber nicht).

Neue Messung verbessert die Schätzung

Gehen wir also davon aus, das gerade das GPS Signal verloren gegangen ist und das Navi völlig im Unklaren, wo man sich befindet. Die Varianz ist hoch, die Kurve entsprechend platt. Es herrscht Unsicherheit.

Nun kommt eine Geschwindigkeitsmessung vom Sensor, welche mit entsprechender Varianz ebenfalls “ungenau” ist. Diese beiden Unsicherheiten müssen nun miteinander verknüpft werden. Mit Hilfe der Bayes Regel geht die Addition mathematisch korrekt durchzuführen. Prof. Thrun erklärt das im Udacity Kurs CS373 sehr anschaulich:

Der neue Mittelwert ist:

Diese beiden Informationen (eine für die aktuelle Position und eine für die Messunsicherheit des Sensors) ergeben jetzt tatsächlich ein besseres Ergebnis! Die Matlab Implementierung davon ist:

function [new_mean, new_var]=update(mean1,var1,mean2,var2)
%% [new_mean, new_var]=update(mean1,var1,mean2,var2)
% Berechnet geschätzte Position nach Messung der Position

new_mean=(var2*mean1 + var1*mean2) / (var1+var2);
new_var = 1/(1/var1 +1/var2);

Umso schmaler die Normalverteilung, umso sicherer ist das Ergebnis.

Bewegung verschlechtert die Schätzung

Das Fahrzeug bewegt sich natürlich auch, was sich nachteilig auf die Genauigkeit der Positionsbestimmung auswirkt. Ein Sensor kann z.B. die Umdrehung des Rades bestimmen und über die Annahme, welchen Radius das Rad hat, einen Rückschluss auf die zurückgelegte Strecke treffen, dennoch wird das immer etwas ungenau bleiben. Diese Bewegungsungenauigkeit wird ebenfalls mit einer Normalverteilung beschrieben. Das Zusammenrechnen mit der aktuellen Schätzung läuft dieses mal etwas anders, da die “Bewegung” auch “Vorhersage” genannt werden kann. Man kann nach der Berechnung schätzen, wo man zum nächsten (Mess-)Zeitpunkt sein wird, folglich muss das Ergebnis ungenauer sein, man schaut ja in die Zukunft.

In unserem Beispiel ist das µ einfach v*dt, nämlich die Distanz, die man zurücklegt in der Berechnungszeit.

Die Matlab Implementierung davon ist:

function [new_mean, new_var]=predict(mean1,var1,mean2,var2)
%% [new_mean, new_var]=predict(mean1,var1,mean2,var2)
% Berechnet neuen Mittelwert und neue Varianz nach Bewegung mit Mittelwert
% und Varianz von Ausgangsposition und Unsicherheit der Bewegung sowie
% vorhergesagter Bewegungsentfernung
% new_mean = mean1 + mean2;
% new_var = var1 + var2;
new_mean=mean1+mean2;
new_var =var1+var2;

Warum nun das ganze?

Messen & Updaten: Das Kalman-Filter

Das Kalman-Filter macht nichts anderes als immer wieder diese beiden Funktionen zu berechnen.

%% 1D-Kalman Filter
% Aufbau und Erläuterung nach Udacity Kurs CS373 taught by Sebastian Thrun
% Paul Balzer
clear all, clc

%% Ausgangsbedingungen
% sigma=Standardabweichung, m = Mittelwert
sigma_mess = 4; % Standardabweichung Messung
sigma_move = 2; % Standardabweichung Bewegung

mu = 0; % Startposition
sig = 10000; % Unsicherheit zu Beginn

%% Kalman-Berechnung

messung = [5, 6, 7, 9, 10];
bewegung= [1, 1, 2, 1, 1];

for i=1:length(messung)
 [mu,sig]=predict(mu,sig,bewegung(i),sigma_move);
 disp(['Predict: ' num2str(mu) ', ' num2str(sig)])
 [mu,sig]=update(mu,sig,messung(i),sigma_mess);
 disp(['Update: ' num2str(mu) ', ' num2str(sig)])
end

Das Filter überschreibt zyklisch den Mittelwert (das was mich interessiert) und die Varianz des Ergebnisses. Beim Schritt Messen (update) steigt die Sicherheit des Ergebnisses, beim Schritt Bewegen (predict) sinkt sie. Insgesamt wird sich das Filter allerdings immer sicherer, wo es sich befindet, so lange die Messwerte nicht zu stark vom vorhergesagten Wert abweichen.

Beispiel Positionsbestimmung

Beispielhaft die Berechnung der Position in einem Tunnel, wenn der Predict-Schritt eine Distanz von 20m mit einer Varianz von 0.2 vorhersagt, die tatsächliche Entfernung aber mit einer Varianz von 20 ermittelt wird.

Schaut man in den Kern des Kalman-Filters während der 6 Iterationsschritte, so erkennt man, wie die Varianz sinkt (Wahrscheinlichkeit richtig zu liegen steigt!).

Umgangssprachlich: Da die gemessenen Werte (in update) relativ gut zu den vorhergesagten (durch predict) passen, wird sich das Filter immer sicherer, dass es richtig liegt (Normalverteilungen werden schmaler und höher), obwohl die Werte verrauscht sind.

Kalman-Filter, war das nicht eigentlich das mit den ganze Matrizen?

Ja, war es. Das habe ich hier aber alles weg gelassen, weil man es so wesentlich besser versteht. Das Problem ist: Ohne Matrizen kann man nur mit einer Dimension rechnen und das ist nett, aber nicht ausreichend. Deshalb gibt es im 2. Teil:

Multidimensionales Kalman Filter