dXd () ( )ˆ - ra.cs.uni-tuebingen.de · Beispiel Kalmanfilter a = 0.9802 2 u = 0.039 2 n = 1 s = 0...
10
Das Schätzproblem Kalmanfilter Geg.: N-elementiger Datensatz X = { x[0], x[1], .. , x[N-1] }, der von einem unbekannten Parameter abhängt. Ges.: Bestimmung von mit Hilfe eines Schätzers, d.h. einer Funktion g mit Grundidee: Modellierung der Wahrscheinlichkeitsdichte- funktion (WDF) der Daten in Abhängigkeit von Klassische Schätztheorie: LVWXQEHNDQQW DEHUGHWHUPLQLVWLVFKGK SDUDPHWULVLHUW die WDF von x Bayes‘sche Schätztheorie: LVWZLHx die Realisierung einer Zufallsvariable => Modellierung über eine gemeinsame WDF S; S;_ S Optimale Schätzer werden über Optimalitätskriterien gesucht, z.B. Minimierung der Varianz ohne systematische Fehler (MVU), kleinste Fehlerquadrate oder Minimie- rung des mittleren quadratischen Fehlers. ( ) ] 1 [ ],.., 1 [ ], 0 [ ˆ − = N x x x g θ Mittlerer quadratischer Fehler Kalmanfilter Ges.: Schätzer, der den (Bayes‘schen) mittleren quadratischen Fehler minimiert. Bayes‘sches Theorem: S; S;_ S => Wähle so, daß e minimiert wird : Ergebnis: d.h. der optimale Schätzer, der den mittleren quadratischen Fehler minimiert (MMQF-Schätzer), ist der Mittelwert der a-posteriori-WDF (a posteriori : nach Beobachtung der Daten). () ( ) ( ) ∫∫ − = θ θ θ θ θ dXd X p e , ˆ ˆ 2 () ( ) ( ) ∫∫ − = dX x p d X p e ) ( | ˆ ˆ 2 θ θ θ θ θ ( ) ( ) 0 | ˆ ˆ 2 = − ∂ ∂ ∫ θ θ θ θ θ d X p ^ ∫ = = ) | ( ) | ( ˆ X E d X p θ θ θ θ θ a-priori-WDF: vor Beobachtung der Daten (Vorwissen)
Transcript of dXd () ( )ˆ - ra.cs.uni-tuebingen.de · Beispiel Kalmanfilter a = 0.9802 2 u = 0.039 2 n = 1 s = 0...
Das SchätzproblemKalmanfilter
Geg.: N-elementiger Datensatz X = { x[0], x[1], .. , x[N-1] }, der von einem unbekannten Parameter abhängt.
Ges.: Bestimmung von mit Hilfe eines Schätzers, d.h. einer Funktion g mit
Grundidee: Modellierung der Wahrscheinlichkeitsdichte-funktion (WDF) der Daten in Abhängigkeit von �
Klassische Schätztheorie: ������������������ ���������������� ���� ��������������die WDF von xBayes‘sche Schätztheorie: ���������x die Realisierung einer Zufallsvariable => Modellierung über eine gemeinsame WDF ����� �������� ���� �
Optimale Schätzer werden über Optimalitätskriterien gesucht, z.B. Minimierung der Varianz ohne systematische Fehler (MVU), kleinste Fehlerquadrate oder Minimie-rung des mittleren quadratischen Fehlers.
( )]1[],..,1[],0[ˆ −= Nxxxgθ
Mittlerer quadratischer FehlerKalmanfilter
Ges.: Schätzer, der den (Bayes‘schen) mittleren quadratischen Fehler
minimiert.
Bayes‘sches Theorem: ����� �������� ���� �
=>
Wähle so, daß e minimiert wird :
Ergebnis:
d.h. der optimale Schätzer, der den mittleren quadratischen Fehler minimiert (MMQF-Schätzer), ist der Mittelwert der a-posteriori-WDF (a posteriori : nach Beobachtung der Daten).
( ) ( ) ( )∫∫ −= θθθθθ dXdXpe ,ˆˆ 2
( ) ( ) ( )∫ ∫
−= dXxpdXpe )(|ˆˆ 2
θθθθθ
( ) ( ) 0|ˆˆ
2=−
∂∂∫ θθθθ
θdXp^
∫ == )|()|(ˆ XEdXp θθθθθ
a-priori-WDF: vor Beobachtung der Daten (Vorwissen)
Multivariate GaußverteilungKalmanfilter
( )
−−−= − )()(
2
1exp
det2
1)( 1
xxT
x
xN
xCxC
xp µµπ
Cx: Kovarianzmatrix (N x N, symmetrisch, positiv definit => invertierbar) mit [Cx]ij = E((xi - E(xi)) (xj -E(xj������ x: Mittelwert• Ist C diagonal, dann sind die xi unkorreliert und statistisch unabhängig, p faktorisiert in ein Produkt von 1D-Gaußfunktionen.• Jede lineare Transformation von x mit y = Ax + b ergibt wieder eine Gaußverteilung mit y = � x + b und Cy = ACxA
T.
• Konturen konstanter Wahr-scheinlichkeit sind Ellipsen• Bedingte Wahrscheinlichkeiten sind wieder gaußverteilt mit
• Sind x1 und x1 unkorreliert, so gilt E(y|x1, x2) = E(y|x1) + E(y|x2)
xyxxyxyyxy
xxxy
CCCCC
xExCCyExyE1
|
1 ))(()()|(−
−
−=−+=
MMQF-Schätzer für y
OrthogonalitätsprinzipKalmanfilter
MMQF-Schätzer für Gaußverteilung:
Annahme ������ ����0 =>d.h. Schätzer ist eine Linearkombination der Daten
Anwendungsfall: Geg. seien Daten x[1] bis x[N-1], geschätzt werden soll x[N].
Innovation: Schätzfehler
Korrelation:
))(()(ˆ 1 xExCCE xxx −+= −θθθ
xCC xxx1ˆ −= θθ
xCCx xxxN1ˆ~ −−=−= θθθθ
( )( )0
)()~(1
11
=−=−=
−=⋅−=⋅−
−−
θθθθ
θθθθ
xxxxxxxx
xxxxxxxn
CCCCCC
xxECCCxxCCExxE
• Schätzfehler bzw. Innovation ist nicht mit den Daten korreliert, d.h. Innovation und Daten sind zueinander orthogonal• MMQF-Schätzer nutzt die Korrelation der Daten untereinander aus bzw. „dekorreliert“ die Zeitreihe
(gilt für jeden linearen Schätzer)
Stochastische ProzesseKalmanfilter
Zufallsvariable: Regel, die jedem Ausgang eines Zufallexperimentes eine Zahl zuweist.Stochastischer Prozeß: Regel, die jedem Ausgang eines Zufallexperimentes eine Funktion bzw. eine Zeitreihe zuweist (d.h. ein Ensemble von Funktionen mit Wahrscheinlichkeitsdichte p(x,t) ).t
ZustandMessung
Charakterisierung: • Mittelwert:
• Autokorrelation: R(t,t): Durchschnittsleistung
• Autokovarianz: C(t,t): Varianz
Stationärer Prozeß (im weiteren Sinne, WSS): Mittelwert konstant, Autokorrelation hängt nur von Zeitdifferenz ab.
Diskreter WSS-Prozeß:
( ) ( )( )txEtx =µ
( ) ( ) ( )( )2121, txtxEttR =
( ) ( ) ( ) ( )212121 ,, ttttRttC µµ−=
( ) [ ] [ ] [ ]( )1, 21 +=→ nxnxEkrttR xx
( ) [ ] [ ] 221, xxxxx krkcttC µ−=→
Skalarer Gauß-Markov-ProzeßKalmanfilter
Statistische Modellierung eines Signals als korrelierter stochastischer Prozeß mit dem Zustandsmodell
s[n] = as[n - 1] + u[n], n >= 0
mit Zustand s[n], ���� ���� 2u), s[-1] ~
� s�� 2s) und s[-1] unabhängig vom
Anregungsrauschen u (Gauß-Markov-Prozeß 1. Ordnung). • Zustand des Systems hängt nur vom vor-
herigen Zustand und vom Eingangswert uab.• Für |a| < 1 ist der Prozeß stabil, d.h. s[n] bleibt beschränkt für alle n.• Für |a| -> 1 ist der Prozeß stark, für |a| -> 0 schwach korreliert.• Prozeß ist nicht stationär, wird es aber für unendliches n.• s[n] bleibt gaußverteilt für alle n.
Statistik des Gauß-Markov-ProzessesKalmanfilter
Mittelwert
Varianz
Gleichgewichts-korrelation
0])[(
])1[(
])[(])1[(
])[(
→⇒
−=+−=
∞→nnsE
nsaE
nuEnsaE
nsE
[ ][ ]
2
2
22
2
2
1])[var(
])1[var(
]))1[(][]1[(
]))[(][(])[var(
ans
nsa
nsaEnunasE
nsEnsEns
un
u
− →⇒
+−=−−+−=
−=
∞→ σσ
kuss a
akr
2
2
1][
−= σ
=> Für unendliches n wird der Prozeß stationär mit Autokorrelationsfunktion
Fortpflanzungsgleichungen:
Vektorielles Gauß-Markov-ModellKalmanfilter
Erweiterung des skalaren Modells auf p x 1- Signal-vektoren s[n] und r x 1-dimensionales Anregungs-rauschen u[n]:
s[n] = As[n - 1] + Bu[n]
mit p x p - Matrix A und p x r - Matrix B, u ~ N(0, Q), s[-1] ~ N( s, Cs) und s[-1] unabhängig von der Eingangsgröße u. A und B dürfen zeitlich variieren.
Fortpflanzungsgleichungen:E(s[n]) = AE(s[n - 1]) E(s[n])
C[n] = AC[n - 1]AT + BQBT C[n]
• für konstantes A und B wird der Prozeß asymptotisch stationär.• für ein stabiles System müssen die Eigenwertbeträge von A kleiner als 1 sein• andere Eigenschaften analog zum skalaren Modell
∑∞
=∞→
∞→
→
→
0
0
k
kTkn
n
T
ABQBA
Skalarer Kalmanfilter - VoraussetzungenKalmanfilter
1. „Beobachtung“: Schätzung des Zustands ohne Berücksichtigung desMeßrauschens„Filterung“: Schätzung des Zustands unter Berücksichtigung des Meßrauschens=> Kalman-“Filter“
2. Skalares Gauß-Markov-Signalmodell: s[n] = as[n - 1] + u[n], n >= 0Meßmodell (additives Gauß-Rauschen): x[n] = s[n] + w[n] mit w ~ ���� 2
n[n]) unabh. von u und s[-1]
3. Vereinfachung: s = 0
4. A-priori-Wissen über den im Signalmodell gegebenen zeitlichen Zusammenhang soll verwendet werden => Bayes‘scher Ansatz.
5. Optimalitätskriterium: Minimierung des mittleren quadratischen Fehlers
6. Ges.: MMQF-Schätzer für s[n] mit sequenzieller Korrektur, d.h. neue Schätzung soll sich aus der alten Schätzung s[n - 1] und der aktuellen Beobachtung x[n] berechnen.
]1|1[ˆ]1|[ −−=− nnsanns
22 ]1|1[]1|[ unnMannM σ+−−=−
]1|[
]1|[][
2 −+−=nnM
nnMnK
nσ
])1|[ˆ][]([]1|[ˆ]|[ −−+−= nnsnxnKnnsnns
]1|[])[1(]|[ −−= nnMnKnnM
Skalarer Kalmanfilter*Kalmanfilter
Vorhersage des Signals:
Vorhersage des MMQF:
Kalman-Gain:
Korrektur der Signalschätzung:
Korrektur des MMQF:
Initialisierung:2]1|1[,]1|1[ ss Ms σµ =−−=−−
K[n] 1 - K[n]
a2z-1
M[n|n]
M[n|n-1]
2u
2u
+
+
Schätzschleife
Gainschleife
*identische Gleichungen für s = 0
BeispielKalmanfilter
a = 0.98022u = 0.039
2n = 1
s = 02s = 1
Kalmanfilter - EigenschaftenKalmanfilter
• Gainberechnung läuft ohne Rückkopplung der Daten in einer unabhängigen Schleife• Gain kann schon im voraus berechnet werden• Gainschleife kann instabil werden• Kalmanfilter ist ein rekursiver linearer Filter erster Ordnung mit zeitvarianten Koeffizienten• Keine Matrixinversion notwendig• Kalmanfilter liefert gleichzeitig sein eigenes Konfidenzmaß, den MMQF M[n|n]• MMQF steigt im Vorhersageschritt und sinkt im Korrekturschritt.• Kalmanfilter ist der bestmögliche lineare Filter, selbst wenn Gauß‘sche Annahme nicht zutrifft.• kann auf Vektorzustände und -messungen verallgemeinert werden.
][][]1|1[ˆ])[1(
])1|1[][]([
]1|1[ˆ]|[
nxnKnnsnKa
nnsanxnK
nnsanns
+−−−=−−−+−−=
Vektorieller Zustand - skalare MessungKalmanfilter
]1|1[ˆ]1|[ˆ −−=− nnAnn ss
TT BQBAnnAMnnM +−−=− ]1|1[]1|[
][]1|[][
][]1|[][
2 nnnMn
nnnMn
Tn hh
hK
−+−=
σ
])1|[ˆ][][]([]1|[ˆ]|[ˆ −−+−= nnnnxnnnnn T shKss
]1|[])[][1(]|[ −−= nnMnnnnM ThK
ss CM =−−=−− ]1|1[,]1|1[s
Vorhersage des Signals:
Vorhersage des MMQF (p x p - Matrix):
Kalman-Gain-Vektor (p x 1):
Korrektur der Signalschätzung:
Korrektur des MMQF:
Initialisierung:
Signalmodell: Vektorielles Gauß-Markov-Modell
s[n] = As[n - 1] + Bu[n]mit Zusatzannahme Q diagonal.
Meßmodell:x[n] = hT[n]s[n] + w[n]
mit w ~ ���� 2n[n]) unabh. von u
und s[-1]
• Kalman-Gain ist jetzt ein Vektor, MMQF eine Matrix• Es müssen genügend Messungen vorhanden sein• A und h müssen bestimmten Bedingungen genügen (Beobachtbarkeit)
Vorhersage des Signals:
Vorhersage des MMQF (p x p - Matrix):
Kalman-Gain-Matrix (p x m):
Korrektur der Signalschätzung:
Korrektur des MMQF:
Initialisierung:
Vektorieller Zustand - vektorielle MessungKalmanfilter
]1|1[ˆ]1|[ˆ −−=− nnAnn ss
TT BQBAnnAMnnM +−−=− ]1|1[]1|[
( ) 1][]1|[][][][]1|[][
−−+−= nHnnMnHnCnHnnMnK TT
])1|[ˆ][][]([]1|[ˆ]|[ˆ −−+−= nnnHnxnKnnnn sss
]1|[])[][1(]|[ −−= nnMnHnKnnM
ss CM =−−=−− ]1|1[,]1|1[s
Signalmodell: wie vorher.
Lineares Meßmodell:Messungen sind m x 1- Vektoren
x[n] = H[n]s[n] + w[n]mit w ~ N(0, C[n]) unabh. von uund s[-1]
• Kalman-Gain und MMQF sind Matrizen• Achtung: Matrixinversion für Berechnung der Gainmatrix erforderlich.• Erweiterungen:- A, B, Q zeitvariant => identische Gleichungen- zeitliche Korrelationen in w- deterministisches u
Erweiterter Kalmanfilter (EKF)Kalmanfilter
Erweiterung auf nichtlineares Signal- und Meßmodell mit additiven Rauschtermen:
s[n] = a(s[n - 1]) + Bu[n] und x[n] = h(s[n]) + w[n]
mit nichtlinearen Funktionen a und h. Geschlossene Lösung für MMQF-Schätzer ist nicht bekannt => Näherungslösung durch Linearisierung um s[n - 1|n - 1] und s[n|n - 1]
])1|[ˆ][(][
])1|[ˆ(])[(
])1|1[ˆ]1[(]1[
])1|1[ˆ(])1[(
]1|[ˆ][
]1|1[ˆ]1[
−−∂∂+−≈
−−−−−∂
∂+−−≈−
−=
−−=−
nnnn
nnn
nnnn
nnn
nnn
nnn
sssh
shsh
sss
asasa
ss
ss
]1|[ˆ][
]1|1[ˆ]1[
][][
]1[]1[
−=
−−=−
∂∂=⇒
−∂∂=−⇒
nnn
nnn
nnH
nnA
ss
ss
sh
sa
])1|1[ˆ(]1|[ˆ −−=− nnnn sas
]))1|[ˆ(][]([]1|[ˆ]|[ˆ −−+−= nnnxnKnnnn shss
Achtung:• keine Optimalitätseigenschften• M ist hier kein verläßliches Konfidenzmaß• A[n-1] H[n] müssen in jedem Zeitschritt neu berechnet werden.• K und M können nicht im voraus berechnet werden.
Filtergleichungen ändern sich nicht, bis auf
Vorhersage:
und Korrektur:
^ ^
Anwendungsbeispiel SpurwarnerKalmanfilter
Aufgabe: Detektion der Fahrbahnrandmarkie-rungen im Kamerabild, daraus Schätzung der Fahrzeugposition innerhalb des Spurverlaufs mit einem erweiterten Kalmanfilter und ggf. Warnung bei Verlassen der Spur.
Kameras
3 Koordinatensysteme: Fahrbahn-, Fahrzeug-und Kamerakoordinatensystem:
yc
zc
xc
yV
xV
zV
zE
xE
yE
Modellierung eines ZeitschrittsKalmanfilter
� �alt
neu
�neu
�alt
zE
zV
xExV
Fahrbahnrand
• Spurverlauf wird als Polynom 3. Grades beschrieben:
mit Krümmung
mit Krümmungsänderung
�� ������ ������������� � �
3120
62)( EEEE z
cz
cxzx ++∆=
2
2
0E
E
dz
xdc =
3
3
1E
E
dz
xdc =
• Bewegung im Zeitschritt��������ückweise konstant,
d.h. erst Translation ����� �������������� �� ���Drehung parallel zum Spur-verlauf.• Straßen-KS ist tangential zur Spur.• ���� � ������������
Aufstellen des SignalmodellsKalmanfilter
1. Spurposition:
⇒≈∆≈ →
∆−++∆=∆
<< θθ
θ
θ tan,
tan62
1
3120
tvz
tvzc
zc
xx
E
EEaltneu
tvtvc
tvc
xx alt
altalt
altneu ∆−∆+∆+∆=∆ θ3120 )(6
)(2
2. Krümmung und Krümmungsänderung:
⇒∆∂∂+=
=E
zE
altneu zz
ccc
E 0
000 altneu
altaltneu
cc
tvccc
11
100
=
∆+=
3. Gier-winkel:
⇒++
=+≈+=
210 2
tan
EEalt
E
Ealt
E
Ealtneu
zc
zc
dz
dx
dz
dx
θ
θθθ
2)(21
0tv
ctvc
altalt
altneu ∆+∆+=θθ
=> Signalmodell: s[n] = As[n - 1] + u[n] mit Tccxn ),,,(][ 10 θ∆=s und
=
∆∆
∆∆−∆∆
=
∆
2
2
2
2
221
3612
21
000
000
000
000
,
1)(0
0100
010
)()(1
1
0
θσσ
σσ
c
c
x
Q
tvtv
tv
tvtvtv
A
Annahme: Kleine Gierwinkel und Krümmungen
Aufstellen des MeßmodellsKalmanfilter
1. N Beobachtungen pro Framemit
+
++
=
NNbb
bb
bb
wynx
wynx
wynx
n
)],[(
)],[(
)],[(
][
,
22,
11,
s
s
s
x�
2. Transformation Welt-Kamera:
dzxz
hyy
zxx
EEC
EC
EEC
−+=−=
−=
θθ
θθ
cossin
sincos
mit Montagehöhe h und Entfer-nung zur Hinterachse d
3. Perspektivische Abbildungfür ������
dzx
hyfy
dzx
zxfx
EE
Eb
EE
EEb
−+−=
−+−=
θ
θθ
4. Auflösen nach zE
für �zE << zE :
⇒−≈b
E y
fhdz
5. Meßgleichung (nichtlinear):
dzzc
zc
x
zzc
zc
xfx
E
E
b
EE
EE
−+
++∆
−++∆=
3120
3120
62
62
θ
θeinsetzen
=> erweiterter Kalmanfilter benötigt linearisierte Meß-gleichung mit den Ableitungen
( ) 10
,,,c
x
c
xx
x
x bbbb
∂∂
∂∂
∂∂
∆∂∂
θ
BeispieleKalmanfilter
