卡爾曼濾波的推導

1 最小二乘法

在一個線性系統中，若\(x\)為常量，是我們要估計的量，關于\(x\)的觀測方程如下：

\[ y = Hx + v \tag{1.1}\]

\(H\)是觀測矩陣（或者說算符），\(v\)是噪音，\(y\)是觀察量。若關于\(x\)的最佳估計為\(\hat{x}\),誤差可定義為\(\epsilon_y\):

\[ \epsilon_y = y - H\hat{x} \tag{1.2}\]

定義代價函數\(J\)為：

\[ J = \epsilon_{y1}^2 + \epsilon_{y2}^2 + ... + \epsilon_{yk}^2 \tag{1.3}\]

將方程\((1.2)\)代入\((1.3)\)可得：

\[ J=\left(y-H\hat{x}\right)^T\left(y-H\hat{x}\right)=y^Ty-x^TH^Ty-y^TH\hat{x}+\hat{x}^TH^TH\hat{x} \tag{1.4} \]

使代價函數\(J\)最小的就是最佳估計，對\(J\)求偏導，并使之為0：

\[ \frac{\partial J}{\partial \hat{x}}=-2y^TH+2\hat{x}^TH^TH=0 \tag{1.5}\]

解得：

\[ \hat{x} = \left(H^TH\right)^{-1}H^Ty \tag{1.6} \]

2 遞推最小二乘法

線性系統的遞推估計值可以寫成以下形式：

\[ y_k=H_kx+v_k \tag{2.1} \]
\[ \hat{x}_k = \hat{x}_{k-1} + K_k\left(y_k-H_k\hat{x}_{k-1}\right) \tag{2.2} \]

其中是\(K_k\)待定的增益矩陣，\(y_k-H_k\hat{x}_{k-1}\)為修正項。先考慮線性遞推估計器的估計誤差均值,要注意的是這里和式\((1.2)\)有所不同，但本質都是一樣東西：

\[ \begin{split} E\left(\epsilon_{x,k}\right) &= E\left(x-\hat{x}_k\right) \cr &= E\left(x- \hat{x}_{k-1}-K_k\left(y_k-H_k\hat{x}_{k-1}\right)\right) \newline &= E\left(\epsilon_{x,k-1}-K_k\left(H_kx+v_k-H_k\hat{x}_{k-1}\right)\right) \cr &= E\left(\epsilon_{x,k-1}-K_kH_k\left(x-\hat{x}_{k-1}\right)-K_kv_k\right) \cr &=\left(I-K_kH_k\right)E\left(\epsilon_{x,k-1}\right)-K_kE\left(v_k\right) \end{split} \tag{2.3} \]

類似地，代價函數\(J_k\)表達為：

\[ J_k=E(\epsilon_{x1,k}^2+\epsilon_{x2,k}^2+...+\epsilon_{xn,k}^2)=E(\epsilon_{x,k}^T\epsilon_{x,k})=E(Tr(\epsilon_{x,k}\epsilon_{x,k}^T))=Tr(P_k) \tag{2.4}\]

其中\(P_k\)為估計誤差的協方差矩陣，利用式\((2.3)\)可以得到：

\[ \begin{split} P_k &= E\left(\epsilon_{x,k}\epsilon_{x,k}^T\right) \newline &=E\left\{\left[(I-K_kH_k)E(\epsilon_{x,k-1})-K_kE(v_k)\right]\left[(I-K_kH_k)E(\epsilon_{x,k-1})-K_kE(v_k)\right]^T\right\} \cr &=(I-K_kH_k)E(\epsilon_{x,k}\epsilon_{x,k}^T)(I-K_kH_k)^T - K_kE(v_k\epsilon_{x,k-1}^T) \cr & \quad -(I-K_kH_k)E(\epsilon_{x,k-1}v_k^T)K_k^T+K_kE(v_kv_k^T)K_k^T \end{split} \tag{2.5} \]

由于\(\epsilon_{x,k-1}\)與\(v_k\)是相互獨立的，所以有：

\[ E(v_k\epsilon_{x,k-1}^T)=E(v_k)E(\epsilon_{x,k-1})=0 \tag{2.6} \]

令\(R_k=E(v_kv_k^T)\)為噪聲的協方差矩陣，式\((2.5)\)變成：

