به نام خدا عنوان فیلتر کالمن Kalman Filter سیدمحمد حسینی SeyyedMohammad Hosseini Seyyedmohammad [@] iasbs.ac.ir تحصیالت تکمیلی علوم پایه زنجان Institute for Advanced Studies in Basic Sciences تابستان 95
مقدمه فیلترکالمن 1 یک ابزار عمومی و قدرتمند برای ترکیب اطالعات در محیط های عدم قطعیت 3 و داینامیک می باشد.دربیشتر موارد اطالعات استخراج شده توسط این فیلتر بسیار دقیق است. فیلتر کالمن در مواقعی که اطالعات جمع آوری شده یا همان ورودی ها با تداخل همراه باشد و به دور از واقعیت کامل باشد نیز می تواند به خوبی کار کند و پیش بینی خوبی از آنچه که رخ داده است داشته باشد. فیلتر کالمن نیاز به نگهداری اطالعات قبلی در هر مرجله را ندارد و پاسخگوی سریعی می باشد و همین موضوع 4 گزینه مناسبی باشد. باعث شده برای سیستم های بهنگام فیلتر کالمن توانایی انجام چه اعمالی را دارد با یک مثال ساده شروع می کنیم فرض می کنیم یک ربات داریم که در جنگل سرگردان است و نیاز دارد دقیقا بداند که به کجا می تواند حرکت کند. ربات ما فقط در هر لحظه دارای حالتی 5 شامل مکان 6 و سرعت 7 است.باید توجه داشته باشیم هر حالت شامل یک جفت عدد است.این اطالعات می تواند در ربات دیگری متفاوت باشد و شامل اطالعات دیگری مثل دمای ماشین عمق مایع درون مخزن و... باشد. هر حالت را به صورت زیر نشان می دهیم: x k = (p, v ) رباتی که در اختیار داریم دارای یک سنسور مکان یاب 8 است.دقت این سنسور 10 متر است یعنی امکان خطا در اطالعات به دست آمده تا 10 متر می باشد.از طرفی هنگامی که ربات در حال حرکت است تصور می شود که می توانیم با توجه به زمان سپری شده و سرعت ربات مقدار مسافت طی شده را پیش بینی کنیم ولی ممکن است این مقدار به دست آمده دقیق نباشد و مسیر دارای موانع و پستی بلندی هایی باشد که این مقدار را با خطا همراه بسازد. 1 Kalman Filter uncertainty 3 dynamic 4 realtime 5 State 6 position 7 velocity 8 GPS
9 همراه کند. این اشتباه ها می تواند تشخیص ربات از وضعیت موجود را با اختالل پس پیش بینی های ما به صورت عدم قطعیت می باشد و دقیق نیست.پس به نظر می رسد ابتدا برروی داده های دریافتی تخمینی زده بشود و سپس تصمیم گیری صورت بگیرد. فیلتر کالمن چگونه به مشکل نگاه می کند با توجه به داده های دریافتی برای هرحالت می توان طیف وسیعی از ترکیبات ممکن را پیش بینی کرد ولی احتمال برخی نسبت به سایر بیشتر است.تصویر زیر گویای این واقعیت است. فیلتر کالمن فرض می کند که هر دو متغیر سرعت و مکان به صورت تصادفی و براساس توزیع گوسی توزیع شده اند.هر متغیر دارای مقدار میانگین µ است که مرکز توزیع تصادفی و همچنین دارای مقدار است که مقدار عدم قطعیت برای هر متغیر را نشان می دهد. برابر با واریانس در تصویر زیر متغیرهای سرعت و مکان هیچگونه اطالعاتی درباره متغیر دیگر نمی دهد. چراکه فاکتورهای سرعت و مکان هیچگونه همبستگی با هم ندارند. 9 noise
اما در تصویر زیر فاکتورهای سرعت و مکان با هم همبستگی دارند چرا که احتمال مشاهده کردن یک موقعیت خاص بستگی به این دارد که سرعت وسیله چقدر باشد. این حالت زمانی رخ می دهد که سرعت در حال کم و زیاد شدن است و ما در حال تخمین موقعیت بر اساس موقعیت قبلی باشیم. در دو حالت فوق حالت دوم از اهمیت بیشتری برخوردار است چرا که اطالعات بیشتری از وضعیت ربات در اختیار ما قرار می دهد و بدین ترتیب می توانیم رابطه بین متغیرها را پیدا کنیم و همچنین باعث می شود محدوده احتماالتی تصمیم گیری را کمتر کنیم.
10 به این همبستگی بین متغیرها ماتریس کوواریانس گفته می شود. در این ماتریس هر عنصر a ij همبستگی بین متغیر i و j را بیان می کند. از طرفی می توان حدس زد که ماتریس کوواریانس متقارن استتت پس می توان جای jوi را با هم تعویض نمود.اغلب ماتریس کوواریانس را با نشان می دهندو هر عنصر عضو این ماتریس را به. ij صورت بیان مسئله با استفاده از ماتریس ها ابتدا حاالت و اطالعات در مورد ربات را با توجه به زمان )k( را با x k نشان می دهیم.همچنین ماتریس کوواریانس برای حالت در زمان k را یا تقسیم بندی می کنیم و بهترین تخمین در زمان k P k نشان می دهیم. داریم : x k = position velocity P k = pp vp pv vv در مرحله بعد نیاز داریم راهی دا شته با شیم تا به توجه به زمان 1-K پیش بینی کنیم. و ضعیت فعلی د ستر سی دا شت ه با شیم. می توانیم اینکار را با می تواینم این پیش بینی را با استفاده از یک ماتریس بنام F k به جایی که پیش بینی می کنیم درست است منتقل می کند. انجام دهیم.این ماتریس ما را از جایی که هستیم 10 covariance matrix
حال سوالی که ایجاد می شود این است که این تابع چگونه ما را به وضعیت جدید منتقل می کند این کار را با استفاده از فرمول حرکتی انجام می دهیم: P k = P k 1 + tv k 1 V k = V k 1 به عبارت دیگر داریم : x k = [ 1 t 0 1 ] x k 1 = F k x k 1 11 که با توجه به رابطه فوق می توانیم حالت بعدی را پیش بینی کنیم.برای برروزرسانی ماتریس کوواریانس نیز نیاز به رابطه ای داریم.این رابطه را به شکل زیر بیان می کینم : حال با استفاده ازروابط فوق داریم : Cov(x) = Σ Cov(Ax) = AΣA T x k = F k x k 1 P k = F k P k 1 F k T 11 update
نفوذ خارجی 1 تا کنون حرفی از وقایع خارجی نداشتتته ایم ولی ممکن استتت یکستتری حواد رخ بدهد که ارتباطی با حالت ندا شته با شد ولی بر و ضعیت کلی سی ستم تاثیرگذار با شد.برای مثال اگر درحال مدل کردن وضعیت یک قطار باشتتیم ممکن استتت راننده قطار اهرم گاز را فشتتار دهد آنگاه ستترعت قطار افزایش پیدا می کند و یا در مثال ناوبری ربات ستتیستتتم ناوبری ممکن استتت یک فرمان برای چرخیدن چرخ و یا توقف را صتتادر کند.اگر ما این اطالعات ا ضافی را دا شته با شیم می توانیم این اطالعات را در ماتری سی به نام a k بینی های خود استفاده کنیم. قرار بدهیم و از آن در پیش با توجه به مطلب گفته شده فرمول های بیان شده را مورد بازنگری قرار می دهیم. روابط فوق را به فرم ماتریسی به صورت زیر داریم : P k = P k 1 + tv k 1 + 1 a t V k = V k 1 + x t t x k = F k x k 1 + [ ] a = F k x k 1 + β k U k t در رابطه فوق β را مارتریس کنترل و U را بردار کنترل می نامیم.برای سیستم های ساده و بدون نفوذ می توانیم این موارد را حذف کنیم. حال اگر پیش بینی های ما صد در صد درست نباشد چه اتفاقی می افتد عدم قطعیت خارجی 13 همه چیز عالی می شود اگر حالت جدید براساس خواص واقعی حالت قبلی ایجاد شود و یا دقیقا بدانیم چه عواملی باعث ایجاد تغییرات در حالت جدید می شود.اما زمانی که اطالعات دقیقی از عواملی که باعث تغییر می شود نداریم چه اتفاقی رخ می دهد مثال اگر وسیله ما یک ربات پرنده باشد ممکن است توسط اثرات باد جابجا شود. 1 External influence 13 External uncertainty
پس اگر حوادثی از این قبیل رخ بدهد پیش بینی ها اشتتتباه می شتتود.می توانیم عدم قطعیت را با اضتتافه کردن برخی از فاکتورهای جدید بعد از هر مرحله پیش بینی ایجاد کنیم. هر حا لت در تخمین فعلی ما می توا ند ما را ب ه یک رنج از x k 1 Q k حاالت دیگری منتقل کند.پس هر نقطه در یک ماتریس گووسین با استفاده از کوواریانس کند. ما را به متصل می که در نهایت یک ماتریس گووستین جدید به ما می دهدکه با توجه به کوواریانس های متفاوت ایجاد شده است. حال می توانیم گام های پیش بینی خود را بهتر کنیم.داریم: x k = F k x k 1 + β k U k P k = F k P k 1 F T k + Q k به عبارت دیگر بهترین تخمین این است که بر اساس بهترین تخمین قبلی ایجاد شود و همچنین یک تصحیح برای نفوذ خارجی اعمال کنیم. همچنین عدم قطعیت جدیدی که براساس عدم قطعیت قدیمی پیش بینی می شود همراه با یکسری عدم قطعیت های جدید است که از محیط دریافت می شود.حال زمانی که اطالعات از سنسورها دریافت می کنیم چه اتفاقی رخ می دهد
)خالص کردن( پاالیش برآوردهای اندازه گیری شده 14 ممکن است ربات ما چندین سنسور داشته باشد و اطالعات را از محیط جمع آوری کند و این اطالعات به صورت غیر مستقیم اطالعات مفیدی برای تصمیم گیری در مورد حالت ربات باشد. از طرفی ممکن استت واحد مقیاس چیزی که در حال خواندن از ستنستورها هستتیم با چیزی که واحد مقیاس ا صلی ما ا ست متفاوت با شد.اطالعاتی که از سن سورها داریم را می توانیم تو سط یک ماتریس براساس آن حدس بزنیم که به کجا در حال حرکت هستیم.داریم: H k مدل کنیم و μ expected = H k x k T expected = H k P k H k یکی از عواملی که باعث بهبود کارایی فیلتر کالمن شده است طریقه برخورد آن با نویز سنسورها است.از آنجایی که سن سورهای ما تا حدودی غیرقابل اعتماد ا ست پس هر حالت در تخمین ا صلی در طیف و سیعی از مقادیر است ولی می دانیم که بعضی از حاالت ایجاد شده بیشتر شبیه به واقعیت است. 14 Refining the estimate with measurements
به کوواریانس ناشی از سنسورها R k می گوییم. مقدار Z k را برابر با توزیع میانگین مقادیر خوانده شده از سنسورها قرار می دهیم.پس گوسین جدیدی داریم که از اطالعات خوانده شده از سنسورها و اطالعات پیش بینی به وجود آمده است. حال در تالش هستیم تا این اختالف در پیش بینی ها را از بین ببریم. سوالی که ایجاد می شود این است که کدام به حالت واقعی شبیه تر است دو حالت را در نظر می گیریم : حالت اول : اینکه مقادیر اندازه گیری شده توسط سنسورها همان مقادیر واقعی باشد. حالت دوم : احتمال اینکه تخمین های قبلی برای مقادیر پارامترها نسبت به مقادیری که سنسورها گزارش داده اند به مقادیر واقعی نزدیکتر باشند. حال چون نمی داینم کدام یک به واقعیت نزدیکتر است از ترکیب هر دو برای ایجاد گووسین جدید استفاده می کنیم و این حالت بهترین تخمیمن ممکن را ایجاد می کند.
ترکیب گووسین ها سعی می کنیم برای ترکیب گووسین ها موجود راهی را پیدا کنیم.ساده ترین راه این است که از یک بعد به ماجرا نگاه کنیم. منحنی گوسی یک بعدی با واریانس و میانگین µ به صورت زیر تعریف می شود: ℵ(x, μ, σ) = 1 (x μ) σ π e σ حال می خواهیم بدانیم وقتی دو نمودار گوسی را در هم ضرب می کنیم چه اتفاقی می افتد به عبارتی داریم : ℵ(x, u 0, σ 0 ). ℵ(x, u 1, σ 1 ) =? ℵ(x,u,σ) μ = u 0 + σ 0 (μ q μ 0 ) σ 0 + σ 1 σ = σ 0 + σ 0 4 با توجه به فرمول های بیان شده و جایگزین کردن در آن فرمول ها می توانیم داشته باشیم : σ 0 + σ 1 در روابط فوق می توانیم بخشی از آن را فاکتور بگیریم و مقدار آنرا با k جایگزین کنیم. k = σ 0 σ 0 +σ 1 در نتیجه خواهیم داشت : μ = μ 0 + k(μ 1 + μ 0 ), σ = σ 0 kσ 0
: از طرفی چون محاسبات ما به صورت ماتریسی است پس باید به فرم ماتریسی تبدیل کنیم تا بتوانیم در محاسبات استفاده نماییم. اگر برابر با ماتریس کوواریانس برای ماتریس گووس باشد و μ برابر با میانگین مقدار هر محور باشد آنگاه داریم μ = μ 0 + K(μ 1 μ 0 ) k = 0 ( 0 + 1 ) 1 = 0 K 0 K یک ماتریس است و به آن Kalman Gain می گویند.بدین ترتیب می توانم پیش بینی را انجام دهیم.