פרויקט א'דוח פרויקט:

הנושא:

ניווט אינרציאלי מבוסס עלUnscented Kalman Filter

מגישים:ודים אוחרימנקואיליה אוסדצ'י

326952843326899226 מנחה:

ד''ר גבי דוידוב

אביב: סמסטרתשע: שנה

הטכניון - מכון טכנולוגי לישראל TECHNION - ISRAEL INSTITUTE OF TECHNOLOGY

הפקולטה להנדסת חשמלהמעבדה לבקרה ורובוטיקה

Table of Contents

Table of Figures..............................................................................................................34...................................................................................................................תקציר.14........................................................................................................רשימת קיצורים.24.....................................................................................................................מבוא.35...........................................................................................................רקע תיאורטי.4

5......................................................................................................ניווט אינרציאליא.Kalman Filter.......................................................................................................7ב.

Linear Kalman Filter...........................................................................................7Unscented Kalman Filter....................................................................................9Rao-Blackwellised Unscented Kalman Filter........................................................12Rao-Blackwellised Additive Unscented Kalman Filter............................................13

14......................................................................................................תיאור הפרויקט.514.......................................................................................................מימוש בסיסיא.18...................................................................................................הורדת סיבוכיותב.

18.....................................................................................רעש מדי תאוצה אדיטיבי19.....................................................................................רעש גירוסקופים אדיטיבי

scaling factor....................................................................................20התחשבות ב-ג.21................................................................................................................תוצאות.6

21............................................................................................................מסלוליםא.23...............................................................................השוואה בין מימושים – סיבוכיותב.26.............................................................................השוואה בין מימושים – קנה מידהג.

29.......................................................................................................סיכום מסקנות.630......................................................................................................רשימת מקורות.7

2

Table of Figures

Figure 1: Euler angles..................................................................................................................... 5Figure 2: basic Kalman filter...........................................................................................................7Figure 3: sigma points..................................................................................................................... 9Figure 4: covariance estimation....................................................................................................10Figure 5: block diagram................................................................................................................14Figure 6: RMSE dependency on noise (untira1)...........................................................................24Figure 7: route estimation with higher noise (untira1)...................................................................24Figure 8: RMSE dependency on noise (unnis3)...........................................................................25Figure 9: route estimation with higher noise (unnis3)...................................................................25Figure 10: scaling factor in acceleration (untira1).........................................................................26Figure 11: scaling factor in angular rate (untira1).........................................................................27Figure 12: scaling factor with low noise........................................................................................27Figure 13: scaling factor with medium noise.................................................................................28Figure 14: scaling factor with high noise.......................................................................................28

3

תקציר.1

, ברמה של תוכנה )UKFהפרויקט מתאר בנייה של מערכת לניווט אינרציאלי מבוססת על

Matlab( מבצעים השוואה בין מספר גרסאות של המסנן .)RBUKF, RBAUKFמבחינת דיוק )

השערוך וסיבוכיות חישובית. משווים דיוק השערוך בין מימוש בסיסי לבין מימוש שבנוסף מתחשב

בשגיאת קנה מידה של חיישנים.

רשימת קיצורים.2INS = Inertial Navigation System

NED = North-East-Down (inertial axis system)

UT = Unscented transformation

SP = Sigma points

UKF = Unscented Kalman Filter

RBUKF = Rao-Blackwellised UKF

RBAUKF = Rao-Blackwellised Additive UKF

מבוא.3

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

זולות ומדויקות. פיתרון אחד של בעיית ניווט כולל גוף נוסף ששולח אותות )למשל גלי רדיו( לגוף

. אבל לפיתרון הזה יש מספר חסרונות, למשל האותGPSהמנווט, דוגמא מפורסמת היא מערכת

יכול להיחסם או להשתבש, בנוסף לא ניתן להעביר כל סוג של אות לכל מקום )כמו מתחת לים או

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

בסנסורים המודדים תאוצה ומהירות זוויתית של הגוף ומחשבים מצב של גוף על סמך מצב הקודם

