2.由于积分的关系 ，  计算量会非常大 ，  一般通过将运动模型和观测模型 线性化来解决或通过蒙特卡洛积分 。
有很多的科研成果聚焦在如何解决这两个问题 ，  代表性的如本文下面讨论 的 EKF ，  Particl Filter ,Sigmapoint Kalman Filter 等 。另外在 Bayes Filter 近似算法中很重要一点是保持马尔科夫性 。
2.3、EKF
EKF 在近似 Bayes Filter 的时候采用的近似方案是将模型线性化 ， 假设噪声是高斯的 。emmm, 捂脸 ， .... 说的就是因为模型非线性、噪声非高斯 。
所以回到 Bayes Filter ， 然后 Bayes Filter 有难以克服的障碍 ， 需要近似解 ， 而解决方案绕一圈竟然又回到了线性高斯假设的原点 ， 这是骗鬼呢么？
真的 ，  这就是著名的 EKF 。在Kalman,1960发表了他的Kalman FIlter论文后 ，  他遇到了 NASA Ames Reserach Center 的 Stanley F.Schmidt ， 他们一起对 Kalman Filter 做了改进 ， 最终应用在 NASA Apollo 项目的航空器轨迹估计上 。看EKF曾经多么NB闪闪 。
他们的改进主要集中在 3 个方面：
i 将原有的工作扩展到非线性模型和非高斯噪声领域 。
ii 对当前最优解做线性化 ， 以减轻非线性带来的影响 。
iii 将原来的滤波器改造为现在标准的预测和校正两步 。

文章插图


文章插图

原来的 Bayes Filter 经过线性近似和高斯假设后变为：

文章插图

可以看出公式 (1.18) 与公式 (1.6) 出发点是一样的 ，  只是 (1.18) 给出了可 性的均值方差求解公式 。
对照公式 ((1.18)) 左右两边 ， 可以得出：
状态预测：

文章插图

卡尔曼增益：

文章插图

修正项

文章插图

03 编程实现3.1 几个重要的矩阵
本文只侧重 SLAM 中有关的应用 。文末有示例代码链接 。
首先简单介绍 EKF 中的几个重要矩阵 。
3.2 系统状态矩阵 X
由机器人 robot 位姿和 n 个路标 landmank 的坐标组成的 (3 + 2 × n,1) 矩阵 ，  称为系统状态矩阵 。
位移变量的单位可以是米或厘米 ， 角度变量的单位是度或弧度 。

文章插图

3.3 协方差矩阵 P
协方差矩阵 P 在 SLAM 中十分重要 ， 描述了机器人位姿间的相关性 ， 机器人位姿与路标间的相关性以及路标与路标间的相关性 。P 是对称的 。

文章插图

P 矩阵是对称的 ， E=D ， F=G(代表了最后一个路标与第一个路标的协 方差项) 。在增加新的路标点的时候 ， 不仅要在对角线上增加各自的协方差项 ， 还要增加与机器人的协方差项 (第一行、第一列) ， 以及与其他路标的协方差项 。
矩阵内容大致如下：

文章插图

协方差矩阵最初如果没有观测到路标点 ， 会只包含 A ， 随着新的路标点不断加入 ， 维度会越来越大 。

• 顶级风水宝地 至尊蓝月风水宝地
• 各种牡丹寓意和象征风水 蓝色牡丹寓意风水好吗
• 世界十大地理奇观欣赏：伯利兹蓝洞上榜，石浪居第一位
• 富德无线鼠标连接方法 蓝牙鼠标怎么连接电脑呢
• 蓝色钱包真的不好吗 蓝色钱包有风水吗吗
• 为什么有人眼白是淡蓝色 白眼球发蓝图片
• 2022长安cs75蓝鲸版1.5t配6at 长安cs75价格及图片大全新款怎么样
• 江西蓝天学校 江西南昌蓝天学院校长
• 世界罕见的十大天文现象盘点：日全食蓝月亮均上榜
• 看完就知道该咋办了 带的蓝牙入耳式耳机老是滑出来怎么办