Kalman Filter zur Rekonstruktion von Messsignalen Denis Schetler 15. Dezember 2006
description
Transcript of Kalman Filter zur Rekonstruktion von Messsignalen Denis Schetler 15. Dezember 2006
1
Kalman Filter zur Rekonstruktion
von Messsignalen
Denis Schetler15. Dezember 2006
2
Agenda
1. Kalman Filter2. Motivation3. Anwendungsbereiche4. Funktionale Übersicht5. Beispiel: Wurfparabel6. Objektverfolgung7. Zusammenfassung8. Ausblick Masterarbeit9. Externe Motivation
360° Laserscanner
kyrteAbstandswe
knWeg kLenkwinkel
3
FAUST Projekt
Kamera + Enthält alle Informationen
- Begrenzter Öffnungswinkel- Aufwendige Verarbeitung
- Wetter und Licht
- Genauigkeit der Abstands- bestimmung
Laserscanner+ horizontaler Schnitt der Fahrbahn+ Genauigkeit der Abstandsbestimmung
- Straßenmarkierung und Schilder
Ein Sensortyp reicht für robuste Objektverfolgung nicht aus!Sensordatenfusion
4
FAUST Projekt
Bildverarbeitung
Rohbild
Laserscanner-messwerte
Sensordatenfusion
Focus of Expansion /
optischer Fluss
Nachbarschafts-operatoren
Kanten- und Linienerkennung Skelettierung
Rekonstruktion von gestörten Messsignalen
Kalman Filter
Objekt-verfolgung
Fahrer-assistenz-systeme
AutonomesFahren
Objekt Klassifikation
y y
5
1. Kalman Filter: optimale Schätzwerte für die
Systemszustandsgröße x
Varianz des Messfehlers
kRSystem-rauschen
kQ
kP
1kP
PSchätzfehler-kovarianz
Berechnung Messfehlerkorrektur
Prädiktion
Initialer Schätzwert der Systemzustände
Korrigierter Schätzwert der Systemzustände mit der Messung
Schätzwert der Systemzustände
Laserscanner-messwerte
0x
1ˆkx
ky
kx H
kySchätzwert der Messwerte
6
2. Motivation1. Überholvorgang: Objektverfolgung basierend auf
Laserscanner, Radar oder Bildverarbeitung
2. Berichte zu Forschungsprojekten vom „3rd International Workshop on Intelligent Transportation“ in Hamburg März 2006z.B.: Intersection Safety
3. Masterarbeit Bremsassistent und AusweichassistentStandard: Moving Average Filter
durch Kalman Filter ersetzen.
7
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
8
4. Funktionale Übersicht:
Zustands-beobachter
Modellierung dynamischer Systeme mit
Zustandsvariablen
StochastikSchätzung
Luenberger-Beobachter
Querverbindungzum
Kalman-Filter
Riccati-Entwurf-Regler
-Gleichung
zeitd
iskr
etze
itkon
tinui
erlic
h Das erweiterte Kalman-Filter
(EKF)
Herleitung des diskreten Kalman-
Filters aus dem kontinuierlichen
Übersicht zum diskreten Kalman-
Filter
Simulations-beispiel
Betrachtung Laufzeit und Ressourcen
9
4. Zustandsbeobachter nach Luenberger
u(t) : Soll vx(t) : Zustandsvektory(t) : Abstandswerte,
Drehgebermesswerte
A : SystemmatrixB : EingangsmatrixH : Ausgangsmatrix
L : Rückführmatrixr : Rückführvektor
uBxAx
ruBxAx
ˆ
H
00ˆ xx
unbekanntx ,0
L
Fahrzeugx
x
y
y
yy ˆ
r
u
Modellmath.
H
10
4. Diskrete System- und Messgleichung
Messgleichung kkkk vxHy
Diskrete Systemzustandsgleichung kkkkkk wuBxAx 1
Abstandswerte, Drehgebermesswerte
Messrauschen
ky
kv
m-dimensional
m-dimensional
MessmatrixkH (m · n)-Matrix
Systemmatrix
Zustand Fahrzeugkx
kA
Soll Geschwindigkeitku
n-dim.
l-dim.
(n · n)-Matrix
Systemrauschen
EingabematrixkB (n · l)-Matrix
kw n-dim.
11
4. Regelschleife des Kalman Filters
kK
kP
Prädiziere Systemzustand
Prädiziere Fehlerkovarianz
Berechnung der Kalman Verstärkung
Korrektur der Schätzung mit der Messung yk
Korrektur der Fehlerkovarianz
Varianz des Messfehlers kR
kx
1ˆ kx
1kP
1 Tkkkk
Tkkk HPHRHPK kkkkkk xHyKxx ˆˆˆ
kkkk PHKIP
kkkkk uBxAx ˆˆ 1
kTkkkk QAPAP
1
Qk Zufallswert für das Systemrauschen
Berechnung Messfehlerkorrektur
Prädiktion
0x
1ˆ kx
ky
kx
ky
kP
kP
H
12
4. Rekursive Arbeitsweise
0x
0P
0000 PHKIP
000000 ˆˆˆ xHyKxx
10000000
TT HPHRHPK
Initialer Schätzwert der Systemzustände
Berechnung Messfehler-korrektur
Prädiktion 00001 ˆˆ uBxAx
00001 QAPAP T
0y
Konstante Matrizen 0A 0Q0H 0R
13
5. Simulationsbeispiel: Ideale Wurfparabel .0 const
dtdxvvx dt
dytvy )( 281,9smg
dtdvy
vx
vy
14
5.1 Diskrete Systemzustandsgleichung
vx : vy :
x :
x
x
x
xx
vtkxkxvtkxkx
vtx
vtxv
dtdx
)()1()()1(
tgkvkv
tgkvkv
tgv
gatv
adtdv
yy
yy
y
yy
)()1(
)()1(.
)()1(0
constvkvkv
vv
x
xx
x
15
5.1 Diskrete Systemzustandsgleichungyv y
ydtvy
tgkvkvgdtdv
yyy )()1(
Integration
aus vy:
y :
Trapezintegration:
2
211
1211
tgktvkyky
kvkvtktvkyky
y
yyy
t t3t2
)(kv y
)1( kv y
t
)(kv y
trvallAbtastinte
16
5.2 Gleichungssystem für die WurfparabelDie stochastische Systemzustandsgleichung:
Die Messgleichung:
k
ky
x
ky
x
wg
t
t
vvyx
tt
vvyx
0
000
210
00
10000100
010001
2
1
k
ky
xkm
m v
vvyx
yx
00100001
kkk wuBxAx 0001
kkk vxHy 0
17
5.3 Die Simulationsparameter (allgemein)
Startschätzung des Systemzustands
Startschätzung der Fehlerkovarianzmatrix
Varianz des Messfehlers
Systemrauschen (Luftwirbel)
1100
ˆ0x
1000010000100001
0P
01,00001,0
kR
5
5
5
5
10000010000010000010
kQ
18
5.3 Die Simulationsparameter (speziell Beispielrechnung)
Abtastintervall
Messung bei k = 0
Erdbeschleunigung
st 01,0
08,00
0y
281,9smg
19
5.4 Beispielrechnung für Schritt 0
000099,00099,0
00001001
1000010000100001
00100001
01,00001,0
00001001
1000010000100001
1
0K
10000
Tk
Tkk HPHRHPK
11
0792,00
1100
00100001
08,00
000099,00099,0
1100
ˆkx
kkkkk xHyKxx ˆˆˆ 0
100001000001,0000001,0
1000010000100001
00100001
000099,00099,0
1000010000100001
kP
kkk PHKIP 0
Berechnung der Kalman Verstärkung
Korrektur der Schätzung mit der Messung yk
Korrektur der Fehlerkovarianz
20
5.4 Beispielrechnung für Schritt 0
9019,01
08915,001,0
0
01,0000
01,0210
00
11
0792,00
1000010001,0010001,001
ˆ2
1 gxk
0001 ˆˆ uBxAx kk
0001 QAPAP
Tkk
00001,1001,00000001,1001,001,0001001,00001,0001001,0
10000010000010000010
1001,0001001,000100001
100001000001,0000001,0
1000010001,0010001,001
1
5
5
5
5
1
k
k
P
P
Prädiziere Systemzustand
Prädiziere Fehlerkovarianz
21
5.3 Matlab-Simulationsparameter
Abtastintervall
Messung x_mes = x_idealy_mes = y_ideal + random
random entspricht Zufallswert von -0,1 bis +0,1sollte y_mes negativ sein, dann y_mes = 0
Erdbeschleunigung
st 005,0
280665,9smg
22
5.5 Matlab-Simulationsergebnis
blau: Ideale Wurfparabelgrün: Messpunkterot: Wurfparabel rekonstruiert durch Kalman Filter
23
6 ObjektverfolgungStörsimulation: Kurzzeitige Verdeckung der Objektbahn
blau: Ideale Wurfparabelgrün: Messpunkterot: Wurfparabel rekonstruiert durch Kalman Filter
24
6 Objektverfolgung
blau: Ideale Wurfparabelgrün: Messpunkterot: Wurfparabel rekonstruiert durch Kalman Filter
Lösung: Suchfensteraußerhalb des Suchfenster: Rk = 1
25
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
26
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.
27
9. Externe Motivation: Siemens VDO Automotive
28
9. Externe Motivation: Ibeo
29
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
30
Vielen Dank für die Aufmerksamkeit