ונתוני הסנסורים; קידום המצב הינו טרנספורמציה לא ליניארית כי מתחשבים בסיבוב של הגוף.

ניווט אינרציאלי פשוט עובד טוב אם אין רעשים, אבל בסביבה רועשת ביצוע אינטגרציה על

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

particle filter או Kalman filter לא ליניארי. בפרויקט אנחנו משתמשים במסנן Unscented

4

Kalman Filter-הוא נותן דיוק טוב בסיבוכיות סבירה. כמו שנראה בהמשך, גם ל ,UKFיש מספר

גרסאות.

. נניח שהגוף המנווטUKFמטרתנו לממש ולהשוות בין גרסאות שונות של ניווט אינרציאלי בעזרת

הינו רכב, הוא יכול לנוע במרחב תלת מימדי אבל לא יכול להיות מכוון אנכית למעלה או למטה.

המקבלת ככניסה מדידות של גירוסקופים ומדי תאוצה ובנוסףMatlabהמערכת תכלול תוכנה ב-

מדידה של אודומטר שהיא מרחק קווי שעבר הגוף.

אנחנו מניחים שכל מדידות הסנסורים מרועשות ברעש לבן גאוסי ויכולות לסבול משגיאות היסט

מניחים כי מדידת אודומטר סובלת רק.(1)( שקבועות בזמן bias and scalingושגיאות קנה מידה )

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

שונים.

– ננסה להפריד בין חלק הליניארי והלא ליניארי שלUKFנשווה בין מימושים שונים של

טרנספורמציה. ובנוסף נשווה בין גרסאות שמתחשבות בשגיאות היסט וקנה מידה )מנסות לשערך

אותן ומכילות אותן כחלק מווקטור המצב( לבין גרסאות שמתחשבות רק בשגיאת היסט )מניחות

שאין שגיאת קנה מידה(. ההשוואה תכלול דיוק הניווט המחושבת על פי סימולציות ובנוסף תכלול

הערכה למורכבות החישוב וזמן ביצוע.

רקע תיאורטי.4

ניווט אינרציאליא.

בביעה שלנו התנועה הינה תלת מימדית לכן לכל גוף יש שלושה רכיבים של מיקום, שלושה

Roll, Pitch, Yawרכיבים של מיהורות ושלושה רכיבים של כיוון. לתיאור של כיוון נשתמש בזוויות

בהתאמה(.φ, θ, ψ)מסומנים

לניתוח הבעיה נגדיר שתי מערכות הבאות:

הינוZ הינו ימינה וציר Y הינו קדימה יחסית לגוף, ציר X( שבה ציר XYZמערכת הגוף )-

למלה

Down הינו Z וציר East הינו Y, ציר North הינו X( שבה ציר NEDמערכת כדור הארץ )-

סביבYaw במעבר ממערכת הארץ למערכת הגוף קודם מפעילים סיבוב בזווית Figure 1רואים ב-

)שהסתובב יחד עם גוף בסיבוב הראשון( ולבסוף בזוויתY סביב ציר Pitch, אחר כך בזווית Zציר

Roll סביב ציר X)(2 ) )שמסובב בשני סיבובים קודמים .

5

Figure 1: Euler angles

נרצה לדעת מיקום הגוף במערת הארץ לכן נתרגם כל המדידות המתקבלות במערכת הגוף. כדי

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

המתוארים למעלה:

T roll

CB→N=(1 0 00 cos φ sin φ0 −sinφ cos φ)⏞

T pitch

×(cosθ 0 −sin θ0 1 0

sin θ 0 cosθ )⏞

T yaw

×( cosψ sinψ 0−sinψ cosψ 0

0 0 1)⏞

. נעשה את זה על ישי שימוש(3 )בנוסף יש צורך לתרגם מהירות זוויתית ממערכת למערכת

בנוסחה הבאה:

