卡尔曼滤波：究竟滤了谁？

在SLAM系统中，后端优化部分有两大流派：一派是基于马尔科夫性假设的滤波器方法，认为当前时刻的状态只与上一时刻的状态有关；另一派是非线性优化方法，认为当前时刻状态应该结合之前所有时刻的状态一起考虑。

如果采用滤波器方法，那一定会听到一个如雷贯耳的名字——卡尔曼滤波(Kalman Filtering)。我听到过这个名字已经很久了，可是一直没有花时间弄懂究竟是什么东西，最近看了一些资料，就来总结一下，看看卡尔曼大佬究竟滤了谁？

1 什么是滤波

在了解卡尔曼滤波之前，本科就学过一些滤波的方法，也就是从混合在一起的信号里提取出所需要的信号，就好比吃辣子鸡丁时只把鸡丁挑出来啃光，而辣椒被你抛弃掉了。

例如，在模拟电路中，我们利用低通滤波的方法，将信号中掺杂的一些频率较高的噪声滤除掉，从而提取有效的较低频信号。同样地，根据不同需要，还有高通滤波、带通滤波和带阻滤波。

再比如，在图像处理中，如果存在椒盐噪声（不能吃的！是在图像中随机出现的黑白点），我们会选择用中值滤波，将$n\times n$个像素值的中值取出来以代替中心点的值，这样就能滤除椒盐噪声。

但是，卡尔曼滤波和上述说的这些滤波略显不同，它并没有很直观地从一些信号或者数据里面提取某些信号或数据，而是在有干扰的条件下，通过数据的结合得到相对更准确的估计数据。

卡尔曼滤波全程只关注两个东西，一个是估计的最佳值，另一个是该值的不确定性（此处联想一下高斯分布的两个参数）。

打个比方，假设你蒙着眼睛在屋子里走，要从客厅走到卧室，你可以通过数步数来预测你当前所在的位置，但是因为你每次迈步的幅度和方向不是精准的，所以你每多走一步所估计位置的不确定性就会越来越大，最后有可能走到浴室去了。

如果利用卡尔曼滤波，那么你对自己每一步的位置估计就会准确很多，具体怎么做呢？卖个关子，后面再讲，先解释一下什么是状态估计。

2 状态估计

在SLAM中，运用卡尔曼滤波是为了状态估计，那什么才是需要估计的状态呢？

状态可以看作是机器人或者环境中可能会对未来产生某些变化的因素，比如说机器人的位姿$\mathbf{R}$和$\mathbf{t}$、机器人的运动速度$\mathbf{v}$(这个在纯视觉SLAM中是没有的)、环境中的路标点$\mathbf{l}$等等，不同的SLAM系统拥有不同的状态。

我们假设${\mathbf{x}}_k$为机器人的状态，在SLAM的整个过程中，我们能获取到两种数据：控制数据和测量数据。

• 控制数据是机器人记录自身运动的传感器获取的数据，比如IMU中的陀螺仪可以测量角速度、加速度计可以测量运动的加速度。

• 测量数据则是机器人记录环境信息的传感器获取的数据，比如相机可以将环境转化为二维的图像像素、激光雷达捕捉环境中的信息生成点云。

假设控制数据为${\mathbf{u}}_k$，运动噪声为${\mathbf{\epsilon }}_k$，那么机器人的运动可以用一个运动方程来表达，因为假设了马尔科夫性，所以当前时刻状态只与上一时刻有关，它表示从k-1时刻到k时刻机器人状态发生了怎样的变化。
$${\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)+{\mathbf{\epsilon }}_k$$

假设测量数据为${\mathbf{z}}_k$，测量噪声为${\mathbf{\delta }}_k$，那么机器人的测量可以用一个观测方程来表达，它表示在k时刻所在的位置观测到路标点产生测量数据。
$${\mathbf{z}}_k=h\left({\mathbf{x}}_k\right)+{\mathbf{\delta }}_k$$

那么，状态估计其实就是用过去的数据来估计当前的状态。由于状态不是直接得到的，而且方程还受到噪声的影响，因此状态其实是符合某种概率分布的随机变量，而状态估计实际上是估计当前的状态分布。

我们用置信度$bel({\mathbf{x}}_k)$来表示当前时刻的状态分布，它是以控制数据和测量数据为条件的后验概率，即
$$bel({\mathbf{x}}_k)=p\left({\mathbf{x}}_k|{\mathbf{z}}_{1:k},{\mathbf{u}}_{1:k}\right)$$

而在获得当前时刻测量数据之前的状态分布可以用$\overline{bel}\left({\mathbf{x}}_k\right)$来表示，它表示的后验概率为
$$\overline{bel}({\mathbf{x}}_k)=p\left({\mathbf{x}}_k|{\mathbf{z}}_{1:k-1},{\mathbf{u}}_{1:k}\right)$$

同时，如果用概率来表达运动过程的话，则是$p\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)$，这表示了从k-1时刻到k时刻机器人的状态转移概率。而用概率来表达观测过程，则是$p\left({\mathbf{z}}_k|{\mathbf{x}}_k\right)$，表示第k时刻机器人的测量概率。

所以，如下图所示，每一时刻的状态${\mathbf{x}}_k$只与前一时刻的状态${\mathbf{x}}_{k-1}$和当前时刻的控制${\mathbf{u}}_k$有关，而每一时刻的测量${\mathbf{z}}_k$只与当前时刻的状态${\mathbf{x}}_k$有关。这就是一个隐马尔可夫模型，状态不能直接得到，但是可以通过测量观察到。

3 贝叶斯滤波

有了状态分布的表达方式，还有运动方程和观测方法的概率表示，接下来就可以名正言顺地献上著名的贝叶斯公式了(前方多式警告！)
p ( x k ∣ z 1 : k , u 1 : k ) = p ( z k ∣ x k , z 1 : k − 1 , u 1 : k ) p ( x k ∣ z 1 : k