卡爾曼濾波（Karman Filter）
卡爾曼濾波器是什么？
對於卡爾曼濾波器，實際上用濾波器來描述卡爾曼濾波器算法其實並不准確。卡爾曼濾波器最好地叫法是最優化遞歸數字處理算法（Optimal Recursive Data Processing Algorithm）,本質上更加像一個觀測器。
卡爾曼濾波器的作用？
卡爾曼濾波器是用來處理我們生活中的不確定性的算法。我們生活中充滿了不確定性，無論是測量的數據，還是估計的數據都不是准確的，我們就需要這些數據來得到一個最優的估計值。
不確定性：
- 不存在完美的數學模型
- 系統的擾動不可控，也很難建模
- 測量的傳感器也存在誤差
實例講解卡爾曼濾波器（迭代）
用一把尺子測量一枚硬幣，每次測量的結果為\({Z}_{i}\)，那么\(Z_k\)就是第\(k\)次的測量結果。\({Z}_{1}\)=50.1mm、\({Z}_{2}\)=50.4mm、\({Z}_{3}\)=50.2mm。估計真實的硬幣寬度。
估計真實的硬幣數據很容易就想到求取平均值。
\[\begin{aligned}\hat{x}_{k}&=\frac{1}{k}\left(z_{1}+z_{2}+\cdots \cdot+z_{k}\right)\\&=\frac{1}{k}\left(z_{1}+z_{2}+\cdots+z_{k-1}\right)+\frac{1}{k} z_{k}\\&=\frac{1}{k} \frac{k-1}{k-1}\left(z_{1}+z_{2}+\cdots+z_{k-1}\right)+\frac{1}{k} z_{k}\ 其中\frac{1}{k-1}\left(z_{1}+z_{2}+\cdots \cdot+z_{k-1}\right)表示的是\hat{x}_{k-1}\\&=\frac{k-1}{k} \hat{x}_{k-1}+\frac{1}{k} z_{k}\\&=\hat{x}_{k-1}-\frac{1}{k} \hat{x}_{k-1}+\frac{1}{k} z_{k}\\\hat{x}_{k}&=\hat{x}_{k-1}+\frac{1}{k}\left(z_{k}-\hat{x}_{k-1}\right)\end{aligned} \]
分析：\(k\)逐漸的增大，\(\frac{1}{k}\)就會逐漸趨於0，\(\hat{x}_{k}\)—>\(\hat{x}_{k-1}\)。也就是說隨着\(k\)的增加，再繼續測量已經不太重要了。<大量測量已經對結果有信心了>。相反\(k\)減小，\({z}_{k}\)的作用就表較大。
\(\hat{x}_{k}=\hat{x}_{k-1}+\frac{1}{k}\left(z_{k}-\hat{x}_{k-1}\right)\) 表示的是：
當前的估計值=上一次的估計值+系數 *（當前測量值 - 上一次的估計值）
\(\frac{1}{k}\)用\({K}_{k}\)表示，再卡爾曼濾波器里面表示卡爾曼增益（Karman Gain）。可以發現上面的表達式中，當前的估計值跟上一次的估計值有關，而上一次的估計值又跟上上次的估計值有關。這個就是遞歸思想。
示例作圖顯示？
引入參數：
估計誤差\({e}_{EST}\)，表示的是估計值與真實值的誤差。\(EST\)表示Estimate
測量誤差\({e}_{MEA}\)，表示的是測量值與真實值的誤差。\(MEA\)表示Measure
定義\({K}_{k}\)（怎么來的不清楚，感覺跟后面關聯性不大）。
\[k_{k}=\frac{e_{E S T_{k-1}}}{e_{E ST_{k-1}}+e_{M E A_{k}}} \]
同理可以分析
- \(e_{E S T_{k-1}} \gg e_{M E A_{k}}\)時，\(K_{k}\)——>1
- \(e_{E S T_{k-1}} \ll e_{M E A_{k}}\)時，\(K_{k}\)——>0
例子：多次測量一枚硬幣，真實值時50mm，初始估計值為40mm，方差為5mm，初始測量值為51mm，方差為3mm。
可以發現多測測量與估計后，估計值逐漸的接近於50mm。
幾個關鍵名詞？
數據融合（Data Fusion）
兩個稱稱東西，
一個稱得\({Z}_{1}= 30\)，
\[\begin{array}{l} z_{1}=30 \mathrm{~g} \quad \sigma_{1}=2g \quad\begin{array}{r} \text { Natural Distribution 正態分布 } \end{array}\\ z_{2}=32 g \quad \sigma_{2}=4g \quad \end{array}\]
估計真實得值\(\hat{Z}\)？
利用卡爾曼增益思想，\(\hat{Z}={Z}_{1}+K*({Z}_{2}-{Z}_{1})\)
估計值的表達式我們已經給出上，如上面所示。那么我們現在不知道的就是\(K\),所以我們現在要求的就是\(K\)使得\(\hat{Z}\)的方差\(var(\hat{Z})\)最小。方差實際上表示的是測量值與真實值之間的的波動情況，越小自然波動也就越小，接近真實值的概率也就越大。
\[\begin{aligned}\sigma_{z}^{2}&=\operatorname{Var}\left(z_{1}+k\left(z_{2}-z_{1}\right)\right)=\operatorname{var}\left(z_{1}-k z_{1}+k z_{2}\right)=\operatorname{var}\left((1-k) z_{1}+k z_{2}\right)\\&=\operatorname{Var}\left((1-k) z_{1}\right)+\operatorname{Van}\left(k z_{2}\right)=(1-k)^{2} \operatorname{Var}\left(z_{1}\right)+k^{2} \operatorname{Var}\left(z_{2}\right)\\&=(1-k)^{2} \sigma_{1}^{2}+k^{2} \sigma_{2}^{2}\end{aligned} \]
對方差進行求導，
\[\frac{d \sigma_{z}^{2}}{d k}=0 \]
\[-2(1-k) \sigma_{1}^{2}+2 k \sigma_{2}^{2}=0 \Rightarrow-\sigma_{1}^{2}+k v_{1}^{2}+k \sigma_{2}^{2} \]
\[k\left(\sigma_{1}^{2}+\sigma_{2}^{2}\right)=\sigma_{1}^{2} \Rightarrow k=\frac{\sigma_{1}^{2}}{\sigma_{1}^{2}+\sigma_{2}^{2}} \]
\[=\frac{2^{2}}{2^{2}+4^{3}}=\frac{4}{4+16}=0.2 \]
\[\hat{z}=z_{1}+k\left(z_{2}-z_{1}\right)=30+0.2(32-30)=30.4 g \]
根據兩個稱的特點（利用方差，正態分布）基於兩個稱做出最優的估計30.4g並通過數學證明這是最優解。這個過程就叫做數據融合。<根據兩個都不准確的數，得到一個准確度更高的結果，基於這兩個數>
協方差矩陣
協方差矩陣，把方差和協方差在一個矩陣中表現出來（體現變量之間的聯動，指的是兩個變量之間的相關性）
\(\sigma_{x} \sigma_{y}\)表示協方差。
具體協方差內容查看
狀態空間表達式
\[\begin{aligned}\left[\begin{array}{l} \dot{x}_{1} \\ \dot{x}_{2} \end{array}\right]&=\left[\begin{array}{cc} 0 & 1 \\ -\frac{k}{m} & -\frac{\beta}{m} \end{array}\right]\left[\begin{array}{l} x_{1} \\ x_{2} \end{array}\right]+\left[\begin{array}{l} 0 \\ \frac{1}{m} \end{array}\right] u\\\left[\begin{array}{l} z_{1} \\ z_{2} \end{array}\right]&=\left[\begin{array}{ll} 1 & 0 \\ 0 & 1 \end{array}\right]\left[\begin{array}{l} x_{1} \\ x_{2} \end{array}\right]\end{aligned}\]
簡寫為：
\[\begin{aligned}X_{k}&=A X_{k-1}+B U_{k}+\omega_{k-1} &&( \omega_{k-1}表示過程噪聲)\\Z_{k}&=H X_{k}+v_{k}&&(v_{k}表示測量噪聲)\end{aligned} \]
卡爾曼增益（Karman Gain）的詳細推導
狀態空間方程：
\[\begin{aligned}X_{k}&=A x_{k-1}+B u_{k-1}+w_{k-1} \\ Z_{k}&=H x_{k}+v_{k}\end{aligned}\]
\[\begin{aligned} P(\omega) &\sim(0, Q)\\Q&=E\left[\begin{array}{ll} \omega * \omega ^T \end{array}\right]\left[\begin{array}{l} x_{1} \\ x_{2} \end{array}\right] \rightarrow\left\{\begin{array}{l} \omega_{1} \\ w_{2} \end{array}\right]\\ \operatorname{VAR}(x)&=E\left(x^{2}\right)+E^{2}(x)^{=0}=E(x^{2})\\E\left[\left[\begin{array}{l} w_{1} \\ w_{2} \end{array}\right]\left[\begin{array}{ll} w_{1} & w_{2} \end{array}\right]\right]&=E\left[\begin{array}{cc} w_{1}^{2} & w_{1} w_{2} \\ w_{2} w_{1} & w_{2}^{2} \end{array}\right]\\&=\left[\begin{array}{cc} \sigma_{1}^{2} & \sigma_{1} \sigma_{2} \\ \sigma_{2} \sigma_{1} & \sigma_{2}^{2} \end{array}\right]&就是協方差矩陣 \end{aligned} \]
同理\(P(v) \sim(0, R)\)也是服從正態分布，\(R\)為協防差矩陣，和\(Q\)一樣，推導過程和右上角推導同理。
- \(\hat{X}_{k}^{-}=A \hat{X}_{k-1}+B u_{k-1}\)，\(\hat{X}_{k}^{-}\)右上角有一個
- 表示先驗估計，這個時沒有加噪聲的估計。計算出來的結果。
- \(Z_{k}=H{X_{k}} \longrightarrow \hat{x}_{k M E A}=H^{-1} Z_{k}\)，這個\(\hat{x}_{k M E A}\)表示的是測量結果。
\(\hat{X}_{k}^{-}\)和\(\hat{x}_{k M E A}\)都不准確，從而需要根據這兩個值來估計最優值。
\[\hat{X}_{k}=\hat{x}_{k}+G\left(H^{-}Z_{ k}-\hat{X}_{k}\right) \]
\(\hat{X}_{k}\)表示最優估計值，又因為\(G=K_{k} H\)，故卡爾曼濾波器\(\hat{X}_{k}=\hat{X}_{k}+K_{k}\left(Z_{k}-H \hat{X}_{k}\right)\)，\(K_{k} \in\left[0 , H^{-1}\right]\)。
現在的目標就是尋找\(K_k\)，使得\(\hat{X}_{k}\)趨近於真實值\({X}_{k}\)（實際值）。
引入誤差：\(e_{k}=x_{k}-\hat{x}_{k}\)，而且\(e_k\)服從正態分布\(P\left(e_{k}\right) \sim(0, P)\)。\(P\)就是協方差，跟前面的\(Q\)，\(R\)一樣
\[P=E\left[e * e^{T}\right]=\left[\begin{array}{ll} \sigma_{e1}^{2} & \sigma_{e1}\sigma_{e2}\\ \sigma_{e2}\sigma_{e1} & \sigma_{e2}^{2} \end{array}\right]\]
因為期望為0，所以方差越小，那么誤差就越接近於0，誤差就越小。（因為正太分布方差越小，圖像就越尖，概率值就越向期望0靠近。）
因為\(P\)為協方差矩陣，所以我們希望\(P\)的對角線上的元素和最小，即便就是\(P\)的跡最小。
\[\begin{aligned} P_{k} &=E[e* e^{T}] \\ &=E\left[\left(\hat{x}_{k}-\hat{x}_{k}\right)\left(x_{k}-\hat{x}_{k}\right)^T\right]\\&=E\left\{\left[\left(I-k_{k} H\right) e_{k}-k_{k} v_{k}\right]\left[\left(I-k_{k} H\right) e_{k}-k_{k} v_{k}\right]^{T}\right] \end{aligned}\]
然后就是一些列的化簡得到，
\[=\left(I-k_{k} H\right) E\left(e _{k}^{-} e_{k}^{-T}\right)\left(I-k_{k} H\right)^{T}+k_{k} E\left(v_{k} v_{k}^{T}\right) k_{k}^{T} \]
\[P_{k}=P_{k}^{-}-k_{k} H P_{k}^{-}-P_{k}^{-} H^{T} k_{k}^{T}+k_{k} H P_{k}^{-}H^{T} K_{k}^{T}+K_{k} R K_{k}^{T} \]
\[\operatorname{tr}\left(P_{k}\right)=\operatorname{tr}\left(P_{k}^{-}\right)-2 t_{r}\left(k_{k} H P_{k}^{-}\right)+\operatorname{tr}\left(k_{k} H_{k}^{-} H^{T} K_{k}\right)+\operatorname{tr}\left(k_{k} R K_{k}\right) \]
\[\begin{aligned} x_{k}-\hat{x}_{k} &=x_{k}-\left(\hat{x}_{k}^{-}+k_{k}\left(z_{k}-H \hat{x}_{k}^{-}\right)\right.\\ &=x_{k}-\hat{x}_{k}^{-}-k_{k} Z _k+k_{k} H \hat{x}_{k}^{-} \\ &=x_{k}-\hat{x}_{k}^{-}-k_{k}\left(H x_{k}+V_{k}\right)+k_{k} H x_{k} ^{-}\\ &=x_{k}-\hat{x}_{k}^{-}-k_{k} H x_{k}-k_{k} v_{k}+k_{k} H \hat{x}_{k}^{-}\\& =\left(x_{k}-\hat{x}_{k}^{-}\right)-k_{k} H\left(x_{k}-x_{k}^{-}\right)-k_{k} V_{k} \\ &=\left(I-k_{k }H\right)\left(x_{k}-\hat{x}_{k}^{-}\right)-k_{k} V_{k}\\&e_{k}^{-}=x_{k}-\hat{x}_{k}^{-}表示為e_k的先驗誤差 \end{aligned}\]
求\(K_k\)使得\(tr(p_k)\)有最小值。有\(\frac{d tr\left(p_{k}\right)}{d k_{k}}=0\)，即便就是分別對每一項求導。
\[\frac{d t r\left(P_{k}\right)}{d{k_ k}}=0-2\left(H P_{k}^{-}\right)^{T}+2 k_{k} H P_{k}^{-} H^T+2 k_{k} R=0 \]
協方差的轉置等於本身，\(P_{k}^{-T}=P_{k}\)
\[-P_{k}H ^T+K_{k}\left(H P_{k}^{-} H^{T}+R\right)=0 \]
得到卡爾曼濾波器核心公式（卡爾曼增益公式）：
\[K_{k}=\frac{P_{k}^{-} H ^T}{H P_{k}^{-} H ^T+R} \]
- \(R\)特別大。\(K_k\)就趨近於0，\(X_k=\hat X_k^{-}\)。
- \(R\)特別小，\(K_k\)就趨近於H-，\(X_k = H^{-}Z_{k}\)。
卡爾曼增益公式中的\(P_{k}^{-}\)是未知的
求\(P_{k}^{-}\)，\(P_{k}^{-}=E\left[e_{k}^{-} \cdot e_{k}^{-T}\right]\)
\[\begin{aligned} P_{k}^{-} &=E\left[\left(A e _{k-1}+w_{k-1}\right)\left(A e_{k-1}+w_{k-1}\right)^{T}\right] \\ &\left.=E\left[( A e_{k-1}+w_{k-1}\right)\left(e_{k-1}^{T} A^{T}+W_{k-1}^{T}\right)\right] \\ &=E\left[A ( e_{k-1}^{T} A^{T}+{A} e_{k-1} w_{k-1}+w_{k-1} e _{k-1} ^{T}A^{T}+w_{k-1}w_{k-1}^{T}\right.\\&=A*E \left(e_{k-1} e _{k-1}^{T}\right)A^{T}+E\left(w_{k-1} \omega_{k-1}^{T}\right) \end{aligned}\]
\(P_{k-1 }=E \left(e_{k-1} e _{k-1}^{T}\right)\)而且\(Q = E\left(w_{k-1} \omega_{k-1}^{T}\right)\)
\[P_{k}^{-}=A P_{k-1} A^{T}+{Q} \]
\[\begin{aligned} e_ k^{-} &={x}_{k}-\hat{x} _{k}^{-} \\ &=A x_{k-1}+B u_{k-1}+w_{k-1}-A \hat{x}_{k-1}+B U_{k-1} \\ &=A\left(x_{k-1}-\hat{x}_{k-1}\right)+w_{k-1} \end{aligned}\]
因為\(e_{k-1}=x_{k-1}-\hat x_{k-1}\)
卡爾曼濾波的幾個部分：
預測部分：
先驗：\(\hat{X}_{k}^{-}=A \hat{X}_{k-1}+B U_{k-1}\)
先驗校正：\(P_{k}^{-}=A P_{k-1} A^{T}+{Q}\)
最開始的時候需要初始化\(\hat X_0\)，\(P_0\)
校正部分：
卡爾曼增益：\(K_{k}=\frac{P_{k}^{-} H ^T}{H P_{k}^{-} H ^T+R}\)
后驗估計：\(\hat{X}_{k}=\hat{X}_{k}^{-}+K_{k}\left(Z_{k}-H \hat{X}_{k}^{-}\right)\)
更新誤差協方差：
\[P_{k}=\left(I-K_{k} H\right) P_{k}^{-} \]
過程就是因為需要求最優估計，由\(\hat{X}{k}=\hat{X}{k}+K_{k}\left(Z_{k}-H \hat{X}_{k}\right)\)\(K_k\)是未知的，由 \(K_{k}=\frac{P_{k}^{-} H ^T}{H P_{k}^{-} H ^T+R}\)可以得到\(K_k\)；但是\(P_{k}^{-}\)是未知的，由\(P_{k}^{-}=A P_{k-1} A^{T}+{Q}\)可以求得；又因為下一次迭代需要\(P_{k-1}\)，因此需要更新\(P_{k}=\left(I-K_{k} H\right) P_{k}^{-}\)。
\(P_{k}=\left(I-K_{k} H\right) P_{k}^{-}\)怎么來的？
\[P_{k}=P_{k}^{-}-k_{k} H P_{k}^{-}-P_{k}^{-} H^{T} k_{k}^{T}+k_{k} H P_{k}^{-}H^{T} K_{k}^{T}+K_{k} R K_{k}^{T} \]
化簡后得到：
\[P_{k}=P_{k}^{-}-\mathrm{K_kHP_k}^{-}=(I-\mathrm{K_k} \mathrm{H}) \mathrm{P_k}^{-} \]
詳細視頻見【卡爾曼濾波器】1_遞歸算法_Recursive Processing 講得很好，這篇博客基本就是這個視頻轉過來的文字版本。控制工程的寶藏博主。
