ניווט אינרציאלי מבוסס על UNSCENTED KALMAN FILTER

ניווט אינרציאלי מבוסס על ניווט אינרציאלי מבוסס על UNSCENTED UNSCENTED

KALMAN FILTERKALMAN FILTER

מגישות:מגישות:011949286011949286שירלי ליבנה שירלי ליבנה

034719625034719625הילה זלוטו הילה זלוטו

מנחה:מנחה:דר' גבי דוידובדר' גבי דוידוב

נושאי המצגתנושאי המצגת

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

רקע תיאורטירקע תיאורטימבנה מערכת אינרציאליתמבנה מערכת אינרציאלית

סימולציה לכיולסימולציה לכיולמעבר בין מערכות ,השפעת מהירות סיבוב כד"וא ,שלבים במימוש מעבר בין מערכות ,השפעת מהירות סיבוב כד"וא ,שלבים במימוש

UKFUKFהמערכת, הוספת רעשים והשפעתם , מימוש מסנן קלמן המערכת, הוספת רעשים והשפעתם , מימוש מסנן קלמן

סימולציה אמיתיתסימולציה אמיתיתסימולציה אמיתית- מימוש המסלולים, סימולציה אמיתית- מימוש המסלולים,

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

סיכום ומסקנותסיכום ומסקנותתודותתודותביבלוגרפיהביבלוגרפיה

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

נתונה ע"י שימוש  במדידים אינרציאלים מדויקים  נתונה ע"י שימוש  במדידים אינרציאלים מדויקים  UKf -Unscented Kalman UKf -Unscented Kalmanתוך שימוש במשערך תוך שימוש במשערך

FilterFilter

הגדרת הבעיההגדרת הבעיה

IMUIMU= = Inertial Inertialנתון רכב נוסע ממוכשר עם נתון רכב נוסע ממוכשר עם 1.1.measurment unitmeasurment unit גירוסקופים ושלושה גירוסקופים ושלושה 33 שכולל שכולל

