卡尔曼滤波详细推导

前言

虽然卡尔曼滤波作为一种经典的算法在多源数据融合、slam后端优化等很多方面都取得了较好的应用，但是我们经常都会有一个疑问，卡尔曼滤波到底是什么呢？
简要的说：“卡尔曼滤波是一种优化估算算法”

卡尔曼滤波可以适用于线性高斯系统，如果我们考虑非线性系统则可以考虑扩展卡尔曼滤波

其可以解决两个问题：

1. 状态如何在无法被直接测量的情况下，进行估算系统的状态。
2. 如何在数据源存在噪声的情况下，通过卡尔曼滤波估计系统的状态。（比如说惯导、轮式里程计、GNSS三种传感器数据均存在误差，且频率不一，如何融合三种数据源进行精确的估计汽车的位置）

我们先来看一个简单的例子：
一辆行驶在路上的小车，其状态包括位置、速度两个信息，用$x$表示小车的状态，其中$p$表示位置，$v$表示速度。则可以写成下述形式：

$$\overrightarrow{x}=\left|\begin{array}{c}p\\ v\end{array}\right|$$

假设式中的位置变量$p$ 与 速度变量$v$，都为随机变量且符合正态分布，则进而可以通过均值 描述可能的估计 和 对应的方差 衡量变量的不确定性。

匀速行驶

在一个比较理想的状态下，我们假设小车在路上匀速行驶，则可以对小车行驶由$t-1$时刻到$t$时刻之间的过程描述成下式中的数学模型：
$$p_t=p_{t-1}+v_{t-1}\ast △t$$
$$v_t=v_{t-1}$$
针对 $t$ 时刻与 $t-1$ 时刻的状态的估计量，${\hat{x}}_t=\left|\begin{array}{c}{\hat{p}}_t\\ {\hat{v}}_t\end{array}\right|$，${\hat{x}}_{t-1}=\left|\begin{array}{c}{\hat{p}}_{t-1}\\ {\hat{v}}_{t-1}\end{array}\right|$

写成矩阵形式如下：
$${\hat{x}}_t=\left|\begin{array}{cc}1& △t\\ 0& 1\end{array}\right|\ast {\hat{x}}_{t-1}$$
令上式中：
$$F_t=\left|\begin{array}{cc}1& △t\\ 0& 1\end{array}\right|$$
则可以写成：
$${\hat{x}}_t=F_t\ast {\hat{x}}_{t-1}$$
称上式中的 $F_t$ 为预测矩阵，其描述了从上一时刻状态估计到下一时刻状态估计之间的关系。

但是，上述例子中的位置变量$p$ 与 速度变量$v$都不是确定的，两者之间的关系也不是确定的，但是二者之间却有一定的相关性，我们通过协方差矩阵 $P$ 描述两者之间的相关程度：

(注意这里的协方差矩阵 $P$ 与位置变量$p$符号之间的区别)

$$P=\left|\begin{array}{cc}{\Sigma }_{pp}& {\Sigma }_{pv}\\ {\Sigma }_{vp}& {\Sigma }_{vv}\end{array}\right|$$
同时，考虑到两者之间的预测矩阵F_{t}，则给定上一时刻的协方差矩阵 $P_{t-1}$，则下一时刻的协方差矩阵 $P_t$可以写成：
$$P_t=F_t{P}_{t-1}{F}_t^T$$

非匀速行驶

上述的例子种过于理想，实际情况中小车不可能都是在匀速行驶。因此，我们考虑加入一个加速度$a$，即小车的行驶速度会随着时间而发生改变。
重新定义小车的状态量如下所示：
$$p_t=p_{t-1}+v_{t-1}\ast △t+\frac{1}{2}a△t^2$$
$$v_t=v_{t-1}+a△t$$
令${\hat{u}}_t$作为加速度$a$在$t$时刻的估计（又称之为输入）。
则上式又可以写为：
$$p_t=p_{t-1}+v_{t-1}\ast △t+\frac{△t^2}{2}\ast {\hat{u}}_t$$
$$v_t=v_{t-1}+△t\ast {\hat{u}}_t$$
整理$B_t=\left|\begin{array}{c}\frac{△t^2}{2}\\ △t\end{array}\right|$
得：
$${\hat{x}}_t=F_t\ast {\hat{x}}_{t-1}+B_t\ast {\hat{u}}_t$$
同时，使用$Q_t$表示引入的输入变量$u_t$的方差，则对两者之间协方差的影响又可以写成如下形式：
$$P_t=F_t{P}_{t-1}{F}_t^T+Q_t$$
总结一下，引入新的输入之后，对估计 ${\hat{x}}_t$ 和不确定性 $P_t$ 都产生了影响。

卡尔曼滤波公式推导

