卡爾曼濾波

在訊號與系統裡面，我們研究一個系統的特性，通常是通過系統的輸入輸出來求解系統函式，這樣的研究思路相當於將系統當成了一個黑箱，並未對系統本身特性進行建模處理，因此也未將關於系統模型的一些先驗知識應用上去。現在我們嘗試用另外一種思路來對一個系統進行研究，在此需要引入狀態空間的概念。在這種研究思路下，系統的特效能通過系統的狀態變化完全體現出來。通常，系統的狀態具有如下兩個特點：一是系統的狀態能夠體現系統本身的特性，它的變化遵循一定的物理規律，可對其進行建模，進而將與之相關的一些先驗知識應用起來；二是系統的狀態通常難以直接測量得到，因此需要依賴特定的觀測向量來了解系統的狀態。根據上面的描述我們建立下面的狀態方程和觀測方程

\[\begin{equation} \begin{aligned} {\rm State \quad Equation:}\quad &\boldsymbol{x}_{n+1}=f(\boldsymbol{x}_n,\boldsymbol{v}_n)\\ {\rm Observation \quad Equation:}\quad &\boldsymbol{y}_n=g(\boldsymbol{x}_n,\boldsymbol{w}_n) \end{aligned} \end{equation}\tag{1} \]

在上面的表示式中狀態方程表示不同時刻系統狀態之間的變化規律，它反映了系統最本質的特性，由系統模型決定。觀測方程表示了觀測向量和系統狀態之間的轉化關係。其中\(\boldsymbol{v}_n\)

，\(\boldsymbol{w}_n\)分別表示狀態噪聲和觀測噪聲，通常我們假設兩者為零均值白噪聲。(1)中兩個方程是一般化的情況，在實際應用中通常可用線性模型進行轉化，以方便計算，為此，將(1)改寫為

\[\begin{equation} \begin{aligned} {\rm State \quad Equation:}\quad &\boldsymbol{x}_{n+1}=\boldsymbol{F}_n \boldsymbol{x}_n+\boldsymbol{v}_n\\ {\rm Observation \quad Equation:}\quad &\boldsymbol{y}_n=\boldsymbol{H}_n \boldsymbol{x}_n+\boldsymbol{w}_n \end{aligned} \end{equation}\tag{2} \]

在上面的表示式中\(\boldsymbol{F}_n\)

，\(\boldsymbol{H}_n\)分別表示狀態轉移矩陣和觀測矩陣，需要注意的是這兩者都帶下標\(n\)，說明這兩者是時變的，這也就意味著，此時允許系統是非平穩的。

---

基於上面的狀態方程和觀測方程，我們的目的是更好地估計系統的狀態。為此，我們先定義如下一些符號，方便後續說明：

