6.3 Bayes Filter
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
Beispiel
6.0)|( =openzp 3.0)|( =¬openzp
5.0)( =openp 5.0)( =¬openp
)()|()()|()()|()|(
openpopenzpopenpopenzpopenpopenzpzopenp
¬⋅¬+⋅⋅
=
67.032
5.03.05.06.05.06.0)|( ≈=⋅+⋅
⋅=zopenp
Die Beobachtung z erhöht die Wahrscheinlichkeit dafür,dass die Tür geöffnet ist
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
Ü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: