17.03.06 Kalman Filter zur Rekonstruktion von Messsignalen Denis Schetler 15. Dezember 2006

Agenda 360° Laserscanner Kalman Filter Motivation Anwendungsbereiche 17.03.06 Agenda 360° Laserscanner Kalman Filter Motivation Anwendungsbereiche Funktionale Übersicht Beispiel: Wurfparabel Objektverfolgung Zusammenfassung Ausblick Masterarbeit Externe Motivation Sensonen, mikrocontroller, fahrerassistenzsysteme, transportplattform für lager

17.03.06 FAUST Projekt Ein Sensortyp reicht für robuste Objektverfolgung nicht aus! Sensordatenfusion Laserscanner + horizontaler Schnitt der Fahrbahn + Genauigkeit der Abstandsbestimmung - Straßenmarkierung und Schilder Kamera + Enthält alle Informationen - Begrenzter Öffnungswinkel - Aufwendige Verarbeitung - Wetter und Licht - Genauigkeit der Abstands- bestimmung

17.03.06 FAUST Projekt Skelettierung: Verdünnung von Linien und Extraktion von Formparameter  Skelettlinien (Mittelinen) Optischer Fluss: Die Grauwertverschiebung in der Bildebene zwischen aufeinander folgenden Bildern. Nachbarschaftsoperatoren: Vorverarbeitung für die Kanten- und Linienerkennung und die Skeletierung. Kanten und Linien: Extraktion von Kanten und Linien aus dem Bild.

optimale Schätzwerte für die Systemszustandsgröße x 17.03.06 1. Kalman Filter: optimale Schätzwerte für die Systemszustandsgröße x Prädiktion: zeitliche Entwicklung des Systems. Berchnung Messfehlerkorrektur: Gleichungen für die Rückkopplung durch die Messung. P Erwartete Fehler für die Abweichung der Systemzustände e(k)- = x(k) – x^(k)- P(k)- = E[e(k)- * e(k)-T] Qk Bodenwelle; Wasserfütze Rk abhängig von der Toleranz des Laserscanners

17.03.06 2. Motivation Überholvorgang: Objektverfolgung basierend auf Laserscanner, Radar oder Bildverarbeitung Berichte zu Forschungsprojekten vom „3rd International Workshop on Intelligent Transportation“ in Hamburg März 2006 z.B.: Intersection Safety Masterarbeit Bremsassistent und Ausweichassistent Standard: Moving Average Filter durch Kalman Filter ersetzen. Objektverfolgung bei Bildbasierten Systemen, linke und rechte Kante des Objektes extrahiert. Durch Verwendung dynamischen Modells des Fahrzeuges mit anschließender Kalmanfilterung lassen sich die erwarteten Kantenpositionen im Bild präzisieren, was die Zuverlässigkeit der Objektverfolgung erhöht Bei Intersection Safety wird das Kalman Filter für das Object Tracking benutzt. MA Filter Das FIR-Filter berechnet den gefilterten Messwert anhand einer Anzahl N Faktoren an vorangegangenen Messwerten, die mit jeweils einem Faktor gewichtet werden. Eine spezielle Variante des FIR-Filters ist das Moving-Average-Filter, bei dem alle Messwerte mit dem Faktor 1/(N+1) multipliziert werden.

Messsignalverarbeitung 17.03.06 3. Anwendungsbereiche Messsignalverarbeitung System- und Messrauschen unterdrücken Schätzen des „wahren“ Systemzustands Bildverarbeitung Objektverfolgung, anhand von Kanten im Bild Positionsbestimmung, anhand von Landmarken Fahrer-Assistenz-Systeme Bilddatencodierung Höherer Datenkompressionsfaktor bei MPEG4 durch Segmentierung der bewegten Objekte und deren präzisen Bewegungsschätzung Zu 1: verrauschte Konstante, Wurfparabel Zu 2: Bildbasierte Navigation, FahrerAssistenzSysteme: Fahrbahnverfolgung, Hinderniserkennung

4. Funktionale Übersicht: 17.03.06 4. Funktionale Übersicht: Beobachtermatrix L lässt sich auch durch einen Riccati Entwurf bestimmen. Erweitere Kalman Filter ist: nicht-lineare Übergangs- und Messfunktion jeweils um den aktuellen Schätzwert linearisieren.