\[\begin{equation} \begin{aligned} \hat{\boldsymbol{x}}_{n|n}&={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_n\\ \hat{\boldsymbol{x}}_{n+1|n}&={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_{n+1}\\ \hat{\boldsymbol{x}}_{n+1|n+1}&={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n,\boldsymbol{y}_{n+1})}\boldsymbol{x}_{n+1}\\ \end{aligned} \end{equation}\tag{3} \]

其中\(\boldsymbol{x}_n,\boldsymbol{x}_{n+1}\)分別表示\(n\)時刻和\(n+1\)時刻的狀態真值，\(\hat{\boldsymbol{x}_{n|n}}\)表示用\(1-n\)時刻的觀測向量\((\boldsymbol{y}_1,...,\boldsymbol{y}_n)\)估計得到的\(n\)時刻的狀態向量，\(\hat{\boldsymbol{x}_{n+1|n}}\)表示用\(1-n\)時刻的觀測向量\((\boldsymbol{y}_1,...,\boldsymbol{y}_n)\)估計得到的\(n+1\)時刻的狀態向量，\(\hat{\boldsymbol{x}_{n+1|n+1}}\)表示用\(1-n+1\)時刻的觀測向量\((\boldsymbol{y}_1,...,\boldsymbol{y}_n,\boldsymbol{y}_{n+1})\)估計得到的\(n+1\)時刻的狀態向量。\({\rm Proj}\)表示投影操作，且有\({\rm Proj}_\boldsymbol{Y}\boldsymbol{X} ={\rm E}(\boldsymbol{X}\boldsymbol{Y}^{\rm T})[{\rm E}(\boldsymbol{Y}\boldsymbol{Y}^{\rm T})]^{-1}\boldsymbol{Y}\)。

我們在\(n\)​時刻擁有的資訊包括：觀測向量\(\boldsymbol{y}_1,...,\boldsymbol{y}_n)\)​，\(n\)​時刻的狀態估計向量\(\hat{\boldsymbol{x}}_{n|n}\)​。同時在\(n+1\)​時刻，我們將得到一個新的觀測向量\(\boldsymbol{y}_{n+1}\)​。我們想要利用上述資訊，更好地估計系統在\(n+1\)​時刻的狀態向量，即\(\hat{\boldsymbol{x}}_{n+1|n+1}\)​。一個很自然的思路就是，首先利用\(\hat{\boldsymbol{x}}_{n|n}\)​結合狀態轉移矩陣\(\boldsymbol{F}_n\)​(即系統模型)，可以預測預測得到\(\hat{\boldsymbol{x}}_{n+1|n}\)​。此時，由於系統建模不一定完全準確(\(\boldsymbol{F}_n\)​不一定完全符合實際模型)或者狀態噪聲的影響，使得\(\hat{\boldsymbol{x}}_{n+1|n}\)​並不一定準確。因此，我們需要利用新增加的資訊\(\boldsymbol{y}_{n+1}\)​對上述估計結果進行修正，得到\(\hat{\boldsymbol{x}}_{n+1|n+1}\)​。修正的過程我們越簡單越好，因此我們可以大致猜測修正的方法為:\(\hat{\boldsymbol{x}}_{n+1|n+1}=\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{K}_{n+1}(\boldsymbol{y}_{n+1}-\hat{\boldsymbol{y}}_{n+1})\)​。該方法用觀測向量的估計誤差來進行修正，\(\boldsymbol{K}_{n+1}\)​為修正係數，一方面它用於表示我們相信模型預測結果和相信觀測值的程度，另一方面可以將其理解為對觀測向量和狀態向量之間相互轉換的矩陣，因為觀測向量和狀態向量不僅可能量綱不同，甚至物理含義也不一樣，因此不能直接用觀測向量的估計誤差來修正狀態向量的預測誤差，因此需要在兩者之間做一個轉化。顯然上面的研究思路遵循了預測->修正的思路，即由\(\hat{\boldsymbol{x}}_{n|n} \rightarrow \hat{\boldsymbol{x}}_{n+1|n} \rightarrow \hat{\boldsymbol{x}}_{n+1|n+1}\)​的路線。以下通過嚴謹的推導來證明上述思路的可行性。

---

（1）\(\hat{\boldsymbol{x}}_{n|n} \rightarrow \hat{\boldsymbol{x}}_{n+1|n}\)的過程推導