\[ P_k = (I-K_kH_k)P_{k-1}(1-K_kH_k)^T + K_kR_kK_k^T \tag{2.7} \]

我們的目標是求出待定的增益矩陣\(K_k\)，對\(J_k\)求偏導，并使之為0可得：

\[ \frac{\partial J_k}{\partial K_k}=2(I-K_kH_k)P_{k-1}(-H_k)^T+2K_kR_k = 0 \tag{2.8} \]

可\(K_k\)解得以下：

\[ \begin{split} (I-K_kH_k)P_{k-1}H_k^T=K_kR_k \cr K_k(R_k+H_kP_{k-1}H_k^T)=P_{k-1}H_k^T \cr K_k=P_{k-1}H_k^T\left(R_k+H_kP_{k-1}H_k^T\right)^{-1} \end{split} \tag{2.9} \]

遞推最小二乘法總結如下:

a. 初始估計值：

\[ \hat{x}_0 = E(x) \tag{2.10} \]
\[ P_0 = E\left[(x - \hat{x}_0)(x-\hat{x}_0)^T\right] \tag{2.11}\]

b. 對于k=1,2,3…:
假設線性系統的觀測方程如下：

\[ y_k = H_kx + v_k \tag{2.12} \]

其中是\(v_k\)均值為0，協方差矩陣為\(R_k\)的隨機變量，每步測量的噪聲都是相互獨立的，則矩陣為\(R_k\)對角矩陣，也就是說測量的噪聲為白噪聲。

更新方程如下：
\[K_k = P_{k-1}H_k^T\left(R_k + H_kP_{k-1}H_k^T\right)^T \tag{2.13}\]
\[\hat{x}_k = \hat{x}_{k-1} + K_k(y_k - H_k\hat{x}_{k-1}) \tag{2.14}\]
\[ P_k = (I-K_kH_k)P_{k-1}(I-K_kH_k)^T+K_kR_kK_k^T \tag{2.15}\]

3 遞推最小二乘法的更簡潔表示

首先要為估計誤差協方差表達式尋找一個新的形式。把式\((2.14)\)代入式\((2.15)\)中，可以得到：

\[ P_k=\left[I-P_{k-1}H_k^TH_k\right]P_{k-1}\left[I-P_{k-1}H_k^TH_k\right]^T + K_kR_kK_k^T \tag{3.1}\]

其中\(S_k=R_k+H_kP_{k-1}H_k^T\),再把\(K_k\)代入并展開可得(考慮到\(S_k\)與\(P_k\)的對稱性)：

