Kalman-Filter und Target Tracking

Transkript

1 Kalman-Filter und Target Tracking Peter Poschmann Hochschule für Technik und Wirtschaft Dresden Fakultät Informatik/Mathematik 23. März 2016

2 Inhalt 1 Kalman-Filter Einleitung Eindimensionaler Kalman-Filter Mehrdimensionaler Kalman-Filter 2 Erweiterter Kalman-Filter Linearisierung Extended Kalman Filter 3 Target Tracking Bewegungsmodelle Beobachtungsmodelle Data Association Problem 4 Abschließendes Weitere Informationen Praktikum 1 / 30

3 Kalman-Filter 1 Kalman-Filter Einleitung Eindimensionaler Kalman-Filter Mehrdimensionaler Kalman-Filter 2 Erweiterter Kalman-Filter Linearisierung Extended Kalman Filter 3 Target Tracking Bewegungsmodelle Beobachtungsmodelle Data Association Problem 4 Abschließendes Weitere Informationen Praktikum 2 / 30

4 Motivation Roboter soll sich autonom bewegen Kollision mit Personen und anderen bewegten Objekten vermeiden Ausreichend Abstand zu Personen einhalten (sollen keine Angst haben) Wo sind Personen/Objekte und wohin bewegen sie sich? Interaktion mit Personen Wo sind (interaktionsbereite) Personen? Tracking von Personen/Objekten nötig Messwerte sind fehlerbehaftet Kalman-Filter ist eine Methode zur Lösung dieses Problems Einleitung 3 / 30

5 Kalman-Filter Rekursiver Zustandsschätzer Bayes-Filter kontinuierlicher (Zustands-)raum Modelliert Unsicherheit mittels Normalverteilung bel(x) = N(x; µ, σ2) bzw. bel(x) = N(x; µ, Σ) Zwei Schritte, abwechselnd ausgeführt - analog zu Bayes-Filter Motion update (prediction) Measurement update (correction) Voraussetzung/Einschränkung: Zustandsübergang, Beobachtung und Zustand sind normalverteilt (stochastische Messfehler) Lineare Gleichungen Einleitung 4 / 30

6 Eindimensionale Normalverteilung Oder auch: Gauss-Verteilung, Gauss-Kurve Unimodal (nur ein Maximum) { } p(x) = 1 exp (x µ)2 2πσ 2 2σ 2 µ... Erwartungswert σ... Standardabweichung σ2... Varianz Fläche unter der Kurve ist 1 (wie bei allen Wahrscheinlichkeitsdichtefunktionen) Je größer σ bzw. σ2 (höhere Unsicherheit), desto niedriger und breiter das Maximum Einleitung 5 / 30

7 Measurement update Measurement update / correction Normalverteilung nach Update Weniger Unsicherheit als vor Update Weniger Unsicherheit als Beobachtung Informationsgewinn Update (1D): Zustand (z.b. Position): N(µ, σ 2 ) Beobachtung: N(z, r 2 ) K = σ2 σ 2 +r... Kalman Gain 2 µ = µ + K(z µ) σ 2 = (1 K)σ 2 Eindimensionaler Kalman-Filter 6 / 30

8 Motion update Motion update / prediction Normalverteilung nach Update Mehr Unsicherheit als vor Update Informationsverlust Update (1D): Zustand (z.b. Position): N(µ, σ 2 ) Zustandsänderung (z.b. Bewegung/Geschwindigkeit): N(u, q 2 ) µ = µ + u σ 2 = σ 2 + q 2 Eindimensionaler Kalman-Filter 7 / 30

9 Zusammenfassung 1D Kalman-Filter Satz µ t = µ t 1 + u t σ t 2 = σt q2 t K t = σ2 t σ 2 t +r2 t µ t = µ t + K t (z t µ t ) σ 2 t = (1 K t ) σ 2 t Eindimensionaler Zustandsschätzer mit Modellierung der Unsicherheit Vorraussetzung: Control u (Änderung bzw. Bewegung) muss bekannt sein Beim Tracking typischerweise nicht bekannt Muss mitgeschätzt werden Weitere Dimension(en) Eindimensionaler Kalman-Filter 8 / 30

