前兩篇文章介紹了離散時間的批量估計、離散時間的遞歸平滑，本文著重介紹離散時間的遞歸濾波。

前兩篇位置：【狀態(tài)估計】線性高斯系統(tǒng)的狀態(tài)估計——離散時間的批量估計、【狀態(tài)估計】線性高斯系統(tǒng)的狀態(tài)估計——離散時間的遞歸平滑。

離散時間的遞歸濾波算法

批量優(yōu)化的方案及其對應的平滑算法方案，是LG問題下能找到的最好的方法了。它利用了所有能用的數(shù)據(jù)，來估計所有時刻的狀態(tài)。不過這個方法有一個致命的問題：無法在線運行，因為它需要用未來時刻的信息估計過去的狀態(tài)。為了在實時場合下使用，當前時刻的狀態(tài)只能由它之前時間的信息決定，而卡爾曼濾波則是對這樣一個問題的傳統(tǒng)解決方案。

之前講述了如何從Cholesky分解推導出卡爾曼濾波，實際上并不需要這么復雜，接下來介紹幾種推導卡爾曼濾波的方法。

通過MAP推導卡爾曼濾波

假設已經(jīng)有了$k?1$時刻的前向估計：

{ x ^ k ? 1 , P ^ k ? 1 } \{\hat x_{k-1},\hat P_{k-1}\} {x^k?1?,P^k?1?}

這兩個量是根據(jù)初始時刻到$k?1$時刻的數(shù)據(jù)推導出來的。目標是計算：

{ x ^ k , P ^ k } \{\hat x_k,\hat P_k\} {x^k?,P^k?}

其中，需要用到直到$k$時刻的數(shù)據(jù)。實際上沒有必要再從初始時刻開始，而是簡單地用$k?1$時刻的狀態(tài)，以及$k$時刻的$v_k$、$y_k$就能估計出$k$時刻的狀態(tài)了：

{ x ^ k ? 1 , P ^ k ? 1 , v k , y k } ? ? > { x ^ k , P ^ k } \{\hat x_{k-1},\hat P_{k-1},v_k,y_k\}-->\{\hat x_k,\hat P_k\} {x^k?1?,P^k?1?,vk?,yk?}??>{x^k?,P^k?}

為了推導這個過程，定義：

$$z=\left[\begin{array}{c}{\hat{x}}_{k?1}\\ v_k\\ y_k\end{array}\right]$$

$$H=\left[\begin{array}{cc}1& \\ ?A_{k?1}& 1\\ & C_k\end{array}\right]$$

$$W=\left[\begin{array}{ccc}{\hat{P}}_{k?1}& & \\ & Q_k& \\ & & R_k\end{array}\right]$$

$$\hat{x}=\left[\begin{array}{c}{\hat{x}}_{k?1}^{{}^{\prime }}\\ {\hat{x}}_k\end{array}\right]$$

其中，${\hat{x}}_{k?1}^{{}^{\prime }}$表示使用了直到$k$時刻的數(shù)據(jù)計算的$k?1$時刻的狀態(tài)估計，而${\hat{x}}_{k?1}$表示使用了直到$k?1$時刻的數(shù)據(jù)計算的$k?1$時刻的狀態(tài)估計，兩者之間相差一個$k$時刻的后向估計。

通常MAP的最優(yōu)解$\hat{x}$寫成：

$$(H^T{W}^{?1}H)\hat{x}=H^T{W}^{?1}z$$

將上面的定義代入：

$$\left[\begin{array}{cc}{\hat{P}}_{k?1}^{?1}+A_{k?1}^T{Q}_k^{?1}{A}_{k?1}& ?A_{k?1}^T{Q}_k^{?1}\\ ?Q_k^{?1}{A}_{k?1}& Q_k^{?1}+C_k^T{R}_k^{?1}{C}_k\end{array}\right]\left[\begin{array}{c}{\hat{x}}_{k?1}^{{}^{\prime }}\\ {\hat{x}}_k\end{array}\right]=\left[\begin{array}{c}{\hat{P}}_{k?1}^{?1}{\hat{x}}_{k?1}?A_{k?1}^T{Q}_k^{?1}{v}_k\\ Q_k^{?1}{v}_k+C_k^T{R}_k^{?1}{y}_k\end{array}\right]$$

由于并不關心${\hat{x}}_{k?1}^{{}^{\prime }}$的實際值，因此可以將它邊緣化，等式兩側左乘：