\[ \begin{split} P_k &= P_{k-1} - 2P_{k-1}H_k^TS_k^{-1}H_kP_{k-1}+P_{k-1}H_k^TS_k^{-1}H_kP_{k-1}H_k^TS_k^{-1}H_kP_{k-1} \cr &\quad + P_{k-1}H_k^TS_k^{-1}R_kS_k^{-1}H_kP_{k-1} \cr &=P_{k-1} - 2P_{k-1}H_k^TS_k^{-1}H_kP_{k-1}+P_{k-1}H_k^TS_k^{-1}S_kS_k^{-1}H_kP_{k-1} (合并最后兩項）\cr &=P_{k-1}-P_{k-1}H_k^TS_k^{-1}H_kP_{k-1} \end{split} \tag{3.2} \]

由\(K_k=P_{k-1}H_k^TS_k^{-1}\)可以得到：

\[ P_k = (I - K_kH_k)P_{k-1} \tag{3.3}\]

對于式\((3.2)\)，可以得到以下的表達：

\[ P_k = P_{k-1} - P_{k-1}H_k^T\left(R_k+H_kP_{k-1}H_k^T\right)^{-1}H_kP_{k-1} \tag{3.4}\]

兩邊求逆，并用矩陣反演定理\(\left(A-BD^{-1}C\right)^{-1}=A^{-1}+A^{-1}B\left(D-CA^{-1}B\right)^{-1}CA^{-1}CA^{-1}\)可以得到：

\[ \begin{split} P_k^{-1} &= P_{k-1}^{-1}+P_{k-1}^{-1}P_{k-1}H_k^T\left(R_k+H_kP_{k-1}H_k^T-H_kP_{k-1}P_{k-1}^{-1}P_{k-1}H_k^T\right)^{-1}H_kP_{k-1}P_{k-1}^{-1} \cr &=P_{k-1}^{-1}+H_k^TR_k^{-1}H_k \end{split} \tag{3.5} \]

由式\((2.13)\)可得：

\[ \begin{split} K_k&=P_kP_k^{-1}P_{k-1}H_k^T\left(R_k+H_kP_{k-1}H_k^T\right)^{-1} \cr &=P_k\left(P_{k-1}^{-1}+H_k^TP_{k-1}^{-1}H_k\right)P_{k-1}H_k^T(R_k+H_kP_{k-1}H_k^T)^{-1} \cr &=P_k(H_k^T+H_k^TR_k^{-1}H_kP_{k-1}H_k^T)(R_k+H_kP_{k-1}H_k^T)^{-1} \cr &=P_kH_k^T(I+R_k^{-1}H_kP_{k-1}H_k^T)(R_k+H_kP_{k-1}H_k^T)^{-1} \cr &=P_kH_k^TR_k^{-1}(R_k+H_kP_{k-1}H_k^T)(R_k+H_kP_{k-1}H_k^T)^{-1} \cr &=P_kH_k^TR_k^{-1} \end{split} \tag{3.6} \]

可以得到式\((2.13)\)的簡潔形式是\((3.3)\)，式\((2.15)\)的簡潔形式是\((3.6)\)。

4 協方差的傳播

對于離散時間的線性系統，可以以下式子表達：

\[ x_k = F_{k-1}x_{k-1}+G_{k-1}u_{k-1}+w_{k-1} \tag{4.1} \]

其中，\(u_k\)是已知的輸入，\(w_k\)是零均值的白噪聲，協方差為\(Q_k\)。那么狀態\(x_k\)的均值\(\overline{x}_k\)隨時間有怎樣的變化？如果對式\((4.1)\)兩邊取期望將會得到狀態隨著時間的傳播方程：

\[ \overline{x}_k = E(x_k) =F_{k-1}\overline{x}_{k-1}+G_{k-1}u_{k-1} \tag{4.2} \]

\(x_k\)的協方差隨時間會有怎么樣的變化？通過式\((4.1)\)和式\((4.2)\)可以得到：

\[ \begin{split} &\quad (x_k-\overline{x}_k)(x_k - \overline{x}_k)^T \cr &=(F_{k-1}\overline{x}_{k-1}+G_{k-1}u_{k-1}-\overline{x}_k)(F_{k-1}\overline{x}_{k-1}+G_{k-1}u_{k-1}-\overline{x}_k)^T \cr &=[F_{k-1}(x_{k-1}-\overline{x}_{k-1})+w_{k-1}][F_{k-1}(x_{k-1}-\overline{x}_{k-1})+w_{k-1}]^T \cr &=F_{k-1}(x_{k-1}-\overline{x}_{k-1})(x_{k-1}-\overline{x}_{k-1})^TF_{k-1}^T+w_{k-1}w_{k-1}^T+\cr &\quad F_{k-1}(x_{k-1}-\overline{x}_{k-1})w_{k-1}^T+w_{k-1}(x_{k-1}-\overline{x}_{k-1})^TF_{k-1}^T \cr \end{split} \tag{4.3} \]

對上述的式子求期望就能到得協方差。由于\((x_{k-1}-\overline{x}_{k-1})\)與\(w_{k-1}\)相互獨立，所以它們之間的協方差為0，因些可以得到：

\[ P_k=E((x_k-\overline{x}_k)(x_k - \overline{x}_k)^T)=F_{k-1}P_{k-1}F_{k-1}^T+Q_{k-1} \tag{4.4}\]

這個就是協方差的傳播方程。

5 離散卡爾曼濾波的推導

終于來我們最重要的環節了，就是要推導卡爾曼濾波。和之前一樣，先來假設一個線性離散系統，如下：

\[ \begin{split} x_k &= F_{k-1}x_{k-1}+G_{k-1}u_{k-1}+w_{k-1} \cr y_k &= H_kx_k+v_k \cr \end{split} \tag{5.1} \]

\(w_k\)與\(v_k\)是零均值且相互獨立的噪聲，有已知的協方差矩陣\(Q_k\)和\(R_k\)，可以有以下：

\[ \begin{split} w_k \quad &\tilde{} \quad (0, Q_k) \cr v_k \quad &\tilde{} \quad (0, R_k) \cr E[w_kw_k^T] &= Q_k\delta_{k-j} \cr E[v_kv_k^T] &= R_k\delta_{k-j} \cr E[v_kw_k^T] &= 0 \cr \end{split} \tag{5.2} \]

其中\(\delta_{k-j}\)是\(Kronecker-\delta\)函數，是信號與處理中常用的單位脈沖函數，如果\(k=j\)，那么\(\delta_{k-j}=1\)，否則等于0。我們的目的是在已知的系統動態方程和帶噪聲測量{\(y_k\)}的基礎上估計狀態量\(x_k\)。對于狀態估計可以的信息量，取決于我們要解決問題的本身。

如果我們利用包括k時刻和k時刻之前的測量信息來估計\(x_k\)，那么能得到一個后驗估計\(\hat{x}_k^+\)，上標的“+”號表示這個估計的后驗，獲取后驗估計\(\hat{x}_k^+\)是在k時刻與k時刻之前的測量值的條件下計算\(x_k\)的期望值，即：

\[ \hat{x}_k^+ = E\left[x_k | y_1, y_2,...,y_k\right]=后驗估計 \tag{5.3} \]

如果利用k時刻之前但不包括k時刻的測量值來估計\(x_k\)，那么能得到一個先驗估計\(\hat{x}_k^-\),同樣可以計算相應的期望值得到先驗估計：

\[ \hat{x}_k^- = E\left[x_k | y_1, y_2,...,y_{k-1}\right]=先驗估計 \tag{5.4} \]

如果我們已經知道了k時刻之后的測量值，可以利用這些信息對\(x_k\)進行估計，這個叫做平滑估計，即：

\[ \hat{x}_{k|k+N} = E\left[x_k | y_1, y_2,...,y_{k+N}\right]=平滑估計 \tag{5.5} \]

如果我們已經知道了k時刻之前有\(M\)個測量值未知，可以利用這些信息對\(x_k\)進行估計，這個叫做預測估計，即：

\[ \hat{x}_{k|k-M} = E\left[x_k | y_1, y_2,...,y_{k-M}\right]=預測估計 \tag{5.6} \]

從利用信息的多少可以看到關于\(x_k\)的最優估計次序依次是平滑估計、后驗估計、先驗估計、預測估計，因為用多的信息量肯定能得到不比信息量少的結果差。

用\(\hat{x}_0^+\)來表示沒有使用任何測量結果（信息）得到的初始值，第一個測量值是在時間\(k=1\)時測量得到的。我們可以用初始狀態\(x_0\)的期望來得到\(\hat{x}_0^+\)，即有：

\[ \hat{x}_0^+= E(x_0) \tag{5.7}\]

類似地，定義\(P_k^-\)與\(P_k^+\)分別是先驗估計與后驗估計的協方差，即：

\[ \begin{split} P_k^+=E\left[(x_k-\hat{x}_k^+)(x_k-\hat{x}_k^+)^T\right] \newline P_k^-=E\left[(x_k-\hat{x}_k^-)(x_k-\hat{x}_k^-)^T\right] \newline \end{split} \tag{5.8} \]

系統以\(\hat{x}_0^+\)為最優的初始狀態，那么怎么推算到下一個時間點（也就是\(\hat{x}_1^-\)）呢？參加式\((4.2)\)的狀態傳播方程，可以得到：

\[ \hat{x}_1^- = F_0\hat{x}_0^+ + G_0u_0 \tag{5.9} \]

上述的方程可以推廣到更一般的情況，即為狀態\(\hat{x}\)的時間更新方程：

\[ \hat{x}_k^- =F_{k-1}\hat{x}_{k-1}^++G_{k-1}u_{k-1} \tag{5.10} \]

同理由式\((4.4)\)可以得到協議差\(P\)的時間更新方程：

\[ P_k^- = F_{k-1}P_{k-1}^+F_{k-1}^T+Q_{k-1} \tag{5.11}\]

上面我們已經得到狀態\(\hat{x}\)與協方差\(P\)的時間更新方程，這個更新方程是不用測量值來參與的，如果在\(k\)時刻測得到了\(y_k\)，那么如何在\(\hat{x}_k^-\)與\(y_k\)的基礎上得到后驗估計\(\hat{x}_k^+\)呢？參考遞推最小二乘法，我同樣可以得到測量的更新方程：

\[ \begin{split} K_k &= P_k^-H_k^T(H_kP_k^-H_k^T+R_k)^{-1}=P_k^+H_k^TR_k^{-1} \cr \hat{x}_k^+ &= \hat{x}_k^- + K_k(y_k - H_k\hat{x}_k^-) \cr p_k^+ &= (I-K_kH_k)P_k^-(I-K_kH_k)^T+K_kR_kK_k^T \cr &=[(P_k^-)^{-1}+H_k^TR_k^{-1}H_k]^{-1} = (I - K_kH_k)P_k^- \cr \end{split} \tag{5.12} \]

其中\(\hat{x}_k\)和\(P_k\)叫做測量更新方程，\(K_k\)叫做卡爾曼濾波增益。下表是最小二乘估計的遞推形式與卡爾曼濾波器的對比關系：

遞推最小二乘估計	卡爾曼濾波器
\(\hat{x}_{k-1}\)是\(y_k\)處理前的估計值	\(\hat{x}_k^-\)先驗估計
\(P_{k-1}\)是\(y_k\)處理前的協方差估計值	\(P_k^-\)先驗協方差估計
\(\hat{x}_k\)是\(y_k\)處理后的估計值	\(\hat{x}_k^+\)后驗估計
\(P_k\)是\(y_k\)處理后的協方差估計值	\(P_k^+\)后驗協方差估計

離散卡爾曼濾波的總結如下：

1. 動態線性系統的方程如下：

\[ \begin{split} & x_k = F_{k-1}x_{k-1}+G_{k-1}u_{k-1}+w_{k-1} \cr & y_k = H_kx_k+v_k \cr & E[w_kw_k^T] = Q_k\delta_{k-j} \cr & E[v_kv_k^T] = R_k\delta_{k-j} \cr & E[v_kw_k^T] = 0 \cr \end{split} \tag{5.13} \]

2. 卡爾曼濾波器的初始化如下：

\[ \begin{split} \hat{x}_0^+ &= E(x_0) \newline P_0^+ &=E\left[(x_0-\hat{x}_0^+)(x_0-\hat{x}_0^+)^T\right] \newline \end{split} \tag{5.14} \]

3. 卡爾曼濾波器的時間更新方程與測量更新方程如下， \(k=1,2,3,...\)：

\[ \begin{split} P_k^- &= F_{k-1}P_{k-1}^+F_{k-1}^T+Q_{k-1} \cr K_k &= P_k^-H_k^T(H_kP_k^-H_k^T+R_k)^{-1}=P_k^+H_k^TR_k^{-1} \cr \hat{x}_k^- &=F_{k-1}\hat{x}_{k-1}^++G_{k-1}u_{k-1} \cr \hat{x}_k^+ &= \hat{x}_k^- + K_k(y_k - H_k\hat{x}_k^-) \cr p_k^+ &= (I-K_kH_k)P_k^-(I-K_kH_k)^T+K_kR_kK_k^T \cr &=[(P_k^-)^{-1}+H_k^TR_k^{-1}H_k]^{-1} \cr &= (I - K_kH_k)P_k^- \cr \end{split} \tag{5.15} \]

從上述的方程可以看到：1). \(P_k^-\)、\(P_k^+\)與\(K_k\)是不取決于量測值\(y_k\)的，所以可以脫機運算，預先計算好這些量可以使計算機達到實時運行的需求；2). 如果\(x_k\)是一個常量，那么\(F_k=I\)、\(Q_k=0\)和\(u_k=0\)，這種情況下卡爾曼濾波就變成了最小二乘估計，反過來說卡爾曼濾波是最小二乘法的推廣，本質上是通過減少動態系統狀態的方差來達到最優的估計；3).要注意的是\(p_k^+\)的第一個形式比第三個形式在數值計算上更加穩定、魯棒性更好，因為第一個表達式只要\(p_k^-\)是對稱的正定矩陣，那么\(p_k^+\)也一定是對稱的正定矩陣。