ارائه درس روباتيکز extended kalman filter فريد ملازم 84131069 استاد...

کزکزييارائه درس روباتارائه درس روبات

Extended Kalman Extended Kalman FilterFilter

د مالزمد مالزمييفرفر8413106984131069

استاد مربوطهاستاد مربوطهييررييدکتر شدکتر ش

اطالعاتيوتر و فناوري دانشکده کامپ–ر ير کبيدانشگاه ام

نگنگييلترلترييفف

نگنگييلترلترييمساله فمساله ف

يينگ عمومنگ عمومييلترلترييففيينگ عمومنگ عمومييلترلترييفف

x is system state vectorx is system state vector f is system dynamicsf is system dynamics u is control vectoru is control vector w is system error sourcesw is system error sources y is observation vectory is observation vector h is measurement functionh is measurement function v is measurement error sourcev is measurement error source

ييداده ها و خروجداده ها و خروج

:داده ها:داده هاff، ، hhههييت اولت اولييز، وضعز، وضعيي نو نويي ها هاييژگژگيي، و، ويي کنترل کنترلييرهارهاييمجموعه متغمجموعه متغ u(0)u(0)، ، u(1)u(1)، ، ……، ، u(k-u(k-

1)1)يي ها هاييمجموعه خروجمجموعه خروج y(1)y(1)، ، y(2)y(2)، ، ... ... ، ، y(k)y(k)

ييخروجخروجيين بران برايين تخمن تخمييبهتربهتر x(k)x(k)

ييمعيار بهينه سازمعيار بهينه ساز

مم کردن خطا با مم کردن خطا با ييننيين متناظر با من متناظر با ميين تخمن تخمييمساله بهترمساله بهترار خاص است.ار خاص است.ييک معک معييتوجه به توجه به

ع احتمال ع احتمال ييک تابع توزک تابع توزييلتر در مرحله لتر در مرحله يي ف فييززييدگاه بدگاه بيياز داز د دهد. دهد.يي را از مراحل قبل انتشار م را از مراحل قبل انتشار مييشرطشرط

ستمستمييعملکرد سعملکرد س

ستمستمييمراحل عملکرد سمراحل عملکرد سx0x0داده شده است داده شده است

ز ز يينونوw0w0شود شودييعت وارد معت وارد ميي از طب از طب u0u0شود شودييستم داده مستم داده ميي به س به سيي به عنوان ورود به عنوان ورود ت ت ييستم به وضعستم به وضعييسسx1x1شود شوديي وارد م وارد م ييرريياندازه گاندازه گ y1y1شود شوديي انجام م انجام م

يين بران برايين تخمن تخمييسوال: بهترسوال: بهتر x1x1ست؟ست؟يي چ چ پاسخ با توجه به پاسخ با توجه به

ستمستمييعملکرد سعملکرد س

توانند توانند يي م ميي متفاوت متفاوتيينه سازنه سازيي به بهييارهاارهاييمعمع ختم ختم يي متفاوت متفاوتييانتخاب گردند که به پاسخ هاانتخاب گردند که به پاسخ ها

گردندگردندmeanmeanار ار ييع احتمال، متناظر با معع احتمال، متناظر با معيي: مرکز توز: مرکز توز

MSEMSEmodemodeن احتمال، متناظر با ن احتمال، متناظر با يي بزرگتر بزرگتريي: مقدار دارا: مقدار دارا

MAPMAPmedianmedian

KFKF: : Mean-Square ErrorMean-Square Error:متناظر با:متناظر باييشکل کلشکل کل::

رشته رشتهuuاست استيي قطع قطع

است است ييگاس گاس ععييتوزتوزmeanmean ، ، modemode و وmedianmedian يي. هستند .کسان هستند کسان

توز توز شکل ع:ع:ييشکل

ععييانتشار توزانتشار توز

ييع نمع نميياز به انتشار کل توزاز به انتشار کل توزيين حالت نن حالت نييدر ادر ا باشدباشد

ابندابنديي ييانس انتشار مانس انتشار ميين و وارن و وارييانگانگييتنها متنها م انتشار در هر مرحله با توجه به اطالعات انتشار در هر مرحله با توجه به اطالعات

