擴展卡爾曼濾波新手教程（三）----中文版

擴展卡爾曼濾波新手教程（三）

說明：本文內容翻譯自外文網站The Extended Kalman Filter: An Interactive Tutorial for Non-Experts，僅供學習和參考。
本文是擴展卡爾曼濾波新手教程系列的第三篇，上一篇博客請看：擴展卡爾曼濾波新手教程（二）。

Part11: 線性代數

前面我們得到了根據速度和時間描述距離的方程：
$distance_{current}=distance_{previous}+velocity_{previous}*timestep$

但我們想將該方程歸納爲更通用的形式:
$x_k=ax_{k-1}$

幸運的是，很久以前，數學家們就發明了一種“神奇的技巧”，從而實現用同樣的形式來表示這兩種方程。這種技巧就是不要把一種情況（比如系統狀態）看作一個單一的數字，而是看作一種稱爲向量（vector）的數字列，它就像Excel電子表格中的一列。向量的大小（元素的數量）對應於我們想要對狀態編碼的量的個數。例如對於距離和速度，我們的向量中有兩個元素（item）：
$x_k\equiv \begin{bmatrix} {distance_{k}}\\ {velocity_{k}}\\ \end{bmatrix}$

這裏我使用了恆等於號"$\equiv$"來表示這是一個定義：當前狀態被定義爲一個包含當前距離和當前速度的向量。
那麼這能對我們有什麼幫助？嗯。。。我們還需要線性代數中的另一個工具----矩陣。如果一個向量就像電子表格中的一列，那麼一個矩陣就像是整個電子表格。當我們用一個向量和矩陣相乘時，我們會得到同樣大小的另一個向量：
$\begin{bmatrix} {a}&b\\ c&d\\ \end{bmatrix} \begin{bmatrix} {x}\\ {y}\\ \end{bmatrix}=\\ \begin{bmatrix} {ax+by}\\ {cx+dy}\\ \end{bmatrix}$

例如：
$\begin{bmatrix} {8}&8\\ 4&7\\ \end{bmatrix} \begin{bmatrix} {2}\\ {5}\\ \end{bmatrix}=\\ \begin{bmatrix} {31}\\ {43}\\ \end{bmatrix}$

向量和矩陣可以是任意維度的，只要它們的維度相匹配（即矩陣的列數和向量的長度相等）：
$\begin{bmatrix} a&b&c\\ d&e&f\\ g&h&i\\ \end{bmatrix} \begin{bmatrix} {x}\\ {y}\\ z\\ \end{bmatrix}=\\ \begin{bmatrix} {ax+by+cz}\\ {dx+ey+fz}\\ {gx+hy+iz}\\ \end{bmatrix}$

我們也可以將兩個矩陣相乘得到另一個矩陣：
$\begin{bmatrix} a&b&c\\ d&e&f\\ g&h&i\\ \end{bmatrix} \begin{bmatrix} r&s&t\\ u&v&w\\ x&y&z\\ \end{bmatrix}=\\ \begin{bmatrix} {ar+bs+cw}&{as+bv+cy}&{at+bw+cz}\\ {dr+eu+fx}&{ds+ev+fy}&{dt+ew+fz}\\ {gr+hu+ix}&{gs+hv+iy}&{gt+hw+iz}\\ \end{bmatrix}$

兩個矩陣相加則更爲簡單:
$\begin{bmatrix} a&b&c\\ d&e&f\\ g&h&i\\ \end{bmatrix} \begin{bmatrix} r&s&t\\ u&v&w\\ x&y&z\\ \end{bmatrix}=\\ \begin{bmatrix} {a+r}&{b+s}&{c+t}\\ {d+u}&{e+v}&{f+w}\\ {g+x}&{h+y}&{i+z}\\ \end{bmatrix}$

現在回到我們剛纔的任務，我們定義一個矩陣，並且遵循使用大寫字母表示矩陣的慣例：
$A= \begin{bmatrix} 1&timestep\\ 0&1\\ \end{bmatrix}$

這樣我們就能得到具有通用形式的方程：
$x_k=Ax_{k-1}$

它能像我們期望的那樣發揮作用：
$\begin{bmatrix} distance_k\\ velocity_k\\ \end{bmatrix}= \begin{bmatrix} {1}&timestep\\ {0}&1\\ \end{bmatrix} \begin{bmatrix} {distance_{k-1}}\\ {velocity_{k-1}}\\ \end{bmatrix}\\= \begin{bmatrix} {distance_{k-1}+timestep*velocity_{k-1}}\\ {velocity_{k-1}}\\ \end{bmatrix}$

