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)