ردرديي گ گييمراحل قبل صورت ممراحل قبل صورت م

مراحل انتشارمراحل انتشار

ردرديي گ گييات انتشار در دو مرحله صورت مات انتشار در دو مرحله صورت مييعملعمل مرحله مرحلهpredictpredict مرحله مرحلهfilteringfiltering

مراحل انتشارمراحل انتشار

د که در مورد د که در مورد يي گو گويي به ما م به ما مx(k x(k م م ييم داشته باشم داشته باشيي توان توانيي م مييننييش بش بيي چه پ چه پ(1 +(1 +

افت افت يي را در را درy(k + 1)y(k + 1) يينکه خروجنکه خروجييش از اش از اييپپم.م.ييکرده باشکرده باش

مرحله مرحلهfilteringfilteringدهد که دهد که يي به ما نشان م به ما نشان م حاصل از مرحله حاصل از مرحله x(k + 1)x(k + 1)م م يي توان توانييچگونه مچگونه م

مميي بهبود بخش بهبود بخشy(k + 1)y(k + 1)قبل را با مشاهده قبل را با مشاهده

مراحل انتشارمراحل انتشار

ييع شرطع شرطيي تابع توز تابع توزيي برا براييع گاسع گاسييبا توجه به توزبا توجه به توز م:م:يير را داشته باشر را داشته باشييف زف زييم تعارم تعاريي توان توانييمم

مراحل انتشارمراحل انتشار

گام اولگام اولفاز ييتخمتخم( فاز ن( نstate predictionstate prediction ) )

مراحل انتشارمراحل انتشار

گام دومگام دومن )فاز ن )فاز ييتخمتخمMeasurement Measurement

PredictionPrediction))ن مرحله روند محاسبات مانند مرحله ن مرحله روند محاسبات مانند مرحله ييدر ادر اstate state

predictionpredictionباشد. باشد.يي م م نگ(نگ(ييلترلترييگام سوم )فگام سوم )فدگاه دگاه يينکه و از دنکه و از دييبا توجه به ابا توجه به ا

م: م: ييکسان هستند دارکسان هستند داريي يياطالعاتاطالعات

مراحل انتشارمراحل انتشارممييسسيير بنور بنوييم به صورت زم به صورت زيي توان توانييجه گام سوم را مجه گام سوم را ميينتنت

پارامتر پارامترKalman GainKalman Gain

Filtered state estimate = predicted state Filtered state estimate = predicted state estimate + Gain * errorestimate + Gain * error

خالصه مراحلخالصه مراحل

م داشت:م داشت:ييبه طور خالصه خواهبه طور خالصه خواه PredictionPrediction

FilteringFiltering

Initial ConditionsInitial Conditions

د د بلوک ف ييبلوک ف اگرام نگنگييلترلتريياگرام

غ حالت به غ بسط حالت به خط ييبسط خط ر يير غ حالت غ در حالت خط ييدر خط ر يير

Linearize xi about and use Kalman Linearize xi about and use Kalman Filter prediction step Filter prediction step

Linearize zi about and use Kalman Linearize zi about and use Kalman Filter update stepFilter update step

تمتمييخالصه الگورخالصه الگور

ممييرريي گ گيين را در نظر من را در نظر ميين تخمن تخمييآخرآخريي م مييرابطه را حول خطرابطه را حول خط

ممييکنکن مرحله مرحلهpredictionpredictionشده شده ييستم خطستم خطيي س سيي را رو را رو

مميي کن کنيياعمال ماعمال مييرابطه را حول خطرابطه را حول خط

مميي کن کنييمم مرحله مرحلهupdateupdateشده شده ييستم خطستم خطيي س سيي را رو را رو

م م يي کن کنيياعمال ماعمال م

تمتمييخالصه الگورخالصه الگور

شود شوديير خالصه مر خالصه ميي به صورت ز به صورت زييييتم نهاتم نهاييالگورالگور مرحله مرحلهpredictpredict

مرحله مرحلهupdateupdate