最近在学习MSCKF代码，在这里记录一下。
参考博客：
MSCKF那些事儿
一步步深入了解S-MSCKF
一步一步推导S-MSCKF系列
1、MSCKF原理
MSCKF的目标是解决EKF-SLAM的维数爆炸问题。传统EKF-SLAM将特征点加入到状态向量中与IMU状态一起估计，当环境很大时，特征点会非常多，状态向量维数会变得非常大。MSCKF不是将特征点加入到状态向量，而是将不同时刻的相机位姿(位置 [公式]和姿态四元数 [公式])加入到状态向量，特征点会被多个相机看到，从而在多个相机状态（Multi-State）之间形成几何约束（Constraint），进而利用几何约束构建观测模型对EKF进行update。由于相机位姿的个数会远小于特征点的个数，MSCKF状态向量的维度相较EKF-SLAM大大降低，历史的相机状态会不断移除，只维持固定个数的的相机位姿（Sliding Window），从而对MSCKF后端的计算量进行限定。
对于MSCKF来说，EKF预测步骤与INS一样，区别在EKF观测更新，需要用视觉信息来构建观测模型，从而对IMU预测的状态进行更新。INS中GPS可以直接给出位置 [公式]的观测，而视觉通常只能提供多个相机之间相对位姿关系的约束。那观测模型要怎么构建呢？
视觉中，约束通常都是特征点到相机的重投影误差（空间中一个3D特征点根据相机的姿态和位置投影到相机平面，与实际观测的特征点之间的误差）：
2、MSCKF算法步骤
① IMU积分：先利用IMU加速度和角速度对状态向量中的IMU状态进行预测，一般会处理多帧IMU观测数据。
② 相机状态扩增：每来一张图片后，计算当前相机状态并加入到状态向量中, 同时扩充状态协方差.
③ 特征点三角化：然后根据历史相机状态三角化估计3D特征点
④ 特征更新：再利用特征点对多个历史相机状态的约束，来更新状态向量。注意：这里不只修正历史相机状态，因为历史相机状态和IMU状态直接也存在关系(相机与IMU的外参)，所以也会同时修正IMU状态。
⑤ 历史相机状态移除：如果相机状态个数超过N，则剔除最老或最近的相机状态以及对应的协方差.
图中X表示状态向量，P表示对应的协方差矩阵，红色表示当前步骤发生改变的量。
首先初始化状态向量和协方差
然后进行IMU积分，状态向量和协方差都发生改变
接着将新的相机状态加入到状态向量中，扩充协方差矩阵（新相机自身的协方差以及对 [公式]的协方差）
进行观测更新，所有状态和协方差都会发生改变。（注意：第一次因为只有一个相机状态，形成不了重投影约束，所以第一次观测更新并不会做任何事情）
当相机状态个数超过限制时，删除最历史的一个相机状态及其对应的协方差项。
重复2-5。
3、静止初始化
静止初始化(initializeGravityAndBias)：将前200帧加速度和角速度求平均, 平均加速度的模值g作为重力加速度, 平均角速度作为陀螺仪的bias, 计算重力向量(0,0,-g)和平均加速度之间的夹角(旋转四元数), 标定初始时刻IMU系与world系之间的夹角. 因此MSCKF要求前200帧IMU是静止不动的
4、模型建立
MSCKF中的运动模型和观测模型如下：
运动模型和观测模型都是关于误差状态向量X~的，其中 FGH是线性化的Jacobian矩阵。需要注意的是，运动模型只与IMU状态有关，相机状态不发生改变，但是相机与IMU状态之间的协方差会发生改变。观测模型其实只与相机状态有关，但会通过协方差同步对IMU状态进行修正。
3、前端