Kalman Filter. Stephan Meyer Matrix nennt man ein rechteckiges Zahlenschema der Form: a 11 a 12 a 13

Transkript

1 Kalman Filter Ortsbezogene Anwendungen und Dienste Stephan Meyer Zusammenfassung: Der Kalman Filter stellt ein mathematisches Regelwerk zur Verfügung, welches Werteschätzung in linearen dynamischen Systemen erlaubt. Er wird hauptsächlich für die Korrektur von fehlerbehafteten Messwerten verwendet. Die im Folgenden beschriebenen Eigenschaften prädestinieren den Kalman Filter besonders für den Einsatz in der Satellitennavigation. Die vorliegende Studienarbeit beschreibt einerseits die Funktionsweise als auch Anwendungsbeispiele. Nicht behandelt werden die mathematischen Hintergründe des Kalman-Filters und dessen Herleitung. 1 Grundlagen Da der Kalman Filter ein Vorwissen über sehr verschiedene Teilbereiche der Mathematik erfordert, wird an dieser Stelle kurz auf die benötigten Grundlagen eingegangen. Da dies den Rahmen der Studienarbeit sprengen könnte, wird jedoch nicht jedes Detail behandelt. Alle im Folgenden beschriebenen Grundlagen stützen sich auf das von Peter Stingl herausgegebene Buch Mathematik für Fachhochschulen (siehe [Stingl04]). 1.1 Matrizen Matrix nennt man ein rechteckiges Zahlenschema der Form: a 11 a 12 a 13 A 43 = a 21 a 22 a 23 a 31 a 32 a 33 a 41 a 42 a 43 Üblicherweise werden Matrizen mit Groÿbuchstaben bezeichnet, die Zahlen im Index stehen für Zeile und Spalte. Es gelten folgende Rechengesetze und Regeln: Addition: ( ) ( ) ( ) a 11 a 12 b 11 b 12 a 11 + b 11 a 12 + b 12 A 22 + B 22 = + = a 21 a 22 b 21 b 22 a 21 + b 21 a 22 + b 22

2 Multiplikation: a 11 a 12 ( ) a 11 b 11 + a 12 b 21 a 11 b 12 + a 12 b 22 b 11 b 12 A 32 B 22 = a 21 a 22 = a 21 b 11 + a 22 b 21 a 21 b 12 + a 22 b 22 b 21 b 22 a 31 a 32 a 31 b 11 + a 32 b 21 a 31 b 12 + a 32 b 22 Transponierung: Spiegeln der Matrix an der Hauptdiagonalen. a 11 a 12 a 13 a 11 a 21 a 31 A 33 = a 21 a 22 a 23, A T 33 = a 12 a 22 a 32 a 31 a 32 a 33 a 13 a 23 a 33 Invertierung: Berechnung einer Matrix A 1 für die gilt: A A 1 = E wobei E die Einheitsmatrix darstellt, eine Matrix mit Einsen auf der Hauptdiagonalen und Nullen auf allen anderen Positionen. Auf die Berechnung der inversen Matrix wird an dieser Stelle nicht eingegangen, ein praktikables Verfahren ist der Gauÿ-Jordan-Algorithmus (siehe [Stingl04], S. 210). 1.2 Zufallsvariablen Werden die Versuchsausgänge eines Zufallsexperiments auf reelle Zahlen abgebildet, so nennt man diese Zufallsvariablen. Man unterscheidet zwischen stetigen Zufallsvariablen, welche nur bestimmte Werte annehmen können wie z.b. die Augensumme bei mehrmaligem Würfeln, sowie den diskreten Zufallsvariablen, welche alle Werte eines Spektrums annehmen können. Für die Verteilung der Werte von Zufallsvariablen gibt es zwei Richtwerte, den Erwartungswert µ, der einen ungefähren höchstwahrscheinlichen Wert angibt, den die Zufallsvariable annehmen kann, sowie die Varianz σ 2 die ein Maÿ für die Abweichung von dem Erwartungswert darstellt. 1.3 Gauÿ- bzw. Normalverteilung Wichtig für die Beschreibung einer Zufallsvariablen ist auÿerdem noch die Art der Verteilung. Da der Kalman Filter alle Messwerte als gauÿverteilt ansieht, wird hier nur auf diese spezielle Verteilungsart eingegangen. Die Gauÿverteilung beschreibt die Verteilung nach einer Dichtefunktion, welche den in Abbildung 1 dargestellten Graphen erzeugt. Die Wahrscheinlichkeit, dass der Wert der Zufallsvariablen im Bereich zwischen zwei x-werten liegt, erhält man, indem man eine Integration durchführt, also die Fläche berechnet, welche der Graph und beide Werte einschlieÿen. Dass eine Gauÿverteilung mit einem bestimmten

