非线性优化
路标点越多矩阵越大，计算量越大
粒子滤波器相当于自身位置的后验估计（现在已经得到的观测能得到什么样的位置），星为机器人的实际位置（但是不知道），然后随机撒红点，然后运用机器人本体的运动模型，根据观测，删除一些区域，再撒点，直到收敛
滤波只能抑制噪声但是不能消除，绿色为实际轨迹，紫色为预测轨迹
回环检测可以消除
EKF相邻两帧的特征放入矩阵，之前帧特征不在矩阵里
只算当前X和路标的关系放到状态矩阵里，没有之前的X
根据估计的R,T计算特征的位置与观测作差就是误差
入为权重
A,B,C,D为位置，l为路标点