点击上方“”，选择加""或“”
重磅干货，第一时间送达
本文由知乎 郑纯然 授权转载原文链接：https://zhuanlan.zhihu.com/p/146593826
先讨论滤波器的概念，这意味着机器人在正确位置对应的概率越高越好。也就是说，可以理解为：过滤错误位置的概率，过滤正确位置的概率。
假设一个出现在下面的场景中，他一开始不知道自己在哪里(小R还没看到他面前的门)，所以他在这个场景中任何位置的概率都是一样的。如果是这样，应该是均匀分布的直线。
突然，机器人看到了门，假设机器人事先知道有三扇门，所以小R现在知道他可能在任何门前，也就是说，三扇门对应于正态分布。此时的概率波形可以理解为。
小R继续向前走到第二扇门，他通过自己发现自己走了个单位。
根据之前的概率分布，小R可以预测他的位置应该向右移动d单位。然后将以前的概率分布向右移动d单位，以获得通过传感器获得的概率分布。此时的概率波形可以理解为。
小R突然发现自己看到了第二扇门。根据目前的观察，小R知道自己在三扇门前的概率是一样的，可以得到前三个正态分布。根据传感器预测和先验信息的分布:
两个波形信号可以通过卷积融合获得:
这样，小R在第二扇门处(正确位置)的概率就变大了，在其他位置的概率也变小了，从而达到了滤波的目的。
。
我们假设机器人的状态xk 为 即位置和速度。根据运动学公式：
向量化：
这样就引出了：
从公式(1)可以看出，新的最优状态估计是基于上一刻最优估计的预测，再加上已知外部控制量的修正(如控制油门加速)。
从公式（2）可以看出，新的不确定性是通过上一刻的不确定性预测得到的，再加上外部环境的干扰。此时，我们对系统的变化进行了模糊的估计。更新状态(平均值)和不确定性(协方差)分别为公式(1)和(2)。预测过程相当于将D单位向右转移的过程。
？，我们可以通过使用传感器来猜测系统的当前状态，但由于传感器的噪声，传感器读数（平均值）和噪声（方差） ：传感器读数的单位和比例可能与公式（1）中跟踪状态的单位和比例不同。我们将使用矩阵Ck 为预测模型的平均值和方差改为传感器建模
这时，我们已经得到了两个，类似于之前机器人小R目前获得的两个分布，网上对两个高斯分布融合的过程有很多推导，这里就不赘述了，直接利用结果。
先计算卡尔曼增益K，如式(3)。
然后计算后验概率的分布:
式中Zk表示实际的观测， 观察和预测的差异乘以卡尔曼增益 K，再加上原来的预测，得到融合的结果。这个过程类似于前两个波形卷积。
新的最优估计可以在下一刻不断迭代。
以上是经典卡尔曼滤波器的五个公式，给出了线性高斯系统的最佳无偏差估计。我们可以用这些公式为任何线性系统建立准确的模型。对于非线性系统，我们使用扩展卡尔曼滤波器。区别在于EKF多了一个把预测和测量部分进行线性化的过程。
这时，我们来看看这个高赞无公式推导的答案，回顾全局。一切突然变得豁然开朗:无公式解释卡尔曼滤波
假设你有两个传感器，测量相同的信号。但是每次读数都不一样。我该怎么办？假设你知道昂贵的传感器应该准确，便宜的传感器应该更糟。有没有比平均水平更好的方法？K如何加权？假设两个传感器的误差都符合正态分布，假设你知道这两个正态分布的方差，使用这两个方差（这里省略了几个数学公式），你可以得到一个最佳的权重。接下来，重点是：假设你只有一个传感器，但你也有一个数学模型。模型可以帮助你计算一个值，但不太准确。我该怎么办？OK，最后一点说明:你的模型其实只是一步长，也就是说，你知道x(k)，我可以求x(k 1)。问题是x(k)是多少？x(k)是你在卡尔曼滤波器的最后一步获得的，所谓的加权平均值，xk时刻的最佳估计值。于是也有了。这是卡尔曼滤波器。（无公式）
推荐传感器融合的经典论文：
[1] Bloesch M, Omari S, Hutter M, et al. Robust visual inertial odometry using a direct EKF-based approach[C]//2015 IEEE/RSJ international conference on intelligent robots and systems (IROS). IEEE, 2015: 298-304.
[2] Qiu X, Zhang H, Fu W, et al. Monocular Visual-Inertial Odometry with an Unbiased Linear System Model and Robust Feature Tracking Front-End[J]. Sensors, 2019, 19(8): 1941.
[3] Xiong X, Chen W, Liu Z, et al. DS-VIO: Robust and Efficient Stereo Visual Inertial Odometry based on Dual Stage EKF[J]. arXiv preprint arXiv:1905.00684, 2019.
[4] Bloesch M, Burri M, Omari S, et al. Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback[J]. The International Journal of Robotics Research, 2017, 36(10): 1053-1072.
解决办法是看完文章，顺手点下文末右下角的看，系统会认为我们的文章符合你的口味，以后发文章会第一时间推给你。~
在「」公众号后台回复：下载全网第一份OpenCV中文版扩展模块教程，涵盖等20多章。
在「」微信官方账号后台回复:可下载包括等待31个视觉实战项目，帮助快速学校计算机视觉。
在「」微信官方账号后台回复:即可下载内容个基于实现20个，实现OpenCV学习进阶。
交流群
欢迎加入微信官方账号读者群与同行交流。SLAM、三维视觉、传感器、自动驾驶、摄影计算、检测、分割、识别、医学图像、GAN、算法竞赛等微信群(以后会逐渐细分)，请扫描以下微信号加群，备注:昵称 学校/公司 研究方向，例如：张三 上海交大 视觉SLAM“。请按格式备注，否则不予通过。加成功后会根据研究方向邀请进入相关微信群。在群内发送广告，否则会请出群，谢谢理解~