3 Abbildung 1: Gauÿsche Glockenkurve Erwartungswert µ und einer Varianz σ 2 vorliegt, beschreibt man mit der Formel p(x) = N(µ, σ 2 ) (1) Ein Spezialfall der Gauÿverteilung ist die so genannte Normalverteilung, hier ist der Erwartungswert gleich Null und die Varianz Eins. p(x) = N(0, 1) (2) 1.4 Kovarianz und Kovarianz-Matrizen Die Kovarianz stellt ein Maÿ für die Abhängigkeit von zwei Zufallsvariablen untereinander dar. Besteht kein Zusammenhang, nimmt sie den Wert Null an, ansonsten entweder positive oder negative Werte, die entweder einen linearen oder indirekt linearen Zusammenhang beschreiben. Je höher der Betrag der Kovarianz, desto höher ist auch die Abhängigkeit. Berechnet wird die Kovarianz mit folgenden Formeln: Cov(X, Y ) := µ((x µ(x))(y µ(x))) (3) Cov(X, X) := σ 2 (X) (4) Stellt man mehrere Zufallsvariablen in einem Vektor x dar, so kann man die zugehörige Kovarianzmatrix Cov(x) angeben: X Cov(X, X) Cov(X, Y ) Cov(X, Z) x = Y, Cov(x) = Cov(Y, X) Cov(Y, Y ) Cov(Y, Z) (5) Z Cov(Z, X) Cov(Z, Y ) Cov(Z, Z)

4 2 Der Kalman Filter Der Kalman Filter wurde 1960 von dem ungarisch-amerikanischem Mathematiker Rudolf Emil Kalman in dem Dokument A New Approach to Linear Filtering and Prediction Problems (siehe [Kalman60]) vorgestellt. Zu den Vorteilen des Kalman Filters zählen unter anderem die rekursive Implementierung und der geringe Rechenaufwand, was ihn heute besonders für die Positionsbestimmung in mobilen Geräten attraktiv macht. Rekursiv bedeutet hier, dass der Filter immer nur sein zuletzt berechnetes Ergebnis benötigt und nicht weitere Werte speichern muss, wodurch auch der Speicherbedarf gering gehalten wird. 2.1 Modell Der Kalman Filter ist ein rekursives Verfahren, das in jedem Rechenschritt basierend auf einer fehlerbehafteten Messung des Systemszustands und auf der vorherigen Schätzung eine Abschätzung für den aktuellen Zustand des Systems abgibt. Der aktuelle Zustand des zu schätzenden Prozesses wird mit einem Zustandsvektor x k angegeben, wobei k für die Nummer des Schrittes steht. x k 1 gibt somit den vorherigen Zustand an. Es wird eine Matrix A benötigt, die den vorherigen Zustand in den aktuellen Zustand umrechnet, z.b. wenn x k 1 Werte für Position und Geschwindigkeit enthält, rechnet A anhand der dieser Werte eine neue Position für x k aus. Weiterhin kann der Systemzustand von auÿen z.b. durch menschlichen Eingri beeinusst werden. Diesen Einuss kann man mit der Gröÿe u k und der Transformationsmatrix B in das Verfahren mit einrechnen. Schlieÿlich kann noch der Prozessfehler zu einer Verfälschung der Schätzung führen, weshalb dies mit dem gauÿverteilten Faktor w k 1 berücksichtigt werden kann. Aus diesen Angaben kann man nun die Formel für x k angeben: x k = A x k 1 + B u k + w k 1 (6) Die aktuelle fehlerbehaftete Messung des Zustandes wird mit z k angegeben. Die nachfolgende Formel zeigt die Modellvorstellung für z k : z k = H x k + v k (7) Zu beachten ist hier, dass sich die Elemente des Vektors z k durchaus von den Elementen von x k unterscheiden dürfen. Beispielsweise könnte x k Werte für Position oder Geschwindigkeit enthalten, z k jedoch nur Positionsdaten. Die überüssigen Geschwindigkeitsdaten können mit der Transformationsmatrix H herausgenommen werden. Da Hx k den eigentlichen Systemzustand darstellt, muss der Fehler der Messung mit dem Vektor v k hinzugefügt werden.