也就是說，當前距離等於之前的距離加上之前的速度乘以時間步長，當前速度等於之前的速度。如果我們想對一個速度隨時間變化的系統建模，我們可以很容易地修改我們的向量和矩陣來包含加速度：
$\begin{bmatrix} distance_k\\ velocity_k\\ acceleration_k\\ \end{bmatrix}= \begin{bmatrix} {1}&timestep&0\\ {0}&1&timestep\\ 0&0&{1}\\ \end{bmatrix} \begin{bmatrix} {distance_{k-1}}\\ {velocity_{k-1}}\\ {acceleration_{k-1}}\\ \end{bmatrix}$

注：有人給我發郵件問我爲什麼把0放在這個矩陣的右上角，而不是我們高中物理課上學到的$0.5t^2$。這主要涉及到一個簡化問題：你可以嘗試構建一個離散的（non-chaotic）運動模型，讓距離只依賴於前一時刻的距離和速度，而速度只依賴於前一時刻的速度和加速度。(爲了方便，可以直接運行這個Python小程序，它能在恆定加速度下生成預期的拋物線距離曲線。)此外，對於一個比較小的時間步長，平方運算會使右上角的值接近於零。故爲了簡化操作，我們將其置爲0。

Part12: 回看預測和更新

下面是我們修改後的系統狀態方程：
$x_k=Ax_{k-1}$

這裏$x$是一個向量，而A是一個矩陣。之前我們得到的狀態方程的形式是：
$x_k=ax_{k-1}+bu_k$

這裏的$u_k$是一個控制信號，而$b$是一個縮放係數。我們之前也得到了：
$z_k=cx_{k}+v_k$
這裏的$z_k$是測量（觀測，或傳感器）信號，$v_k$是來自傳感器測量精度的噪聲信號。那麼我們該如何修改這些原始方程以使它們匹配現在的“向量/矩陣”形式呢？線性代數讓這一切都變得簡單：我們將$b$和$c$都改寫成大寫字母，使它們成爲矩陣，而不是單一的值：
$x_k=Ax_{k-1}+Bu_k \\ z_k=Cx_{k}+v_k$

然後，所有的變量（系統狀態、觀測值、噪聲信號、控制信號）都變成了向量的形式，然後我們就得到了一系列的“矩陣*向量”的乘法。事實上，我們可以將最初得到的方程看作是線性代數形式方程的一種特殊情況，相當於在每個矩陣中只有一個位置具有非零值。
那我們之前推導得出的預測和更新方程呢?讓我們來回顧一下它們：
預測：
$\hat{x_k}=a\hat{x_{k-1}}+bu_k \\ {p_k}=a{p_{k-1}} a$
更新：
${g_k}=p_kc/(c{p_{k}}c+r) \\ \hat{x_k}\leftarrow\hat{x_{k}}+ g_k(z_k-c\hat{x_k}) \\ {p_k}\leftarrow(1-g_kc){p_k}$

我們可能也想把所有的常量$a,b,c,r$全部變成大寫字母$A,B,C,R$，然後就搞定了。但是對於線性代數來說，不能直接這樣做。你可能會記得我之前說過要解釋爲什麼我沒有將$ap_{k-1}a$簡化成$a^2p_{k-1}$。答案是線性代數乘法不像普通的乘法那樣直接。要得到正確的答案，我們必須以確定的順序執行乘法；例如，$AxB$不一定和$BxA$相等。我們也可能會需要對矩陣進行轉置，該操作用矩陣右上角的符號$T$表示。對矩陣進行轉置的過程實際上是將矩陣的行和列元素進行互換，下面是一些示例：
$\begin{bmatrix} a&b&c\\ d&e&f\\ g&h&i \\ \end{bmatrix}^T= \begin{bmatrix} a&d&g\\ b&e&h\\ c&f&i \\ \end{bmatrix}$

有了這些理論基礎，我們可以重寫預測方程：
$\hat{x_k}=A\hat{x_{k-1}}+Bu_k \\ {P_k}=A{P_{k-1}} A^T$

注意這裏我們對$A$和$P_k$都寫成了大寫字母，以表示它們都是矩陣形式。我們已經知道爲什麼$A$是矩陣；至於爲什麼$P_k$也是矩陣，我們暫且放一放，後面會討論這個問題。

那我們的更新方程呢?第二個更新公式,直接變成:
$\hat{x_k}\leftarrow\hat{x_{k}}+ g_k(z_k-C\hat{x_k})$

但是卡爾曼增益的更新還包含了除法:
${g_k}=p_kc/(c{p_{k}}c+r)$

