卡爾曼濾波系列（五）——奇異值魯棒的卡爾曼濾波算法

1 簡介

傳統的卡爾曼濾波算法假定了噪聲服從高斯分佈，而實際應用場景中，由於傳感器受到各種因素的影響，可能存在部分遠偏離預期值的觀測結果，稱爲奇異值。此時觀測噪聲不再是高斯分佈的，而類似於student-t分佈，對於這樣的情況，傳統的卡爾曼濾波算法在估計系統狀態時會極大地受到奇異值帶來的影響，從而導致系統狀態估計結果遠偏離預期，此時需要對卡爾曼濾波做一些調整，使其能適用於非高斯的噪聲分佈。

---

2 原理

首先有狀態轉移方程和觀測方程如下，與標準卡爾曼濾波中的形式一致。

${{\bf{\theta }}_k} = {\bf{A}}{{\bf{\theta }}_{k - 1}} + {{\bf{s}}_k} \\ {{\bf{z}}_k} = {\bf{C}}{{\bf{\theta }}_k} + {{\bf{v}}_k}$

其中${\bf{\theta }}_k$表示第${k}$時刻的系統狀態， ${\bf{z}}_k$表示對${k}$時刻系統狀態的觀測結果，而$\bf{A}$和$\bf{C}$分別代表狀態轉移矩陣和觀測矩陣，另外${\bf{s}}_k$爲狀態噪聲，${{\bf{s}}_k}\sim N\left( {{\bf{0}},{\bf{Q}}} \right)$， ${{\bf{r}}_k}$爲觀測噪聲，${{\bf{r}}_k}\sim N\left( {{\bf{0}},{\bf{R}}/w_k} \right)$，權重 ${{w}_k}\sim Gamma\left( {a,b} \right)$。這裏${\bf{Q}} \in {\mathbb{R}^{N \times N}}$是一個對角陣，對角線上的元素爲${\bf{q}} \in {\mathbb{R}^{N \times 1}}$，${\bf{R}} \in {\mathbb{R}^{M \times M}}$也是一個對角陣，對角陣上的元素爲${\bf{r}} \in {\mathbb{R}^{M \times 1}}$。

對於標準的卡爾曼濾波而言，矩陣${\bf{Q}}$和矩陣${\bf{R}}$表徵的是狀態噪聲和觀測噪聲的協方差，而卡爾曼濾波算法的最終狀態估計，說到底就是根據矩陣${\bf{Q}}$和矩陣${\bf{R}}$的值去權衡系統的估計偏向預測和偏向觀測的程度。舉個例子，如果矩陣${\bf{Q}}$遠大於${\bf{R}}$矩陣 ，也就是說狀態噪聲十分劇烈，相對的觀測噪聲很小，那麼系統的估計就非常接近觀測的結果；反之矩陣${\bf{Q}}$遠小於矩陣${\bf{R}}$，也就是說觀測噪聲相當大，相對的狀態噪聲很小，那麼系統的狀態估計就更多地偏向於根據上一個時刻的狀態估計所預測的當前時刻系統狀態。

我們可以將這樣的模型及求解模型的問題視爲期望最大化（EM）學習問題，然後通過最大化對數似然函數，學習這些變量的值。有似然函數爲

$\ln p({{\bf{\theta }}_{0:k}},{{\bf{z}}_{1:k}},{w_{1:k}}) = \sum\limits_{i = 1}^k {\ln } p\left( {{{\bf{z}}_i}|{{\bf{\theta }}_i},{w_i}} \right) + \sum\limits_{i = 1}^k {\ln } p\left( {{{\bf{\theta }}_i}|{{\bf{\theta }}_{i - 1}}} \right) + \ln p\left( {{{\bf{\theta }}_0}} \right) + \sum\limits_{i = 1}^k {\ln } p\left( {{w_i}} \right) \\ = \frac{1}{2}\sum\limits_{i = 1}^k {\ln {w_i}} - \frac{k}{2}\ln \left| {\bf{R}} \right| - \frac{k}{2}\ln \left| {\bf{Q}} \right| - \frac{1}{2}\sum\limits_{i = 1}^k {{w_i}{{\left( {{{\bf{z}}_i} - {\bf{C}}{{\bf{\theta }}_i}} \right)}^T}{{\bf{R}}^{ - 1}}\left( {{{\bf{z}}_i} - {\bf{C}}{{\bf{\theta }}_i}} \right)} \\- \frac{1}{2}\sum\limits_{i = 1}^k {\ln } {w_i} - \frac{1}{2}\sum\limits_{i = 1}^k {{{\left[ {{{\bf{\theta }}_i} - {\bf{A}}{{\bf{\theta }}_{i - 1}}} \right]}^T}{{\bf{Q}}^{ - 1}}\left[ {{{\bf{\theta }}_i} - {\bf{A}}{{\bf{\theta }}_{i - 1}}} \right]} - \frac{1}{2}\ln \left| {{{\bf{Q}}_0}} \right| \\- \frac{1}{2}\left( {{{\bf{\theta }}_0} - {{{\bf{\hat \theta }}}_0}} \right){\bf{Q}}_0^{ - 1}\left( {{{\bf{\theta }}_0} - {{\widehat {\bf{\theta }}}_0}} \right) + \sum\limits_{i = 1}^k {a\ln {w_i}} - \sum\limits_{i = 1}^k b {w_i} + {\rm{const}}$

