https://mp.weixin.qq.com/s/II7xYWtgG6cQd-kP-O47mA

本篇文章分上、中、下三篇，上篇從標準卡爾曼濾波開始，中篇加入更真實的系統(tǒng)模型，下篇從傳感器的數(shù)據(jù)融合中實現(xiàn)擴展卡爾曼濾波。

1. 一個例子

想象一架飛機正準備著陸，雖然有很多方面需要考慮，比如風速、燃料等，但是大家最關(guān)心的應(yīng)該是飛機的高度。理想情況下，我們假設(shè)當前的飛機高度是前一時刻飛機飛行高度的一部分，我們假設(shè)飛機每次觀測時都降低 2%，那么它在當前時刻的高度是上一次飛行高度的 98%，因此寫成公式是這個樣子：
$altitude_{current}=0.98\times altitude_{previous}$

上面就是一個典型的遞歸計算，要計算當前值，必須引用前一個值。

由此可以得出一條高度和時間的曲線：

image

2. 噪聲處理

實際應(yīng)用中飛機的飛行高度是通過“GPS”或者氣壓計等傳感器獲取的，這些傳感器的精度往往不同，存在一定的誤差，也會在不同的時刻引入不同的噪聲，因此我們通過傳感器觀察到的飛機的實際飛行高度是存在一定噪聲的：

$observedAltitude_{current}=altitude_{current}+noise_{current}$

下圖表示當噪聲占觀測高度的10%時的曲線

image

3. 組合方程

現(xiàn)在我們有兩個方程描述飛機的狀態(tài)：

$altitude_{current}=0.98\times altitude_{previous}$
$observedAltitude_{current}=altitude_{current}+noise_{current}$

這兩個方程很容易理解，但是他們還不足以處理復(fù)雜的系統(tǒng)，為了使方程更通用，我們用 $x$ 、 $y$ 表示變量，用 $a$ 、 $b$ 表示常量，下標 $k$ 表示當前時刻，因此上面的方程可以表示為：

$x_k=ax_{k-1}$
$z_k=x_k+v_k$

方程中的 $x_k$ 表示系統(tǒng)的當前狀態(tài)， $x_{k-1}$ 表示系統(tǒng)的前一個狀態(tài)， $a$ 是一個固定值，我們這里是 0.98； $z_k$ 表示系統(tǒng)的當前觀測值， $v_k$ 是當前噪聲測量值。

卡爾曼濾波一個非常流行的原因就是它允許我們通過觀測值 $z_k$ ，常量 $a$ ，還有測量噪聲 $v_k$ 等得到一個較好的當前狀態(tài)的估計值。

大家都知道飛機在實際飛行中，飛機高度所走過的路徑不可能完全平滑，飛機通常會遇到一些湍流的影響，我們將這種湍流定義為噪聲，因此這種噪聲可以視為另一種噪聲信號：

$altitude_{current}=0.98\times altitude_{previous}+turbulence_{current}$

用更通用的方程表示：
$x_k=ax_{k-1}+w_k$

方程中的 $w_k$ 稱為過程噪聲，因為像湍流一樣，它是過程的固有部分，不是觀測或測量的偽影。

接下來我們將忽略過程噪聲一會兒，以便于更好的討論其他部分，在傳感器融合部分，我們將繼續(xù)探討過程噪聲。

4. 狀態(tài)估計

我們的目標是從觀測值獲取系統(tǒng)當前狀態(tài) $x_k$ ，因此將方程觀測方程寫為：
$x_k=z_k-v_k$

問題是我們不知道當前的觀測噪聲 $v_k$ ，它是不可預(yù)知的。幸運的是，卡爾曼濾波具有強大的洞察力，我們可以通過當前觀測值以及前一個狀態(tài)估計值來綜合考慮當前狀態(tài)的估計值。我們使用 $\hat{x}_k$ 表示系統(tǒng)的當前狀態(tài)估計值:

$\hat{x}_k=\hat{x}_{k-1}+g_k\cdot(z_k-\hat{x}_{k-1})$

因此，我們可以將當前狀態(tài)的估計值表示為前一個狀態(tài)的估計值與當前狀態(tài)的觀測值之間的一個折中， $g_k$ 表示增益，在這里可以理解為權(quán)重，這個方程將會是我們直接實現(xiàn)卡爾曼濾波的一個重要方程。

看起來有點復(fù)雜，接下來我們將 $g_k$ 給定兩個極端值再來看一下，當 $g_k=0$ 時：

$\hat{x}_k=\hat{x}_{k-1}+0\times(z_k-\hat{x}_{k-1})=\hat{x}_{k-1}$

也就是說，當增益是零的時候，觀測是沒有影響的。當前狀態(tài)與前一個狀態(tài)是相等的；如果設(shè)定$g_k=1，那么：

