فیلتر کالمن
فیلتر کالمن یک روش ریاضی برای تخمین حالت یک سیستم پویا است؛ سیستمی که هم تحت تأثیر نویز فرایند و هم تحت تأثیر نویز اندازه‌گیری قرار دارد.
ایدهٔ اصلی بدین صورت است: یک مدل ریاضی برای چگونگی تغییر حالت سیستم در طول زمان داریم. اندازه‌گیری‌هایی از سیستم دریافت می‌کنیم که پر از خطا و نویز هستند. فیلتر کالمن با ترکیب مدل و اندازه‌گیری‌ها، تخمینی بهینه از حالت واقعی سیستم ارائه می‌دهد.
مدل اندازه‌گیری (Measurement Model)
در لحظهٔ $t$، بردار اندازه‌گیری $Z_t$ به شکل زیر تعریف می‌شود:
\[Z_t = H x_t + v_t , \quad v_t \sim N(0, R_t)\] \[x_t \in \mathbb{R}^{n \times 1}, \quad Z_t \in \mathbb{R}^{m \times 1}, \quad H \in \mathbb{R}^{m \times n}\]در اینجا:
- $x_t \in \mathbb{R}^{n \times 1}$: بردار حالت سیستم در زمان $t$.
- $Z_t \in \mathbb{R}^{m \times 1}$: بردار اندازه‌گیری (خروجی حسگرها) در زمان $t$.
- $H \in \mathbb{R}^{m \times n}$: ماتریس نگاشت که حالت را به فضای اندازه‌گیری تبدیل می‌کند.
- $v_t$: نویز اندازه‌گیری که فرض می‌شود نرمال چندمتغیره با میانگین صفر و کوواریانس $R_t$ است.
- $R_t \in \mathbb{R}^{m \times m}$: ماتریس کوواریانس نویز اندازه‌گیری.
مدل انتقال حالت (State Transition Model)
پیش‌بینی حالت در گام بعدی (قبل از اعمال اندازه‌گیری جدید) به صورت زیر است: این معادله نشان می‌دهد که پیش‌بینی حالت آینده از روی مدل دینامیک ساخته می‌شود و عدم قطعیت سیستم با یک نویز تصادفی مدل می‌گردد. - معادلهٔ اندازه‌گیری رابطهٔ بین حالت واقعی و آنچه حسگرها مشاهده می‌کنند را بیان می‌کند (به‌علاوهٔ نویز اندازه‌گیری). معادلهٔ انتقال حالت مدل حرکت سیستم در زمان را توصیف می‌کند (به‌علاوهٔ نویز فرایند).
\[\hat{x}_{t+1}^- = F \hat{x}_t^+ + w_t , \quad w_t \sim N(0, Q_t), \quad F \in \mathbb{R}^{n \times n}\] \[R_t \in \mathbb{R}^{m \times m}, \quad Q_t \in \mathbb{R}^{n \times n}\]ماتریس‌های کوواریانس نویزهاهستند.
هدف:
می‌خواهیم مقدار به‌روز شده (a posteriori) یعنی $ \hat{x}_t^+ $ را بیابیم.
فرمول به‌روزرسانی حالت به صورت ساده به شکل زیر است:
\[\hat{x}_t^+ = \hat{x}_t^- + C_t (Z_t - H \hat{x}_t^-)\]در این رابطه:
- $ \hat{x}_t^- $ برآورد پیش‌بینی شده (a priori) از حالت در زمان $t$ است.
- $ Z_t $ بردار اندازه‌گیری در زمان $t$ است.
- $ H $ ماتریس نگاشت اندازه‌گیری است.
- $ C_t $ ماتریس کالمن (Kalman gain) است که وزنِ تصحیحِ ناشی از اندازه‌گیری را تعیین می‌کند.
- عبارت داخل پرانتز $ (Z_t - H \hat{x}_t^-) $ را غالباً «نوآورد» (innovation یا measurement residual) می‌نامند؛ این عبارت نشان‌دهندهٔ اختلاف بین آنچه حسگر مشاهده کرده و آنچه مدل پیش‌بینی کرده است.
برای تعیین ماتریس کالمن $C_t$، از کمینه‌سازی کوواریانس خطا استفاده می‌شود. اگر ماتریس کوواریانس برآورد پیش‌بینی شده را با $P_t^-$ تعریف کنیم، آنگاه کالمن گِین به‌صورت زیر به‌دست می‌آید:
\[C_t = P_t^- H^T \big( H P_t^- H^T + R_t \big)^{-1}\]که در آن:
- $P_t^- \in \mathbb{R}^{n \times n}$ کوواریانس خطای پیش‌بینی (a priori) است.
- $R_t \in \mathbb{R}^{m \times m}$ کوواریانس نویز اندازه‌گیری است.
تعویض یا به‌روزرسانی کوواریانس پس از اعمال اندازه‌گیری نیز معمولاً به یکی از دو صورت زیر نوشته می‌شود (هر دو فرم معادل‌اند؛ انتخاب فرم بستگی به سادگی محاسبات و پایداری عددی دارد):
\[P_t^+ = (I - C_t H) P_t^-\]یا فرم کامل‌تر و متقارن:
\[P_t^+ = (I - C_t H) P_t^- (I - C_t H)^T + C_t R_t C_t^T\]توضیح کوتاه:
- کالمن گِین $C_t$ تعیین می‌کند چه‌قدر به اندازه‌گیری جدید اعتماد کنیم نسبت به مدل پیش‌بینی. وقتی عدم قطعیت مدل (یعنی عناصر $P_t^-$) بزرگ باشد یا نویز اندازه‌گیری $R_t$ کوچک باشد، آنگاه $C_t$ بزرگ‌تر می‌شود و اندازه‌گیری تاثیر بیشتری در تصحیح حالت خواهد داشت.
- پس از محاسبهٔ $C_t$ و به‌روزرسانی حالت $ \hat{x}_t^+ $، کوواریانس خطا نیز با $P_t^+$ به‌روزرسانی می‌شود تا نشان دهد عدم قطعیت پس از ترکیب مدل و اندازه‌گیری چگونه تغییر کرده است.
- این دو مرحله (پیش‌بینی و سپس به‌روزرسانی) در هر گام زمانی تکرار می‌شوند و فیلتر کالمن برآوردی آنلاین و بهینه (برای سیستم‌های خطی با نویزهای گاوسی) فراهم می‌کند.
روش weighted least square (WLS)
فرمول هزینه و به‌دست آوردن برآورد بهینه
می‌دانیم با استفاده از نتایج بهینه‌سازی، بردار برآورد به‌روز شده به صورت زیر تعریف می‌شود:
\[\hat{x}_{t+1}^+\]این بردار به گونه‌ای تعیین می‌شود که تابع هزینه زیر کمینه گردد:
\[J = \| Z_t - H \hat{x}_t^+ \|^2_{R_t^{-1}} + \| \hat{x}_t^+ - F \hat{x}_{t-1}^+ \|^2_{P_{t|t-1}^{-1}}\]توضیح اجزای تابع هزینه:
- عبارت اول:
نشان‌دهندهٔ خطای مشاهده است (اختلاف بین اندازه‌گیری واقعی و مقدار پیش‌بینی‌شده توسط مدل اندازه‌گیری، وزن‌دهی‌شده با $R_t^{-1}$).
- عبارت دوم:
نشان‌دهندهٔ خطای تخمین است (اختلاف بین حالت برآوردشده و حالت پیش‌بینی‌شده با مدل دینامیک، وزن‌دهی‌شده با کوواریانس خطا).
{ نکته درباره‌ی نرم نرم دو وزن دار به صورت زیر تعریف می‌شود البته به آن Energy norm هم می گویند
\[\| x \|_A^2 = x^T A x\]- اگر
آنگاه این رابطه همان نرم ۲ معمولی خواهد بود.
- برای محاسبات می‌توان عبارات ماتریسی مانند
و موارد مشابه را به‌طور صریح صفر در نظر گرفت تا ساده‌سازی انجام شود. }
تابع هزینه داریم:
\[J = \| Z_t - H \hat{x}_t^+ \|^2_{R_t^{-1}} + \| \hat{x}_t^+ - F \hat{x}_{t-1}^+ \|^2_{P_{t|t-1}^{-1}}\]مشتق نسبت به \(\frac{\partial J}{\partial \hat{x}_t^+}\)
\[\frac{\partial J}{\partial \hat{x}_t^+} = -2 H^T R_t^{-1}(Z_t - H \hat{x}_t^+) + 2 P_{t|t-1}^{-1}(\hat{x}_t^+ - F \hat{x}_{t-1}^+) = 0\]بنابراین:
\[P_{t|t-1}^{-1} \hat{x}_t^+ - P_{t|t-1}^{-1} F \hat{x}_{t-1}^+ = H^T R_t^{-1} Z_t - H^T R_t^{-1} H \hat{x}_t^+\]اگر فرض کنیم:
\[\hat{x}_t^- = F \hat{x}_{t-1}^+\]داریم:
\[(P_{t|t-1}^{-1} + H^T R_t^{-1} H)\hat{x}_t^+ = H^T R_t^{-1} Z_t + P_{t|t-1}^{-1} \hat{x}_t^-\]حال اگر حل کنیم:
\[\hat{x}_t^+ = \hat{x}_t^- + (P_{t|t-1}^{-1} + H^T R_t^{-1} H)^{-1} H^T R_t^{-1}(Z_t - H \hat{x}_t^-)\]که معادل است با:
\[\hat{x}_t^+ = \hat{x}_t^- + K_t (Z_t - H \hat{x}_t^-)\]که در آن:
\[K_t = (P_{t|t-1}^{-1} + H^T R_t^{-1} H)^{-1} H^T R_t^{-1}\]محاسبه‌ی کوواریانس خطا
اگر داشته باشیم:
\[y = \sum A_i n_i\]که $ n_i $ uncorrelated باشند
آنگاه:
\[\text{cov}(y) = \sum A_i \, \text{cov}(n_i) \, A_i^T\]پس برای محاسبه‌ی
\[P_{t\|t}\]طبق رابطه‌ی بالا داریم:
\[P_{t|t} = (I - K_t H) P_{t|t-1} (I - K_t H)^T + K_t R_t K_t^T\]