卡尔曼滤波（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 讲得很好，这篇博客基本就是这个视频转过来的文字版本。控制工程的宝藏博主。
