卡尔曼滤波学习笔记（含公式推导）

卡尔曼滤波（Kalman Filter）能用于各种状态的预测（温度、湿度、距离等可量化值），并基于测量情况对预测结果进行校正。

卡尔曼滤波主要基于两组数据：

• 预测的状态和预测误差
• 测量的状态和测量误差

因为各种噪声的存在，预测误差和测量误差都不可忽略，两者的大小共同决定了相信预测多一些还是相信测量多一些。
假设FK用于对位置的预测，那么我们要预测变量有二：位置和速度。
本文把公式的推导分为6个步骤：

1. 状态预测
2. 协方差预测
3. 已知外部因素
4. 未知外部因素
5. 预测空间映射至测量空间
6. 校正

最终得出卡尔曼滤波的5个公式。

1. 状态预测
在当前k时刻，p和v分别表示位置（position）和速度（velocity）：
$$\widehat{x_k}=(p_k,v_k)$$
每个预测的元素之间都会相互关联，相互间的影响也会造成预测的不确定性。我们用协方差矩阵（covarance matrix）表示：
$$s_k=\left[\begin{array}{cc}{\sum }_{pp}& {\sum }_{pv}\\ {\sum }_{vp}& {\sum }_{vv}\end{array}\right]$$
我们从k时刻预测k+1时刻，假设没有加速度，那么位置和速度的转换公式如下：
$$\{\begin{array}{rl}& p_{k+1}=p_k+△t\cdot v_k\\ & v_{k+1}=0+1\cdot v_k\end{array}$$
用状态转移矩阵$F_{k+1}$来表示转换矩阵，则公式可简写为：
$$F_{k+1}=\left[\begin{array}{cc}1& △t\\ 0& 1\end{array}\right],\widehat{x_k}=\left[\begin{array}{c}{p}_k\\ v_k\end{array}\right]$$
$$\widehat{x_{k+1}}=F_{k+1}\cdot \widehat{x_k}$$

2. 协方差预测
根据协方差矩阵的计算公式：
$$Cov(F_{k+1}\widehat{x_k})=F_{k+1}Cov(\widehat{x_k})F_{k+1}^T$$
可得出整体的转换结果为：
$$\{\begin{array}{rl}& \widehat{x_{k+1}}=F_{k+1}\cdot \widehat{x_k}\\ & s_{k+1}=F_{k+1}\cdot s_k\cdot F_{k+1}^T\end{array}$$

3. 已知外部因素
原本对状态的预测为：
$$\widehat{x_{k+1}}=\{\begin{array}{rl}& p_{k+1}=p_k+△t\cdot v_k\\ & v_{k+1}=0+1\cdot v_k\end{array}$$
我们加上加速度（外力）的因素：
$$\begin{gathered} \widehat{x_{k+1}}=\{\begin{array}{rl}& p_{k+1}=p_k+△t\cdot v_k+\frac{1}{2}a△t^2\\ & v_{k+1}=0+1\cdot v_k+a△t\end{array} \\ \begin{array}{rl}\widehat{x_{k+1}}& =F_{k+1}\cdot \widehat{x_k}+\left[\begin{array}{c}\frac{1}{2}△t^2\\ △t\end{array}\right]\cdot a\\ & =F_{k+1}\cdot \widehat{x_k}+B_k\cdot u_k\end{array} \\ {B}_k=\left[\begin{array}{c}\frac{1}{2}△t^2\\ △t\end{array}\right],u_k=a \end{gathered}$$
其中，$B_k$称控制矩阵，表示外部因素如何控制状态转移；$u_k$称控制向量，表示外部因素影响的大小。

4. 未知外部因素
除了已知的，可用公式表示的外部因素（如外力），还有很多未知或难以测量的因素，这些未知因素会造成预测误差，我们统称为噪声。
噪声的大小根本上影响预测状态的协方差，因此以$Q_{k+1}$加进协方差中：
$$s_{k+1}=F_{k+1}\cdot s_k\cdot F_{k+1}^T+Q_{k+1}$$
综上，单纯预测的状态预测方程为：
$$\{\begin{array}{rl}& \widehat{x_{k+1}}=F_{k+1}\cdot \widehat{x_k}+B_k\cdot u_k\\ & s_{k+1}=F_{k+1}\cdot s_k\cdot F_{k+1}^T+Q_{k+1}\end{array}$$
这也是KF的五个公式中的前两个。

5. 预测空间映射至测量空间
预测的维度和测量维度可能不一致，需要通过矩阵的相乘，把预测值的维度映射到测量值的维度，才能用测量值对预测进行校准。
$H_{k+1}$取决于预测和测量的维度的差别，通常仅用于维度的映射，不做数值大小的改变。
$$\begin{gathered} \widehat{x_{\mu }}=H_{k+1}\cdot \widehat{x_{k+1}} \\ {s}_{\mu }=H_{k+1}\cdot s_{k+1}\cdot H_{k+1}^T \end{gathered}$$

6. 校正
预测值与测量值都存在误差，且都服从正态分布。
令预测值~$N({\mu }_0,{\sigma }_0)$，测量值~$N({\mu }_1=z_{k+1},{\sigma }_1=\sqrt{R_{k+1}})$。
要通过两个正态分布取得最合适的结果，我们要取两个正态分布的交集。
两个正态分布的交集也服从正态分布，且等于两者的积：
$$N({\mu }^{\prime },{\sigma }^{\prime })=N({\mu }_0,{\sigma }_0)\cdot N({\mu }_1,{\sigma }_1)=\frac{1}{\sqrt{2\pi }{\sigma }^{\prime }}{e}^{-\frac{(x-{\mu }^{\prime }{)}^2}{2{\sigma }^{\prime 2}}}$$
（两个高斯函数的乘积可参考：两个高斯分布相乘结果是什么？）
可得：
$$\{\begin{array}{rl}& {\mu }^{\prime }={\mu }_0+\frac{{\sigma }_0^2({\mu }_1-{\mu }_0)}{{\sigma }_0^2+{\sigma }_1^2}\\ & {\sigma }^{\prime 2}={\sigma }_{0}2-\frac{{\sigma }_0^4}{{\sigma }_0^2+{\sigma }_1^2}\end{array}$$
定义K简化公式：
$$\begin{gathered} K\equiv \frac{{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2} \\ \{\begin{array}{l}{\mu }^{\prime }={\mu }_0+K\left({\mu }_1-{\mu }_0\right)\\ {\sigma }^{\prime 2}={\sigma }_0^2-K{\sigma }_0^2\end{array} \end{gathered}$$
因为预测状态和协方差分别代表了正态分布的均值和方差，测量值也是如此，所以两者可以等价起来（注意这里的${\sigma }^{\prime 2}$是方差）：
$$\{\begin{array}{rl}& ({\mu }_0,{\sigma }_0^2)=(\widehat{x_{\mu }},s_{\mu })=(H_{k+1}{\hat{x}}_{k+1},H_{k+1}\cdot S_{k+1}\cdot H_{k+1})\\ & ({\mu }_1,{\sigma }_1^2)=(z_{k+1},R_{k+1})\end{array}$$
把他们代入上式${\mu }^{\prime }$和${\sigma }^{\prime 2}$中：
$$\begin{gathered} \{\begin{array}{rl}& {\mu }^{\prime }=H_{k+1}\widehat{x_{k+1}}+K(z_{k+1}-H_{k+1}\widehat{x_{k+1}})\\ & {\sigma }^{\prime 2}=H_{k+1}\cdot S_{k+1}\cdot H_{k+1}-K{H}_{k+1}\cdot S_{k+1}\cdot H_{k+1}\end{array} \\ K=H_{k+1}\cdot S_{k+1}\cdot H_{k+1}(H_{k+1}\cdot S_{k+1}\cdot H_{k+1}+R_{k+1}{)}^{-1} \end{gathered}$$
我们把卡尔曼滤波最终求得的状态和协方差定义为：$\widehat{x_{k+1}^{\prime }}$和$s_{k+1}^{\prime }$。
正如预测状态要通过映射矩阵$H_{k+1}$映射到高斯分布中，最终的结果$\widehat{x_{k+1}^{\prime }}$和$s_{k+1}^{\prime }$也同样要进行映射，所以：
$$\{\begin{array}{rl}& H_{k+1}\widehat{x_{k+1}^{\prime }}={\mu }^{\prime }=H_{k+1}\widehat{x_{k+1}}+K(z_{k+1}-H_{k+1}\widehat{x_{k+1}})\\ & H_{k+1}{s}_{k+1}^{\prime }{H}_{k+1}^T={\sigma }^{\prime 2}=H_{k+1}\cdot S_{k+1}\cdot H_{k+1}-K{H}_{k+1}\cdot S_{k+1}\cdot H_{k+1}\end{array}$$

两边同时约掉$H_{k+1}$和$H_{k+1}^T$后，最终得到的卡尔曼滤波的五个公式就是：
$$\begin{gathered} \{\begin{array}{rl}& {\hat{x}}_{k+1}=F_{k+1}\cdot {\hat{x}}_k+B_k\cdot u_k\\ & {\mathbf{s}}_{k+1}={\mathbf{F}}_{k+1}\cdot {\mathbf{s}}_k\cdot {\mathbf{F}}_{k+1}^T+Q_{k+1}\end{array} \\ {K}^{\prime }=S_{k+1}\cdot H_{k+1}^T\cdot {\left(H_{k+1}\cdot S_{k+1}\cdot H_{k+1}^T+R_{k+1}\right)}^{-1} \\ \{\begin{array}{rl}& {\hat{\mathbf{x}}}_{k+1}^{\prime }={\hat{\mathbf{x}}}_{k+1}+K^{\prime }\left(z_{k+1}-H_{k+1}\cdot {\hat{\mathbf{x}}}_{k+1}\right)\\ & {\mathbf{s}}_{k+1}^{\prime }=s_{k+1}-K^{\prime }\cdot H_{k+1}\cdot s_{k+1}\end{array} \end{gathered}$$
其中，前两个公式为预测（Predict），公式三为增益（Gain），后两个公式为校正（Correction）。

总结
当没有传感器（Sensor）做测量并校正预测结果时，FL就只用了前两个公式，状态转移矩阵$F_{k+1}$是根据经验设定的。
当有一个或多个传感器工作，则FL也会使用后三个公式做校准。
从后两个公式看出，最终的状态${\hat{\mathbf{x}}}_{k+1}^{\prime }$和方差${\mathbf{s}}_{k+1}^{\prime }$，是以预测的状态${\hat{\mathbf{x}}}_{k+1}$和方差$s_{k+1}$为起点，加上或减去增益$K^{\prime }$乘以某个矩阵。
公式4增益后的$\left(z_{k+1}-H_{k+1}\cdot {\hat{\mathbf{x}}}_{k+1}\right)$，可以看作测量值均值和映射后预测值的距离，即测量方差$R_{k+1}$越小，增益$K^{\prime }$越大，${\hat{\mathbf{x}}}_{k+1}^{\prime }$向$z_{k+1}$偏移得越多。
增益$K^{\prime }$的大小取决于预测和测量各自的方差的大小。也正如文初所说的，预测误差和测量误差都不可忽略，两者的大小共同决定了相信预测多一些还是相信测量多一些。