我們可以從正態和Gamma分佈的標準形式中得出最終的EM更新方程，即對於當前時間步${k}$，有如下算法：
E-Step：
${{\bf{\Sigma }}_k} = \left\langle {{w_k}} \right\rangle {\left( {{\bf{C}}_k^T{\bf{R}}_k^{ - 1}{{\bf{C}}_k} + {\bf{Q}}_k^{ - 1}} \right)^{ - 1}}$

$\left\langle {{{\bf{\theta }}_k}} \right\rangle = {{\bf{\Sigma }}_k}\left( {{\bf{Q}}_k^{ - 1}{{\bf{A}}_k}\left\langle {{{\bf{\theta }}_{k - 1}}} \right\rangle + \left\langle {{w_k}} \right\rangle {\bf{C}}_k^T{\bf{R}}_k^{ - 1}{{\bf{z}}_k}} \right)$

$\left\langle {{w_k}} \right\rangle = \frac{{a + \frac{1}{2}}}{{b + \frac{1}{2}{{\left( {{{\bf{z}}_k} - {\bf{C}}{{\bf{\theta }}_k}} \right)}^T}{\bf{R}}_k^{ - 1}\left( {{{\bf{z}}_k} - {\bf{C}}{{\bf{\theta }}_k}} \right)}}$

M-Step：

${{\bf{C}}_k} = \left( {\sum\limits_{i = 1}^k {\left\langle {{w_i}} \right\rangle {{\bf{z}}_i}} {{\left\langle {{{\bf{\theta }}_i}} \right\rangle }^T}} \right){\left( {\sum\limits_{i = 1}^k {\left\langle {{w_i}} \right\rangle } \left\langle {{{\bf{\theta }}_i}{\bf{\theta }}_i^T} \right\rangle } \right)^{ - 1}}$

${{\bf{A}}_k} = \left( {\sum\limits_{i = 1}^k {\left\langle {{{\bf{\theta }}_i}} \right\rangle {{\left\langle {{{\bf{\theta }}_{i - 1}}} \right\rangle }^T}} } \right){\left( {\sum\limits_{i = 1}^k {\left\langle {{{\bf{\theta }}_{i - 1}}{\bf{\theta }}_{i - 1}^T} \right\rangle } } \right)^{ - 1}}$

${r_{km}} = \frac{1}{k}\sum\limits_{i = 1}^k {\left\langle {{w_i}} \right\rangle \left\langle {{{\left( {{{\bf{z}}_{im}} - {{\bf{C}}_k}(m,:){{\bf{\theta }}_i}} \right)}^2}} \right\rangle }$

${q_{kn}} = \frac{1}{k}\sum\limits_{i = 1}^k {\left\langle {{{\left( {{{\bf{\theta }}_{in}} - {{\bf{A}}_k}(n,:){{\bf{\theta }}_{i - 1}}} \right)}^2}} \right\rangle }$

其中${r_{km}}$表示向量${{\bf{r}}_k}$的第$m$個元素，${q_{kn}}$表示向量${{\bf{q}}_k}$的第$n$個元素，$a$和$b$是先驗的Gamma分佈的尺度參數，當$k$時刻的傳感器採樣結果獲得時，便可以進行上述的算法。

