通过稀疏扩展信息滤波实现SLAM,并与EKF-SLAM进行性能对比
综述：对于EKF-SLAM需要对已存在的环境特征点和机器人本身，计算一个符合高斯分布的后验概率
模型，不幸的是EKF-SLAM计算需要很大的内存空间，并且随着路标的增加，内存需求和计算量成平
方倍增长，计算量大约为路标N的平方。对于EKF-SLAM只能处理包含几百个特征点（路标信息）的
地图。
1.我们在EKF-SLAM方法的基础上，在本文中我们提出一种SLAM算法用最近特征点
之间的信息，和地图中特征点与机器人位置的信息表示成一个后验概率函数（或者
说与后验概率函数导出的转移矩阵）。这种方法称为扩展信息滤波（EIF），这个滤
波器包含了一个信息矩阵，我们在EIF的基础上，提出了一种稀疏扩展信息滤波，这
是本文所做的贡献。
我们来看稀疏扩展信息滤波，SLAM算法的后验分布概率函数由地图中临近结点的相对
链接决定，我们来举一个例子。
在图1的左边，是机器人和它周围50个路标的位置信息，中间部分是机器人和它
路标的标准化协方差，右图是归一化后的转移矩阵。
我们发现归一化后的信息矩阵是十分稀疏的，这引导我们寻找一种方法，用稀疏
的转移矩阵来表示SLAM的过程。
下面我们来看图2：
我们发现，不同于图1，此时机器人只跟特定的路标相关联（表现在图中为黑色实心圆）
，这些点被称为活动特征点，我们发现存储一个稀疏信息矩阵，转移矩阵的大小现在
只跟特征点数目线性成比例（而不是原来的平方成比例）.
2.首先介绍EIF
EIF的转移矩阵如上图所示，转移矩阵的大小与路标N，构成一个N*N的矩阵，因此在
密集型的信息矩阵中，计算量的大小与路标N的平方成比例。
（1）对于EIF的测量信息更新
从图a到b的过程中，机器人在a图中观测到了路标y1，而在图b中新增了观测路标y2,
对于EIF，每一个观测路标之间，以及观测路标与机器人之间都是相关的，因此转移矩阵中
增加了机器人与路标之间的联系信息.
（2）对于EIF进行位置更新
我们发现从图a到图b中，根据a中路标的位置和路标转移矩阵，确定了y1,y2之间的联系.
即确定了已有路标之间的相关性。
3.对于SEIF的介绍
对于密集的扩展信息矩阵，所有观测到的路标都与机器人相联系，而在稀疏扩展信息矩
阵中，我们定义了一个约束变量X，X表示与机器人相关的路标的约束数目，约束变量y表
示路标之间的相关性约束！
好处：减少运算量，而且误差也不会有太大的变化(相比于密集型的扩展信息矩阵）
我们来看对比a和b图，我们发现，a是密集型的扩展信息矩阵与b不同的是，b中的
y1与路标是不相连的（因为我们定义了约束变量x），因此在b中的转移矩阵中，缺
少了y1xt和xty2项，矩阵由密集变为稀疏，减少了存储空间和计算量。
4.比较SEIF与VIO的仿真结果
在第一次的仿真实验中，我们对比了VIO和SEIF，我们仿真的数据集中，假设一个汽
车在一个载满树的公园里面运动，汽车上装了一个激光测距器以及测试转向角和汽车速度的装置，
我们可以看到如果直接用vio，我们理论上希望的是采用激光测距器观测到树作为我们的路
标，但是实际情况下，其他的影响（比如道路上的其他汽车？）会引入额外的误差，使得我们所
绘制的运动模型与理论相差太远。
但是如果采用了SEIF情况则会改变，在短短的3.5KM的运动中，相比于GPS精准定位，SEIF的定
位误差在0.5米左右。下图为在仿真过程中实际观察到的路标信息。