$$\left[\begin{array}{cc}1& 0\\ Q_k^{?1}{A}_{k?1}({\hat{P}}_{k?1}^{?1}+A_{k?1}^T{Q}_k^{?1}{A}_{k?1}{)}^{?1}& 1\end{array}\right]$$

這是一元線性方程的行操作，于是變成：

$$\left[\begin{array}{cc}{\hat{P}}_{k?1}^{?1}+A_{k?1}^T{Q}_k^{?1}{A}_{k?1}& ?A_{k?1}^T{Q}_k^{?1}\\ 0& Q_k^{?1}?Q_k^{?1}{A}_{k?1}({\hat{P}}_{k?1}^{?1}+A_{k?1}^T{Q}_k^{?1}{A}_{k?1}{)}^{?1}{A}_{k?1}^T{Q}_k^{?1}+C_k^T{R}_k^{?1}{C}_k\end{array}\right]\left[\begin{array}{c}{\hat{x}}_{k?1}^{{}^{\prime }}\\ {\hat{x}}_k\end{array}\right]=\left[\begin{array}{c}{\hat{P}}_{k?1}^{?1}{\hat{x}}_{k?1}?A_{k?1}^T{Q}_k^{?1}{v}_k\\ Q_k^{?1}{A}_{k?1}({\hat{P}}_{k?1}^{?1}+A_{k?1}^T{Q}_k^{?1}{A}_{k?1}{)}^{?1}({\hat{P}}_{k?1}^{?1}{\hat{x}}_{k?1}?A_{k?1}^T{Q}_k^{?1}{v}_k)+Q_k^{?1}{v}_k+C_k^T{R}_k^{?1}{y}_k\end{array}\right]$$

因此，只需要關注${\hat{x}}_k$：

$$(Q_k^{?1}?Q_k^{?1}{A}_{k?1}({\hat{P}}_{k?1}^{?1}+A_{k?1}^T{Q}_k^{?1}{A}_{k?1}{)}^{?1}{A}_{k?1}^T{Q}_k^{?1}+C_k^T{R}_k^{?1}{C}_k){\hat{x}}_k=Q_k^{?1}{A}_{k?1}({\hat{P}}_{k?1}^{?1}+A_{k?1}^T{Q}_k^{?1}{A}_{k?1}{)}^{?1}({\hat{P}}_{k?1}^{?1}{\hat{x}}_{k?1}?A_{k?1}^T{Q}_k^{?1}{v}_k)+Q_k^{?1}{v}_k+C_k^T{R}_k^{?1}{y}_k$$

根據(jù)SMW恒等式：

$$Q_k^{?1}?Q_k^{?1}{A}_{k?1}({\hat{P}}_{k?1}^{?1}+A_{k?1}^T{Q}_k^{?1}{A}_{k?1}{)}^{?1}{A}_{k?1}^T{Q}_k^{?1}=(Q_k+A_{k?1}{\hat{P}}_{k?1}{A}_{k?1}^T{)}^{?1}$$

定義：

$${\check{P}}_k=Q_k+A_{k?1}{\hat{P}}_{k?1}{A}_{k?1}^T$$

$${\hat{P}}_k=({\check{P}}_k^{?1}+C_k^T{R}_k^{?1}{C}_k{)}^{?1}$$

原式可化簡為：

$$\begin{array}{rl}{\hat{P}}_k^{?1}{\hat{x}}_k& =Q_k^{?1}{A}_{k?1}({\hat{P}}_{k?1}^{?1}+A_{k?1}^T{Q}_k^{?1}{A}_{k?1}{)}^{?1}({\hat{P}}_{k?1}^{?1}{\hat{x}}_{k?1}?A_{k?1}^T{Q}_k^{?1}{v}_k)+Q_k^{?1}{v}_k+C_k^T{R}_k^{?1}{y}_k\\ & =Q_k^{?1}{A}_{k?1}({\hat{P}}_{k?1}^{?1}+A_{k?1}^T{Q}_k^{?1}{A}_{k?1}{)}^{?1}{\hat{P}}_{k?1}^{?1}{\hat{x}}_{k?1}+(Q_k^{?1}?Q_k^{?1}{A}_{k?1}({\hat{P}}_{k?1}^{?1}+A_{k?1}^T{Q}_k^{?1}{A}_{k?1}{)}^{?1}{A}_{k?1}^T{Q}_k^{?1})v_k+C_k^T{R}_k^{?1}{y}_k\\ & ={\check{P}}_k^{?1}{A}_{k?1}{\hat{x}}_{k?1}+{\check{P}}_k^{?1}{v}_k+C_k^T{R}_k^{?1}{y}_k\\ & ={\check{P}}_k^{?1}(A_{k?1}{\hat{x}}_{k?1}+v_k)+C_k^T{R}_k^{?1}{y}_k\\ & ={\check{P}}_k^{?1}{\check{x}}_k+C_k^T{R}_k^{?1}{y}_k\end{array}$$

