Kalman Filter فيلتر كالمن در سال 1960 توسط R.E.Kalman در مقاله اي تحت عنوان زير معرفي شد. “A new approach to liner filtering & prediction problem” Transactions of the ASME-Journal of Basic Engineering, Vol 82, pp 35-45, March 1960. 1

Kalman Filter, Problem definition The Kalman filter is an efficient recursive filter that estimates the state of a dynamic system from a series of incomplete and noisy measurements. فیلتر کالمن یک فیلتر بازخوردی کار آمد است که وضعیت یک سیستم دینامیک را با استفاده از یک سری اندازه گیری های ناقص و نویز دار انجام میدهد.

Kalman Filter, Problem definition فرض كنيد ميخواهيم يك شئي متحرك را در يك محيط نويزي ردگيري كنيم. ولي بعلت نويزي بودن و يا دلايل ديگر كه ناشي از سيستم تصوير برداري ، سرعت و جهت حركت شئي متحرك است ،‌ شئي متحرك مورد نظر بدرستي قابل تشخيص نيست.

Kalman Filter, Problem definition از مشاهده اي كه توسط سيستم تصوير برداري بدست مي آيد ،‌ يك بردار ويژگي بنام Z درست ميكنيم. با استفاده از اين بردار ويژگي Z (مشاهده) ميخواهيم شئي متحرك مورد نظر را بصورت يك بردار ويژگي X مدل كنيم. لازم به ياد آوري است كه بردار هاي ويژكي X, Z ممكن است از نظر ابعاد با هم مساوي نباشند.

Kalman Filter فيلتر کالمن شامل دو مرحله prediction و correction است که به طور تناوبی تکرار می شوند prediction correction

Kalman Filter در شرايطي كه مشاهده ما از طريق دنباله اي از تصاوير (image sequences) صورت ميگيرد ،‌ مدل ها و مشاهدات خود را ميتوانيم به دو روش زير مورد استفاده قرار دهيم: شماره فريم و يا مفهوم زمان استK 7

Kalman Filter در فيلتر كالمن يك مكانيزم باز خوردي (Feedback) پيشنهاد ميشود كه توسط آن ميتوانيم : Zk را مشاهده كنيم،‌ Xk (مدل) را تقريب بزنيم ، Xk+1 را پيش بيني كنيم ،‌ و بنا بر اين ،‌ Zk+1 را پيش بيني كنيم و سپس Zk+1 را مشاهده نمائيم. با استفاده از مشاهدات و پيش بيني هاي فوق ، Xk+1 (وضعيت مدل در زمان k+1 ) را update ‌ ميكنيم.

Kalman Filter سيكل تكرار در فيلتر كالمن: Time Update : رويداد ها را درفريم بعدي پيش بيني ميكند. Measurement Update : با استفاده از مشاهدات انجام شده در فریم حاضر پيش بيني هاي انجام شده در فریم قبلی (برای فریم حاضر) را اصلاح ميكند.

Kalman Filter فيلتر كالمن كه بطور گسترده اي در كاربرد هاي ردگيري بكار ميرود فرض ميكند كه سيستم مورد بر رسي يك سيستم خطي است. يعني : الف - مشاهدات با استفاده از توابع خطي از شرايط مورد بر رسي بدست مي آيند. ب - نويز در سيستم و در اندازه گيري ،‌ از نوع نويز گوسين فرض میشود.

Kalman Filter 11

Kalman Filter Wk ~ N (0, Qk) Process noise with 0 mean and covariance of Qk, Vk ~ N (0, Rk) Observation noise with 0 mean and covariance of Rk Covariance of observation noise at frame k 12

Kalman Filter 13

Kalman Filter 14

Kalman Filter 15

Kalman Filter 16

Kalman Filter تقريب اوليه قبل از مشاهده 17

Kalman Filter Kalman gain at frame k كواريانس نويز در مشاهده zk 18

Kalman Filter, An example

Kalman Filter, An example 20

Kalman Filter 21

Kalman Filter 22

Kalman Filter r : واريانس نويز اسكالر ثابت x 23

Kalman Filter 24

Kalman Filter 25