到此为止，我们对上述的一个小车行驶过程进行了很好的数学建模，但是我们还没有进行一个较好的公式推导。
下面我们就进行这个推导的过程：
上述推导过程我们得到了关于小车在$t$时刻预测值${\hat{x}}_t$满足一个高斯分布$N_p({\mu }_p,{\sigma }_p)$。
同时，基于小车携带的传感器又可以获得一个状态的观测值，这个观测值也会受到环境和传感器自身的影响产生众多噪声，我们仍假设观测值服从一个高斯分布，记为：$N_{ob}({\mu }_{ob},{\sigma }_{ob})$

因此，对于当前时刻$t$，我们有了两组参考值，一组是预测值${\hat{x}}_t$，一组是测量值$x_t$。那么那组值是正确的呢？怎么样才能得到最终较好的结果呢?
卡尔曼滤波器做的一个工作就是把预测值的高斯分布$N_p({\mu }_p,{\sigma }_p)$与测量值的高斯分布$N_{ob}({\mu }_{ob},{\sigma }_{ob})$进行相乘，得到一个新的分布。（注：这两个高斯分布的乘积不再是一个正态分布，因为其概率密度求和之后不为1）

这个新的分布会正比于一个高斯分布，因此，我们可以通过指数相等来进行计算，即我们可以得到如下关系：
$$\frac{1}{{\sigma }_{comb}^2}(x-{\mu }_{comb}{)}^2+C=\frac{1}{{\sigma }_p^2}(x-{\mu }_p{)}^2+\frac{1}{{\sigma }_{ob}^2}(x-{\mu }_{ob}{)}^2$$
上式中，${\mu }_p$，${\sigma }_p$分别为预测的均值和方差，${\mu }_{ob}$，${\sigma }_{ob}$分别为观测的均值和方差。而${\mu }_{comb}$，${\sigma }_{comb}$分别为融合之后的分布所对应均值和方差，且$C$为一个常数（即正比于一个高斯分布）。
我们把上式展开
$$\frac{1}{{\sigma }_{comb}^2}{x}^2-\frac{2{\mu }_{comb}}{{\sigma }_{comb}^2}x+\frac{{\mu }_{comb}^2}{{\sigma }_{comb}^2}+C=(\frac{1}{{\sigma }_p^2}+\frac{1}{{\sigma }_{ob}^2})x^2-(\frac{2{\mu }_p}{{\sigma }_p^2}+\frac{2{\mu }_{ob}}{{\sigma }_{ob}^2})x+(\frac{{\mu }_p^2}{{\sigma }_p^2}+\frac{{\mu }_{ob}^2}{{\sigma }_{ob}^2})$$
为了使得上式的关系成立，需满足一个必要条件为左右两侧 $x$ 的二次项系数与一次项系数均相等。则由此可以得到下述关系：
$$\frac{1}{{\sigma }_{comb}^2}=\frac{1}{{\sigma }_p^2}+\frac{1}{{\sigma }_{ob}^2}$$
$$\frac{{\mu }_{comb}}{{\sigma }_{comb}^2}=\frac{{\mu }_p}{{\sigma }_p^2}+\frac{{\mu }_{ob}}{{\sigma }_{ob}^2}$$
首先，对于第一个式子，可以变换为下述形式：
$$\begin{gathered} {\sigma }_{comb}^2=\frac{{\sigma }_p^2{\sigma }_{ob}^2}{{\sigma }_p^2+{\sigma }_{ob}^2} \\ {\sigma }_{comb}^2=\frac{{\sigma }_p^2({\sigma }_p^2+{\sigma }_{ob}^2)-{\sigma }_p^4}{{\sigma }_p^2+{\sigma }_{ob}^2}={\sigma }_p^2-\frac{{\sigma }_p^4}{{\sigma }_p^2+{\sigma }_{ob}^2} \end{gathered}$$
同时，代入并变换第二个式子（右侧）：
$${\mu }_{comb}\frac{1}{{\sigma }_{comb}^2}={\mu }_{comb}\frac{{\sigma }_p^2+{\sigma }_{ob}^2}{{\sigma }_p^2{\sigma }_{ob}^2}=\frac{{\mu }_p{\sigma }_{ob}^2+{\mu }_{ob}{\sigma }_p^2}{{\sigma }_p^2{\sigma }_{ob}^2}$$
同时去除分母，得：
$${\mu }_{comb}({\sigma }_p^2+{\sigma }_{ob}^2)={\mu }_p{\sigma }_{ob}^2+{\mu }_{ob}{\sigma }_p^2$$
则：
$$\begin{gathered} {\mu }_{comb}=\frac{{\mu }_p{\sigma }_{ob}^2+{\mu }_{ob}{\sigma }_p^2}{({\sigma }_p^2+{\sigma }_{ob}^2)} \\ {\mu }_{comb}=\frac{{\mu }_p{\sigma }_p^2+{\mu }_p{\sigma }_{ob}^2+{\mu }_{ob}{\sigma }_p^2-{\mu }_p{\sigma }_p^2}{({\sigma }_p^2+{\sigma }_{ob}^2)} \\ {\mu }_{comb}={\mu }_p+\frac{{\mu }_{ob}{\sigma }_p^2-{\mu }_p{\sigma }_p^2}{({\sigma }_p^2+{\sigma }_{ob}^2)} \end{gathered}$$
令：
$$K=\frac{{\sigma }_p^2}{{\sigma }_p^2+{\sigma }_{ob}^2}$$
称$K$为卡尔曼增益，则我们可以把变量${\mu }_{comb}$与变量${\sigma }_{comb}^2$通过$K$表示为：

