Kalman-Filter und Target Tracking

Kalman-Filter und Target Tracking

Peter Poschmann

Hochschule für Technik und Wirtschaft DresdenFakultät Informatik/Mathematik

23. März 2016

Kalman-Filter Erweiterter Kalman-Filter Target Tracking Abschließendes

Inhalt

1 Kalman-FilterEinleitungEindimensionaler Kalman-FilterMehrdimensionaler Kalman-Filter

2 Erweiterter Kalman-FilterLinearisierungExtended Kalman Filter

3 Target TrackingBewegungsmodelleBeobachtungsmodelleData Association Problem

4 AbschließendesWeitere InformationenPraktikum

1 / 30


Kalman-Filter





2 / 30


Motivation

Roboter soll sich autonom bewegenKollision mit Personen und anderen bewegten ObjektenvermeidenAusreichend Abstand zu Personen einhalten (sollen keineAngst haben)Wo sind Personen/Objekte und wohin bewegen sie sich?

Interaktion mit PersonenWo sind (interaktionsbereite) Personen?

Tracking von Personen/Objekten nötigMesswerte sind fehlerbehaftetKalman-Filter ist eine Methode zur Lösung dieses Problems

Einleitung 3 / 30


Kalman-Filter

Rekursiver ZustandsschätzerBayes-Filterkontinuierlicher (Zustands-)raumModelliert Unsicherheit mittels Normalverteilungbel(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 sindnormalverteilt (stochastische Messfehler)Lineare Gleichungen

Einleitung 4 / 30


Eindimensionale Normalverteilung

Oder auch: Gauss-Verteilung, Gauss-KurveUnimodal (nur ein Maximum)p(x) = 1√

2πσ2 exp{− (x−µ)2

2σ2

}µ ... Erwartungswertσ ... Standardabweichungσ2 ... VarianzFläche unter der Kurve ist 1 (wie bei allenWahrscheinlichkeitsdichtefunktionen)Je größer σ bzw. σ2 (höhere Unsicherheit), desto niedrigerund breiter das Maximum

Einleitung 5 / 30


Measurement update

Measurement update / correctionNormalverteilung nach Update

Weniger Unsicherheit als vor UpdateWeniger Unsicherheit als BeobachtungInformationsgewinn

Update (1D):Zustand (z.B. Position): N(µ, σ2)Beobachtung: N(z, r2)K = σ2

σ2+r2 ... Kalman Gainµ′ = µ+K(z − µ)σ′2 = (1−K)σ2

Eindimensionaler Kalman-Filter 6 / 30


Motion update

Motion update / predictionNormalverteilung nach Update

Mehr Unsicherheit als vor UpdateInformationsverlust

Update (1D):Zustand (z.B. Position): N(µ, σ2)Zustandsänderung (z.B. Bewegung/Geschwindigkeit): N(u, q2)µ′ = µ+ uσ′2 = σ2 + q2



Zusammenfassung 1D Kalman-Filter

Satzµt = µt−1 + utσ2t = σ2

t−1 + q2t

Kt = σ2t

σ2t +r2

t

µt = µt +Kt(zt − µt)σ2t = (1−Kt)σ2

t

Eindimensionaler Zustandsschätzer mit Modellierung derUnsicherheitVorraussetzung: Control u (Änderung bzw. Bewegung) mussbekannt sein

Beim Tracking typischerweise nicht bekanntMuss mitgeschätzt werdenWeitere Dimension(en)



Multivariate Normalverteilung

Multivariat = mehrdimensionalp(x) = 1

(2π)k/2|Σ|1/2 exp{−1

2(x− µ)TΣ−1(x− µ)}

µ ∈ Rk ... ErwartungswertvektorΣ ∈ Rk×k ... Kovarianzmatrixk ... Anzahl Dimensionen

Mehrdimensionaler Kalman-Filter 9 / 30


Mehrdimensionaler Kalman-Filter

Beobachtbare und versteckte ZuständeInteragieren miteinanderBeobachtbare Zustände geben Information über versteckteZuständeKalman-Filter kann versteckte Zustände schätzen (auch andereFilter, wie z.B. Partikelfilter, können das)

Prozessmodell mit normalverteiltem Prozessrauschenxt = Atxt−1 +Btut + εt with εt ∼ N(0, Qt)Beim Target Tracking ist Steuervektor ut meist unbekannt,Term Btut fällt dann raus

Messmodell mit normalverteiltem Messrauschenzt = Ctxt + δt with δt ∼ N(0, Rt)Transformiert Zustand in (erwartete) BeobachtungEntfernt im wesentlichen versteckte Zustände (z.B.Geschwindigkeit, nur Position bleibt übrig)



Kalman-Filter Algorithmus (1)

Satzµt = Atµt−1 +BtutΣt = AtΣt−1A

Tt +Qt oder Σt = AtΣt−1A

Tt +BtQtB

Tt

yt = zt − CtµtSt = CtΣtC

Tt +Rt

Kt = ΣtCTt S−1t

µt = µt +KtytΣt = (I −KtCt)Σt = Σt −KtStK

Tt

Wenn Unsicherheit Qt im Zustandsraum, dann:Σt = AtΣt−1A

Tt +Qt

Wenn Unsicherheit Qt im Steuerraum, dann in Zustandsraumtransformieren: Σt = AtΣt−1A

Tt +BtQtB

Tt



Kalman-Filter Algorithmus (2)

Definitionenµ ... Systemvektor (Zustand)Σ ... Kovarianzmatrix (Unsicherheit) des ZustandsA ... Systemmatrix / state transition matrixu ... Steuervektor (Änderung von außen)B ... SteuermatrixQ ... Kovarianzmatrix (Unsicherheit) der Änderungz ... Messvektor (Beobachtung)R ... Kovarianzmatrix (Unsicherheit) der BeobachtungC ... Messmatrix / measurement transformation matrixy ... ResiduumvektorS ... Kovarianzmatrix des ResiduumsK ... Kalmanverstärkungsmatrix / Kalman GainI ... Einheitsmatrix



Zusammenhang zwischen Bayes-Filter und Kalman-Filter

State transition function with white Gaussian noise:xt = Atxt−1 +Btut + εt with εt ∼ N(0, Qt)Prediction: bel(xt) =

´p(xt|ut, xt−1)bel(xt−1)dxt−1

bel(xt) = N(xt; µt, Σt) ... prior (belief after prediction)bel(xt−1) = N(xt−1;µt−1,Σt−1) ... previous beliefp(xt|ut, xt−1) = N(xt;Atxt−1 +Btut, Qt) ... state transitionprobability

Measurement function with white Gaussian noise:zt = Ctxt + δt with δt ∼ N(0, Rt)Correction: bel(xt) = η p(zt|xt)bel(xt)

bel(xt) = N(xt;µt,Σt) ... posterior (belief after correction)bel(xt) = N(xt; µt, Σt) ... prior (belief before correction)p(zt|xt) = N(zt;Ctxt, Rt) ... measurement probability



Anwendungen

Selbstlokalisierung des RobotersSLAMPersonentrackingPositions- und Orientierungsbestimmung von FlugzeugenSchätzung von Fahrzeugdynamik und -masseGanz allgemein

Zustandsschätzer für dynamische SystemeSchätzung nicht-beobachtbarer Zustände



Erweiterter Kalman-Filter





15 / 30


Nichtlinearität

Funktionen sind häufig nichtlinearTransformation von kartesischen Koordinaten (x, y) zuPolarkoordinaten (LRF-Messungen) (ϕ, r)

ϕ = tan−1 yx

r =√x2 + y2

Kann nicht in Form z′ = Cx dargestellt werden

Angabe der Bewegung in Form von Richtung θ und Distanz dx′ = x+ d cos θy′ = y + d sin θKann nicht in Form x′ = Ax +Bu dargestellt werden

Lösung: LinearisierungFinden einer linearen Funktion, die der tatsächlichen Funktionmöglichst nahe kommt

Linearisierung 16 / 30


Taylor-Entwicklung

Taylor-Reihe von f an Stelle a: P (x)= f(a)+ f ′(a)

1! (x−a)+ f ′′(a)2! (x−a)2+. . .+ f (n)(a)

n! (x−a)n+. . .=

∑∞n=0

f (n)(a)n! (x− a)n

Nutzung der ersten beiden Terme und Weglassen derRestlichen ergibt lineare Funktion

Linearisierung von f an Stelle aT1(x) = f(a) + f ′(a) · (x− a)An Stelle a stimmen f und T1 übereinAn Stelle a stimmen Anstiege von f und T1übereinJe größer der Abstand zu Stelle a, desto größer derUnterschied zwischen f und T1 (typischerweise)

Extended Kalman-Filter nutzt Taylor-Entwicklung ersterOrdnung, um Funktionen zu linearisieren



Linearisierung der Funktionen

Prozessmodellxt = Atxt−1 +Btut + εt wird zu xt = g(ut, xt−1) + εtLinearisierung an ut und µt−1g(ut, xt−1) ≈ g(ut, µt−1) + g′(ut, µt−1)(xt−1 − µt−1)Gt = ∂g(ut,xt−1)

∂xt−1... Gegenstück zu At

Vt = ∂g(ut,xt−1)∂ut

... Gegenstück zu BtGt / Vt enthalten partielle Ableitungen von g nach xt−1 / ut

Messmodellzt = Ctxt + δt wird zu zt = h(xt) + δtLinearisierung an Stelle µth(xt) ≈ h(µt) + h′(µt)(xt − µt)h′(µt) = ∂h(µ)

∂xt= Ht ... Gegenstück zu Ct

Ht enthält partielle Ableitungen von h nach xtGt, Vt, Ht ... Jacobi-Matritzen



Jacobi-Matrix

Vektor Funktionen f =

f1(x)f2(x)...

fm(x)

Jacobi-Matrix Fx =

∂f1∂x1

∂f1∂x2

· · · ∂f1∂xn

∂f2∂x1

∂f2∂x2

· · · ∂f2∂xn... ... . . . ...

∂fm

∂x1∂fm

∂x2· · · ∂fm

∂xn

m ... Anzahl der Funktionen in fn ... Anzahl der Variablen, nach denen abgeleitet wird



Extended Kalman-Filter

Satzµt = g(ut, µt−1)Σt = GtΣt−1G

Tt +Qt oder Σt = GtΣt−1G

Tt + VtQtV

Tt

yt = zt − h(µt)St = HtΣtH

Tt +Rt

Kt = ΣtHTt S−1t

µt = µt +KtytΣt = (I −KtHt)Σt

Wenn Unsicherheit Qt im Zustandsraum, dann:Σt = GtΣt−1G

Tt +Qt

Wenn Unsicherheit Qt im Steuerraum, dann in Zustandsraumtransformieren: Σt = GtΣt−1G

Tt + VtQtV

Tt

Extended Kalman Filter 20 / 30


EKF: Zusammenfassung

Nichtlineare Funktionen (Zustandsübergang und/oderBeobachtung)Linearisierung mittels Taylor-Reihe erster OrdnungGradient im mehrdimensionalen Fall gegeben durchJacobi-MatrixJe höher die Unsicherheit, desto mehr liegt Linearisierungdaneben

Extended Kalman Filter 21 / 30


Target Tracking





22 / 30


Bewegungsmodelle

Im Target Tracking ist Control u nicht bekanntKompensation durch BewegungsmodelleBrownian Motion

Wenn Bewegung kaum vorhersagbar, stark zufällig/chaotischNur Position wird geschätztPositions-Erwartungswert bleibt konstant in VorhersageVorhersage erhöht lediglich die Unsicherheit

Constant VelocityGeschwindigkeit wird mitgeschätztVorhersage ändert Position anhand GeschwindigkeitGeschwindigkeits-Erwartungswert bleibt konstant in Vorhersage

Constant AccelerationGeschwindigkeit und Beschleunigung wird mitgeschätztVorhersage ändert Position anhand Geschw. und Beschl.Vorhersage ändert Geschwindigkeit anhand BeschleunigungBeschleunigungs-Erwartungswert bleibt konstant in Vorhersage

Bewegungsmodelle 23 / 30


Beobachtungsmodelle

Am Beispiel PersonentrackingBackground subtraction

LRF: Umgebung statisch, Karte vorhandenKamera: Hintergrund statisch, Bild ohne Personen vorhanden

MustererkennungLRF: z.B. Minima im Scan, Clustering & AnalyseKamera: z.B. mit Klassifikation auf extrahierten Features(Gesichtsdetektion, ...)

Beobachtungsmodelle 24 / 30


Zuordnungsprobleme

Data Association ProblemMehrere Tracking Targets, mehrere BeobachtungenWelche Beobachtung wurde von welchem Target generiert?Zusätzliche Probleme

Sensor NoiseFalsche DetektionenFehlende Detektionen

LösungsansätzeGutes BewegungsmodellMerkmale, die Identifikation/Unterscheidung ermöglichenMehrere Varianten berücksichtigen

Data Association Problem 25 / 30


Abschließendes





26 / 30


Weitere Informationen

Kurs „Programming a Robotic Car“ von Udacityhttps://www.youtube.com/playlist?list=PLAwxTw4SYaPkCSYXw6-a_aAoXVKLDwnHK(ab Video Nummer 77 - „Tracking Intro“)

Vorlesungsfolien der Arbeitsgruppe Autonome IntelligenteSysteme der Universität Freiburg

http://ais.informatik.uni-freiburg.de/teaching/ss13/robotics/(Kalman Filter und Extended Kalman Filter)http://ais.informatik.uni-freiburg.de/teaching/ws11/robotics2/(Target Tracking)

Weitere Informationen 27 / 30


Praktikumsaufgabe Kalman-Filter

Tracking eines einzelnen Objekts mittels LRFConstant velocity modelZustand x = (x, y, x, y)T

Beobachtung z = (x, y)T

Vanilla Kalman-FilterIm Wesentlichen: Implementierung des Algorithmus,Aufstellen der Matritzen

Praktikum 28 / 30


Praktikumsaufgabe Partikel-Filter

Lokalisierung des RobotersPartikelfilterVorgegebene StartpositionGridkarte der UmgebungLRFIm Wesentlichen: Implementierung von Algorithmus,Bewegungsmodell, Beobachtungsmodell (Partikel bewerten)

Praktikum 29 / 30


Das wars.

Fragen?

Praktikum 30 / 30

Documents

Kalman-Filter und Target Tracking