5 2.2 Fehlervektoren Der Kalman Filter unterscheidet zwischen zwei Arten von Fehlern, welche auf das System Einuss nehmen können. Der Messfehler, welchen man als Measurement Noise bezeichnet, sowie das Prozessrauschen, das Process Noise genannt wird. Die Kovarianzmatrizen, welche beide Fehler beschreiben, werden als konstant angenommen, was eine Vereinfachung darstellt, jedoch in der Realität eher selten der Fall ist. Beispielsweise schwankt bei GPS die Genauigkeit ständig mit der Sichtbarkeit der Satelliten Measurement Noise Der eigentliche Fehler der Messung z k, ausgedrückt mit dem Vektor v k, wird als Measurement Noise bezeichnet. Dieser Messfehler wird als gauÿverteilt mit dem Erwartungswert 0 und der Kovarianzmatrix R angenommen. Der Erwartungswert ist deshalb 0, weil v k nur die Abweichung von z k vom realen Systemzustand beschreibt. R beschreibt die Kovarianz zwischen den einzelnen Elementen von v k. Sollten die Messfehler sich nicht untereinander beeinussen, so ist bei R lediglich die Hauptdiagonale mit den Varianzen der Messwerte belegt und die restlichen Werte 0. Die Formel für die Verteilung lautet: p(v) = N(0, R) (8) Process Noise Der Begri des Prozessrauschens ist etwas schwieriger zu denieren. Es beschreibt die Abweichung des Modell-Systems vom eigentlichen realen Systemzustand. Hintergrund ist, dass zwischen zwei Schritten k und k 1 normalerweise eine gewisse Zeitspanne vergeht, in der das System seinen Zustand auch ändern kann. Die Transformation von x k 1 nach x k rechnet jedoch über die gesamte Zeitspanne mit den Werten von x k 1. Beispiel: Enthält der Zustandsvektor Informationen über Position und Geschwindigkeit, so kann mit der Transformation mit der Matrix A anhand der vorherigen Position und Geschwindigkeit die aktuelle Position berechnet werden. Dies ist aber nur dann genau, wenn sich die Geschwindigkeit zwischen den beiden Schritten konstant bleibt, was nicht immer der Fall sein muss. Zum Beispiel kann bei Navigationssystemen im Auto die Schwankung zwischen zwei Schritten stark variieren, wenn man sich im Stadtverkehr bewegt. Der Vektor w k rechnet diesen Fehler in x k ein. w k ist gauÿverteilt nach p(w) = N(0, Q) (9) wobei Q die Kovarianzmatrix des Fehlers ist.