\[\begin{equation} \begin{aligned} \hat{\boldsymbol{x}}_{n+1|n}&={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_{n+1}={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}(\boldsymbol{F}_n \boldsymbol{x}_n+\boldsymbol{v}_n)\\ &=\boldsymbol{F}_n{\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_n+{\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{v}_n=\boldsymbol{F}_n \hat{\boldsymbol{x}}_{n|n} \end{aligned} \end{equation}\tag{4} \]

在(4)中需要理解的是\({\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{v}_n=0\)​，即\(\boldsymbol{v}_n\)​與\((\boldsymbol{y}_1,...,\boldsymbol{y}_n)\)​是正交的，因為由觀測方程可知，\((\boldsymbol{y}_1,...,\boldsymbol{y}_n)\)​依賴\((\boldsymbol{x}_1,...,\boldsymbol{x}_n)\)​，而根據狀態方程\((\boldsymbol{x}_1,...,\boldsymbol{x}_n)\)​受\(n\)​時刻以前的狀態噪聲影響，並不受\(n\)​時刻的狀態噪聲影響，因此\(\boldsymbol{v}_n\)​與\((\boldsymbol{y}_1,...,\boldsymbol{y}_n)\)​是正交的。

(2) \(\hat{\boldsymbol{x}}_{n+1|n} \rightarrow \hat{\boldsymbol{x}}_{n+1|n+1}\)​​的過程推導

\[\begin{equation} \begin{aligned} \hat{\boldsymbol{x}}_{n+1|n+1}&={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n,\boldsymbol{y}_{n+1})}\boldsymbol{x}_{n+1} \end{aligned} \end{equation}\tag{5} \]

由於在(4)中我們得到了\(\hat{\boldsymbol{x}}_{n+1|n}={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_{n+1}\)，因此我們自然希望若(5)式滿足\({\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n,\boldsymbol{y}_{n+1})}\boldsymbol{x}_{n+1}={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_{n+1}+{\rm Proj}_{\boldsymbol{y}_{n+1}}\boldsymbol{x}_{n+1}\)，則可以利用到已有的結論。但是，遺憾的是，這個等式並不一定滿足，因為\(\boldsymbol{y}_{n+1}\)並不一定與\((\boldsymbol{y}_1,...,\boldsymbol{y}_n,\boldsymbol{y}_{n+1})\)張成的空間正交。因此一個很自然的思路就是對\(\boldsymbol{y}_{n+1}\)進行正交化，令\(\overline{\boldsymbol{y}}_{n+1}=\boldsymbol{y}_{n+1}-{\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{y}_{n+1}\)，據此，可將(5)改寫為

\[\begin{equation} \begin{aligned} \hat{\boldsymbol{x}}_{n+1|n+1}&={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n,\boldsymbol{y}_{n+1})}\boldsymbol{x}_{n+1}={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_{n+1}+{\rm Proj}_{\overline{\boldsymbol{y}}_{n+1}}\boldsymbol{x}_{n+1}\\ &=\hat{\boldsymbol{x}}_{n+1|n}+{\rm E}(\boldsymbol{x}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})[{\rm E}(\overline{\boldsymbol{y}}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})]^{-1}\overline{\boldsymbol{y}}_{n+1} \end{aligned} \end{equation}\tag{6} \]

由於\(\overline{\boldsymbol{y}}_{n+1}=\boldsymbol{y}_{n+1}-{\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{y}_{n+1}=\boldsymbol{y}_{n+1}-{\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}(\boldsymbol{H}_{n+1}\boldsymbol{x}_{n+1}+\boldsymbol{w}_{n+1})=\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n}\)，將其代入(6)有

\[\begin{equation} \begin{aligned} \hat{\boldsymbol{x}}_{n+1|n+1}&=\hat{\boldsymbol{x}}_{n+1|n}+{\rm E}(\boldsymbol{x}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})[{\rm E}(\overline{\boldsymbol{y}}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})]^{-1}\overline{\boldsymbol{y}}_{n+1}\\ &=\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{K}_{n+1}(\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n}) \end{aligned} \end{equation}\tag{7} \]

從(7)可以看出，該修正形式完全符合上面的猜想，即用觀測向量的估計誤差來修正狀態向量的預測值。其中修正權值\(\boldsymbol{K}_{n+1}={\rm E}(\boldsymbol{x}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})[{\rm E}(\overline{\boldsymbol{y}}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})]^{-1}\)​被稱為卡爾曼增益。

(3) \(\boldsymbol{K}_{n+1}={\rm E}(\boldsymbol{x}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})[{\rm E}(\overline{\boldsymbol{y}}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})]^{-1}\)的計算和化簡

\[\begin{equation} \begin{aligned} {\rm E}(\boldsymbol{x}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})&={\rm E}(\boldsymbol{x}_{n+1}(\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})\\ &={\rm E}(\boldsymbol{x}_{n+1}(\boldsymbol{H}_{n+1}\boldsymbol{x}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{w}_{n+1})^{\rm T})\\ &={\rm E}(\boldsymbol{x}_{n+1}(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})\boldsymbol{H}_{n+1}^{\rm T}\\ &={\rm E}((\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})\boldsymbol{H}_{n+1}^{\rm T}=\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T} \end{aligned} \end{equation}\tag{8} \]