(w x

w y

w z)=T roll×T pitch×(0 0 0

0 0 00 0 1)+T roll×(0 0 0

0 1 00 0 0)+(1 0 0

0 0 00 0 0)=¿

¿(1 0 −sin θ0 cos φ sinφ cos φ0 −sinφ cos φ cosθ)⏞

TangN→ B

×(wφ

wθ

wψ)

| אחרת ההגדרה אינה חד משמעית של זוויות ולא ניתן להפוך מטריצה. θ<|90אנחנו מניחים כי

6

בבעיות שבהן הנחה ה''ל לא מתקיימת ניתן להשתמש בייצוג אחר של זוויות, למשל בשיטת

קווטרניונים.

7

Kalman Filterב.

Linear Kalman Filter

קלמן פילטר )פותח על ידי רודולף קלמן( הינו פילטר רקורסיבי יעיל אשר משערך את המצב של

מערכת דינאמית בזמן מתוך סדרה של מדידות רועשות ואף לא שלמות. כאשר המצב המשוערך

מהצעד הקודם והמדידה הנוכחית נדרשים על מנת לחשב את השערוך למצב הנוכחי.

משערך המצב האופטימאלי במובן של שגיאה ריבועית בין ווקטור המצב המשוערך לבין ווקטור

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

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

פילטר קלמן מבוסס על שלבים הבאים:

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

והכניסות.

מציאת השגיאה במדידות והגבר אופטימאלי )למינימום של המומנט השני(.ב.

תיקון החיזוי על ידי משוב של השגיאה לפי הגבר. ג.

מודל המשערך במקרה הלינארי מתואר בסכמה הבאה:

Figure 2: basic Kalman filter

מניחים כי הכניסה אפס וכי רעשים לבנים גאוסיים בעלי תוחלת אפס.

8

9

Unscented Kalman Filter

מציאת המומנט השני של משתנה אקראי לאחר טרנספורמציה לא ליניארית ללא מגבלות על

כמות החישובים יכולה להתבצע על ידי בחירה של כמות גדולה מאוד של נקודות במרחב המדגם,

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

על מנת לקבל תוצאה טובה יש צורך בכמות דגימות גדולה )בשיטה זו מתקבלת תוצאה המאפשרת

לחשב גם את המומנטים מסדר גבוה בצורה פשוטה, אך עבור הפילטר הנבדק אין שימוש