6 2.3 Ablauf Zur Initialisierung des Filters müssen zuerst alle benötigten Matrizen mit Anfangswerten belegt werden. Nach dem Start des Vorgangs wird in jedem Schritt zuerst die sog. Time Update Phase durchgeführt, welche aus dem vorherigen geschätzten Systemzustand eine a priori (lat. Vom Früheren her) Schätzung berechnet. Zusätzlich werden hier auch manuelle Änderungen am System mit eingerechnet. Nach dieser Phase wird die fehlerbehaftete Messung durchgeführt und von der Measurement Update Phase verarbeitet. Diese Phase gibt nun eine a posteriori (lat. was aus der Erfahrung stammt) Abschätzung des Systemzustandes ab, indem sie die Messwerte und die Schätzwerte der Time Update Phase verwendet. Danach kann der Schätzwert ausgegeben werden und die nächste Time Update Phase beginnt. Das Ergebnis der Measurement Update Phase kann als aktuelles Schätz-Ergebnis nun weiterverarbeitet werden. Abbildung 2 illustriert diesen Ablauf. Abbildung 2: Ablauf der Abschätz-Vorgänge Der Kalman Filter kann hier als Black-Box bezeichnet werden, er nimmt nur Änderungsund Messdaten entgegen und liefert daraus ein Schätzergebnis des aktuellen Systemzustandes, ohne dass man sich um die Vorgänge im Filter kümmern muss. Beide Phasen berechnen zusätzlich noch eine Schätzung für die Fehlerkovarianzmatrix P, welche die Varianz des Systemzustandes und zusätzlich die Kovarianz zwischen den Elementen des Zustandsvektors angibt. Beispielsweise hat der Fehler bei der Messung der Position in x-richtung keinerlei Zusammenhang zu der Position in y-richtung; die Kovarianz ist hier Null. Andererseits kann ein Fehler bei der Geschwindigkeitsmessung in x-richtung sehr wohl Einuss auf die Schätzung der x-position haben. Die folgenden Kapitel beschäftigen sich nun genauer mit den beiden Ablauf-Phasen.

7 2.3.1 Time Update Die Time Update Phase liefert die a priori Schätzwerte für x k und P k : ˆx k,pri = A ˆx k 1,pos + B u k (10) P k,pri = A P k 1,pos A T + Q (11) Formel 10 eine gewisse Ähnlichkeit zu dem Modell für x k (siehe Formel 6). Hier wird wie in 6 einerseits die Transformation von ˆx k 1 zu ˆx k mittels der Matrix A durchgeführt und zusätzlich noch mittels u k und B die manuellen Änderungen am System eingerechnet. Die a priori Schätzung für die Fehlerkovarianzmatrix entsteht aus der Transformation der vorherigen a posteriori Fehlerkovarianzmatrix mit A sowie dem Addieren der Kovarianzmatrix des Prozessrauschens Q. Tabelle 1 gibt noch einmal einen kurzen Überblick über die verwendeten Elemente, sofern nicht schon weiter oben erklärt. Symbol ˆx k,pri ˆx k,pos P k,pri P k,pos Bedeutung a priori Schätzwert des Vektors zum Zeitpunkt k a posteriori Schätzwert des Vektors zum Zeitpunkt k a priori Fehler-Kovarianzmatrix zum Zeitpunkt k a posteriori Fehler-Kovarianzmatrix zum Zeitpunkt k Tabelle 1: Symbole und ihre Bedeutungen Measurement Update Das Measurement Update nimmt die a priori Schätzungen der Time Update Phase und die Messung z k entgegen und liefert daraus den a posteriori Schätzwert. Zuerst wird jedoch das so genannte Kalman Gain K k ermittelt (siehe Formel 12), ein Zwischenwert, welcher für die Berechnung von x k,pos und P k,pos benötigt wird. Wichtige Elemente, die in die Berechnung von K k einieÿen, sind die a priori Fehlerkovarianzmatrix sowie die Kovarianzmatrix des Messfehlers. Formel 13 liefert die a posteriori Abschätzung für x k was das Filterergebnis darstellt. Ebenfalls neu berechnet wird die a posteriori Fehlerkovarianzmatrix (Formel 14). K k = P k,pri H T (H P k,pri H T + R) 1 (12) ˆx k,pos = ˆx k,pri + K k (z k H ˆx k,pri ) (13) P k,pos = (E K k H) P k,pri (14) Der Ausdruck (z k H ˆx k,pri ) in Formel 13 liefert nach Formel 7 eine geschätzte Abweichung von z k zu ˆx k, also in etwa den Messfehlervektor v k.

