5.3 Bayes Filter - Technische Universität Chemnitz · Beispiel X t Z t offen (Tür ist geöffnet)...

6.3 Bayes Filter

6.3.1 Allgemeiner Bayes-Filter

Satz von Bayes

∑ ⋅=y

ypyxpxp )()|()(

)()()|()|(

ypxpxypyxp ⋅

=

∑ ⋅⋅

=

')'()'|(

)()|()|(

xxpxyp

xpxypyxp

)()|()|( xpxypyxp ⋅⋅=η

Satz über die totale Wahrscheinlichkeit

)|()|(),|(),|(

zypzxpzxypzyxp ⋅

=

Beispiel

)()()|()|(

zpopenpopenzpzopenp ⋅

=

Beobachtungleichter zu ermitteln

Weitere Beobachtungen

5.0)|( 2 =openzp 6.0)|( 2 =¬openzp32)|( 1 =zopenp

)|(),|()|(),|()|(),|(),|(

112112

11221 zopenpzopenzpzopenpzopenzp

zopenpzopenzpzzopenp¬⋅¬+⋅

⋅=

)|()|()|()|()|()|(),|(

1212

1221 zopenpopenzpzopenpopenzp

zopenpopenzpzzopenp¬⋅¬+⋅

⋅=

Markov Annahme

625.085

31

106

32

21

32

21

),|( 21 ==⋅+⋅

⋅=zzopenp Wahrscheinlichkeit wird kleiner

Weitere Beobachtungen

),|(),|(),,|(),,|(

11

111121

−

−− ⋅=

nn

nnnn zzzp

zzxpzzxzpzzzxpK

KKK

),|(),|()|(),,|(

11

1121

−

−⋅=

nn

nnn zzzp

zzxpxzpzzzxpK

KK

Markov Annahme

),|()|(),,|( 11121 −⋅⋅= nnn zzxpxzpzzzxp KK η

∏=

⋅⋅=n

iinn xpxzpzzzxp

1121 )()|(),,|( ηK

Bayes - Filter

),|()(bel :11:1 tttt uzxpx −=

)|()(bel 111 uxpx =

),|()bel( :1:1 tttt uzxpx =

Bayes - Filter

)bel( von Berechnung rekursive tx

)bel()bel( 1 tt xx →−

tt zu ,n vorgegebe wird)bel( 0x

Algorithmus Bayes - Filter

Eingabe: ttt zux ,),bel( 1−

∑−

−−=1

)bel(),|()(bel 11tx

ttttt xuxxpx

)(bel)|()bel( tttt xxzpx ⋅⋅=η

)bel( tx

do allfor tx

Ausgabe:

müssen bekannt sein

Beweis

)(bel)|(

),|()|( ),|(),,|(

),|(),|(),,|(

),|()(bel

:11:1

:11:1:11:1

:11:1

:11:1:11:1

:1:1

ttt

ttttt

ttttttt

ttt

ttttttt

tttt

xxzp

uzxpxzpuzxpuzxzp

uzzpuzxpuzxzp

uzxpx

⋅⋅=

⋅⋅=⋅⋅=

⋅=

=

−

−−

−

−−

η

ηη

)|()|(),|(),|(

zypzxpzxypzyxp ⋅

=

Bayes

Beweis

∑

∑

∑

∑

−

−

−

−

−−

−−−−

−−−

−−−−

−

⋅=

⋅=

⋅=

⋅=

=

1

1

1

1

)(bel),|(

),|(),|(

),|(),|(

),|(),,|( ),|()(bel

11

1:11:111

:11:111

:11:11:11:11

:11:1

t

t

t

t

xtttt

xtttttt

xtttttt

xttttttt

tttt

xuxxp

uzxpuxxp

uzxpuxxp

uzxpuzxxpuzxpx

6.3.2 Beispiel

Beispiel

Beispiel

tX

tZ

offen (Tür ist geöffnet)

geschlossen (Tür ist geschlossen)2 Zustände

2 Werteoffen

geschlossen

Ein Roboter steht vor einer Tür.

Sensor des Roboters sieht, ob die Tür offen oder geschlossen.

Aber fehlerhaft!

Aktionen

tU 2 Wertekeine

oeffnen (Öffnen der Tür)

(keine Aktion)

Anfangswahrscheinlichkeit

5.0)offen (bel 0 ==X

5.0)n geschlosse (bel 0 ==X

Auftrittswahrscheinlichkeiten

6.0)offen |offen ( === tt XZp

4.0)offen |n geschlosse ( === tt XZp

2.0)n geschlosse |offen ( === tt XZp

8.0)n geschlosse |n geschlosse ( === tt XZp

Übergangswahrscheinlichkeiten1)offen ,oeffnen |offen ( 1 ==== −ttt XUXp

0)offen ,oeffnen |n geschlosse ( 1 ==== −ttt XUXp8.0)n geschlosse ,oeffnen |offen ( 1 ==== −ttt XUXp

2.0)n geschlosse ,oeffnen |n geschlosse ( 1 ==== −ttt XUXp

1)offen , keine |offen ( 1 ==== −ttt XUXp0)offen , keine |n geschlosse ( 1 ==== −ttt XUXp0)n geschlosse , keine |offen ( 1 ==== −ttt XUXp

1)n geschlosse , keine |n geschlosse ( 1 ==== −ttt XUXp

1.Iteration

)(bel )(bel 10 xx →

keine 1 =U offen 1 =Z

)n geschlosse bel(X )ngeschlosse , keine |( )offen bel(X )offen , keine |(

0011

0011

=⋅==+=⋅===

XUxpXUxp

∑=0

)bel(),|()(bel 01011x

xuxxpx

1.Iteration

21

210

211

)n geschlosse bel(X )ngeschlosse , keine |offen ( )offen bel(X )offen , keine |offen (

0011

0011

=⋅+⋅=

=⋅===+=⋅====

XUXpXUXp

)offen (bel 1 =X

21

211

210

)n geschlosse bel(X )ngeschlosse , keine |ngeschlosse ( )offen bel(X )offen , keine |ngeschlosse (

0011

0011

=⋅+⋅=

=⋅===+=⋅====

XUXpXUXp

)ngeschlosse (bel 1 =X

1.Iteration

3.05.06.0 )offen (bel)offen |offen ()offen bel( 1111

⋅=⋅⋅==⋅==⋅==

ηηη XXZpX

1.05.02.0 )ngeschlosse (bel)ngeschlosse |offen ()ngeschlosse bel( 1111

⋅=⋅⋅==⋅==⋅==

ηηη XXZpX

)(bel)|offen ()bel( 1111 xxZpx ⋅=⋅=η

5.21)1.03.0(

==+⋅

ηη

25.01.05.2)ngeschlosse bel( 1 =⋅==X

75.03.05.2)offen bel( 1 =⋅==X

2.Iteration

)(bel )(bel 21 xx →

oeffnen 2 =U offen 2 =Z

)n geschlosse bel(X )ngeschlosse ,oeffnen |( )offen bel(X )offen ,oeffnen |(

1122

1122

=⋅==+=⋅===

XUxpXUxp

∑=1

)bel(),|()(bel 12122x

xuxxpx

2.Iteration

95.0418.0

431

)n geschlosse bel(X )ngeschlosse ,oeffnen |offen ( )offen bel(X )offen ,oeffnen |offen (

1122

1122

=⋅+⋅=

=⋅===+=⋅====

XUXpXUXp

)offen (bel 2 =X

05.0412.0

430

)n geschlosse bel(X )ngeschlosse ,oeffnen |ngeschlosse ( )offen bel(X )offen ,oeffnen |ngeschlosse (

1122

1122

=⋅+⋅=

=⋅===+=⋅====

XUXpXUXp

)ngeschlosse (bel 2 =X

2.Iteration

57.095.06.0 )offen (bel)offen |offen ()offen bel( 2222

⋅=⋅⋅==⋅==⋅==

ηηη XXZpX

01.005.02.0 )ngeschlosse (bel)ngeschlosse |offen ()ngeschlosse bel( 2222

⋅=⋅⋅==⋅==⋅==

ηηη XXZpX

)(bel)|offen ()bel( 2222 xxZpx ⋅=⋅=η

58100

1)01.057.0(

=

=+⋅

η

η

017.0581)ngeschlosse bel( 2 ≈==X

983.05857)offen bel( 2 ≈==X

6.3.3 Diskreter Bayes – Filter

Diskreter Bayes - Filter Werte- , endlich, - kit xxX

),|()bel( :1:1,, ttkttktk uzxXppx ===

),|()(bel :11:1,, ttkttktk uzxXppx −===

Eingabe: tttk zup ,},{ 1, −

do allfor k

∑ −− ⋅===i

tiittkttk pxXuxXpp 1,1, ),|(

tkktttk pxXzpp ,, )|( ⋅=⋅=ηAusgabe: }{ ,tkp

6.3.4 Binary – Bayes – Filter

Binary – Bayes - FilterxxX ¬, Werte2hat (statisch) Zustand

)|(),|()(bel :1:1:1 tttt zxpuzxpx ==

)(1)(log)()( 0 xp

xpxlxl−

==

)(bel1)(bellog)(

t

t

xxxlt −

=)(1

11)(bel xlt tex

+−=

)(bel1)(bel xx tt −=¬

llSensormode inverses - )|( tzxp

Algorithmus

Eingabe:

Ausgabe:

tt zl ,1−

tl

)(1)(log

)|(1)|(log1 xp

xpzxp

zxpllt

ttt −

−−

+= −

Herleitung

)|()|(),|()|(

1:1

1:11:1:1

−

−− ⋅=

tt

tttt zzp

zxpzxzpzxp

)|()|()|(

1:1

1:1

−

−⋅=

tt

tt

zzpzxpxzp

)()()|()|(

xpzpzxpxzp tt

t⋅

=

)|()()|()()|()|(

1:1

1:1:1

−

−

⋅⋅⋅

=tt

tttt zzpxp

zxpzpzxpzxp

analog:

)|()()|()()|()|(

1:1

1:1:1

−

−

⋅¬¬⋅⋅¬

=¬tt

tttt zzpxp

zxpzpzxpzxp

Herleitung

)|()()|()()|()|(

1:1

1:1:1

−

−

⋅⋅⋅

=tt

tttt zzpxp

zxpzpzxpzxp)|()(

)|()()|()|(1:1

1:1:1

−

−

⋅¬¬⋅⋅¬

=¬tt

tttt zzpxp

zxpzpzxpzxp

)()(

)|()|(

)|()|(

)|()|(

1:1

1:1

:1

:1

xpxp

zxpzxp

zxpzxp

zxpzxp

t

t

t

t

t

t ¬⋅

¬⋅

¬=

¬ −

−

)(1)(log

)|(1)|(log)()( 1 xp

xpzxp

zxpxlxlt

ttt −

−−

+= −

6.4 Sensormodell

Sensormodell

Karte ist eine Menge von Landmarken },,{ 1 Nmmm K=

Tjyjxjj smmm ),,( ,,=

Ort Signature

Sensorbeobachtung

Abstand Lage der Landmarke relativ zum Roboter Signature

Tit

it

it sr ),,( φ K,2,1=i

Sensormodell

},,{},,,,{)(2

2

2

1

1

1

21 KKK⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

==

t

t

t

t

t

ti

tttt

s

r

s

rfffzf φφ

Feature Extraktion Anzahl ist mit der Zeit t variabel

Tjyjxjj smmm ),,( ,,=

tmf ji

t Zeitpunktzum und zwischen enzKorrespond

}1,,2,1{ +∈= Ncj it K

},,{ 1 Nmmm K=

1+= Nj Sensor sieht keineLandmarke

Sensormodell T

t yx ),,( θ=x Position des Roboters

⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜

⎝

⎛

+⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜

⎝

⎛

−−−−+−

=⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

2

2

2

),(2atan)()(

,,

2,

2,

s

r

j

xjyj

yjxj

it

it

it

sxmymymxm

s

r

σ

σ

σ

εεε

θφφ

∏=i

tit

it

ittt msrpmzfp ),|,,(),|)(( xx φ

Fehler (Normalverteilung)exakter WertSensorbeobachtung

Gesucht:

Sensormodell

Tt )0,0,0(=μ

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

=Σ2

2

2

000000

s

r

t

σσ

σ

φ

),;(),|,,( ttit

it

it Nmsrp Σ→ tax μφ

⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜

⎝

⎛

−−−−+−

−⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

=⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

j

xjyj

yjxj

it

it

it

sxmymymxm

s

r

aaa

θφ ),(2atan)()(

,,

2,

2,

3

2

1

a

2

2

21

221),prob( b

a

eb

ba⋅−

⋅=π

),prob(),prob(),prob(),|,,( 321 srtit

it

it aaamsrp σσσφ φ ⋅⋅←x

atan2

0,0 falls 2

)(

0 falls 0

0 falls atan)sign(

0 falls atan

),(2atan

⎪⎪⎪⎪

⎩

⎪⎪⎪⎪

⎨

⎧

≠=⋅

==

<⎟⎟⎠

⎞⎜⎜⎝

⎛⎟⎟⎠

⎞⎜⎜⎝

⎛−⋅

>⎟⎠⎞

⎜⎝⎛

=

yxysign

yx

xxyy

xxy

xy

π

π

6.5 Bewegungsmodell

Aktionsmodell

Tt yx ),,(1 θ=−x T

t yx )',','( θ=x

Tt wv ),(=u

Translationsgeschwindigkeit Rotationsgeschwindigkeit

Beschreibung eines Zustandes

Tyx ),,( θ=x

Achse - xRichtung 0⇒=θ

Achse -y Richtung 2⇒=

πθ

Exakte Bewegung

Tt yx )',','( θ=x

