参考：
https://zhuanlan.zhihu.com/p/21381490
https://citeseerx.ist.psu.edu/viewdoc/download;jsessionid=FA1024834F74311ED580F5D811ADC717?doi=10.1.1.208.6289&rep=rep1&type=pdf
https://zhuanlan.zhihu.com/p/45207081
在SLAM中，我们一般使用扩展卡尔曼滤波器基于机器人运动信息与传感器测量特征点信息估计机器人的状态。这里，我们将详细讨论将其应用于SLAM中的具体步骤。
在得到路标点的位置和方位，并且将路标点进行关联后，SLAM的过程分为如下三个部分：
-
基于机器人运动信息更新机器人当前状态；
-
基于路标信息更新估计状态；
-
在当前状态中增加新的状态；
感觉基本就是ekf这套
MSCKF 学习，这个有点像位姿图优化，但是观测中改了一下观测模型，没用range-bearing形式，用了重投影误差，然后再改写了一下，前边的imu的模型这里大概有一些参考文献：
公式出自：
Trawny, Nikolas, and Stergios I. Roumeliotis. “Indirect Kalman filter for 3D attitude estimation.” University of Minnesota, Dept. of Comp. Sci. & Eng., Tech. Rep 2 (2005): 2005.
http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf
https://charlieleee.github.io/post/msckf/#references
https://fzheng.me/2016/11/20/imu_model_eq/
http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf