1
فیلتر کالمن در سال 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.
Kalman Filter
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 ممکن است از نظر ابعاد با هم مساوی نباشند.
فیلتر کالمن شامل دو مرحله prediction و correction است که به طور تناوبی تکرار می شوند
Kalman Filter
7
در شرایطی که مشاهده ما از طریق دنباله ای از تصاویر (image sequences) صورت میگیرد ، مدل ها و مشاهدات خود را میتوانیم به دو روش زیر مورد استفاده قرار دهیم:
Kalman Filter
شماره فریم و یا مفهوم زمان استK
Kalman Filter
در فیلتر کالمن یک مکانیزم باز خوردی (Feedback) پیشنهاد میشود که توسط آن میتوانیم :
Zk را مشاهده کنیم،
Xk (مدل) را تقریب بزنیم ،
Xk+1 را پیش بینی کنیم ،
و بنا بر این ، Zk+1 را پیش بینی کنیم
و سپس Zk+1 را مشاهده نمائیم.
با استفاده از مشاهدات و پیش بینی های فوق ، Xk+1 (وضعیت مدل در زمان k+1 ) را update میکنیم.
Kalman Filter
سیکل تکرار در فیلتر کالمن:
Time Update : رویداد ها را درفریم بعدی پیش بینی میکند.
Measurement Update : با استفاده از مشاهدات انجام شده در فریم حاضر پیش بینی های انجام شده در فریم قبلی (برای فریم حاضر) را اصلاح میکند.
Kalman Filter
فیلتر کالمن که بطور گسترده ای در کاربرد های ردگیری بکار میرود فرض میکند که سیستم مورد بر رسی یک سیستم خطی است.
یعنی :
الف – مشاهدات با استفاده از توابع خطی از شرایط مورد بر رسی بدست می آیند.
ب – نویز در سیستم و در اندازه گیری ، از نوع نویز گوسین فرض میشود.
11
Kalman Filter
12
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
13
Kalman Filter
14
Kalman Filter
15
Kalman Filter
16
Kalman Filter
17
Kalman Filter
تقریب اولیه قبل از مشاهده
18
Kalman Filter
کواریانس نویز در مشاهده zk
Kalman gain at frame k
Kalman Filter, An example
20
Kalman Filter, An example
21
Kalman Filter
22
Kalman Filter
23
r : واریانس نویز اسکالر ثابت x
Kalman Filter
24
Kalman Filter
25
Kalman Filter
26
Wk یک بردار نویز با میانگین صفر و کواریانس Qk برای پیش بینی در فریم k
Vk یک بردار نویز با میانگین صفر و کواریانس Rk بر روی متغییر اندازه گیری شده در فریم k
Qk : ماتریس کوواریانس نویز اعمال شده بر پیش بینی Xk
Kalman Filter
یاد آوری روابط:
27
Kalman Filter
28
Kalman Filter
یک مثال ساده از مدلی که در فضای دوبعدی حرکت میکند.
مرحله اول : تعریف سیستم
تعریف بردار حالت
فرض کنید می خواهیم یک نقطه را که در فضای دوبعدی با سرعت ثابت حرکت می کند، ردگیری کنیم. مدل را به صورت بردار ویژگی زیر تعریف می کنیم و مراحل زیر را دنبال می کنیم:
مختصات شئی
مولفه های سرعت شئی در راستای عمودی و افقی
مرحله اول : تعریف سیستم
تعریف ماتریس انتقال حالت:
فرض کنیم فاصله زمانی بین دو فریم متوالی مساوی باشد. آنگاه ماتریس انتقال حالت سیستم بصورت ذیل در خواهد آمد:
توجه کنید که ماتریس A یک ماتریس انتقال یا Translation است که با ضرب شدن در مقدار بردار ویژگی (مدل) در فریم k ، تقریب بردار ویژگی (مدل) را در فریم بعدی یعنی فریم k+1 بدست میدهد. ماتریس A مفهوم زمان و بردار X مکان شئی و سرعت آنرا میدهد.
بنابراین حاصل ضرب A X (زمان ضربدر سرعت میزان جابجائی را تعیین مینماید).
مرحله اول : تعریف سیستم
تعریف ماتریس H ، ارتباط مشاهده به مدل را نشان میدهد.
بردار ویژکی مشاهده ، در حقیقت از متغیرهای بدست آمده از اندازه گیری مکان هدف در فضای دوبعدی ، در فریم های متوالی بدست می آید.
با این تعریف ماتریس H بصورت ذیل خواهد بود.
مرحله اول : تعریف سیستم
با توجه به رابطه فوق ابعاد ماتریس H بصورت زیر تعیین شده است.
مرحله اول : تعریف سیستم
با توجه به رابطه فوق پیش بینی مدل در فریم بعدی:
مرحله دوم : مقداردهی اولیه
مقداردهی اولیه ماتریس حالت ، A
مقداردهی اولیه ماتریس کواریانس خطای مشاهده Pk
مقداردهی ماتریس کواریانس نویز فرایند پیش بینی مدل Qk
مقداردهی ماتریس کواریانس نویز اندازه گیری (مشاهده) Rk
مرحله سوم: prediction
در هر مرحله از prediction طبق روابط زیر ، حالت بعدی سیستم ، و مشاهده بعدی آن تخمین زده می شود.
تقریب کواریانس خطای مشاهده در فریم k
Qk ماتریس کوواریانس نویز اعمال شده بر پیش بینی Xk
مرحله سوم: تصحیح براساس مشاهده
با رویت مشاهده واقعی حالت تخمینی و عدم قطعیت آن بصورت زیر تصحیح می شود:
Pk کواریانس خطای تقریب مدل ( Xk) در فریم k
و روند فوق ( predict و correct ) متناوبا تا اتمام همه مشاهدات تکرار می شود
Kalman Gain
Estimate of
Error covariance
ماتریس کو واریانس نویز بر روی مشاهده
مرحله سوم: تصحیح براساس مشاهده
Kalman Gain
و روند فوق ( predict و correct ) متناوبا تا اتمام همه مشاهدات تکرار می شود
Feedback Iteration of Kalman Filter
Matrix A
Zi
Wk: noise of
estimation is
assumed to be
zero.
Feedback Iteration of Kalman Filter
. . . .
Update estimate at frame i
Estimate at frame i+1
Extended Kalman Filter
فیلتر کالمن
42
فیلتر کالمن
43
فیلتر کالمن
شکل 2 – شمای کلی فیلتر کالمن ]2[
44
فیلتر کالمن توسعه یافته (EKF)
45
فیلتر کالمن توسعه یافته
46
فیلتر کالمن توسعه یافته
شکل 3 – شمای کلی فیلتر کالمن توسعه یافته ]2[
47