$\left\langle {{w_k}} \right\rangle$的計算公式中，${{\bf{\theta }}_k}$的值可以用上一個時刻的估計結果$\left\langle {{{\bf{\theta }}_{k - 1}}} \right\rangle$ 所預測的當前時刻狀態值${\bf{\theta }}_k^{'}{\rm{ = }}{\bf{A}}\left\langle {{{\bf{\theta }}_{k - 1}}} \right\rangle$代替。也就是說，權值$\left\langle {{w_k}} \right\rangle$近似的與誤差 的平方成反比，假定 時刻的狀態估計 是符合預期的，那麼如果 時刻的傳感器觀測出現奇異值的話，誤差 就會取大值，因此權值 很小，進而觀測噪聲的協方差矩陣就會很小，根據卡爾曼濾波的原理，對當前時刻的狀態做估計時，結果會更偏向於預測而比較不會被觀測到的奇異值所影響。

另外，算法對矩陣${\bf{Q}}$和矩陣${\bf{R}}$的初始值設置較爲敏感，其值應該根據觀測數據的噪聲情況設定，例如對於觀測噪聲較大的數據，可以取值爲${\bf{Q}}{\rm{ = }}{\bf{R}} = 0.01{\bf{I}}$，對於觀測噪聲較小的數據，可以取初始值爲${\bf{Q}}{\rm{ = }}{\bf{R}} = 0.0001{\bf{I}}$，其中${\bf{I}}$表示單位陣。

以上算法與卡爾曼濾波的關係可以通過聯合兩個算法的公式獲得，因此奇異值魯棒的卡爾曼濾波算法也可以通過下列公式計算：
預測：

${\bf{\theta }}_k^ {'}= {{\bf{A}}_k}\left\langle {{{\bf{\theta }}_{k - 1}}} \right\rangle$

${\bf{\Sigma }}_k^{'}= {{\bf{Q}}_k}$

更新：

${{\bf{S}}_k} = {\left( {{{\bf{C}}_k}{\bf{\Sigma }}_k^{'}{\bf{C}}_k^T + \frac{1}{{{w_k}}}{{\bf{R}}_k}} \right)^{ - 1}}$

${{\bf{K}}_k} = {\bf{\Sigma }}_k^{'}{\bf{C}}_k^T{{\bf{S}}_k}$

$\left\langle {{{\bf{\theta }}_k}} \right\rangle = {\bf{\theta }}_k^{'} + {{\bf{K}}_k}\left( {{{\bf{z}}_k} - {{\bf{C}}_k}{\bf{\theta }}_k^{'}} \right)$

${{\bf{\Sigma }}_k} = \left( {{\bf{I}} - {{\bf{K}}_k}{{\bf{C}}_k}} \right){\bf{\Sigma }}_k^{'}$

在進行上述算法之前，可以先用EM算法估計各個參數的值，包括其中的權值${w_k}$，當觀測出現奇異值時， ${w_k}$的參數估計結果就會很小，於是矩陣${{\bf{S}}_k}$的值就會減小，從而導致卡爾曼增益 ${{\bf{K}}_k}$減小，於是${{\bf{K}}_k}\left( {{{\bf{z}}_k} - {{\bf{C}}_k}{\bf{\theta }}_k^{'}} \right)$就會減小，最終導致系統狀態的估計結果偏向於${\bf{\theta }}_k^{'}$，即對當前時刻的狀態預測值${\bf{\theta }}_k^{'} = {{\bf{A}}_k}\left\langle {{{\bf{\theta }}_{k - 1}}} \right\rangle$。

---

3 實驗

在12自由度（DOF）機器狗上評估了所有濾波器，如下圖所示。該機器狗有兩個測量其方向的信號源：運動捕獲（MOCAP）系統和機載慣性測量 單位（IMU）。兩者都提供了機器人方向的四元數q：來自MOCAP的qMOCAP和來自IMU的qIMU。由於IMU無法提供穩定的方向估計，但其信號乾淨，因此qIMU隨時間漂移。 在IMU中發生的漂移在傳感器收集需要集成的數據的系統中非常普遍。相比之下，qMOCAP具有奇異值和噪聲，但沒有漂移。我們想估計qMOCAP和qIMU之間的偏移，該偏移是一個包含奇異值的有噪聲的緩慢漂移信號。


圖中可以看出，觀測結果隨機出現偏離正常波動範圍的奇異值，傳統的卡爾曼濾波算法並不具有檢測奇異值的功能，觀測到奇異值時，算法認爲該時刻的觀測仍然是有效的，認爲奇異值反映了系統狀態出現了較大的改變，所以給出了偏離正常範圍的系統狀態估計結果。而實際上，奇異的觀測並非系統狀態的有效反映，而是噪聲出現離羣值，對於這樣的噪聲，奇異值魯棒的加權卡爾曼濾波算法通過結合上一個時刻的狀態估計以及當前時刻的觀測結果，計算權值，並在估計當前時刻的狀態時，用該權值修正新息，抑制奇異值的影響，從而獲得更魯棒的估計結果。

同理於圖3所示的觀測數據濾波效果對比。

---

4 參考文獻

[1] . Joanne Ting，Evangelos Theodorou，Stefan Schaal. A Kalman Filter for Robust Outlier Detection. 2008

---

原創性聲明：本文屬於作者原創性文章，小弟碼字辛苦，轉載還請註明出處。謝謝~

如果有哪些地方表述的不夠得體和清晰，有存在的任何問題，亦或者程序存在任何考慮不周和漏洞，歡迎評論和指正，謝謝各路大佬。

有需要相關技術支持的可諮詢QQ：297461921