卡尔曼滤波以后再经过低通滤波器_卡尔曼滤波：究竟滤了谁？

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

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

1 什么是滤波

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

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

再比如，在图像处理中，如果存在椒盐噪声(不能吃的！是在图像中随机出现的黑白点)，我们会选择用中值滤波，将

个像素值的中值取出来以代替中心点的值，这样就能滤除椒盐噪声。

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

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

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

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

2 状态估计

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

状态可以看作是机器人或者环境中可能会对未来产生某些变化的因素，比如说机器人的位姿

和

、机器人的运动速度

(这个在纯视觉SLAM中是没有的)、环境中的路标点

等等，不同的SLAM系统拥有不同的状态。

我们假设

为机器人的状态，在SLAM的整个过程中，我们能获取到两种数据：控制数据和测量数据。控制数据是机器人记录自身运动的传感器获取的数据，比如IMU中的陀螺仪可以测量角速度、加速度计可以测量运动的加速度。

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

假设控制数据为

，运动噪声为

，那么机器人的运动可以用一个运动方程来表达，因为假设了马尔科夫性，所以当前时刻状态只与上一时刻有关，它表示从k-1时刻到k时刻机器人状态发生了怎样的变化。

假设测量数据为

，测量噪声为

，