10 Multivariate Normalverteilung Multivariat = mehrdimensional { } 1 p(x) = exp 1 (2π) k/2 Σ 1/2 2 (x µ)t Σ 1 (x µ) µ R k... Erwartungswertvektor Σ R k k... Kovarianzmatrix k... Anzahl Dimensionen Mehrdimensionaler Kalman-Filter 9 / 30

11 Mehrdimensionaler Kalman-Filter Beobachtbare und versteckte Zustände Interagieren miteinander Beobachtbare Zustände geben Information über versteckte Zustände Kalman-Filter kann versteckte Zustände schätzen (auch andere Filter, wie z.b. Partikelfilter, können das) Prozessmodell mit normalverteiltem Prozessrauschen x t = A t x t 1 + B t u t + ε t with ε t N(0, Q t ) Beim Target Tracking ist Steuervektor u t meist unbekannt, Term B t u t fällt dann raus Messmodell mit normalverteiltem Messrauschen z t = C t x t + δ t with δ t N(0, R t ) Transformiert Zustand in (erwartete) Beobachtung Entfernt im wesentlichen versteckte Zustände (z.b. Geschwindigkeit, nur Position bleibt übrig) Mehrdimensionaler Kalman-Filter 10 / 30

12 Kalman-Filter Algorithmus (1) Satz µ t = A t µ t 1 + B t u t Σ t = A t Σ t 1 A T t + Q t oder Σ t = A t Σ t 1 A T t + B t Q t B T t y t = z t C t µ t S t = C t Σt Ct T + R t K t = Σ t Ct T St 1 µ t = µ t + K t y t Σ t = (I K t C t ) Σ t = Σ t K t S t Kt T Wenn Unsicherheit Q t im Zustandsraum, dann: Σ t = A t Σ t 1 A T t + Q t Wenn Unsicherheit Q t im Steuerraum, dann in Zustandsraum transformieren: Σ t = A t Σ t 1 A T t + B t Q t B T t Mehrdimensionaler Kalman-Filter 11 / 30

13 Kalman-Filter Algorithmus (2) Definitionen µ... Systemvektor (Zustand) Σ... Kovarianzmatrix (Unsicherheit) des Zustands A... Systemmatrix / state transition matrix u... Steuervektor (Änderung von außen) B... Steuermatrix Q... Kovarianzmatrix (Unsicherheit) der Änderung z... Messvektor (Beobachtung) R... Kovarianzmatrix (Unsicherheit) der Beobachtung C... Messmatrix / measurement transformation matrix y... Residuumvektor S... Kovarianzmatrix des Residuums K... Kalmanverstärkungsmatrix / Kalman Gain I... Einheitsmatrix Mehrdimensionaler Kalman-Filter 12 / 30

14 Zusammenhang zwischen Bayes-Filter und Kalman-Filter State transition function with white Gaussian noise: x t = A t x t 1 + B t u t + ε t with ε t N(0, Q t ) Prediction: bel(x t ) = p(x t u t, x t 1 )bel(x t 1 )dx t 1 bel(x t ) = N(x t ; µ t, Σ t )... prior (belief after prediction) bel(x t 1 ) = N(x t 1 ; µ t 1, Σ t 1 )... previous belief p(x t u t, x t 1 ) = N(x t ; A t x t 1 + B t u t, Q t )... state transition probability Measurement function with white Gaussian noise: z t = C t x t + δ t with δ t N(0, R t ) Correction: bel(x t ) = η p(z t x t )bel(x t ) bel(x t ) = N(x t ; µ t, Σ t )... posterior (belief after correction) bel(x t ) = N(x t ; µ t, Σ t )... prior (belief before correction) p(z t x t ) = N(z t ; C t x t, R t )... measurement probability Mehrdimensionaler Kalman-Filter 13 / 30

15 Anwendungen Selbstlokalisierung des Roboters SLAM Personentracking Positions- und Orientierungsbestimmung von Flugzeugen Schätzung von Fahrzeugdynamik und -masse Ganz allgemein Zustandsschätzer für dynamische Systeme Schätzung nicht-beobachtbarer Zustände Mehrdimensionaler Kalman-Filter 14 / 30