⎟⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜⎜

⎝

⎛

Δ⋅

Δ⋅+⋅−⋅

Δ⋅+⋅+⋅−

+⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

tw

twwv

wv

twwv

wv

yx

yx

)cos(cos

)sin(sin

'''

θθ

θθ

θθ

Tt yx ),,(1 θ=−x T

t wv ),(=u

Reale Bewegung

⎟⎟⎠

⎞⎜⎜⎝

⎛+⎟⎟⎠

⎞⎜⎜⎝

⎛=⎟⎟

⎠

⎞⎜⎜⎝

⎛

+

+

wv

wv

wv

wv

43

21

ˆˆ

αα

αα

εε

),0(ˆˆ

MNwv

wv

+⎟⎟⎠

⎞⎜⎜⎝

⎛=⎟⎟

⎠

⎞⎜⎜⎝

⎛

⎟⎟⎠

⎞⎜⎜⎝

⎛

++

= 24

23

22

21

00

wvwv

Mαα

αα

Fehler (Normalverteilung)

ängigroboterabh - 0≥iα

Reale Bewegung

⎟⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜⎜

⎝

⎛

Δ⋅

Δ⋅+⋅−⋅

Δ⋅+⋅+⋅−

+⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

tw

twwv

wv

twwv

wv

yx

yx

ˆ

)ˆcos(ˆˆ

cosˆˆ

)ˆsin(ˆˆ

sinˆˆ

'''

θθ

θθ

θθ

⎟⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜⎜

⎝

⎛

Δ⋅+Δ⋅

Δ⋅+⋅−⋅

Δ⋅+⋅+⋅−

+⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

ttw

twwv

wv

twwv

wv

yx

yx

γ

θθ

θθ

θθ ˆˆ

)ˆcos(ˆˆ

cosˆˆ

)ˆsin(ˆˆ

sinˆˆ

'''

Übergangswahrscheinlichkeit

Berechnung der Übergangswahrscheinlichkeit

Eingabe:

),|( 1−ttt xuxp

1,, −ttt xux

θθθθμ

sin)'(cos)'(sin)'(cos)'(

21

xxyyyyxx

−+−−+−

⋅=

)'(2

'* yyxxx −⋅++

= μ

)'(2

'* xxyyy −⋅++

= μ

22 *)(*)(* yyxxr −+−=

Berechnung der Übergangswahrscheinlichkeit

*)*,(2atan*)'*,'(2atan xxyyxxyy −−−−−=Δθ

*ˆ rt

v ⋅ΔΔ

=θ

tw

ΔΔ

=θˆ w

tˆ'ˆ −

Δ−

=θθγ

),ˆprob(),ˆprob(),ˆprob( 654321 wvwvwwwvvv ααγαααα +⋅+−⋅+−

Ausgabe:

5.3 Bayes Filter - Technische Universität Chemnitz · Beispiel X t Z t offen (Tür ist geöffnet)...

Documents

Transcript of 5.3 Bayes Filter - Technische Universität Chemnitz · Beispiel X t Z t offen (Tür ist geöffnet)...