במומנטים אלו(.

( ניתן לקבל את המומנט הראשון והשניSPעל ידי בחירה מושכלת של הנקודות הנבחרות )

. הנקודות נבחרות2L+1 (4), בדיוק )L(O)ממטריצת הקווריאנס( בדרישות חישוביות של

שיאפיינו את שני המומנטים הראשונים של התפלגות המשתנה האקראי לפני טרנספורמציה,

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

שונה לכל נקודה )אנו השתמשנו בווקטור משקל פשוט(.

דוגמה לבחירת נקודות במקרה דו מימדי:

Figure 3: sigma points

אינם פרידים בהתפלגות. הנקודות שנבחרוX,Yהמשתנה האקראי הוא גאוסי כאשר הצירים

ממוקמות על צירי המערכת העצמית )המלוכסנת( של מטריצת הקווריאנס ומתארות בצורה מלאה

את התפלגות.

השיטות השונות להתמודדות עם טרנספורמציה לא3בתרשים הבא ניתן לראות השוואה בין

לינארית:

10

Figure 4: covariance estimation

ניתן לראות כי בדגימה צפופה )שמאל( מתקבל מיפוי טוב של הפילוג החדש ובשיטה של

)ימין( מתקבלת תוצאהSPלינאריזציה )מרכז( מתקבלת סטייה גדולה מאוד. עבור השימוש ב-

נקודות בלבד. 5מאוד דומה לפילוג האמיתי וזאת על פי

אלגוריתם חיזוי המצב הבא.א.

xk−1 , Pk−1xx → χ k−1

i=0..2 L (xk−1 , pk−1 , ξ )→ χk∨k−1i=0..2 L→ xk∨k−1 , Pk∨k−1

xx (xk−1 ,Q )חיזוי המדידה הבאה.ב.

xk∨k−1 ,Pk∨k−1xx → χ k∨k−1

i=0. .2L (xk−1 , pk−1, ξ )→γk∨k−1i=0. .2 L→ y, pxy , pyy( yk−1 , R)

pxyמציאת ההגבר. ג. , pyy →K

xk∨k−1עדכון החיזוי. ד. ,Pk∨k−1xx , y , yk−1 ,K k−1 , Pk−1

yy →xk ,Pkxx

11

Rao-Blackwellised Unscented Kalman Filter

RBUKF הינו פילטר שמבצע קומבינציה בין KF ליניארי לבין UKF(5 ) כדי להוריד סיבוכיות. לחישוב תצפית פילטר משתמש בטרנספורמציה ליניארית:

:RBUKFהאלגוריתם עבודה של מסנן

12

Rao-Blackwellised Additive Unscented Kalman Filter

RBAUKFהינו מסנן שמניח שרעש הסנסורים הינו רעש אדיטיבי לכן הוא מבצע הוספת רעש

: (5 )אחרי הפעלת טרנספורמציה על וקטור מצב

:RBAUKFהאלגוריתם עבודה של מסנן

13

תיאור הפרויקט.5

מימוש בסיסיא.

. נרצה שחיזוי התצפית יתקבל בטרנספורמציה ליניאריתUKFנבנה מערכת ניווט המבוססת על

תהיה ליניארית כדי לצמצםg תהיה לא ליניארית ופונקציה fמתוך חיזוי המצב. כלומר שפונקציה

את סיבוכיות המערכת.

Figure 5: block diagram

וקטור המצב הינו:

xk=[positionNED3x 1

velocity xyz3 x1

anglesNED3 x1

biasa3 x1

biasw3 x1

distance]

positionNED.הינו מיקום הנוכחי של הגוף במערכת הארץ

velocity xyz הינו מהירות הגוף, על מנת לאפשר gליניארי שומרים את המהירויות במערכת צירים

(.N,E,D( במקום מערכת כדור הארץ )x,y,zהמסובבת )

14

anglesNED מכיל זוויות roll, pitch, yaw.

biasa, biasw.הינם שערוך של שגיאות היסט של מדי תאוצה וגירוסקופים בהתאמה

distance.הינו שיערוך של מרחק קווי שעבר הרכב, שאותו ניתן להשוות למדידת אודומטר

N=16אורך הווקטור

. פלטSP, שלב ראשון הינו יצירת UT לא ליניארית ולכן מבצעים חיזוי המצב בעזרת fפונקציה

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

הבאה:

ניצור וקטור מצב מורחב

xk−1∨k−1a =¿

ak−1וקטוריםbody ,w k−1

body בגודל( 3 הינם פלט של מדי תאוצה וגירוסקופים ברגע הנתוןx1)כל אחד

Nx1 הינו המצב הנוכחי בגודל xk−1∨k−1וקטור

ניצור מטריצת קווריאנס של וקטור המורחב, היא תהיה בלוק-אלכסונית כי רעש הסנסורים

בלתי תלוי במצב

P xxa

k−1∨k−1=[P xxk−1∨k−1 0 00 Paa 00 0 Pww

]P xxk−1∨k−1הינה מטריצת קווריאנס של וקטור המצב שאותה שומרים יחד עם וקטור המצב

Paa ,Pww 3 הינן מטריצות אלכסוניותx3של שונות הרעש )הידועה מנתוני יצרן של המדדים או לפי