מדדי תאוצה , ותפוקות של מד מהירות מדדי תאוצה , ותפוקות של מד מהירות גלגל)אודו מטר(.גלגל)אודו מטר(.

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

דרך הפתרוןדרך הפתרון

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

( ( Pitch,Roll, YawPitch,Roll, Yawתאוצה- עבור כל אחד מהצירים תאוצה- עבור כל אחד מהצירים ופולסי אודומטר.ופולסי אודומטר.

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

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

, הפתרון לכך , הפתרון לכך IMUIMUלשם מזעור השגיאות מייקר את עלות ה לשם מזעור השגיאות מייקר את עלות ה הוא חסימת ההתבדרות ע"י סנסורים נוספים. במקרה הוא חסימת ההתבדרות ע"י סנסורים נוספים. במקרה

שלנו זה מד המהירות גלגל האוטו-אודומטר.שלנו זה מד המהירות גלגל האוטו-אודומטר.

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

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

שהכנסנו.שהכנסנו.

קבלת מסלול אמיתי וע"י שימוש במערכת שבנינו קבלת מסלול אמיתי וע"י שימוש במערכת שבנינושיערוך המסלול – מיקום ואוריינטצית הרכבשיערוך המסלול – מיקום ואוריינטצית הרכב

תיאורטיתיאורטירקערקעIMU=Inertial Measurement UnitIMU=Inertial Measurement Unit

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

רכיב רכיב אחר המכיל חיישני תנועה( אחר המכיל חיישני תנועה(

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

)IMU()IMU(מבנה מערכת אינרציאליתמבנה מערכת אינרציאלית ::

ג'ירוסקופג'ירוסקופ

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

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

הארץ(הארץ(

מבנה הג'ירוסקופ-מבנה הג'ירוסקופ-

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

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

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

הסיבוב נקראת גם 'אינרצית הגירוסקופ'.הסיבוב נקראת גם 'אינרצית הגירוסקופ'.

ג'ירוסקופג'ירוסקופ

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

מכניים- המבוססים על דיסקה מסתובבת.מכניים- המבוססים על דיסקה מסתובבת.

טכנולוגיות נוספות:טכנולוגיות נוספות:

טכנולגיות אופטיות כגון טכנולגיות אופטיות כגוןFOGFOG== Fiber Optic Gyro Fiber Optic Gyro

טכנולגיית טכנולגיית RLG=Ring Laser GyroRLG=Ring Laser Gyro

MEMS- Micro electro Mechanical SystemsMEMS- Micro electro Mechanical Systems

לכולם מטרה זהה- מדידת המהירות האינרציאלית של גוף. לכולם מטרה זהה- מדידת המהירות האינרציאלית של גוף.

מד תאוצה/אקסלומטרמד תאוצה/אקסלומטר

מד תאוצה, חד צירי .מד תאוצה, חד צירי .

מורכב ממסה וקפיץ או ממטוטלת.מורכב ממסה וקפיץ או ממטוטלת.

קיימת משמעות רבה לאוריינטציה.

אודומטראודומטראודומטר הינו מד סיבוביםאודומטר הינו מד סיבובים

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

הסיבובים שנעשו על הגלגל בזמן מסוים. הסיבובים שנעשו על הגלגל בזמן מסוים.

תיאור התהליך- דיאגרמת בלוקים-תיאור התהליך- דיאגרמת בלוקים-

השפעהשפעעלעלבדגשבדגשמערכותמערכותביןביןמעברמעבר

אאכדוכדותת

נבטא את נבטא את NEDNEDלעומת זאת במערכת ה לעומת זאת במערכת ה המדדים באופן הבא:המדדים באופן הבא:

NEDW

x

B y

z

w

W w

w

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

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

::)) BodyBodyה-ה-

שימוש במשוואות הניווט:שימוש במשוואות הניווט:

1 0 sin( )

0 cos( ) sin( )cos( )

0 sin( ) cos( )cos( )BW

��������������

1 ( )sin( ) ( ) cos( )

0 cos( ) sin( )

sin( ) cos( )0

cos( ) cos( )

B

tg tg

W

��������������

המשוואות הבאות קושרות בין מדידות הג'ירוסקופים המשוואות הבאות קושרות בין מדידות הג'ירוסקופים למהירויותלמהירויות

או-או-

נציין שהנ"ל נכון רק לזוויות למוגבלות)עד כ ~ (נציין שהנ"ל נכון רק לזוויות למוגבלות)עד כ ~ (

מעלות הנ"ל מתפוצץ ואז צריך לעבור לקווטרניון. מעלות הנ"ל מתפוצץ ואז צריך לעבור לקווטרניון.9090 קרוב ל- קרוב ל-

90

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

cos ( ) cos( ) cos( ) sin( ) sin( ) sin( ) cos( ) sin( ) sin( ) cos( ) sin( ) cos( )

cos( ) sin( ) cos( ) cos( ) sin( ) sin( ) sin( ) sin( ) cos( ) cos( ) sin( ) sin( )

sin( ) sin( ) cos( ) cos( ) cos

Euler

( )

כך ש- מתארת את הגובהכך ש- מתארת את הגובה

מתארת את הסיבוב מתארת את הסיבוב

מתארת את כיוון התנועה מתארת את כיוון התנועה

שימוש במשוואות אוילר:שימוש במשוואות אוילר:

BODY TO NED TRANSFORMATION-BODY TO NED TRANSFORMATION-levelingleveling

"יישור" מדדי התאוצה "יישור" מדדי התאוצה

0

0

B S B S

S B BN

b BN s

V E V

E C

a C a

g

מאחר וקיבלנו תאוצות בכדי לעבור למיקום מאחר וקיבלנו תאוצות בכדי לעבור למיקום ((north, east, downnorth, east, down נבצע אינטגרציה כפולה ) נבצע אינטגרציה כפולה )

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

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

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

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

אינטגרציה כפולהאינטגרציה כפולה

השפעת מהירות סיבוב השפעת מהירות סיבוב כדור הארץ:כדור הארץ:

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

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

את כל הנקודות בעלות אותו אורך גיאוגרפי.המרידיאן העובר דרך "מצפה את כל הנקודות בעלות אותו אורך גיאוגרפי.המרידיאן העובר דרך "מצפה הכוכבים המלכותי של גריניץ , בסמוך ללונדון, מוגדר קו אורך האפס או הכוכבים המלכותי של גריניץ , בסמוך ללונדון, מוגדר קו אורך האפס או

המרידיאן הראשי(המרידיאן הראשי(

מקביל לממוצע ציר הסיבוב של כדוהא מקביל לממוצע ציר הסיבוב של כדוהאZZ * ציר * ציר

משלים יד ימין אורתוגונלי של המערכת משלים יד ימין אורתוגונלי של המערכתYY * ציר * ציר

יחס הסיבוב של המערכת ביחס למערכת העצמית:יחס הסיבוב של המערכת ביחס למערכת העצמית:

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

של :של :

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

ערכים ערכים – – השפעת גיאומטריית כדו"א הארץהשפעת גיאומטריית כדו"א הארץהתחלתיים התחלתיים

2 2 20 0

0 0

0

0 0

: 1 (1 )* ( )

: 2( , 1)

: 0

( )

parameter a e x y

latitude phi atan z a

height h

p cos phi

השפעת גיאומטריית כדור הארץ – חישוב עבור השפעת גיאומטריית כדור הארץ – חישוב עבור כל נקודה:כל נקודה:

0

2 2

2 2 20

( )( ( 1))

( ( ) ( ) )( ) ( )

( ( 1))

( ) ( ( ) ( ) ) *

( ) 2( ( ), ( ))

( ) 2( ( ), ( ))

( ) ( ( ) ( ))* ( ( ))* ( ( ))

( ) ( ( ) ( ))*fix

fix

pRn i

cos phi i

x i y iH i Rn i

cos phi i

a i x i y i exp p

phi i atan z i a i

lamda i atan y i x i

x i Rn i H i cos phi i cos lamda i

y i Rn i H i c

2

( ( ))* ( ( ))

( ) ( ( ))*( ( ) ( )*(1 ));fix

os phi i sin lamda i

z i sin phi i H i Rn i exp

השפעת כדור הארץ – סיכוםהשפעת כדור הארץ – סיכום

בשלב מוקדם של הפרויקט עשינו סימולציה. בשלב מוקדם של הפרויקט עשינו סימולציה.

כאשר אין השפעת כדו"א המסלול התקבל הוא כאשר אין השפעת כדו"א המסלול התקבל הוא מעגלי. ברגע שנוסיף את השפעת סיבוב כדו"א מעגלי. ברגע שנוסיף את השפעת סיבוב כדו"א

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

שלבים במימוש המערכתשלבים במימוש המערכת

מודלים שפותחומודלים שפותחו

IMU to NED )North East Down(IMU to NED )North East Down( NED to IMUNED to IMUרעשים מתווספיםרעשים מתווספיםסימולטור שיערוך מבוסססימולטור שיערוך מבוסס UKFUKF

מודלים מוכנים שנעזרנו בהםמודלים מוכנים שנעזרנו בהם

משערך משערךUKFUKFרעשים מתווספים- מודלים קיימים מהרעשים מתווספים- מודלים קיימים מה MATLABMATLAB

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

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

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

קיבלנו את אותן תוצאות.קיבלנו את אותן תוצאות.

ואכן קיימת התאמה

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

הגורמים לאי דיוק במדידה.הגורמים לאי דיוק במדידה.

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

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

להלן התוצאות:להלן התוצאות:

הוספת רעשים והשפעתם-הוספת רעשים והשפעתם-

::ללא תוספת מהירות סיבוב כדור הארץללא תוספת מהירות סיבוב כדור הארץ

::עם תוספת מהירות סיבוב כדור הארץעם תוספת מהירות סיבוב כדור הארץ

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

UKFUKFמימוש מסנן קלמן מימוש מסנן קלמן

במקרה שלנו, המודל אינו לנארי, לכן לא ניתן להשתמש במקרה שלנו, המודל אינו לנארי, לכן לא ניתן להשתמש

במסנן רגיל. במסנן רגיל.

קיימים שני סוגים שונים עקריים לסינון הרעש: קיימים שני סוגים שונים עקריים לסינון הרעש:

Extended Kalman FilterExtended Kalman Filter א. א.

Unscented Kalman FilterUnscented Kalman Filterב. ב.

– למה לא?? – למה לא??Extended Kalman FilterExtended Kalman Filterמסנן מסנן

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

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

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

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

שיערוך שיערוך המצב הבא המצב הבא

– מימשנו – מימשנוUnscented Kalman FilterUnscented Kalman Filterמסנן מסנן כאשר המודל לא לינארי במיוחד ה כאשר המודל לא לינארי במיוחד הEKFEKF נותן נותן

תוצאות תוצאות מאד לא מדויקות. הסיבה לכך היא שמטריצת מאד לא מדויקות. הסיבה לכך היא שמטריצת

השונות השונות נוצרת דרך לינאריזציה לא המודל הלא לינארי)פיתוח נוצרת דרך לינאריזציה לא המודל הלא לינארי)פיתוח

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

דגימה דטרמינסטית- דגימת מס מינימלי של נקודות דגימה דטרמינסטית- דגימת מס מינימלי של נקודות ( ( Sigma PointsSigma Points דגימה סביב הממוצע.) דגימה סביב הממוצע.)

צפיפות ההסתברויות משוערכת ע"י משתנים צפיפות ההסתברויות משוערכת ע"י משתניםאקראים, אקראים,

מה שמחזיר ערכים הרבה יותר מדויקים מאשר מה שמחזיר ערכים הרבה יותר מדויקים מאשר פיתוח פיתוח

טיילור מסדר ראשון של הפונקציה הלא לינארית של טיילור מסדר ראשון של הפונקציה הלא לינארית של EKFEKF הגירסא היותר יציבה של הגירסא היותר יציבה שלEKFEKF למעשה זה שידרוג )למעשה זה שידרוג(

( ( EKFEKFשל של

מודל המערכתמודל המערכת

ZZ ווקטור המדידה: תאוצה, מדידות הגירו', אורך – ווקטור המדידה: תאוצה, מדידות הגירו', אורך –

המסלול,פולסי האודומטר המסלול,פולסי האודומטר

XXווקטור המצב – ווקטור המצב –

PPמטריצת השונות המשותפת – מטריצת השונות המשותפת –

wwרעש המערכת – רעש המערכת –

QQקווריאנס של רעש המערכת– קווריאנס של רעש המערכת –

vv)רעש לבן )רעש המדידה( – רעש לבן )רעש המדידה –

RRקווריאנס של רעש המדידה –קווריאנס של רעש המדידה–

~ (0, )kW N Q

~ (0, )kV N R

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

sigma sigmaיצירת סט וקטורי מצב על ידי הוספת ה- יצירת סט וקטורי מצב על ידי הוספת ה- 1.1.pointspoints לוקטור מצב הנוכחילוקטור מצב הנוכחי ((xxii(k(k-1-1))))

במודל לקבלת סט במודל לקבלת סטxxii(k-1)(k-1)העברת וקטור של הסט העברת וקטור של הסט 2.2.

( ( xxii(k(k| | k-1)k-1)))

)xxii)k | k-1()k | k-1 . חישוב ממוצע וקווריאנס של הסט. חישוב ממוצע וקווריאנס של הסט33

1| 11| 1 [ [ ]]

Ta T T

k kk k kx x E w

1| 1

1| 1

0

0

k kak k

k

PP

Q

חיזוי וקטור המצב- משוואות עיקריותחיזוי וקטור המצב- משוואות עיקריות

נקודות דגימה : נקודות דגימה :2L+12L+1סט של סט של

01| 1 1| 1

1| 1 1| 1 1| 1

1| 1 1| 1 1| 1

( ( ) ) ) 1,2...

( ( ) ) ) , 1...2

ak k k k

i a ak k k k k k i

i a ak k k k k k i L

x

x L P i L

x L P i L L L

נגדיר:נגדיר:

חיזוי וקטור המצב- משוואות עיקריות חיזוי וקטור המצב- משוואות עיקריות )המשך()המשך(

2

| 1 | 10

Li i ik k s k k

i

x W

| 1 1| 1( ) 0,1..2i ik k k kf i L

לנקודות הדגימה מתוארת כך: לנקודות הדגימה מתוארת כך:ff פונקצית המעבר פונקצית המעבר

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

2

| 1 | 1| 1 | 1 | 10

[ ][ ]L

i i i Tk k k kk k c k k k k

i

P W x x

חיזוי וקטור המצב-משוואות עיקריות)המשך(חיזוי וקטור המצב-משוואות עיקריות)המשך(

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

0

0 2

2

(1 )

1

2( )

( )

s

c

i is c

WL

WL

W WL

L L

,כאשר הם קבועים.כאשר הם קבועים. ,

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

המצב הבא ומטריצת הקווריאנס יחושבו בדיוק באותו המצב הבא ומטריצת הקווריאנס יחושבו בדיוק באותו אופן,אופן,

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

הקווריאנס של המדידות הרועשות הקווריאנס של המדידות הרועשות

1| 11| 1 [ [ ]]T

a T Tk kk k kx x E v

1| 1

1| 1

0

0

k kak k

k

PP

R

נקודות דגימה : נקודות דגימה :2L+12L+1כמו קודם , סט של כמו קודם , סט של

עידכון וקטור המצב-משוואות עיקריותעידכון וקטור המצב-משוואות עיקריות

01| 1 1| 1

1| 1 1| 1 1| 1

1| 1 1| 1 1| 1

( ( ) ) ) 1, 2...

