slam算法说明：
SLAM 算法最早出现在 SmithSelf 和 Cheeseman 的论文中,他 们 运用扩展卡尔曼滤波对状态空间中的机器人位姿和特征 ( 路标 ) 位置进行同 时 估 计 。 计 算 复 杂 度 高 一 直 是 SLAM 的 难 题 之 一 ，在 基 于 特 征 的SLAM 算法中,标准 EKF SLAM 方法的计算复杂度与环境特征个 数 呈二次方关系,这使得它只能在一般不超过上百个特征的较小范围内应用。 因此,研究一种计算量可随地图大小进行缩放的 SLAM 算法成为一个公共难题。由于 SLAM 问题针对未知且不确定的环境,一般使用概率方法来描述该问题。 SLAM 的常用概率解法有两类, EKF 算法 和 粒 子 滤 波 算法。 对于非线性系统的 SLAM 问题 EKF 算法和快速 SLAM(FastSLAM)算法是比较典型的两种算法 。 粒子滤波方法对模型进行直接近似,它不要 求 控 制 向 量 和 观 测 值 满 足 高 斯 分 布 假 设 将 粒 子 滤 波 方 法 应 用 到高维 SLAM 问题,计算量非常大, FastSLAM 算法 把 联 合SLAM状 态 分成运动部分和条件地图部分以缩小采样空间 ,机器人的位姿用不同权值的粒子 表 示 ,地 图 由 独 立 的 高 斯 分 布 解 析 表 示 ,机 器 人 位 姿 状 态 的递归估计采用粒子滤波方法,地图状态的递归估计采用 EKF 方法。
主要算法：
（1）、EKF 的 SLAM 算法
传统的 SLAM 算法需要将不同时间、不同地点的感知信息进行匹配和联合 ,存 在 局 部 数 据 之 间 的 关 联 问 题 ,也 存 在 局 部 数 据 和 全 局 数据的关联与匹配问题 [3] 。随着 SLAM 问题研究的深入, 人们发 现 EKF 方 法 的 瓶 颈 在 于 其计算复杂性 。 对于其最优滤波,无论对滤波方程如何整理和计算,其计算复杂度都至少与地图中特征个数的平方成正比 ,难以满足构建大规模地图和实时性的要求。 对于大规模地图, EKF SLAM 算法难以避免出现不一致现象。 一致性 (consistency) 是指算法能得到无偏估计且能正确地给 出 估 计 方 差 ,近 几 年 ,国 外 许 多 学 者 在 这 方 面 已 经 进 行 了 研究 , 分析了模型误差、线性化误差等因素对一致性的影响。 这些研究表明, EKF 算法并不适合大规模的 SLAM 。基于扩展卡尔曼滤波的 SLAM 算法是一种经典方法,虽然该方法计算复杂度高且无法处理“闭环”问题,但其收敛性是目前所有算法中最好的,因此至今仍在陆地、水下及空中等场合得到广泛的应用。
（2）、FAST SLAM
与基于 Kalman 滤波的 SLAM 不同, RBPF SLAM 采用粒子 来 表 示机器人的位姿,而环境特征的估计依然利用 EKF 解析计算。 这种将状态分解为采样部分和解析部分的粒子滤波方法一般称为 RBPF 。 由于这 种 分 解 , RBPF SLAM 的 计 算 复 杂 度 为 O(NM) ,其 中 N , M 分 别 为 所用粒子的数目及特征个数,如给定粒子数 N , RBPF SLAM 的 计 算 复 杂度与特征个数 M 成线性关系,而传统 EKF SLAM 的复杂度为 O(M) ,因此 RBPF SLAM 也被称为 FAST SLAM , 这种算法降低了计算复杂度 ,是 SLAM 的一种常用算法 。FAST SLAM 算 法 是 同 时 定 位 与 地 图 创 建 领 域 的 一 类 重 要 方 法 ,UPF-IEKF FastSLAM 2.0 算法采用 UPF 估计机器人的路径,地图估计则采用 IEKF 算法 UPF 算法使粒子向后验概率高的区域运动,提高了估 计 精 度 同 时 降 低 计 算 复 杂 度 ; IEKF 算 法 通 过 迭 代 观 测 更 新 过 程 来提高估计精度。 地图估计和路径估计是相互关联的 ,路径和地图估计的精度主要取决于路径估计的方法,采用 UKF 估计地图时,虽然估计精度有所提高,计算量却高于 EKF 算法。 综合考虑估计精度和计算复杂度, 认为 “ UPF-IEKF ” 是一种更合理的 FastSLAM 算法滤波架构,UPF-IEKF FastSLAM 2.0 ( n=2 )算 法 是 FastSLAM 2.0 系 列 算 法 中 最 理想的算法。