基本概念
卡尔曼滤波KF：Kalman Filter，是贝叶斯滤波的具体化，系统满足线性化和高斯分布假设，运动方程和观测方程都是线性。
扩展卡尔曼滤波EKF：Extended-Kalman Filter，考虑到SLAM系统模型为非线性，故将非线性函数泰勒展开并取前两项，即近似为线性函数，从而满足KF条件，再使用kalman Filter。
SLAM问题两大目标方程：
线性化处理后方程：
SLAM实际应用中的数学模型
机器人实际模型
考虑二维机器人运动过程中时间的离散性：
1. 运动方程
2. 观测方程
SLAM过程参数矩阵（均值、方差）
EKF滤波原理
卡尔曼滤波前，landmarks位置由于噪声存在和累加，具有不确定性，观测呈椭圆范围。
卡尔曼滤波后，通过重复观测优化系统状态量，本质是通过计算卡尔曼增益来衡量模型的估计值和观测模型的结果。
EKF实际应用流程
- 状态预测
用机器人模型对机器人当前时刻的状态进行预测，即对均值和方差进行更新。需要更新的部分为图中绿色填充项。
- 状态更新
综合机器人的测量信息对机器人当前时刻的状态预测值进行修正，即对均值和方差进行修正。 - 数据关联
将观测的landmark和状态向量中的landmark相匹配，通常通过设定马氏距离阈值来判定。
编程实现
关于EKF_SLAM的仿真，基于MATLAB代码，结果如图所示，机器人轨迹的真实值(黑色虚线)，运动模型预测的机器人轨迹(蓝色虚线)，EKF滤波后的机器人轨迹(红色虚线)。
总结
EKF_SLAM适用的场景为特征地图，稀疏；需要里程计信息反馈（输入机器人位姿量）