8 2.4 Einschränkung Die wichtigste Annahme im Modell des Kalman Filters ist, dass alle Messwerte gauÿverteilt seien. Deshalb kann man den Kalman Filter nicht auf Systeme anwenden, in denen die Messwerte anderweitig verteilt sind. 2.5 Beispiel Um die Funktionsweise des Kalman Filters darzustellen, soll nun folgendes Beispiel eines einfachen Systems für die Positionsbestimmung via Satellit dienen: Der Systemzustands-Vektor x enthält Werte für Position und Geschwindigkeit in einem als zweidimensional angenommenen Gelände. Der Empfänger des Systems kann jedoch nur Positionsdaten messen, weshalb der Vektor des Messergebnisses z nur Positionsdaten enthält. Die Schätzung der Geschwindigkeit ist somit rein Aufgabe des Kalman Filters. Für die Modellvorstellung, dass sich z aus x und v zusammensetzt, muss x noch auf Form von z gebracht werden, wozu die Matrix H verwendet wird. Die Matrix A ist für die Transformation zuständig, ihre Multiplikation mit x erzeugt die physikalischen Formeln für die Position nach der Geschwindigkeit (x = x 0 + v t). t stellt hier die Zeitdierenz zwischen zwei Schritten dar. Da nur der Filter die Geschwindigkeit schätzt, kann es bei Schwankungen der Geschwindigkeit zu groÿen Abweichungen im Filterergebnis kommen. Hierzu ist es noch möglich, dem Filter mit dem Vektor u Änderungen an der Geschwindigkeit mitzuteilen. u muss mit der Matrix B auf die richtige Form gebracht werden. Die Varianz des Messfehlers soll in x-richtung 10 Meter und in Y-Richtung 12 Meter betragen. Beide Varianzen sind in der Matrix R zu nden. Da das Prozessrauschen schwierig zu denieren ist, wird es in diesem einfachen Beispiel nicht betrachtet und somit die Matrix Q mit Nullen belegt. Die untenstehenden Formeln zeigen noch einmal die Belegungen der Werte, mit denen nun der Kalman Filter gestartet werden könnte. x 1 0 t 0 ( ) 0 0 x = y vx A = t u = vx B = 0 0 vy 1 0 vy R = ( ) x z = y ( ) ( ) H = Q = ( ) x v = y

9 3 Anwendungsbeispiele Während der Kalman Filter für die Positionsbestimmung zu einem elementaren Werkzeug geworden ist, gibt es jedoch auch noch einige andere Anwendungsfälle, in denen ebenfalls eine Glättung von Messwerten erreicht werden muss. 3.1 GPS-/INS-Navigation Die Kombination aus einem GPS-Empfänger und einem INS (Inertial Navigation System) wird vor allem im militärischen Bereich eingesetzt. Ein INS bestimmt die aktuelle Position aufgrund von gemessener Geschwindigkeit und Beschleunigung, die von einem bekanntem Punkt aus auf das Gerät wirkten. Diese Art der Positionsbestimmung wird jedoch mit zunehmender Dauer des Vorgangs immer ungenauer, weshalb zusätzlich ein GPS-Empfänger eingesetzt wird, dessen Messdaten mit einem Kalman-Filter geglättet und zur Korrektur des INS eingesetzt werden. Korrektur bedeutet hier, dass bei zu groÿen Divergenz zwischen GPS- und INS-Messung das INS neu initialisiert wird. Abbildung 3 zeigt schematisch diesen Ablauf. Abbildung 3: Navigationssystem mit INS-/GPS-Kombination 3.2 Navigation von Raumkapseln Eine der ersten Anwendungen des Kalman Filters war die Entwicklung der Apollo-Raumkapseln der NASA. Der Kalman Filter sollte hier bei der Navigation und Steuerung im Weltraum eingesetzt werden. Die seit 1959 betriebene Entwicklung führte unter Einbeziehung von Rudolf Kalman zu der Entwicklung des Extended Kalman Filters, welcher auch auf nicht-lineare Systeme angewandt werden konnte, jedoch unter ungünstigen Rahmenbedingungen aufgrund von Divergenz keine vernünftigen Ergebnisse lieferte. Der da-