$\hat{x}_k=\hat{x}_{k-1}+1\times(z_k-\hat{x}_{k-1})={z}_{k}$

也就是說，如果 $g_k=1$ ，系統(tǒng)的當前狀態(tài)是和前一個狀態(tài)無關(guān)的。我們得到的系統(tǒng)當前狀態(tài)完全來自于觀測。

實際系統(tǒng)中，增益值是介于0-1之間的。

比如我們假定 $g_k=0.5$ ， $x_{k-1}=90$ ， $z_k=100$ ，那么

$\hat{x}_k=\hat{x}_{k-1}+0.5\times(z_k-\hat{x}_{k-1})=90+0.5\times(100-90)=95$

5. 增益計算

從上面的公式我們可以通過系統(tǒng)的前一個狀態(tài)估計值 $\hat{x}_{k-1}$ ，以及系統(tǒng)的當前觀測值 $z_k$ 和當前增益 $g_k$
獲取系統(tǒng)當前狀態(tài)的估計值：

$\hat{x}_k=\hat{x}_{k-1}+g_k\cdot(z_k-\hat{x}_{k-1})$

那么如何計算這個增益值 $g_k$ 呢？目前只能間接的從噪聲中獲取這個值。

回憶一下每一個觀測值與一個特定噪聲值的關(guān)系：

$z_k=x_k+v_k$

我們不知道觀測時的單個噪聲，但是我們通?？梢灾榔骄肼曋怠１热缒硞€傳感器公布的精度，告訴我們它的輸出是多么的嘈雜。暫且將這個平均噪聲值稱為 $r$ ，這個值是不隨時間改變的，因此不必給它下標，它是傳感器的固有屬性。然后我們可以依據(jù)這個 $r$ 計算當前增益值。

$r$ 實際上是噪聲信號的方差，如果允許這個值隨時間變化，卡爾曼濾波器也可以工作，但在大多數(shù)應(yīng)用中，這個值通常假定為常數(shù)。

$g_k=\frac{p_{k-1}}{p_{k-1}+r}$

式中的 $p_k$ 是一個遞歸計算的預(yù)測誤差，它實際上是 $k$ 時刻的估計過程的協(xié)方差，它是我們預(yù)測方差的平均值。實際上，狀態(tài)是一個隨機的變量或矢量，一個隨機過程的瞬時值，它根本沒有一個“真”值，估計僅僅是狀態(tài)描述的過程模型最有可能的值，或者是最接近真實值的值。

$p_k=(1-g_k)p_{k-1}$

先看這兩個公式的含義，假設(shè)我們前一個時刻預(yù)測的誤差為0即 $p_{k-1}=0$ ，那么當前增益 $g_k={0}/{(0+r)}=0$ 。

因此由上面公式可以推出下一個系統(tǒng)狀態(tài)的估計和當前系統(tǒng)狀態(tài)是一樣的。這也恰恰說明了如果我們的預(yù)測是準確的我們就不需要調(diào)整狀態(tài)估計。

我們考慮另一個極端，假如預(yù)測誤差為1，即 $p_{k-1}=1$ ，那么 $g_k=1/(1+r)$ 。如果 $r$ 為零，我們的系統(tǒng)僅存在很小的噪聲，那么增益 $g_k=1$ ，我們的狀態(tài)估計 $x_k$ 將會受到觀測值 $z_k$ 的強烈影響。但是當 $r$ 增大時，增益可以變的任意小，也就是說當系統(tǒng)足夠嘈雜時，一個壞的預(yù)測將不得不被忽略。

我們再看最后那個公式，當 $g_k=0$ 時，$p_k=p_{k-1}，因此與狀態(tài)估計一樣，零增益意味著對預(yù)測誤差沒有更新。

另一方面，當 $g_k=1$ 時， $p_k=0$ ，因此最大增益對應(yīng)于零預(yù)測誤差，此時的當前觀測值僅用于更新當前狀態(tài)。

6. 預(yù)測和更新

到這里，你可能想知道之前方程中的常量 $a$ 哪去了？

$x_k=a*x_{k-1}$

看起來已經(jīng)消失在我們的狀態(tài)估計方程中：

$\hat{x}_k=\hat{x}_{k-1}+g_k\times(z_k-\hat{x}_{k-1})$

實際上我們需要這兩個方程去估計系統(tǒng)的狀態(tài)，這兩個方程都是基于不同類型的信息來表示系統(tǒng)狀態(tài)的估計。原始方程表示一個關(guān)于狀態(tài)的預(yù)測稱為先驗，第二個方程表示基于觀測的系統(tǒng)狀態(tài)預(yù)測的更新稱為后驗，那么將第一個方程重新寫為：

$\hat{x}_k=a*\hat{x}_{k-1}$