在(8)中關鍵的化簡步驟是第3個等號到第4個等號的變換，這邊利用了\(\hat{\boldsymbol{x}}_{n+1|n}\)和\((\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})\)​正交的性質。

\[\begin{equation} \begin{aligned} {\rm E}(\overline{\boldsymbol{y}}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})&={\rm E}((\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n})(\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})\\ &={\rm E}((\boldsymbol{H}_{n+1}\boldsymbol{x}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{w}_{n+1})(\boldsymbol{H}_{n+1}\boldsymbol{x}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{w}_{n+1})^{\rm T})\\ &=\boldsymbol{H}_{n+1}{\rm E}((\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})\boldsymbol{H}_{n+1}^{\rm T}+{\rm E}(\boldsymbol{w}_{n+1}\boldsymbol{w}_{n+1}^T)\\ &=\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}+\boldsymbol{R}_{n+1} \end{aligned} \end{equation}\tag{9} \]

在(9)中\(\boldsymbol{R}_{n+1}\)​表示觀測噪聲協方差矩陣。所以

\[\begin{equation} \begin{aligned} \boldsymbol{K}_{n+1}&={\rm E}(\boldsymbol{x}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})[{\rm E}(\overline{\boldsymbol{y}}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})]^{-1}\\ &=\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}(\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}+\boldsymbol{R}_{n+1})^{-1} \end{aligned} \end{equation}\tag{10} \]

---

上面似乎完成了系統狀態向量的預測和修正過程，但是需要注意的是，\(\boldsymbol{P}_{n+1|n}\)表示狀態向量預測誤差的協方差矩陣，這個東西無法從已知的可用的資訊中獲取，因此上面狀態向量的更新和迭代過程，其實並沒有真正完成。因此我們需要思考如何能夠獲得\(\boldsymbol{P}_{n+1|n}\)？由於系統真實的狀態向量沒有辦法得知，因此直接根據定義是沒有辦法計算\(\boldsymbol{P}_{n+1|n}\)的，這時我們自然想到能不能仿照狀態向量的更新迭代過程，用迭代的形式來獲取\(\boldsymbol{P}_{n+1|n}\)，我們希望完成下面的過程: \(\boldsymbol{P}_{n|n} \rightarrow \boldsymbol{P}_{n+1|n} \rightarrow \boldsymbol{P}_{n+1|n+1}\)。

(4) \(\boldsymbol{P}_{n|n} \rightarrow \boldsymbol{P}_{n+1|n}\)​的推導過程

\[\begin{equation} \begin{aligned} \boldsymbol{P}_{n+1|n}&={\rm E}((\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})\\ &={\rm E}((\boldsymbol{F}_n \boldsymbol{x}_n-\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{v}_n)(\boldsymbol{F}_n \boldsymbol{x}_n-\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{v}_n)^{\rm T})\\ &={\rm E}((\boldsymbol{F}_n \boldsymbol{x}_n-\boldsymbol{F}_n \hat{\boldsymbol{x}}_{n|n}+\boldsymbol{v}_n)(\boldsymbol{F}_n \boldsymbol{x}_n-\boldsymbol{F}_n \hat{\boldsymbol{x}}_{n|n}+\boldsymbol{v}_n)^{\rm T})\\ &=\boldsymbol{F}_n{\rm E}((\boldsymbol{x}_n-\hat{\boldsymbol{x}}_{n|n})(\boldsymbol{x}_n-\hat{\boldsymbol{x}}_{n|n})^{\rm T})\boldsymbol{F}_n^{\rm T}+{\rm E}(\boldsymbol{v}_n\boldsymbol{v}_n)^{\rm T}\\ &=\boldsymbol{F}_n \boldsymbol{P}_{n|n}\boldsymbol{F}_n^{\rm T}+\boldsymbol{Q}_n \end{aligned} \end{equation}\tag{11} \]