由於乘法和除法具有很大的相關性,因此我們用乘法運算來替代矩陣除法. 我們首先利用熟悉的上標$^{-1}$來替換除法運算($a^{-1}=1/a$), 然後重寫更新方程:
${g_k}=p_kc(c{p_{k}}c+r)^{-1}\\$

現在, 我們將$c,r,g$以及常數$1$轉換爲矩陣, 而且還需要用到轉置的$C$($C^T$)----對於它們爲什麼需要時矩陣, 我們再一次先暫且放一放. 轉換之後, 其他兩個更新方程變成了:
${G_k}=P_kC^T/(C{P_{k}}C^T+R)^{-1}\\ P_k\leftarrow(I-G_kC)P_k$

那麼我們應該如何計算$(C{P_{k}}C^T+R)^{-1}$,也就是矩陣$(C{P_{k}}C^T+R)$的逆矩陣呢?就如同普通的倒數運算, $a*a^{-1}=1$, 一個矩陣乘以它的逆矩陣之後能夠得到一個單位矩陣. 而單位矩陣和任何其他矩陣之後, 得到的結果始終和原矩陣相同:
$AA^{-1}=I$
例如對於$3\times3$矩陣,
$I= \begin{bmatrix} 1&0&0\\ 0&1&0\\ 0&0&1 \end{bmatrix}$

計算矩陣的逆並不像對矩陣的每個元素求逆那麼簡單，這在大多數情況下都會得到錯誤的答案。矩陣求逆的細節超出了本教程的範圍，但是有一些優秀的資源，比如MathWorld，可以幫助您學習它。更好的是，通常你不需要自己編寫代碼, 它內置在Matlab等語言中，可以也可以在Python和其他流行語言中通過一些功能包獲取。

Part13: 傳感器融合簡介

現在我們已經以線性代數的形式完成了我們卡爾曼濾波器的一系列方程:
模型：
$\hat{x_k}=A\hat{x_{k-1}}+Bu_k \\ {z_k}=C{x_{k}}+v_k$
預測：
$\hat{x_k}=A\hat{x_{k-1}}+Bu_k \\ {P_k}=A{P_{k-1}} A^T$
更新：
${G_k}=P_kC^T(C{P_{k}}C^T+R)^{-1} \\ \hat{x_k}\leftarrow\hat{x_{k}}+G_k(z_k-C\hat{x_k}) \\ {P_k}\leftarrow(I-G_kC){P_k}$

這看上去似乎只是能夠給我們的狀態變量添加一些額外的元素! 而事實上，藉助線性代數可以使卡爾曼濾波器擁有一個非常有價值的能力，我們稱傳感器融合.
再回到我們之前飛機的例子, 我們注意到飛行員能夠得到的信息(觀測值)遠不止海拔高度: 他們還擁有顯示飛機空速(airspeed)、地速(groundspeed)、航向(heading)、經緯度、外部溫度等的儀表。想象一下，一架飛機只有三個傳感器，每個傳感器都對應於狀態的一個已知部分: 一個顯示海拔高度的氣壓計(barometer)、一個顯示航向的羅盤(compass)和一個顯示空速的流速計( Pitot tube)。暫時假設這些傳感器是完全精確的(無噪聲信號), 這樣觀測方程:
$z_k=Cx_{k-1}+v_k$
就變成了:
$\begin{bmatrix} barometer_k \\ compass_k\\ pitot_k \end{bmatrix}= \begin{bmatrix} 1&0&0 \\ 0&1&0\\ 0&0&1 \end{bmatrix} \begin{bmatrix} altitude_{k-1} \\ heading_{k-1}\\ airspeed_{k-1} \end{bmatrix}$
這裏是第一個系統示例，它不再只是傳感器和狀態值之間的簡單一一對應關係。不過此處我們的$C$矩陣有些不切實際，因爲值可能不是1。不過該示例的重點是矩陣中的非零值對應於傳感器和系統狀態向量的一個元素之間的關係。任何這樣的系統都能爲我們提供傳感器融合的機會; 也就是說，能夠將多個傳感器(氣壓計、GPS)的觀測數據結合起來，從而推斷出系統一部分系統狀態(如海拔高度等)。

就像我們常常會尋求第二位醫生對某一病症的意見一樣，直覺告訴我們，對於一些重要的事情，最好有不止一個信息來源。在下一節中，我們將看到卡爾曼濾波器是如何利用多傳感器融合來實現比使用單一傳感器更好的系統狀態估計的。

Part14: 傳感器融合示例

未完待續…