Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al....
-
date post
21-Dec-2015 -
Category
Documents
-
view
220 -
download
0
Transcript of Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al....
Kalman Filter
תומר באוםBased on ch. 8 in “Principles of robot motion”
By Choset et al.
ב"ה
:תאור כללי
שיטה רקורסיבית להערכת מצב של מערכת •דינמית עם רעשים.
הפלט הוא התפלגות גאוסיאנית )ניתנת ע"י •תוחלת המצב ומטריצת קווריאנס של שגיאת
השיערוך(
ווקטורים אקראיים
יהי וקטור אקראי עם •
פונקציית צפיפות •
תוחלת: ,•
מטריצת הקוווריאנס: נסמן•
,
nX :
x X dxxxfXE )()(
)(xf X
1)( nXE
)))((( TX XXXXEP
XXE )(
nnXP
אנו מחפשים את המצב של הרובוט שמשתנה עם הזמן לפי המערכת הבאה:
הוא התצפית )מה שמקבלים מהגלאים(
הוא וקטור קלט שהמערכת מקבלת )מהירות סיבובים...(
F,G,H הן מטריצות בגדלים מתאימים, נניח שדרגת H.מלאה
ו קווריאנס 0 הוא רעש המדידה: רעש גאוסיאני עם תוחלת
והוא לבן ) ו בלתי תלויים(
הוא רעש המערכת: רעש גאוסיאני לבן
משימה: מציאת מיקום ואוריינטציה רובוט )Localization( של
)()()()(
)()()()()()1(
kwkxkHky
kvkukGkxkFkx
nkx )(
pky )(mku )(
)(kw
)(kv
)1( kw )(kw
)(kW
))(,0( kVN
ניתוח מערכת ללא רעשים )טרום קלמן(
נסמן: המצב המשוערך בזמן בהינתן כל •
התצפיות עד הזמן . )הזמנים האלה יכולים להיות
שווים(
)()()(
)()()()()1(
kxkHky
kukGkxkFkx
)|(ˆ 12 kkx
1k2k
)()()|(ˆ)()|1(ˆ kukGkkxkFkkx
)|1(ˆ)1|1(ˆ kkxkkxx
שני שלבים בשיערוך
חיזוי:
עדכון:
החיזוי מתקבל פשוט ע"י הצבה במשוואת המערכת
)()()|(ˆ)()|1(ˆ kukGkkxkFkkx
))|1(ˆ)1(()()|1(ˆ)1|1(ˆ 1 kkxHkyHHHkkxkkx TT
: עדכון
)|1(ˆ kkx xנשים לב שבהינתן תצפית המצב
אמור להימצא על העל מישור:
יהיה ההטלה של
על העל מישור
)1( ky
)1|1(ˆ kkx
)1|1(ˆ kkx)|1(ˆ kkx
)}1()1(|{ kyxkHx n
נגדיר:
: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
:הערהמבין כל הווקטורים שמקיימים:
מהווה מינימום ביחס לגודל:
בתהליך:
K נקראת מטריצת ההגבר
(INNOVATION נקרא החידוש )
vvv T2x
)|1(ˆ)1( kkxHkyvH v
))|1(ˆ)1(()|1(ˆ)1|1(ˆ kkxHkyKkkxkkx
))|1(ˆ)1(( kkxHky
מערכת עם רעש מערכת בלבד )כמעט קלמן(
כמו שצייננו הפלט של מסנן קלמן הוא ממוצע המצב ומטריצת קווריאנס.
)()()(
)()()()()()1(
kxkHky
kvkukGkxkFkx
נתחיל משלב החיזוי בדומה לטרום קלמן זה מתקבל ע"י הצבה:
החיזוי:
.0נעלם כי התוחלת שלו
כעת אנחנו צריכים חיזוי של קווריאנס השגיאה:
)()()|(ˆ)()|1(ˆ kukGkkxkFkkx
)(kv
TkkxkxkkxkxEkkP )|1(ˆ)1()|1(ˆ)1()|1(
P(k+1|k)חישוב
ו :מהצבת•
מליניאריות התוחלת:•
TT
TT
kvkvkvkkxkxkF
kFkkxkxkkxkxkFEkkP
)()()())|(ˆ)()((2
...)()|(ˆ)()|(ˆ)()()|1(
)1( kx)|1(ˆ kkx
)()()|()(
)()()())|(ˆ)(()(2
...)()|(ˆ)()|(ˆ)()()|1(
kVkFkkPkF
kvkvEkvkkxkxEkF
kFkkxkxkkxkxEkFkkP
T
TT
TT
הסבר: • נתון ש בלתי תלוי ב .
בנוסף למרות שלא הגדרנו עדיין את אנו יודעים שהוא יהיה תלוי ב ובתצפית ולכן אין
לו תלות ב . )ל יש תלות ב (.לכן:
ובנוסף נשתמש בנתון ש:•
TT kvEkkxkxEkvkkxkxE )())|(ˆ)(()())|(ˆ)((
)(kv)|(ˆ kkx
)(kx
0)]([ kvE
)1|(ˆ kkx)(ky
)(kv )1( kx )(kv
:שלב העידכון
נזכור שאנו מניחים שפונקציית הסתברות •)צפיפות( המצב היא:
)()( 121
||)2(
1),()( xxPxx
n
T
eP
PxNxp
אנחנו מחפשים נקודה ב:•
שתמקסם את:•
))|1(ˆ()|1())|1(ˆ( 121
|)|1(|)2(
1
))|1(),|1(ˆ()(
kkxxkkPkkxx
n
T
ekkP
kkPkkxNxp
)}1()1(|{ kyxkHx n
המקסימום מתקבל כאשר המעריך ממוקסם, כלומר
כאשר
מקבל ערך מינימלי.
שהיתה קודם. )קודם Least squaresזה דומה לבעיית דרשנו
ש יהיה מינימלי(
xkkPx
kkxxkkPkkxx
T
T
1
1
)|1(
))|1(ˆ()|1())|1(ˆ(
xxT
:לבעייה זו הפתרון יהיה
כאשר ההגבר הוא:
))|1(ˆ)1(()|1(ˆ)1|1(ˆ kkxHkyKkkxkkx
1))|1(()|1( TT HkkHPHkkPK
P(k+1|k+1)חישוב
ו ב:מהצבת•
נקבל:•
)1( kx)1|1(ˆ kkx
TkkxkxkkxkxEkkP )1|1(ˆ)1()1|1(ˆ)1()1|1(
)|1()|1()1|1( kkKHPkkPkkP
מסנן קלמן
הוא וקטור מקרי עם מטריצת קווריאנס
ההבדל העיקרי בין מקרה זה לקודם הוא שאנו לא יכולים לדרוש שיתקיים:
)()()()(
)()()()()()1(
kwkxkHky
kvkukGkxkFkx
)(kw)(kW
)1()1|1(ˆ)1( kykkxkH
?אז מה כן
נחפש שהוא הפלט הסביר ביותר בהינתן •התצפית עם קווריאנס והתצפית
הצפויה:
עם קווריאנס:•
)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
משפט: מכפלת גאוסיאנים
מכפלת פונקציית צפיפות עם
כאשר: פרופורציונלית ל
)( 1,1 N)( 2,2 N
)( 3,3 N
))((
)()(
11
2113
121
21113
I
כיון ש התהליכים ו
בלתי תלויים. תוחלת ההתפלגות המשותפת שלהם
לפי המשפט תהיה:
בדומה להגדרה שהייתה נגדיר:
כעת נותר לחזור על התהליך של מציאת
))1(ˆ),1(ˆ( kWky))1(),1(( kWky
))1(ˆ())1(ˆ(ˆˆ 1* kyykWWWyy
)}1()1(|{ ** kyxkHx n
*)1|1(ˆ kkx
:לבעייה זו הפתרון יהיה
כאשר:
ואם נציב את נקבל:
כאשר ההגבר:
))|1(ˆ)1(()|1(ˆ)1|1(ˆ * kkxHkyRkkxkkx
1))|1(()|1( TT HkkHPHkkPR*y
))|1(ˆ)1(()|1(ˆ)1|1(ˆ kkxHkyKkkxkkx
1))1()|1(()|1( kWHkkHPHkkPK TT
P(k+1|k+1)חישוב
ו ב:מהצבת•
נקבל:•
)1( kx)1|1(ˆ kkx
TkkxkxkkxkxEkkP )1|1(ˆ)1()1|1(ˆ)1()1|1(
)|1()|1()1|1( kkKHPkkPkkP
:סיכום האלגוריתם
חיזוי:•
עדכון:•
)()()|()()|1(
)()()|(ˆ)()|1(ˆ
kVkFkkPkFkkP
kukGkkxkFkkxT
)|1()|1()1|1(
))|1(ˆ)1(()|1(ˆ)1|1(ˆ
kkKHPkkPkkP
kkxHkyKkkxkkx
Reference:
• “An Introduction to the Kalman Filter” Welch and Bishop, (a Course given in SIGGRAPH 2001)