10 nach entworfene Schmidt-Kalman Filter, benannt nach dem NASA-Ingenieur Stanley F. Schmidt, wandelt den Extended Kalman Filter dementsprechend, dass auf diese Fehlerzustände zufriedenstellend reagiert werden kann. Weitere Informationen bietet [McGSch85]. 3.3 Satellitengestützte Messungen in der Erd-Atmosphäre Messungen von Gasen wie z.b. Ozon in der Erd-Atmosphäre werden ebenfalls als gauÿverteilt angenommen, was die Filterung der Messwerte durch den Kalman Filter erlaubt. In [Bittner03] wird der Kalman Filter mit anderen Verfahren verglichen. 3.4 Seismographen Die Messungen hochempndlicher Seismographen werden oft durch ein Rauschen gestört, welches durch die normalen Erschütterungen aus der Umgebung auftreten. Ein Kalman Filter kann hier helfen, die Messwerte zu glätten und somit Erdstöÿe wie gewünscht zu erkennen. 3.5 Mustererkennung Hier wird der Kalman Filter eingesetzt, um aus verschiedenen Mustern wesentliche Bestandteile herauszultern. Zum Beispiel bei der Erkennung von Personen bei der Videoüberwachung. 4 Fazit Trotz der Tatsache, dass die Entwicklung des Kalman Filters schon fast ein halbes Jahrhundert zurückliegt, erfreute er sich besonders in den letzten Jahren einer gewissen Aufmerksamkeit. Seine Stärken liegen einerseits im geringen Rechenaufwand, der ihn besonders für die Positionsbestimmung auf mobilen Geräten wie Handys oder PDAs attraktiv macht, als auch in seiner Flexibilität. Die vielen Varianten des Kalman Filters erlauben eine Behandlung von diversen Problemstellungen aus der Technik und Wissenschaft. Jedoch bleibt bei dem Kalman Filter die Einschränkung, dass er sich nur für Systeme eignet, deren Fehlerbetrachtung gauÿverteilt ist und so anderweitig verteilte Systeme nicht analysiert werden können.

11 Literatur [Bittner03] M. Bittner (2003) Das Verfahren des Kalman-Filters zur Interpolation von satellitengestützten Ozonmessungen The World Data Center for Remote Sensing of the Atmosphere, Deutsches Zentrum für Luft- und Raumfahrt [BiWe06] Gary Bishop, Greg Welch (2006) An Introduction to the Kalman Filter Technical Report TR , University of North Carolina, Department of Computer Science [Kalman60] Robert E. Kalman (1960) A New Approach to Linear Filtering and Prediction Problems Transactions of the ASME Journal of Basic Engineering, Vol. 82, Series D, pp [McGSch85] Leonard A. McGee, Stanley F. Schmidt (1985) Discovery of the Kalman Filter as a Practical Tool for Aerospace and Industry NASA Technical Memorandum [Stingl04] Peter Stingl Mathematik für Fachhochschulen - Technik und Informatik 7. Auage 2004 Carl Hanser Verlag München Wien ISBN