在上一篇文章卡爾曼濾波器原理之基本思想中，我們分析並推導了基於卡爾曼一步預測的濾波器狀態遞推公式，接下來，我們將完成上一次的推導過程。首先，我們拿來上次的推導結果：
\[\hat x(n + 1|{{\bf{Y}}_n}) = \sum\limits_{k = 1}^{n - 1} {E[x(n + 1){\alpha ^H}(k)]{{\bf{R}}^{ - 1}}(k)\alpha (k)} + E[x(n + 1){\alpha ^H}(k)]{{\bf{R}}^{ - 1}}(k)\alpha (k)\]
右側的求和項由過程方程可改寫為：
\[\begin{array}{l}
\sum\limits_{k = 1}^{n - 1} {E[x(n + 1){\alpha ^H}(k)]{{\bf{R}}^{ - 1}}(k)\alpha (k)} \\
= \sum\limits_{k = 1}^{n - 1} {E\left\{ {\left[ {{\bf{F}}(n + 1,n)x(n) + {v_1}} \right]{\alpha ^H}(k)} \right\}{{\bf{R}}^{ - 1}}(k)\alpha (k)} \\
= {\bf{F}}(n + 1,n)\sum\limits_{k = 1}^{n - 1} {E\left\{ {\left[ {x(n) + {v_1}} \right]{\alpha ^H}(k)} \right\}} {{\bf{R}}^{ - 1}}(k)\alpha (k) \\
= {\bf{F}}(n + 1,n)\sum\limits_{k = 1}^{n - 1} {E\left[ {x(n){\alpha ^H}(k)} \right]} {{\bf{R}}^{ - 1}}(k)\alpha (k) \\
= {\bf{F}}(n + 1,n)\hat x(n|{{\bf{Y}}_{n - 1}}) \\
\end{array}\]
同時，如果我們定義矩陣：
\[{\bf{G}}(n) = E[x(n + 1){\alpha ^H}(n)]{{\bf{R}}^{ - 1}}(n)\]
這里G(n)就是大名鼎鼎的卡爾曼增益。結合上面兩個式子，系統狀態的遞推公式可以簡寫為
\[\hat x(n + 1|{{\bf{Y}}_n}) = {\bf{F}}(n + 1,n)\hat x(n|{{\bf{Y}}_{n - 1}}) + {\bf{G}}(n)\alpha (n)\]
等式右側第二項通常稱為修正項，上式的物理意義是比較明確的，它表明：系統狀態的最小均方估計可以由前一個估計再加上修正項得到。這樣一來，觀測值是已知的，說明新息是很容易求出來的，只要再有了卡爾曼增益G(n)，我們就可以由當前狀態預估下一個狀態，這樣不斷的迭代更新...，慢着！好像忽略了一個問題，那就是第一個系統狀態估計，也就是初始值。其實這個好解決，通常設為系統第一個狀態的經驗均值就可以，只要卡爾曼預測過程是收斂的，就不會有問題。至此，我們唯一剩下的問題是：要以什么樣的方式來計算卡爾曼增益G(n)
首先，我們計算
\[E[x(n + 1){\alpha ^H}(n)] = {\bf{F}}(n + 1,n)E[x(n){\alpha ^H}(n)] = {\bf{F}}(n + 1,n)E[x(n){[{\bf{C}}(n)\varepsilon (n,n - 1) + {v_2}(n)]^H}]\]
因為狀態x(n)與噪聲v2(n)互不相關，所以
\[E[x(n + 1){\alpha ^H}(n)] = {\bf{F}}(n + 1,n)E[x(n){\varepsilon ^H}(n,n - 1)]{{\bf{C}}^H}(n) = {\bf{F}}(n + 1,n)E[(\varepsilon (n,n - 1) + \hat x(n|{{\bf{Y}}_{n - 1}})){\varepsilon ^H}(n,n - 1)]{{\bf{C}}^H}(n)\]
又因為狀態誤差向量與其估計正交，因此
\[E[x(n + 1){\alpha ^H}(n)] = {\bf{F}}(n + 1,n)E[\varepsilon (n,n - 1){\varepsilon ^H}(n,n - 1)]{{\bf{C}}^H}(n) = {\bf{F}}(n + 1,n){\bf{K}}(n,n - 1){{\bf{C}}^H}(n)\]
所以
\[{\bf{G}}(n) = E[x(n + 1){\alpha ^H}(n)]{{\bf{R}}^{ - 1}}(n) = {\bf{F}}(n + 1,n){\bf{K}}(n,n - 1){{\bf{C}}^H}(n){{\bf{R}}^{ - 1}}(n)\]
看來，事情還沒有完，又出現了一個新的變量，狀態誤差相關矩陣。看來，還是無法利索的得到卡爾曼增益。那么，我們試試看能不能導出狀態誤差相關矩陣的遞歸表示。這樣，一切就都完美了！讓我們開始吧，這一步耗死了我不少腦細胞。《自適應濾波器原理》這本書中詳細的細節並沒有給出，下面把詳細過程給出來。先把狀態誤差展開：
\[\begin{array}{l}
\varepsilon (n + 1,n) = x(n + 1) - \hat x(n + 1|{{\bf{Y}}_n}) \\
= {\bf{F}}(n + 1,n)x(n) + {v_1} - \left[ {{\bf{F}}(n + 1,n)\hat x(n|{{\bf{Y}}_{n - 1}}) + {\bf{G}}(n)\alpha (n)} \right] \\
= {\bf{F}}(n + 1,n)\left[ {x(n) - \hat x(n|{{\bf{Y}}_{n - 1}})} \right] - {\bf{G}}(n)\alpha (n) + {v_1}(n) \\
= {\bf{F}}(n + 1,n)\left[ {x(n) - \hat x(n|{{\bf{Y}}_{n - 1}})} \right] - {\bf{G}}(n)\left[ {y(n) - {\bf{C}}(n)\hat x(n|{{\bf{Y}}_{n - 1}})} \right] + {v_1} \\
= {\bf{F}}(n + 1,n)\left[ {x(n) - \hat x(n|{{\bf{Y}}_{n - 1}})} \right] - {\bf{G}}(n)\left[ {{\bf{C}}(n)x(n) + {v_2}(n) - {\bf{C}}(n)\hat x(n|{{\bf{Y}}_{n - 1}})} \right] + {v_1}(n) \\
= {\bf{F}}(n + 1,n)\left[ {x(n) - \hat x(n|{{\bf{Y}}_{n - 1}})} \right] - {\bf{G}}(n){\bf{C}}(n)\left[ {x(n) - \hat x(n|{{\bf{Y}}_{n - 1}})} \right] + {\bf{G}}(n){v_2}(n) + {v_1}(n) \\
= \left[ {{\bf{F}}(n + 1,n) - {\bf{G}}(n){\bf{C}}(n)} \right]\varepsilon (n,n - 1) + {\bf{G}}(n){v_2}(n) + {v_1}(n) \\
\end{array}\]
現在我們可以以此為依據把狀態誤差相關矩陣來計算，看看能推出什么。因為展開的過程中式子會比較長，為了公式排版更好，先簡寫一下符號，定義
\[\begin{array}{l}
{\begin{array}{*{20}{c}}
{{\bf{K}}(n,n - 1) = {{\bf{K}}_{n - 1}}} & {{\bf{G}}(n) = {{\bf{G}}_n}} & {{{\bf{Q}}_2}(n) = {{\bf{Q}}_2}} & {{{\bf{Q}}_1}(n) = {\bf{Q}}} \\
\end{array}_1} \\
\begin{array}{*{20}{c}}
{{\bf{C}}(n) = {{\bf{C}}_n}} & {{{\bf{C}}^H}(n) = {\bf{C}}_n^H} \\
\end{array} \\
\begin{array}{*{20}{c}}
{{\bf{R}}(n) = {{\bf{R}}_n}} & {{{\bf{R}}^{ - 1}}(n) = {\bf{R}}_n^{ - 1}} \\
\end{array} \\
\begin{array}{*{20}{c}}
{{\bf{F}}(n + 1,n) = {{\bf{F}}_n}} & {{\bf{F}}(n + 1,n){\bf{F}}(n,n + 1)} \\
\end{array} = {\bf{I}} \\
\end{array}\]
好了，開始正式展開吧，狀態誤差相關矩陣展開可以表示為（以下推導過程中利用了狀態誤差相關矩陣與其共軛相等的性質）：
\[\begin{array}{l}
{\bf{K}}(n + 1,n) = E\left[ {\varepsilon (n + 1,n){\varepsilon ^H}(n + 1,n)} \right] \\
= \left[ {{\bf{F}}(n + 1,n) - {\bf{G}}(n){\bf{C}}(n)} \right]{\bf{K}}(n,n - 1){\left[ {{\bf{F}}(n + 1,n) - {\bf{G}}(n){\bf{C}}(n)} \right]^H} + {\bf{G}}(n){{\bf{Q}}_2}(n){{\bf{G}}^H}(n) + {{\bf{Q}}_1}(n) \\
= [{{\bf{F}}_n} - {{\bf{G}}_n}{{\bf{C}}_n}]{{\bf{K}}_{n - 1}}{[{{\bf{F}}_n} - {{\bf{G}}_n}{{\bf{C}}_n}]^H} + {{\bf{G}}_n}{{\bf{Q}}_2}{\bf{G}}_n^H + {{\bf{Q}}_1} \\
= [{{\bf{F}}_n} - {{\bf{F}}_n}{{\bf{K}}_{n - 1}}{\bf{C}}_n^H{\bf{R}}_n^{ - 1}{{\bf{C}}_n}]{{\bf{K}}_{n - 1}}{[{{\bf{F}}_n} - {{\bf{F}}_n}{{\bf{K}}_{n - 1}}{\bf{C}}_n^H{\bf{R}}_n^{ - 1}{{\bf{C}}_n}]^H} + {{\bf{G}}_n}{{\bf{Q}}_2}{\bf{G}}_n^H + {{\bf{Q}}_1} \\
= {{\bf{F}}_n}[{\bf{I}} - {{\bf{K}}_{n - 1}}{\bf{C}}_n^H{\bf{R}}_n^{ - 1}{{\bf{C}}_n}]{{\bf{K}}_{n - 1}}{[{\bf{I}} - {{\bf{K}}_{n - 1}}{\bf{C}}_n^H{\bf{R}}_n^{ - 1}{{\bf{C}}_n}]^H}{\bf{F}}_n^H + {{\bf{G}}_n}{{\bf{Q}}_2}{\bf{G}}_n^H + {{\bf{Q}}_1} \\
= {{\bf{F}}_n}\left[ {{{\bf{K}}_{n - 1}} - {{\bf{K}}_{n - 1}}{\bf{C}}_n^H{\bf{R}}_n^{ - 1}{{\bf{C}}_n}{\bf{K}}_{n - 1}^H - {{\bf{K}}_{n - 1}}{\bf{C}}_n^H{\bf{R}}_n^{ - 1}{{\bf{C}}_n}{{\bf{K}}_{n - 1}} + {{\bf{K}}_{n - 1}}{\bf{C}}_n^H{\bf{R}}_n^{ - 1}{{\bf{C}}_n}{{\bf{K}}_{n - 1}}{\bf{C}}_n^H{\bf{R}}_n^{ - 1}{{\bf{C}}_n}{\bf{K}}_{n - 1}^H} \right]{\bf{F}}_n^H + {{\bf{G}}_n}{{\bf{Q}}_2}{\bf{G}}_n^H + {{\bf{Q}}_1} \\
= {{\bf{F}}_n}\left[ {{{\bf{K}}_{n - 1}} - 2{{\bf{K}}_{n - 1}}{\bf{C}}_n^H{\bf{R}}_n^{ - 1}{{\bf{C}}_n}{{\bf{K}}_{n - 1}} + {{\bf{K}}_{n - 1}}{\bf{C}}_n^H{\bf{R}}_n^{ - 1}\left( {{{\bf{R}}_n} - {{\bf{Q}}_2}} \right){\bf{R}}_n^{ - 1}{{\bf{C}}_n}{\bf{K}}_{n - 1}^H} \right]{\bf{F}}_n^H + {{\bf{G}}_n}{{\bf{Q}}_2}{\bf{G}}_n^H + {{\bf{Q}}_1} \\
= {{\bf{F}}_n}\left[ {{{\bf{K}}_{n - 1}} - 2{{\bf{K}}_{n - 1}}{\bf{C}}_n^H{\bf{R}}_n^{ - 1}{{\bf{C}}_n}{{\bf{K}}_{n - 1}} + {{\bf{K}}_{n - 1}}{\bf{C}}_n^H{\bf{R}}_n^{ - 1}{{\bf{R}}_n}{\bf{R}}_n^{ - 1}{{\bf{C}}_n}{\bf{K}}_{n - 1}^H - {{\bf{F}}_{n + 1}}{{\bf{G}}_n}{{\bf{Q}}_2}{\bf{G}}_n^H{\bf{F}}_{n + 1}^H} \right]{\bf{F}}_n^H + {{\bf{G}}_n}{{\bf{Q}}_2}{\bf{G}}_n^H + {{\bf{Q}}_1} \\
= {{\bf{F}}_n}\left[ {{{\bf{K}}_{n - 1}} - 2{{\bf{K}}_{n - 1}}{\bf{C}}_n^H{\bf{R}}_n^{ - 1}{{\bf{C}}_n}{{\bf{K}}_{n - 1}} + {{\bf{K}}_{n - 1}}{\bf{C}}_n^H{\bf{R}}_n^{ - 1}{{\bf{C}}_n}{\bf{K}}_{n - 1}^H} \right]{\bf{F}}_n^H - {{\bf{G}}_n}{{\bf{Q}}_2}{\bf{G}}_n^H + {{\bf{G}}_n}{{\bf{Q}}_2}{\bf{G}}_n^H + {{\bf{Q}}_1} \\
= {{\bf{F}}_n}\left[ {{{\bf{K}}_{n - 1}} - {{\bf{K}}_{n - 1}}{\bf{C}}_n^H{\bf{R}}_n^{ - 1}{{\bf{C}}_n}{{\bf{K}}_{n - 1}}} \right]{\bf{F}}_n^H + {{\bf{Q}}_1} \\
= {{\bf{F}}_n}\left[ {{{\bf{K}}_{n - 1}} - {{\bf{F}}_{n + 1}}{{\bf{G}}_n}{{\bf{C}}_n}{{\bf{K}}_{n - 1}}} \right]{\bf{F}}_n^H + {{\bf{Q}}_1} \\
= {\bf{F}}(n + 1,n)\left[ {{\bf{K}}(n,n - 1) - {\bf{F}}(n,n + 1){\bf{G}}(n){\bf{C}}(n){\bf{K}}(n,n - 1)} \right]{{\bf{F}}^H}(n + 1,n) + {{\bf{Q}}_1}(n) \\
\Rightarrow \left\{ {\begin{array}{*{20}{c}}
{{\bf{K}}(n + 1,n) = {\bf{F}}(n + 1,n){\bf{K}}(n){{\bf{F}}^H}(n + 1,n) + {{\bf{Q}}_1}(n)} \hfill \\
{{\bf{K}}(n) = {\bf{K}}(n,n - 1) - {\bf{F}}(n,n + 1){\bf{G}}(n){\bf{C}}(n){\bf{K}}(n,n - 1)} \hfill \\
\end{array}} \right. \\
\end{array}\]
上述就是著名的Riccati遞歸方程。不容易啊，至此，終於完成了線性卡爾曼濾波的推導過程。總結以上過程，線性卡爾曼濾波器主要步驟表示如下：
\[\begin{array}{l}
{\bf{G}}(n) = {\bf{F}}(n + 1,n){\bf{K}}(n,n - 1){{\bf{C}}^H}(n){[{\bf{C}}(n){\bf{K}}(n,n - 1){{\bf{C}}^H}(n) + {{\bf{Q}}_2}(n)]^{ - 1}} \\
\alpha (n) = y(n) - {\bf{C}}(n)\hat x(n|{{\bf{y}}_{n - 1}}) \\
\hat x(n + 1|{{\bf{y}}_n}) = {\bf{F}}(n + 1,n)\hat x(n|{{\bf{y}}_{n - 1}}) + {\bf{G}}(n)\alpha (n) \\
{\bf{K}}(n) = {\bf{K}}(n,n - 1) - {\bf{F}}(n,n + 1){\bf{G}}(n){\bf{C}}(n){\bf{K}}(n,n - 1) \\
{\bf{K}}(n + 1,n) = {\bf{F}}(n + 1,n){\bf{K}}(n){{\bf{F}}^H}(n + 1,n) + {{\bf{Q}}_1}(n) \\
\end{array}\]
現在，再回想這兩篇博客的分析過程，不難看出，卡爾曼濾波器通過使用過程方程對系統的行為進行建模，再通過測量方程對觀測行為進行建模。盡可能利用測量信息來得到准確的狀態，也是說卡爾曼濾波器的核心是如何求解過程方程和測量方程，難點在於理解均值和協方差的傳播過程。再往大了說，它教給了我們一整套如何去認識系統、觀察分析系統的方法。可能分析到這里，可能很多朋友覺得終於理解了這個重要的濾波器。可事情並沒有這么簡單，應該還有以下疑問需要進一步的解決，不解決掉，很可能沒有真正理解透徹。
首先是過程方程，可以看到，過程方程對系統的建模是，系統的下一個狀態只與前一個狀態有關。可能在有的系統中是可以的，但不夠通用化，也不符合很多情況。實際中最常見到的情景是系統的當狀態不只應該與前面的狀態有關，還與當前的輸入有關，這方面可以去看下Lyapunov方程。
其次，以上推導過程只是前人把理論摸透了，后人整理規整出來的，並不是正常思維下的人們解決問題的思考方法。也就是說，這里並沒有解決濾波器的發明者是如何想到用這個方法去解決碰到的問題
再次，卡爾曼濾波器還有很多變種，這些變種是出於什么目的，為了解決什么問題被發明出來，優劣如何，最好也要搞清楚。
最后，這里理論分析太多了，沒有給出一個或幾個實際例子，真碰到問題，可能還是不會靈活的使用，甚至不會用。這幾個問題先留着吧，后面逐步進行解答，不管怎么樣，先把基本思想理解了。也是一件值得開心的事呢！