梳理一下整個過程：

預測：

$$\begin{array}{rl}{\check{x}}_k& =A_{k?1}{\hat{x}}_{k?1}+v_k\\ {\check{P}}_k& =A_{k?1}{\hat{P}}_{k?1}{A}_{k?1}^T+Q_k\end{array}$$

更新：

$$\begin{array}{rl}{\hat{P}}_k& =({\check{P}}_k^{?1}+C_k^T{R}_k^{?1}{C}_k{)}^{?1}\\ {\hat{P}}_k^{?1}{\hat{x}}_k& ={\check{P}}_k^{?1}{\check{x}}_k+C_k^T{R}_k^{?1}{y}_k\end{array}$$

這是逆協(xié)方差形式（信息形式）的卡爾曼濾波。為了得到經(jīng)典形式的卡爾曼濾波，需要定義卡爾曼增益$K_k$：

$$K_k={\hat{P}}_k{C}_k^T{R}_k^{?1}$$

經(jīng)過化簡，更新：

$$\begin{array}{rl}{K}_k& ={\check{P}}_k{C}_k^T(C_k{\check{P}}_k{C}_k^T+R_k{)}^{?1}\\ {\hat{P}}_k& =(1?K_k{C}_k){\check{P}}_k\\ {\hat{x}}_k& ={\check{x}}_k+K_k(y_k?C_k{\check{x}}_k)\end{array}$$

其中，$y_k?C_k{\check{x}}_k$稱為更新量，指的是實際與期望觀測量的誤差，而卡爾曼增益則是這部分更新量對估計值的權重。

通過貝葉斯推斷推導卡爾曼濾波

使用貝葉斯推斷方法還能夠以更簡潔的方式推出卡爾曼濾波。假設$k?1$時刻的高斯先驗為：

$$p(x_{k?1}|{\check{x}}_0,v_{1:k?1},y_{0:k?1})=N({\hat{x}}_{k?1},{\hat{P}}_{k?1})$$

首先，對于預測部份，考慮最近時刻的輸入$v_k$，來計算$k$時刻的先驗：

$$p(x_k|{\check{x}}_0,v_{1:k},y_{0:k?1})=N({\check{x}}_k,{\check{P}}_k)$$

其中：

$${\check{x}}_k=E[x_k]=E[A_{k?1}{x}_{k?1}+v_k+w_k]=A_{k?1}{\hat{x}}_{k?1}+v_k$$

$$\begin{array}{rl}{\check{P}}_k& =E[(x_k?E[x_k])(x_k?E[x_k]{)}^T]\\ & =A_{k?1}E[(x_{k?1}?{\hat{x}}_{k?1})(x_{k?1}?{\hat{x}}_{k?1}{)}^T]A_{k?1}^T+E[w_k{w}_k^t]\\ & =A_{k?1}{\hat{P}}_{k?1}{A}_{k?1}^T+Q_k\end{array}$$

然后，對于更新部分，將狀態(tài)與最新一次測量（即$k$時刻）寫成聯(lián)合高斯分布的形式：

$$p(x_k,y_k|{\check{x}}_0,v_{1:k},y_{0:k?1})=N(\left[\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right],\left[\begin{array}{cc}{\Sigma }_{xx}& {\Sigma }_{xy}\\ {\Sigma }_{yx}& {\Sigma }_{yy}\end{array}\right])=N(\left[\begin{array}{c}{\check{x}}_k\\ C_k{\check{x}}_k\end{array}\right],\left[\begin{array}{cc}{\check{P}}_k& {\check{P}}_k{C}_k^T\\ C_k{\check{P}}_k& C_k{\check{P}}_k{C}_k^T+R_k\end{array}\right])$$

根據(jù)高斯推斷，可以得到：

$$p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})=N({\mu }_x+{\Sigma }_{xy}{\Sigma }_{yy}^{?1}(y_k?{\mu }_y),{\Sigma }_{xx}?{\Sigma }_{xy}{\Sigma }_{yy}^{?1}{\Sigma }_{yx})$$

