极市导读
卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。本文是一篇关于卡尔曼滤波的基础入门教程，详细阐述了卡尔曼滤波的推导过程以及推广到高维的过程。 >>年度盘点：极市计算机视觉资源汇总，顶会论文、技术视频、数据集等（限时开放下载）
最早接触卡尔曼滤波是在卫星导航课中，GPS 和IMU 结合时常会用到卡尔曼滤波。但学完了也只明白了数学推导，不过是“会做题的机器”。最近在学习SLAM 时想要重新好好温习一下卡尔曼滤波，虽然现在SLAM 的主流趋势是利用图优化，但卡尔曼滤波仍然为我们提供了一个很好的参考。
导论
卡尔曼滤波本质上是一个数据融合算法，将具有同样测量目的、来自不同传感器、(可能) 具有不同单位 (unit) 的数据_融合_在一起，得到一个更精确的目的测量值。
卡尔曼滤波的局限性在于其只能拟合线性高斯系统。但其最大的优点在于计算量小，能够利用前一时刻的状态（和可能的测量值）来得到当前时刻下的状态的最优估计。
本文虽然是小白教程，但还是需要各位至少知道高斯分布，一点点线性代数，还有状态向量这样的名词。
简述
考虑一个SLAM 问题，它由一个运动方程：
和一个观测方程组成：
就把它当作一个线性系统吧（非线性系统请看下一讲扩展卡尔曼滤波），并且为了简化推导，忽略路标的下标j，并把路标y 并入到状态向量一起优化，那么运动方程就可以写为：
其中，
-
为t 时刻的状态向量，包括了相机位姿、路标坐标等信息，也可能有速度、朝向等信息； -
为运动测量值，如加速度，转向等等； -
为状态转换方程，将t-1 时刻的状态转换至t 时刻的状态； -
是控制输入矩阵，将运动测量值 的作用映射到状态向量上； -
是预测的高斯噪声，其均值为0，协方差矩阵为 。
这一步在卡尔曼滤波中也称为预测 (predict)。
类似地，测量方程可以写为：
其中，
-
为传感器的测量值； -
为转换矩阵，它将状态向量映射到测量值所在的空间中； -
为测量的高斯噪声，其均值为0，协方差矩阵为 。
而卡尔曼滤波就是预测 - 测量之间不断循环迭代。当然，对于某些情况，如GPS + IMU，由于IMU 测量频率远比GPS 高，在只有IMU 测量值时，只执行运动更新，在有GPS 测量值时再进行测量更新。
一个小例子
用一个在解释卡尔曼滤波时最常用的一维例子：小车追踪。如下图所示：
状态向量为小车的位置和速度：
而司机要是踩了刹车或者油门，小车就会具有一个加速度， 。
假设t 和t-1 时刻之间的时间差为 。根据物理知识，有：
写成矩阵形式就有
跟之前的运动方程对比，就知道
上式就写为
表示t-1 时刻卡尔曼滤波的状态估计； 则表示中t-1 到t 时刻，预测更新所得的预测值。
再利用运动模型对状态向量进行更新后，还要继续更新状态向量的协方差矩阵P，公式为：
假设 为t 时刻下状态向量的真值（自然是永远未知的），由之前的现形运动方程（式(3)）给出，将式(3) 与式(9) 相减可得：
而
考虑到状态向量和噪声是不相关的，则 ，上式就可以简化为
推导完毕。
可以看到，经过预测更新，协方差矩阵P 变大了。这是因为状态转换并不完美，而且运动测量值含有噪声，具有较大的不确定性。
预测更新实际上相当于“加法”：将当前状态转换到下一时刻（并增加一定不确定性），再把外界的干扰（运动测量值）叠加上去（又增加了一点不确定性）。
上面即为卡尔曼滤波中预测这一步。这一步相对比较直观，推导也较测量更新简单，就只在这里详细给出了。
如果得到了测量值，那么我们就可以对状态向量进行测量更新了，对应的公式为
其中，
为卡尔曼增益。
从这里就可以看到，测量更新显然比预测更新复杂，难点也集中在这里。下面就给出测量更性的详细推导。
推导
一维case
从t-1 时刻起，小车运动后，经过前面所述的预测更新后，我们就得到了t 时刻的小车位置的估计，由于在卡尔曼滤波中，我们使用高斯概率分布来表示小车的位置，因此这个预测的位置可以写为：
为了与前面的通用的推导区别开来，在这个一维的例子中我们使用了新的符号。不过熟悉高斯概率分布的话应该可以马上看出来， 为这个高斯分布的均值， 为方差，而r 为小车的可能位置， 为某个可能位置 (r) 的概率分布。
假设在t 时刻，我们通过某测距仪测得小车距离原点的距离r，由于测量包含噪声（且在面前我们假设了其为高斯噪声），因此该测量值也可以利用高斯概率分布来表示：
除了下标外，其余的字母的含义都和上面的式子一样。
如上图琐事，现在在t 时刻，我们有了两个关于小车位置的估计。而我们所能得到的关于小测位置的最佳估计就是将预测更新和测量更新所得的数据融合起来，得到一个新的估计。而这个融合，就是一个简单的“乘法”，并利用了一个性质：两个高斯分布的乘积仍然是高斯分布。
将上式化简一下：
其中， 为 和 的加权平均， 则是 和 的调和平均的二分一：
最右边的式子是为了后面的计算而准备的。
本质上，这（高斯分布相乘）就是卡尔曼滤波中测量更新的全部了。
那么， 怎么由上面两个简单的一维的式子得到前一节 和 呢？一步一步来。
转换矩阵H 的引入
在刚刚的一维情况的小例子中，我们其实做了一个隐式的假设，即有预测更新得到的位置的概率分布和测距仪所得的测量值具有相同的单位 (unit)，如米 (m)。
但实际情况往往不是这样的，比如，测距仪给出的可能不是距离，而是信号的飞行时间（由仪器至小车的光的传播时间），单位为秒 (s)。这样的话，我们就无法直接如上面一般直接将两个高斯分布相乘了。
此时，就该转换矩阵 闪亮登场了。由于 ，c 为光速。所以此时 （测量方程为 ，可以回去参考一下式(4)）。
预测值就要写为：
而测量值保持不变：
这样，两个高斯概率分布在转换矩阵H 的作用下又在同一个空间下了。根据前面 和 的公式 （式(27) 和式(28)），可得：
将上式两端都乘以c 则可得：
由于 （这里转换矩阵H 不随时间变化而变化，所以把下标t 略去），并记