$$\begin{gathered} {\mu }_{comb}={\mu }_p+K\ast ({\mu }_{ob}-{\mu }_p) \\ {\sigma }_{comb}^2={\sigma }_p^2-\frac{{\sigma }_p^4}{{\sigma }_p^2+{\sigma }_{ob}^2}={\sigma }_p^2-{\sigma }_p^2\ast K={\sigma }_p^2(1-K) \end{gathered}$$
对应于矩阵形式，则可以写为：
$$\begin{gathered} {\overrightarrow{\mu }}_{comb}={\overrightarrow{\mu }}_p+K\ast ({\overrightarrow{\mu }}_{ob}-{\overrightarrow{\mu }}_p) \\ {\Sigma }_{comb}={\Sigma }_p(1-K) \end{gathered}$$

至此，我们就简要的说明了卡尔曼滤波中的均值、方差以及卡尔曼增益用于更新前两者的推导过程。

黄金五条公式总结

可以看出卡尔曼滤波整体包括两个部分：1）预测 2）更新
预测部分包括：
$$\begin{gathered} {\hat{x}}_t=F_t\ast {\tilde{x}}_{t-1}+B_t\ast u_t \\ {\hat{\sigma }}_t^2=F_t{\tilde{\sigma }}_{t-1}^2{F}_t^T+Q_t \end{gathered}$$
式中，${\hat{x}}_t$为$t$时刻的状态量预测值，通过上一时刻$t-1$计算的最优状态估计以及输入$u_t$进行计算。
${\hat{\sigma }}_t^2$为状态估计的不确定性，通过上一时刻的不确定性${\tilde{\sigma }}_{t-1}^2$，预测矩阵$F_t$以及输入$u_t$的不确定性$Q_t$进行计算。

更新部分包括：
$$\begin{gathered} K=\frac{{\hat{\sigma }}_t^2}{{\hat{\sigma }}_t^2+{\sigma }_{ob-t}^2} \\ {\tilde{x}}_t={\hat{x}}_t+K\ast (x_{ob-t}-{\hat{x}}_t) \\ {\tilde{\sigma }}_t^2={\hat{\sigma }}_t^2(1-K) \end{gathered}$$
上式中，$K$为卡尔曼增益，通过$t$时刻的状态估计预测量的不确定性${\hat{\sigma }}_t^2$与$t$时刻观测值的不确定性进行计算。
${\tilde{x}}_t$为$t$时刻结合预测和观测计算出的最优状态估计（即我们想要计算的状态量），通过$t$时刻的预测值${\hat{x}}_t$与观测值$x_{ob-t}$，以及卡尔曼增益$K$进行计算。
${\tilde{\sigma }}_t^2$为在$t$时刻结合预测和观测两方面进行计算的不确定性，通过预测的不确定性${\hat{\sigma }}_t^2$与卡尔曼增益$K$进行计算。

上述五个公式即为“黄金五条”公式。

卡尔曼增益K值的讨论

最后我们再进行一些讨论，更加深入的去了解卡尔曼滤波的内在关系。
我们来假设两个极端情况：
1）如果存在一个情况，观测量特别精确，几乎不存在误差，其方差${\sigma }_{ob-t}^2$的值可以取0。此时，卡尔曼增益$K$的值为1。则更新部分的最优估计和不确定性变为：
$$\begin{gathered} {\tilde{x}}_t={\hat{x}}_t+1\ast (x_{ob-t}-{\hat{x}}_t)=x_{ob-t} \\ {\tilde{\sigma }}_t^2={\hat{\sigma }}_t^2(1-1)=0 \end{gathered}$$
即观测值作为了最优估计，且其不确定性为0。
2）针对另外一个情况，我们构建的小车行驶的数学模型毫无误差，预测量特别精确，几乎不存在误差，其方差${\hat{\sigma }}^2$的值可以取0。此时，卡尔曼增益$K$的值为0。则更新部分的最优估计和不确定性变为：
$$\begin{gathered} {\tilde{x}}_t={\hat{x}}_t+0\ast (x_{ob-t}-{\hat{x}}_t)={\hat{x}}_t \\ {\tilde{\sigma }}_t^2={\hat{\sigma }}_t^2(1-0)={\hat{\sigma }}_t^2 \end{gathered}$$
即以预测值作为最优的估计。

因此，我们得到了一个重要的结论：

卡尔曼增益$K$的值，决定了测量值和预测值对计算$t$时刻状态量${\tilde{x}}_t$的影响程度。

这个知乎大佬放了matlab官方的学习视频，很不错，通俗易懂，感兴趣的可以去看一下：如何通俗并尽可能详细地解释卡尔曼滤波？

码字不易，若有转载还请注明出处！