11

در مقال�ه اي R.E.Kalman توس�ط 1960 فيل�تر ك�المن در س�ال عنوان زير معرفي شد. تحت

“A new approach to liner filtering & prediction problem”

Transactions of the ASME-Journal of Basic Engineering ,Vol 82, pp 35-45, March 1960Vol 82, pp 35-45, March 1960..

Kalman FilterKalman Filter

Kalman Filter, Problem definition

The Kalman filter is an efficient recursive filterthat estimates the state of a dynamic system froma series of incomplete and noisy measurements.

فیلتر کالمن یک فیلتر بازخوردی کار آمد است که وضعیت یک سیستم دینامیک را با استفاده از یک سری اندازه گیری های ناقص و نویز دار انجام میدهد.

Kalman Filter, Problem definition

فرض كنيد ميخواهيم يك شئي متحرك را در يك محيطنويزي ردگيري كنيم.

ولي بعلت نويزي بودن و يا داليل ديگر كه ناشي از سيستم تصوير برداري ، سرعت و جهت حركت شئي متحرك است ،

شئي متحرك مورد نظر بدرستي قابل تشخيص نيست.

Kalman Filter, Problem definition

از مشاهده اي كه توسط سيستم تصوير برداري بدست مي درست ميكنيم.Zآيد ، يك بردار ويژگي بنام

