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