( ( ) ) ) , 1...2

ak k k k

i a ak k k k k k i

i a ak k k k k k i L

x

x L P i L

x L P i L L L

-Observation Function-Observation Function ::נעביר את נקודות הדגימה דרךנעביר את נקודות הדגימה דרךhh

| 1( ) 0,1..2i ik k kh i L

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

מוכפלות במשקלים :מוכפלות במשקלים :

עידכון וקטור המצב-משוואות עידכון וקטור המצב-משוואות עיקריות)המשך(עיקריות)המשך(

2

0

Li i ik s k

i

z W

2

0

[ ][ ]k k

Li i i T

k kz z c k ki

P W z z

חישוב מטריצת הקרוסקווריאנס:חישוב מטריצת הקרוסקווריאנס:

2

| 1| 10

[ ][ ]k k

Li i i T

kk kx z c k k ki

P W x z

עידכון וקטור המצב-משוואות עידכון וקטור המצב-משוואות עיקריות)המשך(עיקריות)המשך(

מטריצת הקרוסקווריאנס כפול מטריצת הקווריאנס מטריצת הקרוסקווריאנס כפול מטריצת הקווריאנס ::UKFUKF יגדירו את מטריצת ההגבר של יגדירו את מטריצת ההגבר של

1

k k k kk x z z zK P P

ולבסוף...ולבסוף...

| | 1

| | 1

( )

k k

kk k k k k k

Tk k k k k z z k

x x K z z

P P K P K

תוצאות סימולציה תיאורתית- בלי מסנן קלמןתוצאות סימולציה תיאורתית- בלי מסנן קלמןהוספת רעשיםהוספת רעשים

ללא מסנן קלמןללא מסנן קלמן

עם התחשבות בתאוצת כדו"אעם התחשבות בתאוצת כדו"א

תוצאות סימולציה תיאורתית- עם מסנן קלמןתוצאות סימולציה תיאורתית- עם מסנן קלמןהוספת רעשיםהוספת רעשים

עם מסנן קלמןעם מסנן קלמן

עם התחשבות במהירות סיבוב כדו"אעם התחשבות במהירות סיבוב כדו"א

ואכן נראה שקיימת התאמה כמעט

מוחלטת עם שימוש במסנן קלמן

תוצאות סימולציה תיאורתית- עם ובלי מסנן קלמןתוצאות סימולציה תיאורתית- עם ובלי מסנן קלמןהוספת רעשיםהוספת רעשים

ללא התחשבות במהירות סיבוב כדו"אללא התחשבות במהירות סיבוב כדו"א

עם שימוש במסנן קלמןללא שימוש במסנן קלמן

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

פרמטרים כמפורט: פרמטרים כמפורט:99עבור כל אחד מהמסלולים התקבלו עבור כל אחד מהמסלולים התקבלו

ג'ירו ציר ג'ירו צירXX ]מעלות לשנייה[ ]מעלות לשנייה[ ג'ירו ציר ג'ירו צירYY ]מעלות לשנייה[ ]מעלות לשנייה[ ג'ירו ציר ג'ירו צירZZ ]מעלות לשנייה[ ]מעלות לשנייה[

מד תאוצה ציר  מד תאוצה ציר XX ]מעלות לשנייה[ ]מעלות לשנייה[ מד תאוצה ציר מד תאוצה צירYY ]מעלות לשנייה[ ]מעלות לשנייה[ מד תאוצה ציר מד תאוצה צירZZ ]מעלות לשנייה[ ]מעלות לשנייה[

אורך המסלול )על מנת לעבור ליחידות של מטר יש לכפול אורך המסלול )על מנת לעבור ליחידות של מטר יש לכפול((0.38460.3846פי-פי-

פולסי אודומטרפולסי אודומטר

שניות שניות0.0050.005זמן דגימה זמן דגימה

מסלולים עם מסנן קלמןמסלולים עם מסנן קלמן

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

ואדומטר- מורעשים.ואדומטר- מורעשים.

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

2מסלול 1מסלול

עמידות האלגוריתם- בדיקת ספי עמידות האלגוריתם- בדיקת ספי התבדרות עבור מסלולים אמיתייםהתבדרות עבור מסלולים אמיתיים

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

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

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

מדידות האודומטר הינן סנסור נוסף שעוזר לדיוק.מדידות האודומטר הינן סנסור נוסף שעוזר לדיוק.

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

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

2

2

_ 0.5

_ ( 8)

a

w

Bias var

Bias var e

:: הרעשים שהוספו הינםהרעשים שהוספו הינם

11מסלול מסלול : :

22מסלול מסלול : :

2

_ 2

_ (1 )

a

w

Bias var

Bias var e

רעשים במערכת - קבוע רעש גבוה מדי, רעשים במערכת - קבוע רעש גבוה מדי, המערכת מתבדרתהמערכת מתבדרת

רעשים במערכת קבוע רעש נורמלי- רעשים במערכת קבוע רעש נורמלי- --11מסלול מסלול המערכת עדיין יציבההמערכת עדיין יציבה

רעשים במערכת קבוע רעש גבוה מדי, רעשים במערכת קבוע רעש גבוה מדי, --11מסלול מסלול המערכת מתבדרתהמערכת מתבדרת

המערכת המערכת מתבדרת מתבדרת

:עבורעבור

2

2

_ 0.5

_ ( 8)

a

w

Bias var

Bias var e

רעשים במערכת קבוע רעש רעשים במערכת קבוע רעש --22מסלול מסלול נורמלי-המערכת עדיין יציבהנורמלי-המערכת עדיין יציבה

רעשים במערכת קבוע רעש גבוה מדי, רעשים במערכת קבוע רעש גבוה מדי, --22מסלול מסלול המערכת מתבדרתהמערכת מתבדרת

המערכת המערכת מתבדרת מתבדרת

:עבורעבור2

_ 2

_ (1 )

a

w

Bias var

Bias var e

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

הבאים- הבאים-

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

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

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

בשלב השלישי הוספנו רעשים למערכתבשלב השלישי הוספנו רעשים למערכת בשלב הרביעי ממישנו מסנן קלמן בשלב הרביעי ממישנו מסנן קלמןUKFUKF ובכך התגברנו על ובכך התגברנו על

הרעשיםהרעשים

לבסוף הרצנו סימולציה אמיתית ובחנו את התוצאות עפ"י לבסוף הרצנו סימולציה אמיתית ובחנו את התוצאות עפ"י

האלגוריתם שיצרנו. האלגוריתם שיצרנו.

מסקנותמסקנות

במערכת שיצרנו )שכללה את חיבור המע' הישירה במערכת שיצרנו )שכללה את חיבור המע' הישירה

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

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

עבור מקדמי רעש מסוימים המערכת העבור מקדמי רעש מסוימים המערכת הUKFUKF מתמודד מתמודד היטב היטב

עם הרעשים כפי שראינו בפרויקט,עבור מקדמי רעש עם הרעשים כפי שראינו בפרויקט,עבור מקדמי רעש גבוהים גבוהים

יותר המע' מתבדרת מאחר והרעש גובר על האות. יותר המע' מתבדרת מאחר והרעש גובר על האות.

מסקנותמסקנות

למערכת ניווט אינרציאלי קיימים חסרונות בלתי מבוטלים:למערכת ניווט אינרציאלי קיימים חסרונות בלתי מבוטלים:

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

פתוח,סובלת משגיאה מצטברת שהולכת וגדלה ביחס פתוח,סובלת משגיאה מצטברת שהולכת וגדלה ביחס

ישר לזמן שחלף מהפעלת המערכת. ישר לזמן שחלף מהפעלת המערכת.

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

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

המערכת. המערכת.

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

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

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

זמן ומצטרפת לטעויות חדשות. זמן ומצטרפת לטעויות חדשות.

)מע' ניווט אינארצילת בסיסית )גירוסופ ומד תאוצה( מע' ניווט אינארצילת בסיסית )גירוסופ ומד תאוצה אינה אינה

