非线性优化

路标点越多矩阵越大，计算量越大

 粒子滤波器相当于自身位置的后验估计（现在已经得到的观测能得到什么样的位置），星为机器人的实际位置（但是不知道），然后随机撒红点，然后运用机器人本体的运动模型，根据观测，删除一些区域，再撒点，直到收敛

滤波只能抑制噪声但是不能消除，绿色为实际轨迹，紫色为预测轨迹

回环检测可以消除

EKF相邻两帧的特征放入矩阵，之前帧特征不在矩阵里

只算当前X和路标的关系放到状态矩阵里，没有之前的X

根据估计的R,T计算特征的位置与观测作差就是误差

入为权重

A,B,C,D为位置，l为路标点