kalman filter תומר באום based on ch. 8 in “principles of robot motion” by choset et al....
Post on 21-Dec-2015
220 views
TRANSCRIPT
![Page 1: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/1.jpg)
Kalman Filter
תומר באוםBased on ch. 8 in “Principles of robot motion”
By Choset et al.
ב"ה
![Page 2: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/2.jpg)
:תאור כללי
שיטה רקורסיבית להערכת מצב של מערכת •דינמית עם רעשים.
הפלט הוא התפלגות גאוסיאנית )ניתנת ע"י •תוחלת המצב ומטריצת קווריאנס של שגיאת
השיערוך(
![Page 3: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/3.jpg)
ווקטורים אקראיים
יהי וקטור אקראי עם •
פונקציית צפיפות •
תוחלת: ,•
מטריצת הקוווריאנס: נסמן•
,
nX :
x X dxxxfXE )()(
)(xf X
1)( nXE
)))((( TX XXXXEP
XXE )(
nnXP
![Page 4: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/4.jpg)
אנו מחפשים את המצב של הרובוט שמשתנה עם הזמן לפי המערכת הבאה:
הוא התצפית )מה שמקבלים מהגלאים(
הוא וקטור קלט שהמערכת מקבלת )מהירות סיבובים...(
F,G,H הן מטריצות בגדלים מתאימים, נניח שדרגת H.מלאה
ו קווריאנס 0 הוא רעש המדידה: רעש גאוסיאני עם תוחלת
והוא לבן ) ו בלתי תלויים(
הוא רעש המערכת: רעש גאוסיאני לבן
משימה: מציאת מיקום ואוריינטציה רובוט )Localization( של
)()()()(
)()()()()()1(
kwkxkHky
kvkukGkxkFkx
nkx )(
pky )(mku )(
)(kw
)(kv
)1( kw )(kw
)(kW
))(,0( kVN
![Page 5: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/5.jpg)
ניתוח מערכת ללא רעשים )טרום קלמן(
נסמן: המצב המשוערך בזמן בהינתן כל •
התצפיות עד הזמן . )הזמנים האלה יכולים להיות
שווים(
)()()(
)()()()()1(
kxkHky
kukGkxkFkx
)|(ˆ 12 kkx
1k2k
)()()|(ˆ)()|1(ˆ kukGkkxkFkkx
)|1(ˆ)1|1(ˆ kkxkkxx
![Page 6: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/6.jpg)
שני שלבים בשיערוך
חיזוי:
עדכון:
החיזוי מתקבל פשוט ע"י הצבה במשוואת המערכת
)()()|(ˆ)()|1(ˆ kukGkkxkFkkx
))|1(ˆ)1(()()|1(ˆ)1|1(ˆ 1 kkxHkyHHHkkxkkx TT
![Page 7: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/7.jpg)
: עדכון
)|1(ˆ kkx xנשים לב שבהינתן תצפית המצב
אמור להימצא על העל מישור:
יהיה ההטלה של
על העל מישור
)1( ky
)1|1(ˆ kkx
)1|1(ˆ kkx)|1(ˆ kkx
)}1()1(|{ kyxkHx n
![Page 8: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/8.jpg)
נגדיר:
:Hנכפיל ב
של המשוואה:LEAST SQUARESהפתרון
כלומר:
(WIKIPEDIA: LINEAR LEAST SQUARES)הסתכלו ב
))|1(ˆ)1(()()|1(ˆ)1|1(ˆ 1 kkxHkyHHHkkxkkx TT
x )|1(ˆ)1|1(ˆ kkxkkxx
))|1(ˆ)1(()( 1 kkxHkyHHHx TT
)|1(ˆ)1(
)|1(ˆ))1|1(ˆ(
kkxHky
kkxHkkxHxH
![Page 9: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/9.jpg)
:הערהמבין כל הווקטורים שמקיימים:
מהווה מינימום ביחס לגודל:
בתהליך:
K נקראת מטריצת ההגבר
(INNOVATION נקרא החידוש )
vvv T2x
)|1(ˆ)1( kkxHkyvH v
))|1(ˆ)1(()|1(ˆ)1|1(ˆ kkxHkyKkkxkkx
))|1(ˆ)1(( kkxHky
![Page 10: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/10.jpg)
מערכת עם רעש מערכת בלבד )כמעט קלמן(
כמו שצייננו הפלט של מסנן קלמן הוא ממוצע המצב ומטריצת קווריאנס.
)()()(
)()()()()()1(
kxkHky
kvkukGkxkFkx
![Page 11: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/11.jpg)
נתחיל משלב החיזוי בדומה לטרום קלמן זה מתקבל ע"י הצבה:
החיזוי:
.0נעלם כי התוחלת שלו
כעת אנחנו צריכים חיזוי של קווריאנס השגיאה:
)()()|(ˆ)()|1(ˆ kukGkkxkFkkx
)(kv
TkkxkxkkxkxEkkP )|1(ˆ)1()|1(ˆ)1()|1(
![Page 12: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/12.jpg)
P(k+1|k)חישוב
ו :מהצבת•
מליניאריות התוחלת:•
TT
TT
kvkvkvkkxkxkF
kFkkxkxkkxkxkFEkkP
)()()())|(ˆ)()((2
...)()|(ˆ)()|(ˆ)()()|1(
)1( kx)|1(ˆ kkx
)()()|()(
)()()())|(ˆ)(()(2
...)()|(ˆ)()|(ˆ)()()|1(
kVkFkkPkF
kvkvEkvkkxkxEkF
kFkkxkxkkxkxEkFkkP
T
TT
TT
![Page 13: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/13.jpg)
הסבר: • נתון ש בלתי תלוי ב .
בנוסף למרות שלא הגדרנו עדיין את אנו יודעים שהוא יהיה תלוי ב ובתצפית ולכן אין
לו תלות ב . )ל יש תלות ב (.לכן:
ובנוסף נשתמש בנתון ש:•
TT kvEkkxkxEkvkkxkxE )())|(ˆ)(()())|(ˆ)((
)(kv)|(ˆ kkx
)(kx
0)]([ kvE
)1|(ˆ kkx)(ky
)(kv )1( kx )(kv
![Page 14: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/14.jpg)
:שלב העידכון
נזכור שאנו מניחים שפונקציית הסתברות •)צפיפות( המצב היא:
)()( 121
||)2(
1),()( xxPxx
n
T
eP
PxNxp
![Page 15: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/15.jpg)
אנחנו מחפשים נקודה ב:•
שתמקסם את:•
))|1(ˆ()|1())|1(ˆ( 121
|)|1(|)2(
1
))|1(),|1(ˆ()(
kkxxkkPkkxx
n
T
ekkP
kkPkkxNxp
)}1()1(|{ kyxkHx n
![Page 16: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/16.jpg)
המקסימום מתקבל כאשר המעריך ממוקסם, כלומר
כאשר
מקבל ערך מינימלי.
שהיתה קודם. )קודם Least squaresזה דומה לבעיית דרשנו
ש יהיה מינימלי(
xkkPx
kkxxkkPkkxx
T
T
1
1
)|1(
))|1(ˆ()|1())|1(ˆ(
xxT
![Page 17: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/17.jpg)
:לבעייה זו הפתרון יהיה
כאשר ההגבר הוא:
))|1(ˆ)1(()|1(ˆ)1|1(ˆ kkxHkyKkkxkkx
1))|1(()|1( TT HkkHPHkkPK
![Page 18: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/18.jpg)
P(k+1|k+1)חישוב
ו ב:מהצבת•
נקבל:•
)1( kx)1|1(ˆ kkx
TkkxkxkkxkxEkkP )1|1(ˆ)1()1|1(ˆ)1()1|1(
)|1()|1()1|1( kkKHPkkPkkP
![Page 19: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/19.jpg)
מסנן קלמן
הוא וקטור מקרי עם מטריצת קווריאנס
ההבדל העיקרי בין מקרה זה לקודם הוא שאנו לא יכולים לדרוש שיתקיים:
)()()()(
)()()()()()1(
kwkxkHky
kvkukGkxkFkx
)(kw)(kW
)1()1|1(ˆ)1( kykkxkH
![Page 20: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/20.jpg)
?אז מה כן
נחפש שהוא הפלט הסביר ביותר בהינתן •התצפית עם קווריאנס והתצפית
הצפויה:
עם קווריאנס:•
)1(* ky)1( ky
)|1(ˆ)1()1(ˆ kkxkHky
)1()|1()1(
)]1())1()|1(ˆ))(1()|1(ˆ)(1([
]))1()1(ˆ))(1()1(ˆ[(ˆ
kHkkPkH
kHkxkkxkxkkxkHE
kykykykyEW
T
TT
T
)1( kW
![Page 21: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/21.jpg)
משפט: מכפלת גאוסיאנים
מכפלת פונקציית צפיפות עם
כאשר: פרופורציונלית ל
)( 1,1 N)( 2,2 N
)( 3,3 N
))((
)()(
11
2113
121
21113
I
![Page 22: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/22.jpg)
כיון ש התהליכים ו
בלתי תלויים. תוחלת ההתפלגות המשותפת שלהם
לפי המשפט תהיה:
בדומה להגדרה שהייתה נגדיר:
כעת נותר לחזור על התהליך של מציאת
))1(ˆ),1(ˆ( kWky))1(),1(( kWky
))1(ˆ())1(ˆ(ˆˆ 1* kyykWWWyy
)}1()1(|{ ** kyxkHx n
*)1|1(ˆ kkx
![Page 23: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/23.jpg)
:לבעייה זו הפתרון יהיה
כאשר:
ואם נציב את נקבל:
כאשר ההגבר:
))|1(ˆ)1(()|1(ˆ)1|1(ˆ * kkxHkyRkkxkkx
1))|1(()|1( TT HkkHPHkkPR*y
))|1(ˆ)1(()|1(ˆ)1|1(ˆ kkxHkyKkkxkkx
1))1()|1(()|1( kWHkkHPHkkPK TT
![Page 24: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/24.jpg)
P(k+1|k+1)חישוב
ו ב:מהצבת•
נקבל:•
)1( kx)1|1(ˆ kkx
TkkxkxkkxkxEkkP )1|1(ˆ)1()1|1(ˆ)1()1|1(
)|1()|1()1|1( kkKHPkkPkkP
![Page 25: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/25.jpg)
:סיכום האלגוריתם
חיזוי:•
עדכון:•
)()()|()()|1(
)()()|(ˆ)()|1(ˆ
kVkFkkPkFkkP
kukGkkxkFkkxT
)|1()|1()1|1(
))|1(ˆ)1(()|1(ˆ)1|1(ˆ
kkKHPkkPkkP
kkxHkyKkkxkkx
![Page 26: Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה](https://reader036.vdocument.in/reader036/viewer/2022062313/56649d5f5503460f94a3f83b/html5/thumbnails/26.jpg)
Reference:
• “An Introduction to the Kalman Filter” Welch and Bishop, (a Course given in SIGGRAPH 2001)