מספיקה למתן תוצאות מדויקות לרוב, בגלל שגיאות מספיקה למתן תוצאות מדויקות לרוב, בגלל שגיאות נפוצות נפוצות

בחיישנים. קיימים מס' פתרונות לבעיה זו, בניהם – כיול בחיישנים. קיימים מס' פתרונות לבעיה זו, בניהם – כיול

המע' )יקר( אנחנו בחרנו להשתמש בחיישנים נוספים המע' )יקר( אנחנו בחרנו להשתמש בחיישנים נוספים

)אודומטר( )אודומטר(

שימוש בניווט באמצעות שימוש בניווט באמצעותExtended Kalman FilterExtended Kalman Filter ( ( EKFEKF.).)

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

UKFUKF נחשב מדויק יותר( אך יחד עם זאת המסנן מאד נחשב מדויק יותר( אך יחד עם זאת המסנן מאד קשה קשה

. למימוש.זהו פתרון חלופי אך לא אידיאלי למימוש.זהו פתרון חלופי אך לא אידיאלי

שימוש ב שימוש ב GPSGPS שגם לו חסרונות רבים- לא מתפקד )שגם לו חסרונות רבים- לא מתפקד(באיזורים באיזורים

סגורים, בזמן עמידה או התקדמות איטית עלולה להיות סגורים, בזמן עמידה או התקדמות איטית עלולה להיות שגיאה שגיאה גדולה( גדולה(

מסקנות- פתרונות נוספיםמסקנות- פתרונות נוספים

תודות...תודות...

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

ולבסוף נרצה להודות תודה מיוחדת לגבי דבידוב ולבסוף נרצה להודות תודה מיוחדת לגבי דבידוב על התמיכה, ההקשבה ,העזרה המקצועית הרבה על התמיכה, ההקשבה ,העזרה המקצועית הרבה

והזמינות.והזמינות.

ביבלוגרפיה...ביבלוגרפיה...

Estimation Techniques for low cost Inertial Estimation Techniques for low cost Inertial NavigationNavigation - -By Eun-Hwan Shin

Sequence Unscented Kalman Filtering AlgorithmSequence Unscented Kalman Filtering Algorithm By Hui-ping Li, De-min Xu ,Jiang Li jun and Fu-bin Zhan

Comparison of Uncsented Kalman FiltersComparison of Uncsented Kalman Filters By Yanling Hao, Zhilan Xiong and Feng Sun , College of Automation and Xiaogang Wang , Common Competence Center

Vehicle Model Aided Inertial Navigation By Xin Ma, Salah Sukkarieh and Jong-Hyuk Kim

A Quaternion-Based Orientation Estimation A Quaternion-Based Orientation Estimation Algorithm Using an Inertial Measurement UnitAlgorithm Using an Inertial Measurement Unit By Anthony Kim- University of waterloo

Basic Inertial Navigation-Basic Inertial Navigation- By Sherryl H.Stovall

An Unscented Kalman Filter for In-Motion An Unscented Kalman Filter for In-Motion Alignment of low cost IMU Alignment of low cost IMU By EUN –Hwan Shin and Naser El- Sheimy

ביבלוגרפיה...ביבלוגרפיה...

סוף המצגתסוף המצגת תודה על ההקשבהתודה על ההקשבה