ارائه درس روباتيکز Extended Kalman Filter فريد ملازم استاد مربوطه دکتر شيري دانشگاه امير کبير – دانشکده کامپيوتر و فناوري اطلاعات

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

فيلترينگ عمومي فيلترينگ عمومي فيلترينگ عمومي x is system state vector x is system state vector f is system dynamics f is system dynamics u is control vector u is control vector w is system error sources w is system error sources y is observation vector y is observation vector h is measurement function h is measurement function v is measurement error source v is measurement error source

داده ها و خروجي داده ها : داده ها : f ، h ، ويژگي هاي نويز، وضعيت اوليه f ، h ، ويژگي هاي نويز، وضعيت اوليه مجموعه متغيرهاي کنترلي u(0) ، u(1) ، …، u(k-1) مجموعه متغيرهاي کنترلي u(0) ، u(1) ، …، u(k-1) مجموعه خروجي هاي y(1) ، y(2) ،... ، y(k) مجموعه خروجي هاي y(1) ، y(2) ،... ، y(k) خروجي خروجي بهترين تخمين براي x(k) بهترين تخمين براي x(k)

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

عملکرد سيستم مراحل عملکرد سيستم مراحل عملکرد سيستم x0 داده شده است x0 داده شده است نويز w0 از طبيعت وارد مي شود نويز w0 از طبيعت وارد مي شود u0 به عنوان ورودي به سيستم داده مي شود u0 به عنوان ورودي به سيستم داده مي شود سيستم به وضعيت x1 وارد مي شود سيستم به وضعيت x1 وارد مي شود اندازه گيري y1 انجام مي شود اندازه گيري y1 انجام مي شود سوال : بهترين تخمين براي x1 چيست؟ سوال : بهترين تخمين براي x1 چيست؟ پاسخ با توجه به پاسخ با توجه به

عملکرد سيستم معيارهاي بهينه سازي متفاوتي مي توانند انتخاب گردند که به پاسخ هاي متفاوتي ختم گردند معيارهاي بهينه سازي متفاوتي مي توانند انتخاب گردند که به پاسخ هاي متفاوتي ختم گردند mean: مرکز توزيع احتمال، متناظر با معيار MSE mean: مرکز توزيع احتمال، متناظر با معيار MSE mode: مقدار داراي بزرگترين احتمال، متناظر با MAP mode: مقدار داراي بزرگترين احتمال، متناظر با MAP median median

KF: Mean-Square Error KF: Mean-Square Error متناظر با : متناظر با : شکل کلي : شکل کلي : رشته u قطعي است رشته u قطعي است

توزيع گاسي است توزيع گاسي است mean ، mode و median يکسان هستند. mean ، mode و median يکسان هستند. شکل توزيع : شکل توزيع :

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

مراحل انتشار عمليات انتشار در دو مرحله صورت مي گيرد عمليات انتشار در دو مرحله صورت مي گيرد مرحله predict مرحله predict مرحله filtering مرحله filtering

مراحل انتشار به ما مي گويد که در مورد x(k + 1) چه پيش بيني مي توانيم داشته باشيم پيش از اينکه خروجي y(k + 1) را دريافت کرده باشيم. به ما مي گويد که در مورد x(k + 1) چه پيش بيني مي توانيم داشته باشيم پيش از اينکه خروجي y(k + 1) را دريافت کرده باشيم. مرحله filtering به ما نشان مي دهد که چگونه مي توانيم x(k + 1) حاصل از مرحله قبل را با مشاهده y(k + 1) بهبود بخشيم مرحله filtering به ما نشان مي دهد که چگونه مي توانيم x(k + 1) حاصل از مرحله قبل را با مشاهده y(k + 1) بهبود بخشيم

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

مراحل انتشار گام اول گام اول تخمين ( فاز state prediction) تخمين ( فاز state prediction)

مراحل انتشار گام دوم گام دوم تخمين ( فاز Measurement Prediction) تخمين ( فاز Measurement Prediction) در اين مرحله روند محاسبات مانند مرحله state prediction مي باشد. در اين مرحله روند محاسبات مانند مرحله state prediction مي باشد. گام سوم ( فيلترينگ ) گام سوم ( فيلترينگ ) با توجه به اينکه و از ديدگاه اطلاعاتي يکسان هستند داريم : با توجه به اينکه و از ديدگاه اطلاعاتي يکسان هستند داريم :

مراحل انتشار نتيجه گام سوم را مي توانيم به صورت زير بنويسيم نتيجه گام سوم را مي توانيم به صورت زير بنويسيم پارامتر Kalman Gain پارامتر Kalman Gain Filtered state estimate = predicted state estimate + Gain * error Filtered state estimate = predicted state estimate + Gain * error

خلاصه مراحل به طور خلاصه خواهيم داشت : به طور خلاصه خواهيم داشت : Prediction Prediction Filtering Filtering Initial Conditions Initial Conditions

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

بسط به حالت غير خطي در حالت غير خطي در حالت غير خطي Linearize xi about and use Kalman Filter prediction step Linearize xi about and use Kalman Filter prediction step Linearize zi about and use Kalman Filter update step Linearize zi about and use Kalman Filter update step

خلاصه الگوريتم آخرين تخمين را در نظر مي گيريم آخرين تخمين را در نظر مي گيريم رابطه را حول خطي مي کنيم رابطه را حول خطي مي کنيم مرحله prediction را روي سيستم خطي شده اعمال مي کنيم مرحله prediction را روي سيستم خطي شده اعمال مي کنيم رابطه را حول خطي مي کنيم رابطه را حول خطي مي کنيم مرحله update را روي سيستم خطي شده اعمال مي کنيم مرحله update را روي سيستم خطي شده اعمال مي کنيم

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