16 Erweiterter Kalman-Filter 1 Kalman-Filter Einleitung Eindimensionaler Kalman-Filter Mehrdimensionaler Kalman-Filter 2 Erweiterter Kalman-Filter Linearisierung Extended Kalman Filter 3 Target Tracking Bewegungsmodelle Beobachtungsmodelle Data Association Problem 4 Abschließendes Weitere Informationen Praktikum 15 / 30

17 Nichtlinearität Funktionen sind häufig nichtlinear Transformation von kartesischen Koordinaten (x, y) zu Polarkoordinaten (LRF-Messungen) (ϕ, r) ϕ = tan 1 y x r = x 2 + y 2 Kann nicht in Form z = Cx dargestellt werden Angabe der Bewegung in Form von Richtung θ und Distanz d x = x + d cos θ y = y + d sin θ Kann nicht in Form x = Ax + Bu dargestellt werden Lösung: Linearisierung Finden einer linearen Funktion, die der tatsächlichen Funktion möglichst nahe kommt Linearisierung 16 / 30

18 Taylor-Entwicklung Taylor-Reihe von f an Stelle a: P (x) = f(a)+ f (a) 1! (x a)+ f (a) 2! (x a) f (n) (a) n! (x a) n +... = f (n) (a) n=0 n! (x a) n Nutzung der ersten beiden Terme und Weglassen der Restlichen ergibt lineare Funktion Linearisierung von f an Stelle a T 1 (x) = f(a) + f (a) (x a) An Stelle a stimmen f und T 1 überein An Stelle a stimmen Anstiege von f und T 1 überein Je größer der Abstand zu Stelle a, desto größer der Unterschied zwischen f und T 1 (typischerweise) Extended Kalman-Filter nutzt Taylor-Entwicklung erster Ordnung, um Funktionen zu linearisieren Linearisierung 17 / 30

19 Linearisierung der Funktionen Prozessmodell x t = A t x t 1 + B t u t + ε t wird zu x t = g(u t, x t 1 ) + ε t Linearisierung an u t und µ t 1 g(u t, x t 1 ) g(u t, µ t 1 ) + g (u t, µ t 1 )(x t 1 µ t 1 ) G t = g(ut,xt 1) x t 1... Gegenstück zu A t V t = g(ut,xt 1) u t... Gegenstück zu B t G t / V t enthalten partielle Ableitungen von g nach x t 1 / u t Messmodell z t = C t x t + δ t wird zu z t = h(x t ) + δ t Linearisierung an Stelle µ t h(x t ) h( µ t ) + h ( µ t )(x t µ t ) h ( µ t ) = h( µ) x t = H t... Gegenstück zu C t H t enthält partielle Ableitungen von h nach x t G t, V t, H t... Jacobi-Matritzen Linearisierung 18 / 30

20 Jacobi-Matrix Vektor Funktionen f = f 1 (x) f 2 (x). f m (x) f 1 f 1 x 1 x 2 f 2 f 2 Jacobi-Matrix F x = x 1 x f m f m x 1 x 2 m... Anzahl der Funktionen in f f 1 x n f 2 x n f m x n n... Anzahl der Variablen, nach denen abgeleitet wird Linearisierung 19 / 30

21 Extended Kalman-Filter Satz µ t = g(u t, µ t 1 ) Σ t = G t Σ t 1 G T t + Q t oder Σ t = G t Σ t 1 G T t + V t Q t Vt T y t = z t h( µ t ) S t = H t Σt Ht T + R t K t = Σ t Ht T St 1 µ t = µ t + K t y t Σ t = (I K t H t ) Σ t Wenn Unsicherheit Q t im Zustandsraum, dann: Σ t = G t Σ t 1 G T t + Q t Wenn Unsicherheit Q t im Steuerraum, dann in Zustandsraum transformieren: Σ t = G t Σ t 1 G T t + V t Q t V T t Extended Kalman Filter 20 / 30

