تعریف فیلتر کالمن (Kalman filter) قسمت 2

بخت حاشیه‌ای

همانند تخمین بازگشتی بیز که پیش‌تر بیان شد، فیلتر کالمن را می‌توان به عنوان یک مدل مولد دید. یعنی فرایندی برای تولید دنباله‌ای از مشاهدات تصادفی (… ,z = (z₀, z₁, z_2. این فرایند به صورت زیر تعریف می‌شود:

حالت پنهان $\mathbf {x} _{0}$ را از توزیع گاوسی پیشین $p(\mathbf {x} _{0})={\mathcal {N}}({\hat {\mathbf {x} }}_{0\mid 0},\mathbf {P} _{0\mid 0})$ نمونه‌گیری کنید.
حالت پنهان $\mathbf {x} _{0}$ $\mathbf {z} _{0}$ را از مدل مشاهده شده $p(\mathbf {z} _{0}\mid \mathbf {x} _{0})={\mathcal {N}}(\mathbf {H} _{0}\mathbf {x} _{0},\mathbf {R} _{0})$ نمونه‌گیری کنید.
برای $k=1,2,3,\ldots$
1. حالت پنهان $\mathbf {x} _{k}$ را از مدل انتقالی $p(\mathbf {x} _{k}\mid \mathbf {x} _{k-1})={\mathcal {N}}(\mathbf {F} _{k}\mathbf {x} _{k-1}+\mathbf {B} _{k}\mathbf {u} _{k},\mathbf {Q} _{k})$ محاسبه کنید.
2. مشاهده $\mathbf {z} _{k}$ را از مدل مشاهده شده $p(\mathbf {z} _{k}\mid \mathbf {x} _{k})={\mathcal {N}}(\mathbf {H} _{k}\mathbf {x} _{k},\mathbf {R} _{k})$ محاسبه کنید.

این فرایند ساختاری مشابه مدل پنهان مارکوف دارد که حالات گسسته در آن به متغیرهای تصادفی پیوسته با توزیع گاوسی تبدیل شده‌است.

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

$p(\mathbf {z} )=\prod _{k=0}^{T}p(\mathbf {z} _{k}\mid \mathbf {z} _{k-1},\ldots ,\mathbf {z} _{0})$

به علاوه چون فیلتر کالمن معرف یک فرایند مارکوف است، تمام دانش بدست آمده از مشاهدات قبلی به تخمین ${\hat {\mathbf {x} }}_{k\mid k-1},\mathbf {P} _{k\mid k-1}$ محدود می‌شود. به این ترتیب بخت حاشیه‌ای به صورت زیر محاسبه می‌شود:

${\begin{aligned}p(\mathbf {z} )&=\prod _{k=0}^{T}\int p(\mathbf {z} _{k}\mid \mathbf {x} _{k})p(\mathbf {x} _{k}\mid \mathbf {z} _{k-1},\ldots ,\mathbf {z} _{0})d\mathbf {x} _{k}\\&=\prod _{k=0}^{T}\int {\mathcal {N}}(\mathbf {z} _{k};\mathbf {H} _{k}\mathbf {x} _{k},\mathbf {R} _{k}){\mathcal {N}}(\mathbf {x} _{k};{\hat {\mathbf {x} }}_{k\mid k-1},\mathbf {P} _{k\mid k-1})d\mathbf {x} _{k}\\&=\prod _{k=0}^{T}{\mathcal {N}}(\mathbf {z} _{k};\mathbf {H} _{k}{\hat {\mathbf {x} }}_{k\mid k-1},\mathbf {R} _{k}+\mathbf {H} _{k}\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{T})\\&=\prod _{k=0}^{T}{\mathcal {N}}(\mathbf {z} _{k};\mathbf {H} _{k}{\hat {\mathbf {x} }}_{k\mid k-1},\mathbf {S} _{k})\end{aligned}}$

رابطه بالا حاصلضرب چند توزیع احتمال گاوسی است که هر یک نمایانگر یک مشاهده z_k تحت فیلتر $\mathbf {H} _{k}{\hat {\mathbf {x} }}_{k\mid k-1},\mathbf {S} _{k}$ است؛ که از آپدیت‌های بازگشتی محاسبه می‌شود. برای راحتی محاسبه بهتر است از log بخت حاشیه‌ای یعنی $\ell =\log p(\mathbf {z} )$ استفاده شود. با فرض $\ell ^{(-1)}=0$ محاسبه به صورت بازگشتی انجام می‌شود.

$\ell ^{(k)}=\ell ^{(k-1)}-{\frac {1}{2}}\left({\tilde {\mathbf {y} }}_{k}^{T}\mathbf {S} _{k}^{-1}{\tilde {\mathbf {y} }}_{k}+\log \left|\mathbf {S} _{k}\right|+d_{y}\log 2\pi \right)$

به‌طوری‌که $d_{y}$ بعد بردار اندازه‌گیری‌ها می‌باشد.

فیلتر اطلاعاتی

در فیلتر اطلاعاتی یا فیلتر کوواریانس معکوس، تخمین کوواریانس و تخمین حالت به ترتیب با ماتریس اطلاعات و تابع اطلاعات جایگزین می‌شوند.

{\textbf {Y}}_{k\mid k}={\textbf {P}}_{k\mid k}^{-1}

{\hat {\textbf {y}}}_{k\mid k}={\textbf {P}}_{k\mid k}^{-1}{\hat {\textbf {x}}}_{k\mid k}

به طریق مشابه کوواریانس و بردار مشاهدات هم با عبارات هم‌ارز اطلاعاتی جایگزین می‌شوند.

{\textbf {Y}}_{k\mid k-1}={\textbf {P}}_{k\mid k-1}^{-1}

{\hat {\textbf {y}}}_{k\mid k-1}={\textbf {P}}_{k\mid k-1}^{-1}{\hat {\textbf {x}}}_{k\mid k-1}

با داشتن ماتریس و بردار مشاهدات که به صورت زیر تعریف شده‌اند

{\textbf {I}}_{k}={\textbf {H}}_{k}^{\text{T}}{\textbf {R}}_{k}^{-1}{\textbf {H}}_{k}

{\textbf {i}}_{k}={\textbf {H}}_{k}^{\text{T}}{\textbf {R}}_{k}^{-1}{\textbf {z}}_{k}

اطلاعات آپدیت شده به صورت زیر نوشته می‌شوند.

{\textbf {Y}}_{k\mid k}={\textbf {Y}}_{k\mid k-1}+{\textbf {I}}_{k}

{\hat {\textbf {y}}}_{k\mid k}={\hat {\textbf {y}}}_{k\mid k-1}+{\textbf {i}}_{k}

مزیت اصلی فیلتر اطلاعاتی این است که N مشاهده می‌توانند در هر بازه زمانی با جمع زدن ماتریس‌ها و بردارهای اطلاعاتی فیلتر شوند.

{\textbf {Y}}_{k\mid k}={\textbf {Y}}_{k\mid k-1}+\sum _{j=1}^{N}{\textbf {I}}_{k,j}

{\hat {\textbf {y}}}_{k\mid k}={\hat {\textbf {y}}}_{k\mid k-1}+\sum _{j=1}^{N}{\textbf {i}}_{k,j}

جهت پیش‌بینی فیلتر اطلاعات ماتریس و بردار اطلاعات به عبارات هم‌ارزشان در فضای حالات سیستم تبدیل می‌شوند. البته پیش‌بینی فضای اطلاعاتی هم قابل انجام است.

{\textbf {M}}_{k}=[{\textbf {F}}_{k}^{-1}]^{\text{T}}{\textbf {Y}}_{k-1\mid k-1}{\textbf {F}}_{k}^{-1}

{\textbf {C}}_{k}={\textbf {M}}_{k}[{\textbf {M}}_{k}+{\textbf {Q}}_{k}^{-1}]^{-1}

{\textbf {L}}_{k}=I-{\textbf {C}}_{k}

{\textbf {Y}}_{k\mid k-1}={\textbf {L}}_{k}{\textbf {M}}_{k}{\textbf {L}}_{k}^{\text{T}}+{\textbf {C}}_{k}{\textbf {Q}}_{k}^{-1}{\textbf {C}}_{k}^{\text{T}}

{\hat {\textbf {y}}}_{k\mid k-1}={\textbf {L}}_{k}[{\textbf {F}}_{k}^{-1}]^{\text{T}}{\hat {\textbf {y}}}_{k-1\mid k-1}

این مقادیر به شرطی قابل محاسبه‌اند که F و Q در زمان ثابت باشند. همچنینF و Q باید معکوس‌پذیر باشند.

تصفیه‌کننده تأخیر زمانی

تصفیه‌کننده بهینه تخمینی بهینه از ${\hat {\textbf {x}}}_{k-N\mid k}$ برای تأخیر ثابت $N$ با استفاده از مشاهدات ${\textbf {z}}_{1}$ تا ${\textbf {z}}_{k}$ ارائه می‌کند. این تخمین به کمک روابط قبلی و برای یک حالت تکمیل شده به صورت زیر بدست می‌آید:

${\begin{bmatrix}{\hat {\textbf {x}}}_{t\mid t}\\{\hat {\textbf {x}}}_{t-1\mid t}\\\vdots \\{\hat {\textbf {x}}}_{t-N+1\mid t}\\\end{bmatrix}}={\begin{bmatrix}{\textbf {I}}\\0\\\vdots \\0\\\end{bmatrix}}{\hat {\textbf {x}}}_{t\mid t-1}+{\begin{bmatrix}0&\ldots &0\\{\textbf {I}}&0&\vdots \\\vdots &\ddots &\vdots \\0&\ldots &I\\\end{bmatrix}}{\begin{bmatrix}{\hat {\textbf {x}}}_{t-1\mid t-1}\\{\hat {\textbf {x}}}_{t-2\mid t-1}\\\vdots \\{\hat {\textbf {x}}}_{t-N+1\mid t-1}\\\end{bmatrix}}+{\begin{bmatrix}{\textbf {K}}^{(0)}\\{\textbf {K}}^{(1)}\\\vdots \\{\textbf {K}}^{(N-1)}\\\end{bmatrix}}{\textbf {y}}_{t\mid t-1}$

به‌طوری‌که:

${\hat {\textbf {x}}}_{t\mid t-1}$ و ${\textbf {y}}_{t\mid t-1}={\textbf {z}}_{t}-{\textbf {H}}{\hat {\textbf {x}}}_{t\mid t-1}$ با یک فیلتر استاندارد کالمن تخمین زده شده‌است.
${\hat {\textbf {x}}}_{t-i\mid t}$ و $i=1,\ldots ,N-1$ متغیرهای جدیدی هستند که در فیلتر کالمن وجود نداشتند.
نتایج کالمن از رابطه زیر بدست می‌آیند:
- ${\textbf {K}}^{(i)}={\textbf {P}}^{(i)}{\textbf {H}}^{T}\left[{\textbf {H}}{\textbf {P}}{\textbf {H}}^{\mathrm {T} }+{\textbf {R}}\right]^{-1}$
- ${\textbf {P}}^{(i)}={\textbf {P}}\left[\left[{\textbf {F}}-{\textbf {K}}{\textbf {H}}\right]^{T}\right]^{i}$

به‌طوری‌که ${\textbf {P}}$ و ${\textbf {K}}$ کوواریانس خطاهای پیش‌بینی شده و نتایج فیلتر استاندارد کالمن هستند. ( ${\textbf {P}}_{t\mid t-1}$ )

اگر تخمین کوواریانس خطا را به صورت زیر تعریف کنیم:

${\textbf {P}}_{i}:=E\left[\left({\textbf {x}}_{t-i}-{\hat {\textbf {x}}}_{t-i\mid t}\right)^{*}\left({\textbf {x}}_{t-i}-{\hat {\textbf {x}}}_{t-i\mid t}\right)\mid z_{1}\ldots z_{t}\right]$

تخمین بهتری از ${\textbf {x}}_{t-i}$ از رابطه زیر حاصل می‌شود.

${\textbf {P}}-{\textbf {P}}_{i}=\sum _{j=0}^{i}\left[{\textbf {P}}^{(j)}{\textbf {H}}^{T}\left[{\textbf {H}}{\textbf {P}}{\textbf {H}}^{\mathrm {T} }+{\textbf {R}}\right]^{-1}{\textbf {H}}\left({\textbf {P}}^{(i)}\right)^{\mathrm {T} }\right]$

تصفیه‌کننده بازه

تصفیه‌کننده بهینه تخمینی بهینه از ${\hat {\textbf {x}}}_{k\mid n}$ ( $k<n$ ) با استفاده از مشاهداتی در بازه ${\textbf {z}}_{1}$ تا ${\textbf {z}}_{n}$ ارائه می‌کند. به این مبحث «تصفیه‌کننده کالمن» هم گفته می‌شود. الگوریتم‌های مختلفی با این منظور موجودند.

Rauch–Tung–Striebel

الگوریتمی دو مرحله‌ای و کارا برای تصفیه کردن بازه است. گام رو به جلو مشابه فیلتر عادی کالمن است. تخمین‌های فیلتر شده پیشین و پسین ${\hat {\textbf {x}}}_{k\mid k-1}$ ، ${\hat {\textbf {x}}}_{k\mid k}$ و ${\textbf {P}}_{k\mid k-1}$ ، ${\textbf {P}}_{k\mid k}$ در گام رو به عقب کاربرد دارند.

در گام رو به عقب تخمین تصفیه‌شده ${\hat {\textbf {x}}}_{k\mid n}$ و ${\textbf {P}}_{k\mid n}$ را محاسبه می‌کنیم. بدین طریق که از آخرین بازه زمانی شروع کرده و به صورت عقب‌گرد معادلات بازگشتی زیر را می‌یابیم:

{\hat {\textbf {x}}}_{k\mid n}={\hat {\textbf {x}}}_{k\mid k}+{\textbf {C}}_{k}({\hat {\textbf {x}}}_{k+1\mid n}-{\hat {\textbf {x}}}_{k+1\mid k})

{\textbf {P}}_{k\mid n}={\textbf {P}}_{k\mid k}+{\textbf {C}}_{k}({\textbf {P}}_{k+1\mid n}-{\textbf {P}}_{k+1\mid k}){\textbf {C}}_{k}^{\mathrm {T} }

به‌طوری‌که

{\textbf {C}}_{k}={\textbf {P}}_{k\mid k}{\textbf {F}}_{k+1}^{\mathrm {T} }{\textbf {P}}_{k+1\mid k}^{-1}

{\textbf {x}}_{k\mid k}

تخمین حالت پسین زمان

k

\mathbf {x} _{k+1\mid k}

تخمین حالت پیشین زمان

k+1

است. درمورد کوواریانس نیز همین نوشتار به کار می‌رود.

تصفیه‌کننده Bryson–Frazier

این روش جایگزینی برای الگوریتم RTS است که توسط بیرمن ارائه شده‌است. این روش همچنین در گام رو به عقب داده‌های بدست آمده در گام رو به جلوی فیلتر کالمن استفاده می‌کند. معادلات رو به عقب شامل محاسبات بازگشتی که پس از هر مشاهده جهت تصفیه حالت و کوواریانس به کار برده می‌شود.

معادلات بازگشتی عبارتند از:

{\tilde {\Lambda }}_{k}={\textbf {H}}_{k}^{T}{\textbf {S}}_{k}^{-1}{\textbf {H}}_{k}+{\hat {\textbf {C}}}_{k}^{T}{\hat {\Lambda }}_{k}{\hat {\textbf {C}}}_{k}

{\hat {\Lambda }}_{k-1}={\textbf {F}}_{k}^{T}{\tilde {\Lambda }}_{k}{\textbf {F}}_{k}

{\hat {\Lambda }}_{n}=0

{\tilde {\lambda }}_{k}=-{\textbf {H}}_{k}^{T}{\textbf {S}}_{k}^{-1}{\textbf {y}}_{k}+{\hat {\textbf {C}}}_{k}^{T}{\hat {\lambda }}_{k}

{\hat {\lambda }}_{k-1}={\textbf {F}}_{k}^{T}{\tilde {\lambda }}_{k}

{\hat {\lambda }}_{n}=0

به‌طوری‌که ${\textbf {S}}_{k}$ کوواریانس باقیمانده‌است و ${\hat {\textbf {C}}}_{k}={\textbf {I}}-{\textbf {K}}_{k}{\textbf {H}}_{k}$ . همچنین حالت و کوواریانس تصفیه‌شده با کمک معادلات زیر قابل محاسبه است.

{\textbf {P}}_{k\mid n}={\textbf {P}}_{k\mid k}-{\textbf {P}}_{k\mid k}{\hat {\Lambda }}_{k}{\textbf {P}}_{k\mid k}

{\textbf {x}}_{k\mid n}={\textbf {x}}_{k\mid k}-{\textbf {P}}_{k\mid k}{\hat {\lambda }}_{k}

یا

{\textbf {P}}_{k\mid n}={\textbf {P}}_{k\mid k-1}-{\textbf {P}}_{k\mid k-1}{\tilde {\Lambda }}_{k}{\textbf {P}}_{k\mid k-1}

{\textbf {x}}_{k\mid n}={\textbf {x}}_{k\mid k-1}-{\textbf {P}}_{k\mid k-1}{\tilde {\lambda }}_{k}.

از مزیت‌های MBF عدم نیاز به یافتن معکوس ماتریس کوواریانس است.

تصفیه‌کننده کمینه واریانس

این روش می‌تواند بهترین خطای ممکن را با استفاده از پارامترها و آماره‌های نویزی شناخته‌شده بدست آورد. این تصفیه‌کننده مدل کلی‌تری از فیلتر غیر علی وینر است. (non-causal Wiener filter)

محاسبات در دو گام انجام می‌شود. محاسبات گام رو به جلو در یک مرحله پیش‌بینی صورت می‌گیرد:

{\hat {\textbf {x}}}_{k+1\mid k}={\textbf {(F}}_{k}-{\textbf {K}}_{k}{\textbf {H}}_{k}){\hat {\textbf {x}}}_{k\mid k-1}+{\textbf {K}}_{k}{\textbf {z}}_{k}

{\alpha }_{k}=-{\textbf {S}}_{k}^{-1/2}{\textbf {H}}_{k}{\hat {\textbf {x}}}_{k\mid k-1}+{\textbf {S}}_{k}^{-1/2}{\textbf {z}}_{k}

این عبارات معکوس وینر-هوف (Wiener-Hopf) است. نتیجه گام رو به عقب $\beta _{k}$ می‌تواند با استفاده بازگشت در زمان و از گام رو به جلو از $\alpha _{k}$ محاسبه شود. در این حالت خروجی سیستم برابر است با:

${\hat {\textbf {y}}}_{k\mid N}={\textbf {z}}_{k}-{\textbf {R}}_{k}\beta _{k}$

با جایگذاری در رابطه بالا

${\hat {\textbf {y}}}_{k\mid k}={\textbf {z}}_{k}-{\textbf {R}}_{k}{\textbf {S}}_{k}^{-1/2}\alpha _{k}$

این معادله برا ی فیلتر کالمن کمینه واریانس همواره یکسان است. حل معادلات بالا واریانس تخمین خطای خروجی را کمینه می‌کند. توجه کنید که در روش Rauch–Tung–Striebel فرض می‌شود که همه توزیع‌ها گاوسی هستند اما در اینجا چنین نیست.

فیلترهای وزن‌دار کالمن

توابع وزن‌دار جهت وزن دادن به میانگین توزیع توان خطا در یک بازه تغییر مشخص استفاده می‌شوند. فرض کنید ${\textbf {y}}$ – ${\hat {\textbf {y}}}$ یک تخمین خطای خروجی توسط فیلتر کالمن و ${\textbf {W}}$ یک تابع تخصیص وزن علی باشد. روش بهینه‌ای که واریانس ( ${\textbf {y}}$ – ${\hat {\textbf {y}}}$ ) ${\textbf {W}}$ را کمینه می‌کند استفاده از ${\textbf {W}}^{-1}{\hat {\textbf {y}}}$ است.

نحوه طراحی ${\textbf {W}}$ فعلاً بی‌پاسخ است. یک راه آن شناسایی سیستمی که تخمین خطا را تولید می‌کند و قرارداد کردن ${\textbf {W}}$ به عنوان معکوس آن سیستم است. این روش می‌تواند جهت محاسبه خطای مربع میانگین استفاده شود تا هزینه فیلتر کاهش یابد. همچنین روش مشابهی جهت یافتن تصفیه‌کننده نیز وجود دارد.

فیلترهای غیرخطی

مبنای فیلتر کالمن، تبدیلات خطی است. اما سیستم‌های پیچیده‌تر می‌توانند غیرخطی باشند. مسئله غیرخطی بودن می‌تواند در مشاهدات، مدلسازی یا هر دو بروز پیدا کند.

فیلتر کالمن بسط‌یافته – EKF

در فیلتر بسط‌یافته کالمن (EKF) انتقال حالات و مشاهدات نیاز به توابع حالت خطی یا غیرخطی دارند. اینها توابعی مشتق‌پذیر هستند.

{\textbf {x}}_{k}=f({\textbf {x}}_{k-1},{\textbf {u}}_{k})+{\textbf {w}}_{k}

{\textbf {z}}_{k}=h({\textbf {x}}_{k})+{\textbf {v}}_{k}

تابع f می‌تواند جهت محاسبه حالت پیش‌بینی شده از تخمین قبلی به کار رود. همچنین تابع h جهت یافتن مشاهده‌ای از حالت قبلی به کار می‌رود. توابع f و h نمی‌توانند مستقیماً به کوواریانس اعمال شوند، بلکه باید ماتریسی از مشتقات جزیی (ماتریس ژاکوبی) آن‌ها محاسبه شود.

در هر بازه زمانی ماتریس ژاکوبی با استفاده از حالات پیش‌بینی شده قبلی محاسبه می‌شود. این ماتریس‌ها در معادلات فیلتر کالمن کاربرد دارد. در واقع این فرایند عمل خطی کردن توابع غیرخطی را حول تخمین فعلی شامل می‌شود.

فیلتر کالمن از نوع UKF – Unscented

وقتی انتقال حالات و مشاهدات، یعنی توابع پیش‌بینی و آپدیت $f$ و $h$ ، کاملاً غیرخطی باشند، فیلتر کالمن بسط‌یافته کارایی پایینی خواهد داشت. به این دلیل که کوواریانس در عمل خطی‌سازی مدل غیرخطی افزایش می‌یابد. فیلتر کالمن Unscented از روش نمونه‌گیری قطعی که به Uncented Transform معروف است، استفاده می‌کند تا مجموعه نمونه مینیمالی از نقاط حول میانگین را جمع‌آوری کند. سپس این نقاط در تابع غیرخطی وارد شده تا میانگین و کوواریانس جدید حاصل شود. نتیجه برای سیستم‌های قطعی با قطعیت بیشتری مقدار میانگین و کوواریانس را ارائه می‌کند.^[۳۰]این روش به عنوان روش مونت‌کارلو یا بسط تیلور برای آماره‌های پسین شناخته شده‌است. در واقع این روش ما را از محاسبه مستقیم ماتریس ژاکوبی که برای بعضی توابع بسیار پیچیده‌است، بی‌نیاز می‌کند.

پیش‌بینی

مشابه EKF، در روش UKF فاز پیش‌بینی در مقایسه با یک آپدیت خطی مستقل از آپدیت UKF انجام می‌شود. تخمین حالت و کوواریانس با کمک میانگین و کوواریانس فرایند بدست می‌آیند.

{\textbf {x}}_{k-1\mid k-1}^{a}=[{\hat {\textbf {x}}}_{k-1\mid k-1}^{\mathrm {T} }\quad E[{\textbf {w}}_{k}^{\mathrm {T} }]\ ]^{\mathrm {T} }

{\textbf {P}}_{k-1\mid k-1}^{a}={\begin{bmatrix}&{\textbf {P}}_{k-1\mid k-1}&&0&\\&0&&{\textbf {Q}}_{k}&\end{bmatrix}}

مجموعه‌ای شامل 2L + ۱ به کمک حالت و کوواریانس از حالت بعد L حاصل می‌شود.

\chi _{k-1\mid k-1}^{0}={\textbf {x}}_{k-1\mid k-1}^{a}

\chi _{k-1\mid k-1}^{i}={\textbf {x}}_{k-1\mid k-1}^{a}+\left({\sqrt {(L+\lambda ){\textbf {P}}_{k-1\mid k-1}^{a}}}\right)_{i},\qquad i=1,\ldots ,L

\chi _{k-1\mid k-1}^{i}={\textbf {x}}_{k-1\mid k-1}^{a}-\left({\sqrt {(L+\lambda ){\textbf {P}}_{k-1\mid k-1}^{a}}}\right)_{i-L},\qquad i=L+1,\dots {},2L

به‌طوری‌که

\left({\sqrt {(L+\lambda ){\textbf {P}}_{k-1\mid k-1}^{a}}}\right)_{i}

iامین ستون ماتریس مربع ریشه $(L+\lambda ){\textbf {P}}_{k-1\mid k-1}^{a}$ است.

با توجه به تعریف ریشه مربعی ${\textbf {A}}$ در ماتریس ${\textbf {B}}$ بدست می‌آید:

${\textbf {B}}\triangleq {\textbf {A}}{\textbf {A}}^{\mathrm {T} }\,$

ریشه مربعی باید به صورت عددی و توسط روش‌هایی مانند تفکیک کولسکی محاسبه شود.

نقاط بدست آمده به عنوان ورودی تابع انتقال f داده می‌شوند:

$\chi _{k\mid k-1}^{i}=f(\chi _{k-1\mid k-1}^{i})\quad i=0,\dots ,2L$

به‌طوری‌که $f:R^{L}\rightarrow R^{|{\textbf {x}}|}$ . از نقاط وزن‌دار جهت محاسبه تخمین حالت و کوواریانس استفاده می‌شود

{\hat {\textbf {x}}}_{k\mid k-1}=\sum _{i=0}^{2L}W_{s}^{i}\chi _{k\mid k-1}^{i}

{\textbf {P}}_{k\mid k-1}=\sum _{i=0}^{2L}W_{c}^{i}\ [\chi _{k\mid k-1}^{i}-{\hat {\textbf {x}}}_{k\mid k-1}][\chi _{k\mid k-1}^{i}-{\hat {\textbf {x}}}_{k\mid k-1}]^{\mathrm {T} }

به‌طوری‌که وزن‌های مربوط به حالات و کوواریانس از روابط زیر بدست می‌آیند:

W_{s}^{0}={\frac {\lambda }{L+\lambda }}

W_{c}^{0}={\frac {\lambda }{L+\lambda }}+(1-\alpha ^{2}+\beta )

W_{s}^{i}=W_{c}^{i}={\frac {1}{2(L+\lambda )}}

\lambda =\alpha ^{2}(L+\kappa )-L\,\!

$\alpha$ و $\kappa$ گستردگی نقاط را کنترل می‌کنند. $\beta$ مربوط به توزیع $x$ است.

اگر توزیع $x$ گاوسی باشد، مقادیر طبیعی برابر $\alpha =10^{-3}$ , $\kappa =0$ و $\beta =2$ هستند. $\beta =2$ بهینه است.

آپدیت

پیش‌بینی حالت و کوواریانس مطابق بالا حاصل می‌شوند به جز زمانی که میانگین و کوواریانس نویز مشاهده شده در دست باشد.

{\textbf {x}}_{k\mid k-1}^{a}=[{\hat {\textbf {x}}}_{k\mid k-1}^{\mathrm {T} }\quad E[{\textbf {v}}_{k}^{\mathrm {T} }]\ ]^{\mathrm {T} }

{\textbf {P}}_{k\mid k-1}^{a}={\begin{bmatrix}&{\textbf {P}}_{k\mid k-1}&&0&\\&0&&{\textbf {R}}_{k}&\end{bmatrix}}

مطابق قبل مجموعه‌ای شامل 2L + ۱ نقطه درنظر می‌گیریم

${\begin{aligned}\chi _{k\mid k-1}^{0}&={\textbf {x}}_{k\mid k-1}^{a}\\[6pt]\chi _{k\mid k-1}^{i}&={\textbf {x}}_{k\mid k-1}^{a}+\left({\sqrt {(L+\lambda ){\textbf {P}}_{k\mid k-1}^{a}}}\right)_{i},\qquad i=1,\dots ,L\\[6pt]\chi _{k\mid k-1}^{i}&={\textbf {x}}_{k\mid k-1}^{a}-\left({\sqrt {(L+\lambda ){\textbf {P}}_{k\mid k-1}^{a}}}\right)_{i-L},\qquad i=L+1,\dots ,2L\end{aligned}}$

اگر پیش‌بینی UKF استفاده شده‌باشد، نقاط به صورت زیر مستقلاً قابل محاسبه‌اند.

$\chi _{k\mid k-1}:=[\chi _{k\mid k-1}^{\mathrm {T} }\quad E[{\textbf {v}}_{k}^{\mathrm {T} }]\ ]^{\mathrm {T} }\pm {\sqrt {(L+\lambda ){\textbf {R}}_{k}^{a}}}$

به‌طوری‌که

${\textbf {R}}_{k}^{a}={\begin{bmatrix}&0&&0&\\&0&&{\textbf {R}}_{k}&\end{bmatrix}}$

نقاط به عنوان ورودی تابع h استفاده می‌شوند

$\gamma _{k}^{i}=h(\chi _{k\mid k-1}^{i})\quad i=0..2L$

نقاط وزن‌دار جهت محاسبه مشاهده و کوواریانس مشاهدات پیش‌بینی شده استفاده می‌شوند.

{\hat {\textbf {z}}}_{k}=\sum _{i=0}^{2L}W_{s}^{i}\gamma _{k}^{i}

{\textbf {P}}_{z_{k}z_{k}}=\sum _{i=0}^{2L}W_{c}^{i}\ [\gamma _{k}^{i}-{\hat {\textbf {z}}}_{k}][\gamma _{k}^{i}-{\hat {\textbf {z}}}_{k}]^{\mathrm {T} }

کوواریانس ضربدری حالات و مشاهدات به صورت زیر محاسبه می‌شود

${\textbf {P}}_{x_{k}z_{k}}=\sum _{i=0}^{2L}W_{c}^{i}\ [\chi _{k\mid k-1}^{i}-{\hat {\textbf {x}}}_{k\mid k-1}][\gamma _{k}^{i}-{\hat {\textbf {z}}}_{k}]^{\mathrm {T} }$

که برای محاسبه نتیجه فیلتر کالمن UKF استفاده می‌شود.

$K_{k}={\textbf {P}}_{x_{k}z_{k}}{\textbf {P}}_{z_{k}z_{k}}^{-1}$

همانند فیلتر کالمن، حالت آپدیت شده از جمع حالت پیش‌بینی شده و وزن‌دار کردن نتیجه کالمن محاسبه می‌شود

${\hat {\textbf {x}}}_{k\mid k}={\hat {\textbf {x}}}_{k\mid k-1}+K_{k}({\textbf {z}}_{k}-{\hat {\textbf {z}}}_{k})$

همچنین کوواریانس آپدیت شده برابر است با تفاضل کوواریانس پیش‌بینی شده و کوواریانس محاسبه پیش‌بینی شده که با نتیجه کالمن وزن‌دار شده‌است.

${\textbf {P}}_{k\mid k}={\textbf {P}}_{k\mid k-1}-K_{k}{\textbf {P}}_{z_{k}z_{k}}K_{k}^{\mathrm {T} }$

فیلتر کالمن – بوسی

این فیلتر حالت پیوسته در زمان فیلتر کالمن می‌باشد که نام آن برگرفته از نام ریچارد اسنودن بوسی می‌باشد.

این فیلتر مبتنی بر فضای نمونه حالت مدل شده‌است

{\frac {d}{dt}}\mathbf {x} (t)=\mathbf {F} (t)\mathbf {x} (t)+\mathbf {B} (t)\mathbf {u} (t)+\mathbf {w} (t)

\mathbf {z} (t)=\mathbf {H} (t)\mathbf {x} (t)+\mathbf {v} (t)

به‌طوری‌که $\mathbf {Q} (t)$ و $\mathbf {R} (t)$ قوت نویزهای سفید $\mathbf {w} (t)$ و $\mathbf {v} (t)$ را بیان می‌کند.

فیلتر از دو معادله دیفرانسیلی بدست می‌آید. یکی برای تخمین حالت و دیگری برای کوواریانس.

{\frac {d}{dt}}{\hat {\mathbf {x} }}(t)=\mathbf {F} (t){\hat {\mathbf {x} }}(t)+\mathbf {B} (t)\mathbf {u} (t)+\mathbf {K} (t)(\mathbf {z} (t)-\mathbf {H} (t){\hat {\mathbf {x} }}(t))

{\frac {d}{dt}}\mathbf {P} (t)=\mathbf {F} (t)\mathbf {P} (t)+\mathbf {P} (t)\mathbf {F} ^{T}(t)+\mathbf {Q} (t)-\mathbf {K} (t)\mathbf {R} (t)\mathbf {K} ^{T}(t)

به‌طوری‌که

$\mathbf {K} (t)=\mathbf {P} (t)\mathbf {H} ^{T}(t)\mathbf {R} ^{-1}(t)$

کوواریانس نویز مشاهده‌شده $\mathbf {R} (t)$ معادل کوواریانس خطای پیش‌بینی شده ${\tilde {\mathbf {y} }}(t)=\mathbf {z} (t)-\mathbf {H} (t){\hat {\mathbf {x} }}(t)$ است. این دو کوواریانس تنها در حالت پیوسته زمان برابرند.

تمایز میان حالت پیش‌بینی و آپدیت فیلتر کالمن در اینجا وجود ندارد.

فیلتر کالمن هیبریدی

بسیاری از سیستم‌های فیزیکی به صورت پیوسته در زمان مدل می‌شوند درحالیکه مشاهدات ورودی توسط یک پردازنده دیجیتال و به صورت گسسته در زمان به آن ارائه می‌شوند. به این ترتیب مدل سیستم و مشاهدات به این صورت بیان می‌شود:

${\begin{aligned}{\dot {\mathbf {x} }}(t)&=\mathbf {F} (t)\mathbf {x} (t)+\mathbf {B} (t)\mathbf {u} (t)+\mathbf {w} (t),&\mathbf {w} (t)&\sim N{\bigl (}\mathbf {0} ,\mathbf {Q} (t){\bigr )}\\\mathbf {z} _{k}&=\mathbf {H} _{k}\mathbf {x} _{k}+\mathbf {v} _{k},&\mathbf {v} _{k}&\sim N(\mathbf {0} ,\mathbf {R} _{k})\end{aligned}}$

به‌طوری‌که

$\mathbf {x} _{k}=\mathbf {x} (t_{k})$

مقداردهی

${\hat {\mathbf {x} }}_{0\mid 0}=E{\bigl [}\mathbf {x} (t_{0}){\bigr ]},\mathbf {P} _{0\mid 0}=Var{\bigl [}\mathbf {x} (t_{0}){\bigr ]}$

پیش‌بینی

${\begin{aligned}&{\dot {\hat {\mathbf {x} }}}(t)=\mathbf {F} (t){\hat {\mathbf {x} }}(t)+\mathbf {B} (t)\mathbf {u} (t){\text{, with }}{\hat {\mathbf {x} }}(t_{k-1})={\hat {\mathbf {x} }}_{k-1\mid k-1}\\\Rightarrow &{\hat {\mathbf {x} }}_{k\mid k-1}={\hat {\mathbf {x} }}(t_{k})\\&{\dot {\mathbf {P} }}(t)=\mathbf {F} (t)\mathbf {P} (t)+\mathbf {P} (t)\mathbf {F} (t)^{T}+\mathbf {Q} (t){\text{, with }}\mathbf {P} (t_{k-1})=\mathbf {P} _{k-1\mid k-1}\\\Rightarrow &\mathbf {P} _{k\mid k-1}=\mathbf {P} (t_{k})\end{aligned}}$

این معادلاتت از حالت پیوسته فیلتر کالمن، بدون آپدیت توسط مشاهدات حاصل می‌شوند به‌طوری‌که $\mathbf {K} (t)=0$ . حالت و کوواریانس پیش‌بینی شده با حل مجموعه‌ای از معادلات دیفرانسیلی دارای مقادیر اولیه تخمین حالت قبلی محاسبه می‌شوند.