代入之前的結果，有：

$$\begin{array}{rl}{K}_k& ={\check{P}}_k{C}_k^T(C_k{\check{P}}_k{C}_k^T+R_k{)}^{?1}\\ {\hat{P}}_k& =(1?K_k{C}_k){\check{P}}_k\\ {\hat{x}}_k& ={\check{x}}_k+K_k(y_k?C_k{\check{x}}_k)\end{array}$$

這與MAP給出的更新步驟的方程是完全一致的。

重申一遍，這件事情的根本在于使用了線性模型，且噪聲和先驗也都是高斯的。在這些條件下，后驗概率也是高斯的，于是它的均值和模正巧是一樣的。然而在使用非線性模型之后就不能保證這個性質了。

從增益最優(yōu)化的角度來看卡爾曼濾波

通常來說，卡爾曼濾波是線性高斯系統(tǒng)下的最優(yōu)解。因此，也可以從其他的角度來看卡爾曼濾波的最優(yōu)特性。下面介紹其中的一個：

假設有一個估計器，形式如下：

$${\hat{x}}_k={\check{x}}_k+K_k(y_k?C_k{\check{x}}_k)$$

但是此時并不知道如何選取$K_k$的值，才能正確地衡量修正部分的權重。如果定義狀態(tài)的誤差為（估計值 - 真值）：

$${\hat{e}}_k={\hat{x}}_k?x_k$$

那么有：

$$\begin{array}{rl}{\hat{P}}_k& =E[{\hat{e}}_k{\hat{e}}_k^T]\\ & =E[({\hat{x}}_k?x_k)({\hat{x}}_k?x_k{)}^T]\\ & =E[({\check{x}}_k+K_k(C_k{x}_k+n_k?C_k{\check{x}}_k)?x_k)({\check{x}}_k+K_k(C_k{x}_k+n_k?C_k{\check{x}}_k)?x_k{)}^T]\\ & =E[((1?K_k{C}_k)({\check{x}}_k?x_k)+K_k{n}_k)((1?K_k{C}_k)({\check{x}}_k?x_k)+K_k{n}_k{)}^T]\\ & =(1?K_k{C}_k)E[({\check{x}}_k?x_k)({\check{x}}_k?x_k{)}^T](1?K_k{C}_k{)}^T+K_{k}E[C_k{C}_k^T]K_k^T\\ & =(1?K_k{C}_k){\check{P}}_k(1?K_k{C}_k{)}^T+K_k{R}_k{K}_k^T\\ & ={\check{P}}_k?K_k{C}_k{\check{P}}_k?{\check{P}}_k{C}_k^T{K}_k^T+K_k(C_k{\check{P}}_k{C}_k^T+R_k)K_k^T\end{array}$$

接下來最小均方差開始正式登場了。由于協(xié)方差矩陣的對角線元素就是方差，這樣一來，把協(xié)方差矩陣的對角線元素求和，用$tr$來表示這種算子，它的學名叫矩陣的跡。

于是，可以由它定義出一個代價函數(shù)：

$$\begin{array}{rl}J(K_k)& =tr(E[{\hat{e}}_k{\hat{e}}_k^T])\\ & =tr({\check{P}}_k)?2tr(K_k{C}_k{\check{P}}_k)+tr(K_k(C_k{\check{P}}_k{C}_k^T+R_k)K_k^T)\end{array}$$

最小均方差就是使得$J(K_k)$最小，對未知量$K_k$求導，令導函數(shù)等于0：

$$\frac{dJ(K_k)}{d{K}_k}=?2(C_k{\check{P}}_k{)}^T+2K_k(C_k{\check{P}}_k{C}_k^T+R_k)$$

因此：

$$K_k={\check{P}}_k{C}_k^T(C_k{\check{P}}_k{C}_k^T+R_k{)}^{?1}$$

這正是卡爾曼增益的通常表達式。

關于卡爾曼濾波的討論

以下是卡爾曼濾波的要點：

1. 對于高斯噪聲的線性系統(tǒng)，卡爾曼濾波器是最優(yōu)線性無偏估計；
2. 必須有初始狀態(tài)： { x ˇ 0 , P ˇ 0 } \{\check x_0,\check P_0\} {xˇ0?,Pˇ0?}；
3. 協(xié)方差部分與均值部分可以獨立地遞推。有時甚至可以計算一個固定的$K_k$，用于所有時刻的均值修正，這種做法稱為固定狀態(tài)的卡爾曼濾波。