22 EKF: Zusammenfassung Nichtlineare Funktionen (Zustandsübergang und/oder Beobachtung) Linearisierung mittels Taylor-Reihe erster Ordnung Gradient im mehrdimensionalen Fall gegeben durch Jacobi-Matrix Je höher die Unsicherheit, desto mehr liegt Linearisierung daneben Extended Kalman Filter 21 / 30

23 Target Tracking 1 Kalman-Filter Einleitung Eindimensionaler Kalman-Filter Mehrdimensionaler Kalman-Filter 2 Erweiterter Kalman-Filter Linearisierung Extended Kalman Filter 3 Target Tracking Bewegungsmodelle Beobachtungsmodelle Data Association Problem 4 Abschließendes Weitere Informationen Praktikum 22 / 30

24 Bewegungsmodelle Im Target Tracking ist Control u nicht bekannt Kompensation durch Bewegungsmodelle Brownian Motion Wenn Bewegung kaum vorhersagbar, stark zufällig/chaotisch Nur Position wird geschätzt Positions-Erwartungswert bleibt konstant in Vorhersage Vorhersage erhöht lediglich die Unsicherheit Constant Velocity Geschwindigkeit wird mitgeschätzt Vorhersage ändert Position anhand Geschwindigkeit Geschwindigkeits-Erwartungswert bleibt konstant in Vorhersage Constant Acceleration Geschwindigkeit und Beschleunigung wird mitgeschätzt Vorhersage ändert Position anhand Geschw. und Beschl. Vorhersage ändert Geschwindigkeit anhand Beschleunigung Beschleunigungs-Erwartungswert bleibt konstant in Vorhersage Bewegungsmodelle 23 / 30

25 Beobachtungsmodelle Am Beispiel Personentracking Background subtraction LRF: Umgebung statisch, Karte vorhanden Kamera: Hintergrund statisch, Bild ohne Personen vorhanden Mustererkennung LRF: z.b. Minima im Scan, Clustering & Analyse Kamera: z.b. mit Klassifikation auf extrahierten Features (Gesichtsdetektion,...) Beobachtungsmodelle 24 / 30

26 Zuordnungsprobleme Data Association Problem Mehrere Tracking Targets, mehrere Beobachtungen Welche Beobachtung wurde von welchem Target generiert? Zusätzliche Probleme Sensor Noise Falsche Detektionen Fehlende Detektionen Lösungsansätze Gutes Bewegungsmodell Merkmale, die Identifikation/Unterscheidung ermöglichen Mehrere Varianten berücksichtigen Data Association Problem 25 / 30

27 Abschließendes 1 Kalman-Filter Einleitung Eindimensionaler Kalman-Filter Mehrdimensionaler Kalman-Filter 2 Erweiterter Kalman-Filter Linearisierung Extended Kalman Filter 3 Target Tracking Bewegungsmodelle Beobachtungsmodelle Data Association Problem 4 Abschließendes Weitere Informationen Praktikum 26 / 30

28 Weitere Informationen Kurs Programming a Robotic Car von Udacity playlist?list=plawxtw4syapkcsyxw6-a_aaoxvkldwnhk (ab Video Nummer 77 - Tracking Intro ) Vorlesungsfolien der Arbeitsgruppe Autonome Intelligente Systeme der Universität Freiburg (Kalman Filter und Extended Kalman Filter) (Target Tracking) Weitere Informationen 27 / 30

29 Praktikumsaufgabe Kalman-Filter Tracking eines einzelnen Objekts mittels LRF Constant velocity model Zustand x = (x, y, ẋ, ẏ) T Beobachtung z = (x, y) T Vanilla Kalman-Filter Im Wesentlichen: Implementierung des Algorithmus, Aufstellen der Matritzen Praktikum 28 / 30

30 Praktikumsaufgabe Partikel-Filter Lokalisierung des Roboters Partikelfilter Vorgegebene Startposition Gridkarte der Umgebung LRF Im Wesentlichen: Implementierung von Algorithmus, Bewegungsmodell, Beobachtungsmodell (Partikel bewerten) Praktikum 29 / 30

31 Das wars. Fragen? Praktikum 30 / 30