با استفاده از اين بردار ويژگيZ مشاهده( ميخواهيم شئي( مدل كنيم.Xمتحرك مورد نظر را بصورت يك بردار ويژگي

الزم به ياد آوري است كه بردار هاي ويژكيX, Z ممكن است از نظر ابعاد با هم مساوي نباشند.

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

Kalman FilterKalman Filter

prediction

correction

77

در شرايطي كه مشاهده ما از طريق دنباله اي از تصاوير در شرايطي كه مشاهده ما از طريق دنباله اي از تصاوير((image sequencesimage sequences)) صورت ميگيرد ، مدل ها و مشاهدات صورت ميگيرد ، مدل ها و مشاهدات

خود را ميتوانيم به دو روش زير مورد استفاده قرار دهيم:خود را ميتوانيم به دو روش زير مورد استفاده قرار دهيم:

Kalman FilterKalman Filter

Kشماره فريم و يا مفهوم زمان است

Kalman FilterKalman Filter

در فيلتر كالمن يك مكانيزم باز خوردي(Feedback ) پيشنهاد ميشود كه توسط آن ميتوانيم :

Zk ،را مشاهده كنيمXk ، مدل( را تقريب بزنيم( Xk+1 ، را پيش بيني كنيم ، و بنا بر اينZk+1 را پيش بيني كنيم و سپسZk+1 .را مشاهده نمائيم

، با استفاده از مشاهدات و پيش بيني هاي فوقXk+1 وضعيت( ميكنيم. update ( را k+1مدل در زمان

Kalman Filter

:سيكل تكرار در فيلتر كالمن

Time Update .رويداد ها را درفريم بعدي پيش بيني ميكند : Measurement Update با استفاده از مشاهدات انجام شده در :

فریم حاضر پيش بيني هاي انجام شده در فریم قبلی )برای فریم حاضر( را اصالح ميكند.

Kalman Filter

فيلتر كالمن كه بطور گسترده اي در كاربرد هاي ردگيري بكارميرود فرض ميكند كه سيستم مورد بر رسي يك سيستم خطي

است.

يعني :

- مشاهدات با استفاده از توابع خطي از شرايط مورد بر رسي الف بدست مي آيند.

- فرض نويز در سيستم و در اندازه گيري ، از نوع نويز گوسينب میشود.

1111

Kalman FilterKalman Filter

1212

Kalman Filter

WWkk ~ N (0, Q ~ N (0, Qkk) ) Process noise with 0 mean and Process noise with 0 mean and covariance covariance

of Qof Qkk, ,

VVkk ~ N (0, R ~ N (0, Rkk) ) Observation noise with 0 mean and Observation noise with 0 mean and covariance of Rcovariance of Rkk

Covariance of observation noise at frame k

1313

Kalman FilterKalman Filter

1414

Kalman FilterKalman Filter

1515

Kalman FilterKalman Filter

1616

Kalman FilterKalman Filter

1717

Kalman FilterKalman Filter

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

1818

Kalman FilterKalman Filter

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

Kalman Filter, An exampleKalman Filter, An example

2020

Kalman Filter, An exampleKalman Filter, An example

2121

Kalman FilterKalman Filter

2222

Kalman FilterKalman Filter

2323

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

Kalman FilterKalman Filter

2424

Kalman FilterKalman Filter

2525

Kalman FilterKalman Filter

2626

WWkk يك بردار نويز با ميانگين صفر و كواريانس يك بردار نويز با ميانگين صفر و كواريانسQQkk براي پيش بيني در فريم براي پيش بيني در فريم kk

VVkk يك بردار نويز با ميانگين صفر و كواريانس يك بردار نويز با ميانگين صفر و كواريانسRRkk گيري شدهگيري شده بر روي متغيير اندازهبر روي متغيير اندازه

kkدر فريم در فريم

QQkk : : ماتريس كوواريانس نويز اعمال شده بر پيش بيني ماتريس كوواريانس نويز اعمال شده بر پيش بينيXXkk

Kalman FilterKalman Filter

یاد آوری :روابط

2727

Kalman FilterKalman Filter

2828

Kalman FilterKalman Filter

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

مرحله اول : تعريف سيستمتعريف بردار حالت

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

دنبال می کنيم:

y

x

y

x

Xمختصات شئی

مولفه های سرعت شئی در راستای

عمودی و افقی

مرحله اول : تعريف سيستم :تعريف ماتريس انتقال حالت فرض کنيم فاصله زمانی بین دو فریم متوالی مساوی باشد. آنگاه

ماتريس انتقال حالت سيستم بصورت ذيل در خواهد آمد:

است كه با ضرب شدن Translation يك ماتريس انتقال يا A توجه كنيد كه ماتريس ، تقريب بردار ويژگي )مدل( را در فريم بعدي kدر مقدار بردار ويژگي )مدل( در فريم

مکان شئی و X مفهوم زمان و بردار A بدست ميدهد. ماتریس k+1يعني فريم سرعت آنرا میدهد.

)زمان ضربدر سرعت میزان جابجائی را تعیین مینماید(.A X بنابراین حاصل ضرب

1000

0100

010

001

A

مرحله اول : تعريف سيستم

تعریف ماتریسH ارتباط مشاهده به مدل را نشان ، میدهد.

بردار ویژکی مشاهده ، در حقيقت از متغيرهای بدست آمده از اندازه گيری مکان هدف در فضای دوبعدی ،

در فريم های متوالی بدست می آيد.

y

xZ

0010

0001H

بصورت ذيل خواهد بود.Hبا اين تعريف ماتريس

مرحله اول : تعريف سيستم

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

مرحله اول : تعريف سيستم

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

مرحله دوم : مقداردهی اوليه

ماتریس حالت ، مقداردهی اوليهA ماتريس کواريانس خطای مقداردهی اوليه

Pkمشاهده

پیش کواريانس نويز فرايندمقداردهی ماتريس Qkبینی مدل

کواريانس نويز اندازه گيریمقداردهی ماتريس Rk)مشاهده(

: سوم predictionمرحله

در هر مرحله ازprediction طبق روابط زير ، حالت بعدی سيستم ، و مشاهده بعدی آن تخمين زده می شود.

تقريب كواريانس خطاي مشاهده در فريمkQQkk ماتريس كوواريانس نويز اعمال شده بر پيش بيني ماتريس كوواريانس نويز اعمال شده بر پيش بينيXXkk

kk

kT

kk

kk

XHZ

QAPAP

XAX

ˆˆ

ˆ

ˆ

1

مرحله سوم: تصحيح براساس مشاهده

با رويت مشاهده واقعی حالت تخمينی و عدم قطعيت آنبصورت زير تصحيح می شود:

Pk ( كواريانس خطای تقریب مدل Xk در فريم )k

kkk

kkkkk

Tk

Tkk

PHKIP

ZZKXX

RHPHHPK

ˆ)(

)ˆ(ˆ

)ˆ(ˆ 1

( متناوبا تا اتمام همه correct و predictو روند فوق ) مشاهدات تکرار می شود

Kalman Gain

Estimate ofError covarianceماتريس كو واريانس نويز بر روي مشاهده

مرحله سوم: تصحيح براساس مشاهده

kkk

kkkkk

Tk

Tkk

PHKIP

ZZKXX

RHPHHPK

ˆ)(

)ˆ(ˆ

)ˆ(ˆ 1

Kalman Gain

kk

kT

kk

kk

XHZ

QAPAP

XAX

ˆˆ

ˆ

ˆ

1

( متناوبا تا اتمام همه مشاهدات تکرار می شودcorrect و predictو روند فوق )

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

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

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

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

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

47