- 运动更新
运动更新时，固定点坐标不变，所以只需要根据运动方程更新(x,y,α)及其协方差与互协方差即可。
- 观测到曾经观测过的固定点
这时候依次对观测到的特征点信息进行EKF更新
Landmark observations are processed in the EKF usually one-by-one
- 观测到新的固定点
这时候观测到新的固定点，需要进行状态增广。根据逆观测方程，使用观测信息推测出新加的增广状态均值与方差，然后加入到总体的状态与协方差矩阵中。
主文件为slam.m，运行即可。
paper文件夹里面有pdf文档，里面的公式可能会有些许笔误，最新的更新请访问CSDN
- 传感器探测范围与路标点
- 第一次状态增广
- 状态持续扩大
- 状态增广已停止
2020/2/21增加了轨迹显示
