從一維到多維理解卡爾曼濾波(通俗易懂)

從一維到多維理解卡爾曼濾波

寫在前面

• 本文以移動機器人定位問題(Localization)爲例
• 借鑑Udacity課程：AI for Robotics
• 對於一切問題，降維思考都是個好辦法

卡爾曼濾波的核心思想

1. 根據上一個狀態得到當前狀態的 “帶誤差的預測值”；
2. 在當前狀態使用某種測量工具得到 “帶誤差的測量值”；
3. 根據上述兩個值計算得到當前狀態的最優值；
4. 在當前狀態的最優值基礎上，加上該狀態要發生的動作，得到下一狀態帶誤差的預測值

一維卡爾曼濾波

一維高斯分佈 (Gaussian Distribution)

要講卡爾曼濾波，就不得不說高斯分佈，因爲**“帶誤差的預測值”和“帶誤差的測量值”均爲高斯分佈**，卡爾曼濾波的工作就是整合這兩個值的高斯分佈。

任意高斯分佈都可以由 $(\mu , \sigma^2)$來表徵，$\mu$ 爲高斯分佈的均值，$\sigma^2$則爲方差

由圖可明顯看出，$(\mu , \sigma^2)$大小對高斯分佈的影響如下：

• $\mu$決定了高斯分佈對稱軸或者說峯值的位置
• $\sigma^2$決定了高斯分佈的寬度（和高度）。
  • $\sigma^2$越大，誤差越大，Gaussian越矮胖；
  • 反之，誤差越小，Gaussian越高瘦；
  • 由於Gaussian曲線下的面積恆等於1，就決定了越寬的Gaussian峯值越小，越窄的Gaussian峯值越高。

公式

這裏有必要給出Gaussian的公式：
$f(x)=\frac{1}{\sqrt{2 \pi \sigma^{2}}} \exp \left[\frac{-1}{2} \frac{(x-\mu)^{2}}{\sigma^{2}}\right]$

卡爾曼濾波利用Gaussian在重複地做這兩件事：

• 測量值更新(Measurement updates)
• 預測值（動作）更新(Prediction/Motion updates)

上述兩個值的更新方式分兩種：直接乘積(product) 和 卷積(convolution)

• 對於測量值使用直接乘積，貝葉斯定理來計算
• 對於預測值使用卷積，

測量值更新（Measurement updates）

現假設你正在定位一個機器人，位置的先驗分佈（上一步的預測值，在下一節介紹）爲黑色曲線，接着由傳感器測量得到了如藍色曲線一樣的測量值曲線。

那麼很明顯兩個數據融合之後的Gaussian變爲

• 均值在 $\mu$ 和 $\nu$之間，且更靠近 $\nu$（因爲藍色曲線的方差更小，對位置的估計更加精確，所以我們更信任它），
• 峯值也較兩個組成的Gaussian更高（因爲在借鑑了兩組數據之後，我們對位置更加確信，方差更小）

公式

測量值Gaussian參數按如下公式更新
$\mu^{\prime}=\frac{r^{2} \mu+\sigma^{2} \nu}{r^{2}+\sigma^{2}}$

${\sigma^{2}}^{\prime}=\frac{1}{\frac{1}{r^{2}}+\frac{1}{\sigma^{2}}}$

預測值（動作）更新(Prediction/Motion updates)

預測值的更新就簡單了，均值和方差都是直接求和。當我們將預測值視作機器人的動作更新的話，就好理解了。藍色曲線代表目前位置的最優估計（根據上一節的測量值更新得到），綠色曲線代表下一步要運動的距離（有誤差，可以理解爲要讓電機轉多少圈）。那麼下一時刻的預測位置是不是就是目前位置+運動距離了？誤差是不是也變成了二者誤差的和了？

預測值Gaussian參數按如下公式更新：
$\begin{array}{l}{\mu^{\prime} = \mu+\nu} \\ {{\sigma^{2}}^{\prime} = \sigma^{2}+r^{2}}\end{array}$

總結

以上即爲一維卡爾曼濾波。總結一下：

1. 我們使用測量值更新來融合 傳感器測量到的帶誤差的位置信息 以及 基於上一狀態的預測信息，得到當前狀態的最優位置估計 (Measurement updates)
2. 根據 下一步要進行的動作 以及 當前狀態的最優位置估計，得到下一狀態的預測值 (Prediction/Motion updates)

卡爾曼濾波就是這麼簡單的兩步循環，不斷藉助多方數據，逼近實際值。

多維卡爾曼濾波

在多維空間中，卡爾曼濾波不僅能估計位置（傳感器僅能夠測量位置），還能夠根據數據估計出速度信息，因此多爲卡爾曼濾波可以綜合速度信息估計出未來位置。

多元高斯

設空間維度爲n

1. 均值是一個n維的向量
2. 方差變爲協方差，是一個 $n \times n$ 階矩陣

以二維高斯爲例， $(x_0,y_0)$表示均值，兩個維度之間是相關的。

等高線越小的Gaussian不確定性越小，下圖橫軸上的不確定性很小，但縱軸方向的不確定性很大。如果將橫軸視作位置，縱軸視作速度，我們根據單次測量值得到的Gaussian就是這樣。（因爲我們假設傳感器只能測量到位置信息，所以速度的不確定性巨大）

以(1,0)爲起點，速度爲1，則下一時刻應在(2,1)點

同理，以(1,0)爲起點，速度爲2，則下一時刻應在(3,2)點

由此可知藍色的一維信息和紅色的二維信息是有關係的。

測量值更新

接下來，單看第二狀態的測量值（綠色等高線），將其與狀態先驗（紅色等高線）相乘，即得到該狀態速度和位置的最優估計（黑色等高線），很明顯得到的估計值只有很小的不確定性。

預測值更新

新位置的預測值只有位置信息，和一維的情況一樣，公式爲
$x^{\prime}=x+u$
新位置也可以視作當前位置加上速度

多維卡爾曼濾波器設計

參數說明	預測值更新公式
$x$: 當前位置估計	$x'=Fx+u$
$P$: 協方差矩陣	$P'=FPF^T$
$F:$ 狀態轉移矩陣	測量值更新公式
$u$: 動作向量	$y=Z-Hx$
$Z:$ 測量值	$S=HPH^T+R$
$H:$ 測量矩陣	$K=PH^TS^{-1}$
$R:$ 測量誤差	$x'=x+Ky$
$I:$ 單位矩陣	$P'=(I-KH)P$