שיערוך לפני תחילת התנועה(.

יוצריםSP( 2 לווקטור המורחב, נקבלL+1( נקודות כאשר )L=N+3+3=22.)

על כל אחת מהנקודות שהתקבלו.fלאחר מכך מפעילים את

נסמן רכיבים של כל נקודה לפי משמעותם:

[ posned ,k−1 vbody ,k−1 angk−1 biasa ,k−1biasw ,k−1 distk−1abodyraw wbody

raw ]= χk−1∨k−1i

מתחשבים בשגיאות היסט המשוערכות:

15

abody¿ =abody

raw +biasa , k−1

wbody¿ =wbody

raw +biasw ,k−1

C❑ ,Tיוצרים מטריצות טרנספורמציה ang לפי הזוויות❑

משערכים ערכי הרכיבים בנקודת זמן הבאה

angk=angk−1+Tang , k−1B→N ∙wbody

¿ ∙ dt

posNED, k=posNED, k−1+v NED, k−1∙ dt

dist k=dist k−1+vx , k−1 ∙ dt

עבור מהירות במערכת הארץ הטרנספורמציה הינה:

vNED ,k=vNED ,k−1+(Ck−1B→ N ∙ abody

¿ +gNED ) ∙ dt

ובמקרה שלנו ווקטור המצב מכיל מהירות במערכת מסובבת ולכן הטרנספורמציה הינה:

vNED ,k−1=C k−1B→N ∙ vbody ,k−1

vbody ,k=CkN→B ∙ vNED ,k−1+(abody

¿ +C kN→B ∙ gNED ) ∙ dt

( ללא שינוי, עדכון של השערוך נעשה יחד עםbiasאנחנו מכניסים לחיזוי מצב שיערוך של היסט )

תיקון המצב.

מקבלים נקודה אחרי הטרנספורמציה:

χk∨k−1i = [ posned , k vbody , kangk biasa ,k−1biasw ,k−1 distk ]

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

16

כאשר

W s0= λ

L+λ

W c0= λ

L+λ+(1−α2+β)

W si=W c

i= λ2(L+λ)

λ=α 2 ∙ (L+κ )−L

: (6)השתמשנו בערכים

α=10−3; β=2 ;κ=1 ;

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

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

, אבל זהxבה בשתי דרכים. ניתן לגזור את המדידה לפי הזמן כדי לקבל מהירות של הרכב בכיוון

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

שומרים את המרחק המשוערך ומשווים למרחק הנמדד, זה מגדיל את וקטור המצב במשתנה אחד

אבל נותן רמת בטחון גבוהה יותר.

ערכים נוספים בתצפית באים מהנחות עבור סוג התנועה שהרכב מבצע. נניח שתנועה בניצב וכיוון

כאשר השונות0 לווקטור התצפית ונשווה ל-y, zאינה סבירה, כלומר נכניס מהירות בכיוונים

המתאימה מגדירה סבירות של התנועה כזאת.

חיזוי תצפית:

yk∨k−1=[distv y

vz]=H ∙ xk∨k−1=¿

¿ [0 0 00 0 00 0 0

0 0 00 1 00 0 1

0 0 00 0 00 0 0

0 0 00 0 00 0 0

0 0 00 0 00 0 0

100 ]∙ xk∨k−1

רעש המדידה:

R=[Pdist 0 00 Pv y

00 0 Pvz

]17

קווריאנסים:

P yy , k=H Pk∨k−1 HT +R

P xy ,k=Pk∨k−1 HT

Kalman gain:

K k=P xy , k (P yy ,k )−1

תיכון מצב וקווריאנס המצב:

xk∨k= xk∨k−1+K k ( yk− y k∨k−1)

P xxk∨k=Pk∨k−1−K k P yy ,k K kT

הורדת סיבוכיותב.

בעלSP(, למשל שלב יצירת xaסיבוכיות המערכת מושפעת באופן חזק ממימד מצב מורחב )

הוא מימד הווקטור. במימוש הבסיסי שלנו מתקבלL(, כאשר L3/3סיבוכיות חישוב )~

L=N+3+3=22כמו שראינו בחלק תיאורטי, אם ניתן לתאר רעש הסנסורים כרעש אדיטיבי אז אין .

.L=Nצורך בהרחבת מצב ומקבלים

הגענו לייצוג הנ''ל בשני שלבים, כאשר בשלב ראשון הצגנו רק רעש מדי תאוצה כאדיטיבי.

רעש מדי תאוצה אדיטיבי

abodyההשפעה של תאוצה בטרנספורמציה מתבטאת במחובר ∙ dtבעדכון של מהירות. ניתן לא

במצב המורחב אלא להעביר אותו כפרמטר לטרנספורמציה. במקרה כזה צריךabodyלכלול את

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

18

abodyבמצב ולכן ניתן פשוט לחבר קווריאנס של ∙ dt כלומר( ~Paa=Paa ∙ dt ( לקווריאנס המהירויות2

שהינו חלק מקווריאנס המצב.

במקרה הזה ווקטור המצב המורחב הינו:

xk−1∨k−1a =[ xk−1∨k−1

¿wk−1body ]Pxx

a =[Pxxk−1∨k−1 00 Pww ]

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

χk∨k−1i =f ( χ k−1∨k−1

i , a )

והקווריאנס מתקבל הפעם:

Pk∨k−1=∑i=0

2L

W ci [ χ k∨k−1

i −xk∨k−1 ] [ χk∨k−1i −xk∨k−1 ]T+[0 … 0

⋮ ~Paa ⋮0 … 0]

רעש גירוסקופים אדיטיבי

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

כזה אין צורך בהרחבת וקטור מצב, כלומר:

xk−1∨k−1a =xk−1∨k−1P xx

a =Pxx k−1∨k−1

Tבעדכון של זוויות מופיע מחובר ang ,k−1B→N ∙w body

¿ ∙ dtנעביר את מהירות הזוויתית הנמדדת כפרמטר .

לטרנספורמציה:

χk∨k−1i =f ( χ k−1∨k−1

i , a ,w )

Tונחבר קווריאנס של ang ,k−1B→N ∙w body

¿ ∙ dt :לקווריאנס המצב יחד עם קווריאנס של תאוצות

19

~Pww=T ang ,k−1B →N ∙Pww∙ (T ang ,k−1

B→N )T ∙ dt 2

Pk∨k−1=∑i=0

2L

W ci [ χ k∨k−1

i −xk∨k−1 ] [ χk∨k−1i −xk∨k−1 ]T+[0 …

⋮ ~Paa

… 0⋱ ⋮

⋮ ⋱0 …

~Pww ⋮… 0

]scaling factorהתחשבות ב-ג.

(.scaling factorכמו שכתבנו בחלק המבוא מדידות הסנסורים יכולות לסבול משגיאת קנה מידה )

נממש מערכת ניווט שמתחשבת בשגיאות אלה. נשמור שיערוך של קנה מידה בווקטור המצב

.(7)ונשתמש בו כדי לקזז את השגיאות בשלב חיזוי מצב ווקטור המצב יהיה:

xk=[positio nNED3x 1

velocit yxyz3x 1

angle sNED3x 1

bia sa3 x1

bia sw3 x1

scalea3x 1

scalew3x 1

distance

]N=22כעט אורך הווקטור

התחשבות בשגיאות היסט וקנה מידה:

abody¿ =(1+scalea , k−1 )∙ abody

raw +biasa ,k−1

wbody¿ =( 1+scalew, k−1 )∙wbody

raw +biasw , k−1

שאר הטרנספורמציה ללא שינוי.

בדומה לשערוך היסט עדכון של שערוך קנה מידה נעשה יחד עם תיקון המצב.

20

21

תוצאות.6

על מנת להשוות בין מימושים השתמשנו בשני סוגי קלט:

כצירוף פונקציות הרמוניותMATLABקלט המחושב לפי מסלול מלאכותי שאותו יצרנו ב--

של זמן.

קלט האמיתי שהוא מידידות מדי תאוצה, גירוסקופים ואודומטר.-

קלט אמיתי כבר מכיל בתוכו רעשים ושגיאות למיניהם, אנחנו משערכים שונות הרעש ושגיאת

(, בנוסף0היסט של סנסורים לפי פרק זמן בתחילת מסלול שבו הרכב עומד במקום )פלט אודומטר

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

הצורך )רעש סנסורים, שגיאת היסט, שגיאת קנה מידה וקוונטיזצית אודומטר( כאשר סדר גודל של

השגיאות נקבע בדומה לקלט אמיתי.

מסלוליםא.

השתמשנו במסלולים הבאים:

untira1

מסלול אמיתי, השערוך ללא הגברת רעש ומפה של אזור:

22

ק''מ 7.4אורך מסלול:

רעש משוערך )סטיית תקן(:

g 0.033מדי תאוצה:

sec °/0.21גירוסקופים:

unnis3

מסלול אמיתי, השערוך ללא הגברת רעש:

מטר285אורך מסלול:

רעש משוערך )סטיית תקן(:

g 0.089מדי תאוצה:

sec °/0.89גירוסקופים:

:)מסלולים מלאכותיים )סימולצייה

23

השוואה בין מימושים – סיבוכיותב.

כדי להעריך מימושים עם סיבוכיות שונה נשווה בין שגיאת שערוך המסלול וזמן ביצועים ב-

Matlab( נבנה גרפים של שגיאת השערוך .RMSE( כפונקציה של רעש הסנסורים )מוסיפים רעש

לבן לקלט עם סטיית תקן שונה(.

: untira1 עבור מסלול אמיתי

Figure 6: RMSE dependency on noise (untira1)

השוואת מסלולים עבור רמת רעש נמוכה ורמה גבוהה:

Figure 7: route estimation with higher noise (untira1)

24

(sec °/0.21, גירוסקופים: g 0.033 – עבור שגיאה מקורית )מדי תאוצה: אדוםמסלול

(sec °/1.01, גירוסקופים: g 0.105 – עבור שגיאה מוגברת )מדי תאוצה: כחולמסלול

29.5 [m](: RMSEהבדל בין מסלולים )

: unnis3 עבור מסלול אמיתי

Figure 8: RMSE dependency on noise (unnis3)

השוואת מסלולים עבור רמת רעש נמוכה ורמה גבוהה:

Figure 9: route estimation with higher noise (unnis3)

25

(sec °/0.89, גירוסקופים: g 0.089 – עבור שגיאה מקורית )מדי תאוצה: אדוםמסלול

(sec °/1.33, גירוסקופים: g 0.13 – עבור שגיאה מוגברת )מדי תאוצה: כחולמסלול

0.25 [m](: RMSEהבדל בין מסלולים )

השוואה בין מימושים – קנה מידהג.

ללא תוספת רעש אך עם הכפלה שלuntira1הרצנו מערכות שלנו על נתונים של המסלול האמיתי

)מימוש בסיסי(,RBUKFפלט הסנסורים בפקטורים אקראיים. המימושים שבדקנו היו:

RBAUKF ,)מימוש עם הורדת סיבוכיות( RBUKF ,עם התחשבות בשגיאת קנה מידה RBAUKF

עם התחשבות בשגיאת קנה מידה.

בדקנו בנפרד עבור שגיאת קנה מידה בתאוצות ועבור שגיאת קנה מידה בגירוסקופים.

עבור תאוצות:

Figure 10: scaling factor in acceleration (untira1)

עבור גירוסקופים:

26

Figure 11: scaling factor in angular rate (untira1)

ראינו שעבור ערכים קטנים של שגיאת קנה מידה המימושים שלא מתחשבים בה נותנים תוצאות

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

את המערכת שלנו על קלט מלאכותי )כי בו קל לשלוט ברמת הרעש(. עבור ערכים שונים של רעש

סנסורים ושגיאת קנה מידה בתאוצות.

(:sec °/0.01, גירוסקופים: g 0.001עבור רעש קטן )מדי תאוצה:

Figure 12: scaling factor with low noise

27

(:sec °/0.1, גירוסקופים: g 0.01עבור רעש בינוני )מדי תאוצה:

Figure 13: scaling factor with medium noise

(:sec °/1, גירוסקופים: g 0.1עבור רעש גדול )מדי תאוצה:

Figure 14: scaling factor with high noise

רואים שרמת שגיאת קנה מידה שעבורה כדאי להתחשב בקנה מידה תלויה ברמת רעשים אחרים

במערכת.

28

סיכום מסקנות.1

המסוגלת להתמודד עם רעשיRBUKFמימשנו מערכת ניווט אינרציאלי מבוסס על •

חיישנים

שיטת רעש אדיטיבי מאפשרת להוריד סיבוכיות המערכת ללא הפסד מהותי בדיוק•

השערוך

)רעש תאוצה אדיטיבי(2מימוש –

17%שיפור זמן כ- •

שגיאה זהה•

– רעש כל החיישנים אדיטיבי(RBAUKF )3מימוש –

35%שיפור זמן כ- •

שגיאה קרובה•

יש יתרון בשערוך שגיאת קנה מידה רק עבור ערכים גדולים שלה•

מימושים ללא שיערוך קנה מידה:–

מתנהגים דומה למצב של רעש מוגבר•

מימושים עם שיערוך קנה מידה:–

עבור ערכים קטנים של שגיאת קנה מידה )יחסית לרעשים אחרים(•

שגיאת השערוך גדלה

הסבר: סדר מודל גדול מדי יכול לספק הפרש קטן בין מדידה•

אמיתית ומשוערכת גם עבור הבדל גדול במצב

עבור ערכים גדולים של שגיאת קנה מידה שגיאת השערוך קטנה לעומת•

מימושים בסיסיים וכמעט לא גדלה עם הגדלת ערך קנה מידה

הסבר: שיערוך קנה מידה נעשה קל יותר כי שגיאת קנה מידה•

בולטת

מערכת רגישה יותר לשגיאת קנה מידה של גירוסקופים–

שיפורים אפשריים לעבודות בעתיד:•

יש חלק ליניארי )למשל תיקון היסט(sigma pointsבטרנספורמציה שמבצעים על –

שמאפשר הורדת סיבוכיות נוספת

קטנה( rollניתן לשקול הנחות מודל נוספות לשלב השוואת מדידה )כגון זווית –

29

אפשר לחקור השפעה של פרמטרי המערכת שלקחנו כקבועים על דיוק השערוך–

(.UT ב-Ws, Wc)למשל ערכי משקלים

רשימת מקורות.2

Bibliography

1. O. J. Woodman. An introduction to inertial navigation. Technical Report UCAM-CL-TR-696,

University of Cambridge, Computer Laboratory, Aug. 2007.

2. Sherryl H. Stovall Basic Inertial Navigation, Naval Air Warfare Center Weapons Division,

September 1997.

3. Hover, Franz, and Harrison Chin. 2.017J Design of Electromechanical Robotic Systems, Fall

2009. (Massachusetts Institute of Technology: MIT OpenCourseWare), Chapter 9: Kinematics of

Moving Frames.

4. Rudolph van der Merwe and Eric A.Wan, Simon Julier. Sigma-Point Kalman Filters for

Nonlinear Estimation and Sensor-Fusion - Applications to Integrated Navigation.

5. Yanling Hao; Zhilan Xiong; Feng Sun; Xiaogang Wang; , "Comparison of Unscented Kalman

Filters," Mechatronics and Automation, 2007. ICMA 2007. International Conference on , vol., no.,

pp.895-899, 5-8 Aug. 2007.

6. Wikipedia. [Online] http://en.wikipedia.org/.

7. E.H. Shin and N. El-Sheimy, “An Unscented Kalman Filter for In-Motion Alignment of Low-Cost

IMUs,” in Proceedings of the IEEE Frames Conference, (Monterey, California), pp. 273–279, April

2004.

30