4. Zustandsbeobachter nach Luenberger 17.03.06 4. Zustandsbeobachter nach Luenberger u(t) : Soll v x(t) : Zustandsvektor y(t) : Abstandswerte, Drehgebermesswerte A : Systemmatrix B : Eingangsmatrix H : Ausgangsmatrix L : Rückführmatrix r : Rückführvektor Der Unterschied zwischen Luenberger-Beobachter und Kalman Filter besteht lediglich in der Bestimmung der Matrx L. Während es beim Luenberger Beobachter mittels Polvorgabe berechnet wird, entspringt sie beim Kalman Filter aus der Optimierung eines quadratischen Gütemaßes. Dieses muss notwendigerweise stochastischen Charakter haben. X(t) bei uns Beschleunigung L Fehlerverstärker Zustandsbeobachter ohne Rückkopplung Problem ist das der Startzustand nicht bekannt ist, so lässt sich am Streckenmodell nicht der Anfangszustand einstellen, somit ist x und x^ verschieden, was sich ohne Rückkopplung auch nicht ändern lasst. Luenberger Beobachter 1964 L ist so zu wählen, das x(t) – x^(t) für t gegen unendlich gegen 0 strebt!! Querverbindung zum Kalman-Filter Beim Luenberger-Beobachter geht man von der Vorstellung aus, daß der Systemzustand nach einer Anfangsstörung rekonstruiert werden soll. Wenn sich solche Störungen von Zeit zu Zeit wiederholen, wird der Beobachter den neuen Zustand immer wieder erfassen, sofern diese Störungen nicht allzu dicht liegen. Aber es ist einleuchtend, daß ein asymptotisch arbeitender Beobachter wie der Luenberger-Beobachter nicht mehr gut funktionieren kann, wenn laufend stochastische Störungen in das System eingreifen, die nicht vernachlässigbar sind. Dann geht man zum Kalman Filter über.

4. Diskrete System- und Messgleichung 17.03.06 4. Diskrete System- und Messgleichung

4. Regelschleife des Kalman Filters 17.03.06 4. Regelschleife des Kalman Filters Zwei Gruppen von Gleichungen: Die Gleichungen für die zeitliche Entwicklung des Systems (Prädiktion) Die Gleichungen für die Rückkopplung durch die Messung, d.h. die Berücksichtigung einer neuen Messung, um aus der a priori Schätzung eine verbesserte a posteriori Schätzung abzuleiten (Korrektur mit der Messung) K: Wenn Rk  0 dann K max, aktueller Messwert wird mehr vertraut Wenn P(k)-  0 dann K  0

4. Rekursive Arbeitsweise 17.03.06 4. Rekursive Arbeitsweise Rekursion versus Iteration • Rekursion und Iteration sind funktional ¨aquivalent, d.h. jede Rekursion kann durch Schleifen realisiert werden und jede Iteration durch eine Rekursion. (Hier ohne Beweis.) • Iterativer Ansatz ist oftmals schneller, da Rekursion intern Verwaltungsoverhead verursacht. • Rekursiver Ansatz oftmals klarer (k¨urzerer Code), insbesondere da viele Probleme rekursive Struktur haben. Rekursiv: int gcd(int n, int m) { if (m == 0) return n; return gcd(m,n % m); }; Iterativ: while (m > 0) h = n; n = m; m = h % m; } // Ergebnis steht in n

5. Simulationsbeispiel: Ideale Wurfparabel 17.03.06 5. Simulationsbeispiel: Ideale Wurfparabel g = Erdbeschleunigung

5.1 Diskrete Systemzustandsgleichung 17.03.06 5.1 Diskrete Systemzustandsgleichung vx : vy : x :

5.1 Diskrete Systemzustandsgleichung 17.03.06 5.1 Diskrete Systemzustandsgleichung y : Trapezintegration: Integration aus vy:

5.2 Gleichungssystem für die Wurfparabel 17.03.06 5.2 Gleichungssystem für die Wurfparabel Die stochastische Systemzustandsgleichung: Die Messgleichung:

5.3 Die Simulationsparameter (allgemein) 17.03.06 5.3 Die Simulationsparameter (allgemein) Startschätzung des Systemzustands Startschätzung der Fehlerkovarianzmatrix Varianz des Messfehlers Systemrauschen (Luftwirbel)

5.3 Die Simulationsparameter (speziell Beispielrechnung) 17.03.06 5.3 Die Simulationsparameter (speziell Beispielrechnung) Abtastintervall Messung bei k = 0 Erdbeschleunigung

5.4 Beispielrechnung für Schritt 0 17.03.06 5.4 Beispielrechnung für Schritt 0

5.4 Beispielrechnung für Schritt 0 17.03.06 5.4 Beispielrechnung für Schritt 0

5.3 Matlab-Simulationsparameter 17.03.06 5.3 Matlab-Simulationsparameter Abtastintervall Messung x_mes = x_ideal y_mes = y_ideal + random random entspricht Zufallswert von -0,1 bis +0,1 sollte y_mes negativ sein, dann y_mes = 0 Erdbeschleunigung

5.5 Matlab-Simulationsergebnis 17.03.06 5.5 Matlab-Simulationsergebnis Kein Suchfenster blau: Ideale Wurfparabel grün: Messpunkte rot: Wurfparabel rekonstruiert durch Kalman Filter

17.03.06 6 Objektverfolgung Störsimulation: Kurzzeitige Verdeckung der Objektbahn Kein Suchfenster, das führt dazu, das die geschätzte Position des Objekts den gestörten Messwerten relativ rasch folgt und anschließend mit einer gewissen Trägheit wieder auf die ungestörten Objektbahn einschenkt. blau: Ideale Wurfparabel grün: Messpunkte rot: Wurfparabel rekonstruiert durch Kalman Filter

