Kalman Filter תומר באום Based on ch. 8 in “Principles of robot motion” By Choset et al. ב"הב"ה

תאור כללי : שיטה רקורסיבית להערכת מצב של מערכת דינמית עם רעשים. הפלט הוא התפלגות גאוסיאנית ( ניתנת ע " י תוחלת המצב ומטריצת קווריאנס של שגיאת השיערוך )

ווקטורים אקראיים יהי וקטור אקראי עם פונקציית צפיפות תוחלת :, מטריצת הקוווריאנס : נסמן,

אנו מחפשים את המצב של הרובוט שמשתנה עם הזמן לפי המערכת הבאה: הוא התצפית (מה שמקבלים מהגלאים) הוא וקטור קלט שהמערכת מקבלת (מהירות סיבובים...) F,G,H הן מטריצות בגדלים מתאימים, נניח שדרגת H מלאה. הוא רעש המדידה: רעש גאוסיאני עם תוחלת 0 ו קווריאנס והוא לבן ( ו בלתי תלויים) הוא רעש המערכת: רעש גאוסיאני לבן משימה : מציאת מיקום ואוריינטציה של )Localization( רובוט

ניתוח מערכת ללא רעשים ( טרום קלמן ) נסמן : המצב המשוערך בזמן בהינתן כל התצפיות עד הזמן. ( הזמנים האלה יכולים להיות שווים )

שני שלבים בשיערוך חיזוי: עדכון: החיזוי מתקבל פשוט ע"י הצבה במשוואת המערכת

עדכון : נשים לב שבהינתן תצפית המצב x אמור להימצא על העל מישור: יהיה ההטלה של על העל מישור

נגדיר : נכפיל ב H: הפתרון LEAST SQUARES של המשוואה : כלומר : ( הסתכלו ב WIKIPEDIA: LINEAR LEAST SQUARES)

הערה : מבין כל הווקטורים שמקיימים : מהווה מינימום ביחס לגודל : בתהליך : K נקראת מטריצת ההגבר נקרא החידוש (INNOVATION)

מערכת עם רעש מערכת בלבד ( כמעט קלמן ) כמו שצייננו הפלט של מסנן קלמן הוא ממוצע המצב ומטריצת קווריאנס.

נתחיל משלב החיזוי בדומה לטרום קלמן זה מתקבל ע"י הצבה: החיזוי: נעלם כי התוחלת שלו 0. כעת אנחנו צריכים חיזוי של קווריאנס השגיאה:

P(k+1|k) חישוב מהצבת ו : מליניאריות התוחלת :

הסבר : נתון ש בלתי תלוי ב. בנוסף למרות שלא הגדרנו עדיין את אנו יודעים שהוא יהיה תלוי ב ובתצפית ולכן אין לו תלות ב. ( ל יש תלות ב ). לכן : ובנוסף נשתמש בנתון ש :

שלב העידכון : נזכור שאנו מניחים שפונקציית הסתברות ( צפיפות ) המצב היא :

אנחנו מחפשים נקודה ב : שתמקסם את :

המקסימום מתקבל כאשר המעריך ממוקסם, כלומר כאשר מקבל ערך מינימלי. זה דומה לבעיית Least squares שהיתה קודם. ( קודם דרשנו ש יהיה מינימלי )

לבעייה זו הפתרון יהיה : כאשר ההגבר הוא :

P(k+1|k+1) חישוב מהצבת ו ב : נקבל :

מסנן קלמן הוא וקטור מקרי עם מטריצת קווריאנס ההבדל העיקרי בין מקרה זה לקודם הוא שאנו לא יכולים לדרוש שיתקיים :

אז מה כן ? נחפש שהוא הפלט הסביר ביותר בהינתן התצפית עם קווריאנס והתצפית הצפויה : עם קווריאנס :

משפט : מכפלת גאוסיאנים מכפלת פונקציית צפיפות עם פרופורציונלית ל כאשר :

כיון ש התהליכים ו בלתי תלויים. תוחלת ההתפלגות המשותפת שלהם לפי המשפט תהיה : בדומה להגדרה שהייתה נגדיר : כעת נותר לחזור על התהליך של מציאת

לבעייה זו הפתרון יהיה : כאשר : ואם נציב את נקבל : כאשר ההגבר :

P(k+1|k+1) חישוב מהצבת ו ב : נקבל :

סיכום האלגוריתם : חיזוי : עדכון :

Reference: “An Introduction to the Kalman Filter” Welch and Bishop, (a Course given in SIGGRAPH 2001)