在(11)的推導過程中用到了(1)的推導結果。\(\boldsymbol{Q}_n\)表示狀態噪聲協方差矩陣。

(5) \(\boldsymbol{P}_{n+1|n} \rightarrow \boldsymbol{P}_{n+1|n+1}\)​的推導過程

\[\begin{equation} \begin{aligned} \boldsymbol{P}_{n+1|n+1}&={\rm E}((\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n+1})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n+1})^{\rm T}) \end{aligned} \end{equation}\tag{12} \]

其中

\[\begin{equation} \begin{aligned} \boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n+1}&=\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n}-\boldsymbol{K}_{n+1}(\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n})\\ &=\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n}-\boldsymbol{K}_{n+1}(\boldsymbol{H}_{n+1}\boldsymbol{x}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{w}_{n+1})\\ &=(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})-\boldsymbol{K}_{n+1}\boldsymbol{w}_{n+1} \end{aligned} \end{equation}\tag{13} \]

所以

\[\begin{equation} \begin{aligned} \boldsymbol{P}_{n+1|n+1}&={\rm E}(((\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})-\boldsymbol{K}_{n+1}\boldsymbol{w}_{n+1})((\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})-\boldsymbol{K}_{n+1}\boldsymbol{w}_{n+1})^{\rm T})\\ &=(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1}){\rm E}((\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})^{\rm T}+\boldsymbol{K}_{n+1}{\rm E}(\boldsymbol{w}_{n+1}\boldsymbol{w}_{n+1}^{\rm T})\boldsymbol{K}_{n+1}^{\rm T}\\ &=(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})\boldsymbol{P}_{n+1|n}(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})^{\rm T}+\boldsymbol{K}_{n+1}\boldsymbol{R}_{n+1}\boldsymbol{K}_{n+1}^{\rm T}\\ &=\boldsymbol{P}_{n+1|n}-\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}\boldsymbol{K}_{n+1}^{\rm T}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}+\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}\boldsymbol{K}_{n+1}^{\rm T}+\boldsymbol{K}_{n+1}\boldsymbol{R}_{n+1}\boldsymbol{K}_{n+1}^{\rm T}\\ &=\boldsymbol{P}_{n+1|n}-\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}\boldsymbol{K}_{n+1}^{\rm T}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}+\boldsymbol{K}_{n+1}(\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}+\boldsymbol{R}_{n+1})\boldsymbol{K}_{n+1}^{\rm T}\\ &=(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})\boldsymbol{P}_{n+1|n} \end{aligned} \end{equation}\tag{14} \]

需要注意的是在(14)中最後一個等式的化簡過程中，用到了(3)的推導結果，並進行合併同類項。

---

至此，我們完成了卡爾曼濾波的整個推導過程，對上面整個推導過程進行整合，可以得到卡爾曼濾波包含以下5個關係式:

\[\begin{equation} \begin{aligned} \hat{\boldsymbol{x}}_{n+1|n}&=\boldsymbol{F}_n \hat{\boldsymbol{x}}_{n|n}\\ \hat{\boldsymbol{x}}_{n+1|n+1}&=\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{K}_{n+1}(\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n})\\ \boldsymbol{K}_{n+1}&=\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}(\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}+\boldsymbol{R}_{n+1})^{-1}\\ \boldsymbol{P}_{n+1|n}&=\boldsymbol{F}_n\boldsymbol{P}_{n|n}\boldsymbol{F}_n^{\rm T}+\boldsymbol{Q}_n\\ \boldsymbol{P}_{n+1|n+1}&=(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})\boldsymbol{P}_{n+1|n} \end{aligned} \end{equation}\tag{15} \]

可以看出，上述5個方程描述了兩個方面的內容，一個是狀態向量的預測和修正(前三個方程)，另一個是預測協方差矩陣的迭代調整(相當於預測誤差的實時調整)。在實際應用過程中需要給定狀態向量初始值、狀態向量預測誤差協方差矩陣初始值、狀態噪聲協方差矩陣、觀測噪聲協方差矩陣，然後系統模型(狀態轉移矩陣)和觀測模型(觀測矩陣)即可按上式進行迭代處理。