6 Objektverfolgung Lösung: Suchfenster 17.03.06 6 Objektverfolgung Lösung: Suchfenster außerhalb des Suchfenster: Rk = 1 Die Störung wird detektiert und die Varianz des Messfehlers wird um einen Faktor 100 hochgesetzt, so das die (falschen) Messwerte im Kalman Filter praktisch nicht mehr berücksichtigt werden. Das Kalman Filter läuft deshalb im Prädiktionsmodus und kann nach dem Ende der Störung wieder an der richtigen Stelle weiterarbeiten. blau: Ideale Wurfparabel grün: Messpunkte rot: Wurfparabel rekonstruiert durch Kalman Filter

7. Zusammenfassung Übersicht zum diskreten Kalman-Filter 17.03.06 7. Zusammenfassung Übersicht zum diskreten Kalman-Filter Simulationsbeispiel + rekursives Filter, gut für Echtzeitanwendungen o Systemzustände müssen durch die Messung beobachtbar sein - Achten auf nötige numerische Genauigkeit des Systems MA Filter Das FIR-Filter berechnet den gefilterten Messwert anhand einer Anzahl N Faktoren an vorangegangenen Messwerten, die mit jeweils einem Faktor gewichtet werden. Eine spezielle Variante des FIR-Filters ist das Moving-Average-Filter, bei dem alle Messwerte mit dem Faktor 1/(N+1) multipliziert werden. Stefan Cordes Masterarbeit Die Ergebnisse haben gezeigt, dass die Kollision zwar sicher, aber nicht in dem letztmöglichen Zeitpunkt vermieden wird. Aufgrund des hohen, durch die Filterabweichung bedingten Sicherheitsabstands kommt das Fahrzeug in den Versuchsreihen mit einem Abstand von ca. 90 cm vor dem jeweiligen Hindernis zum Stehen.

8. Ausblick Masterarbeit 17.03.06 8. Ausblick Masterarbeit Sensordatenfusion von Laserscanner und Kamera Objektverfolgung basierend auf das Kalman Filter für Fahrerassistenzsysteme Dynamisches Modell für das Fahrzeug Moving Average Filter im Faust durch Kalman Filter ersetzen. Bremsassi 90 cm vor Hindernis MA Filter Das FIR-Filter berechnet den gefilterten Messwert anhand einer Anzahl N Faktoren an vorangegangenen Messwerten, die mit jeweils einem Faktor gewichtet werden. Eine spezielle Variante des FIR-Filters ist das Moving-Average-Filter, bei dem alle Messwerte mit dem Faktor 1/(N+1) multipliziert werden. Stefan Cordes Masterarbeit Die Ergebnisse haben gezeigt, dass die Kollision zwar sicher, aber nicht in dem letztmöglichen Zeitpunkt vermieden wird. Aufgrund des hohen, durch die Filterabweichung bedingten Sicherheitsabstands kommt das Fahrzeug in den Versuchsreihen mit einem Abstand von ca. 90 cm vor dem jeweiligen Hindernis zum Stehen.

9. Externe Motivation: Siemens VDO Automotive 17.03.06 9. Externe Motivation: Siemens VDO Automotive VDO = Vereinigte Deuta OTA Deuta Werken OTA  Umbenennung von OSA-Apparate GmbH (OSA = Otto Schulze Autometer) T für Tachometer

9. Externe Motivation: Ibeo 17.03.06 9. Externe Motivation: Ibeo Gründung des Ingenieurbüros für Elektronik und Optik (IBEO GmbH) durch J. Hipp und G. Bröhan

17.03.06 Literatur [1] Föllinger, O.: Regelungstechnik, 8., überarbeitete Auflage, 1994, Hüthig [2] Nischwitz, A., Haberäcker, P.: Masterkurs Computergrafik und Bildverarbeitung, 1. Auflage, 2004, Friedr. Vieweg Sohn Verlag/GWV Fachverlage GmbH, Wiesbaden [3] Kirchner, A.: Dissertation, Sensordatenverarbeitung eines Laserscanners für autonome Fahrfunktionen von Kraftfahrzeugen, 2000, Universität der Bundeswehr Hamburg [4] Stüker, D.: Dissertation, Heterogene Sensordatenfusion zur robusten Objektverfolgung im automobilen Straßenverkehr, 2003, Carl von Ossietzky-Universität Oldenburg [5] Cordes, S.: Masterarbeit, Automatischer Bremsassistent auf Basis einer Laserscanner-Abstandserfassung für ein fahrerloses Transportsystem, 2006, HAW-Hamburg [6] www.siemensvdo.de [7] www.ibeo-as.com

Vielen Dank für die Aufmerksamkeit 17.03.06 Vielen Dank für die Aufmerksamkeit

17.03.06 Anhang

5. Simulationsbeispiel: Ideale Wurfparabel über die Zeit 17.03.06 5. Simulationsbeispiel: Ideale Wurfparabel über die Zeit