前言
虽然卡尔曼滤波作为一种经典的算法在多源数据融合、slam后端优化等很多方面都取得了较好的应用，但是我们经常都会有一个疑问，卡尔曼滤波到底是什么呢？
简要的说：“卡尔曼滤波是一种优化估算算法”
卡尔曼滤波可以适用于线性高斯系统，如果我们考虑非线性系统则可以考虑扩展卡尔曼滤波
其可以解决两个问题：
状态如何在无法被直接测量的情况下，进行估算系统的状态。
如何在数据源存在噪声的情况下，通过卡尔曼滤波估计系统的状态。（比如说惯导、轮式里程计、GNSS三种传感器数据均存在误差，且频率不一，如何融合三种数据源进行精确的估计汽车的位置）
我们先来看一个简单的例子：
一辆行驶在路上的小车，其状态包括位置、速度两个信息，用 x表示小车的状态，其中 p表示位置， v表示速度。则可以写成下述形式：
假设式中的位置变量 p 与 速度变量 v，都为随机变量且符合正态分布，则进而可以通过均值 描述可能的估计 和 对应的方差 衡量变量的不确定性。
匀速行驶
在一个比较理想的状态下，我们假设小车在路上匀速行驶，则可以对小车行驶由 t − 1时刻到 t时刻之间的过程描述成下式中的数学模型：
针对 t时刻与 t − 1 时刻的状态的估计量，
写成矩阵形式如下：
令上式中：
则可以写成：
称上式中的 F t 为预测矩阵，其描述了从上一时刻状态估计到下一时刻状态估计之间的关系。
但是，上述例子中的位置变量 p与 速度变量 v都不是确定的，两者之间的关系也不是确定的，但是二者之间却有一定的相关性，我们通过协方差矩阵 P 描述两者之间的相关程度：
(注意这里的协方差矩阵 P与位置变量 p符号之间的区别)
同时，考虑到两者之间的预测矩阵F_{t}，则给定上一时刻的协方差矩阵 P t − 1，则下一时刻的协方差矩阵 P t1，则下一时刻的协方差矩阵 P t 可以写成：
非匀速行驶
上述的例子种过于理想，实际情况中小车不可能都是在匀速行驶。因此，我们考虑加入一个加速度 a，即小车的行驶速度会随着时间而发生改变。
重新定义小车的状态量如下所示：
令 u ^ t作为加速度 a在 t时刻的估计（又称之为输入）。
则上式又可以写为：
整理
得：
同时，使用 Q t
表示引入的输入变量 u t的方差，则对两者之间协方差的影响又可以写成如下形式：
总结一下，引入新的输入之后，对估计 x ^ t和不确定性 P t都产生了影响。
卡尔曼滤波公式推导
到此为止，我们对上述的一个小车行驶过程进行了很好的数学建模，但是我们还没有进行一个较好的公式推导。
下面我们就进行这个推导的过程：
上述推导过程我们得到了关于小车在 t时刻预测值 x ^ t
满足一个高斯分布 N p ( μ p , σ p )
同时，基于小车携带的传感器又可以获得一个状态的观测值，这个观测值也会受到环境和传感器自身的影响产生众多噪声，我们仍假设观测值服从一个高斯分布，记为： N o b ( μ o b , σ o b )
因此，对于当前时刻 t t t，我们有了两组参考值，一组是预测值 x ^ t，一组是测量值 x t。那么那组值是正确的呢？怎么样才能得到最终较好的结果呢?
卡尔曼滤波器做的一个工作就是把预测值的高斯分布 N p ( μ p , σ p ) 与测量值的高斯分布 N o b ( μ o b , σ o b ) 进行相乘，得到一个新的分布。（注：这两个高斯分布的乘积不再是一个正态分布，因为其概率密度求和之后不为1）
这个新的分布会正比于一个高斯分布，因此，我们可以通过指数相等来进行计算，即我们可以得到如下关系：
上式中， μ p σp
分别为预测的均值和方差， μ o b， σ o b 分别为观测的均值和方差。而 μ c o m b， σ c o m b分别为融合之后的分布所对应均值和方差，且 为一个常数（即正比于一个高斯分布）。
我们把上式展开
为了使得上式的关系成立，需满足一个必要条件为左右两侧 x的二次项系数与一次项系数均相等。则由此可以得到下述关系：
首先，对于第一个式子，可以变换为下述形式：
同时，代入并变换第二个式子（右侧）：
同时去除分母，得：
则：
令：
称 K为卡尔曼增益，则我们可以把变量 μ c o m b与变量 σ c o m b 2通过 K表示为：
对应于矩阵形式，则可以写为：
至此，我们就简要的说明了卡尔曼滤波中的均值、方差以及卡尔曼增益用于更新前两者的推导过程。
黄金五条公式总结
可以看出卡尔曼滤波整体包括两个部分：1）预测 2）更新
预测部分包括：
式中， x ^ t 为 t时刻的状态量预测值，通过上一时刻 t − 1计算的最优状态估计以及输入 u t进行计算。
σ ^ t 2为状态估计的不确定性，通过上一时刻的不确定性 σ ~ t − 1 2，预测矩阵 F t以及输入 u t的不确定性 Q t进行计算。
更新部分包括：
上式中， K为卡尔曼增益，通过 t t t时刻的状态估计预测量的不确定性 σ ^ t 2与 t时刻观测值的不确定性进行计算。
x ~ t 为 t时刻结合预测和观测计算出的最优状态估计（即我们想要计算的状态量），通过 t t t时刻的预测值 x ^ t与观测值 x o b − t，以及卡尔曼增益 K进行计算。
σ ~ t 2
为在 t t t时刻结合预测和观测两方面进行计算的不确定性，通过预测的不确定性 σ ^ t 2 与卡尔曼增益 K进行计算。
上述五个公式即为“黄金五条”公式。
卡尔曼增益K值的讨论
最后我们再进行一些讨论，更加深入的去了解卡尔曼滤波的内在关系。
我们来假设两个极端情况：
1）如果存在一个情况，观测量特别精确，几乎不存在误差，其方差 σ o b − t 2 的值可以取0。此时，卡尔曼增益 K的值为1。则更新部分的最优估计和不确定性变为：
即观测值作为了最优估计，且其不确定性为0。
2）针对另外一个情况，我们构建的小车行驶的数学模型毫无误差，预测量特别精确，几乎不存在误差，其方差 σ ^ 2 的值可以取0。此时，卡尔曼增益 K的值为0。则更新部分的最优估计和不确定性变为：
即以预测值作为最优的估计。
因此，我们得到了一个重要的结论：
卡尔曼增益 K的值，决定了测量值和预测值对计算 t时刻状态量 x ~ t的影响程度。
这个知乎大佬放了matlab官方的学习视频，很不错，通俗易懂，感兴趣的可以去看一下：如何通俗并尽可能详细地解释卡尔曼滤波？
码字不易，若有转载还请注明出处！