Kalman Filter یاد آوری روابط: Wk يك بردار نويز با ميانگين صفر و كواريانس Qk براي پيش بيني در فريم k Vk يك بردار نويز با ميانگين صفر و كواريانس Rk بر روي متغيير اندازه گيري شده در فريم k Qk : ماتريس كوواريانس نويز اعمال شده بر پيش بيني Xk 26

Kalman Filter 27

Kalman Filter 28

یک مثال ساده از مدلی که در فضای دوبعدی حرکت میکند.

مرحله اول : تعريف سيستم تعريف بردار حالت فرض کنيد می خواهيم يک نقطه را که در فضای دوبعدی با سرعت ثابت حرکت می کند، ردگيری کنيم. مدل را به صورت بردار ویژگی زير تعريف می کنيم و مراحل زير را دنبال می کنيم: مختصات شئی مولفه های سرعت شئی در راستای عمودی و افقی

مرحله اول : تعريف سيستم تعريف ماتريس انتقال حالت: فرض کنيم فاصله زمانی بین دو فریم متوالی مساوی باشد. آنگاه ماتريس انتقال حالت سيستم بصورت ذيل در خواهد آمد: توجه كنيد كه ماتريس A يك ماتريس انتقال يا Translation ‌ است كه با ضرب شدن در مقدار بردار ويژگي (مدل) در فريم k‌ ، تقريب بردار ويژگي (مدل) را در فريم بعدي يعني فريم k+1 بدست ميدهد. ماتریس A مفهوم زمان و بردار X مکان شئی و سرعت آنرا میدهد. بنابراین حاصل ضرب A X (زمان ضربدر سرعت میزان جابجائی را تعیین مینماید).

مرحله اول : تعريف سيستم تعریف ماتریس H ، ارتباط مشاهده به مدل را نشان میدهد. بردار ویژکی مشاهده ، در حقيقت از متغيرهای بدست آمده از اندازه گيری مکان هدف در فضای دوبعدی ، در فريم های متوالی بدست می آيد. با اين تعريف ماتريس H بصورت ذيل خواهد بود.

مرحله اول : تعريف سيستم با توجه به رابطه فوق ابعاد ماتریس H بصورت زیر تعیین شده است.

مرحله اول : تعريف سيستم با توجه به رابطه فوق پیش بینی مدل در فریم بعدی:

مرحله دوم : مقداردهی اوليه مقداردهی اوليه ماتریس حالت ، A مقداردهی اوليه ماتريس کواريانس خطای مشاهده Pk مقداردهی ماتريس کواريانس نويز فرايند پیش بینی مدل Qk مقداردهی ماتريس کواريانس نويز اندازه گيری (مشاهده) Rk

مرحله سوم: prediction در هر مرحله از prediction طبق روابط زير ، حالت بعدی سيستم ، و مشاهده بعدی آن تخمين زده می شود. تقريب كواريانس خطاي مشاهده در فريم k Qk ماتريس كوواريانس نويز اعمال شده بر پيش بيني Xk

مرحله سوم: تصحيح براساس مشاهده با رويت مشاهده واقعی حالت تخمينی و عدم قطعيت آن بصورت زير تصحيح می شود: Pk كواريانس خطای تقریب مدل ( Xk) در فريم k Estimate of Error covariance ماتريس كو واريانس نويز بر روي مشاهده Kalman Gain و روند فوق ( predict و correct ) متناوبا تا اتمام همه مشاهدات تکرار می شود

مرحله سوم: تصحيح براساس مشاهده Kalman Gain و روند فوق ( predict و correct ) متناوبا تا اتمام همه مشاهدات تکرار می شود

Feedback Iteration of Kalman Filter Wk: noise of estimation is assumed to be zero. Matrix A Zi

Feedback Iteration of Kalman Filter Update estimate at frame i Estimate at frame i+1 . . . .

Extended Kalman Filter

فیلتر کالمن توسعه یافته (EKF)  

فیلتر کالمن توسعه یافته  

فیلتر کالمن توسعه یافته شکل 3 – شمای کلی فیلتر کالمن توسعه یافته ]2[