参考资料：
MSCKF 公式推导_努力努力努力-CSDN博客_msckf推导
0. 说明
左边代表的是传统EKF-SLAM, 红色五角星是old feature,这个也是保存在状态向量中的,状态向量为之前的所有特征点和最新的位姿；
中间这张可以表示的是keyframe-based SLAM, 它会保存稀疏的关键帧和它们之间相关联的地图点;
最右边这张则可以代表MSCKF的一个基本结构, MSCKF中老的地图点和滑窗之外的相机姿态是被丢弃的, 它只存了滑窗内部的相机姿态和它们共享的地图点.
1. 状态估计
坐标系定义：
I系：imu固定坐标系；G系：ECEF坐标系（忽略地球自转的影响）
目的：
跟踪三维位姿变化（I系相对于G系）
状态向量说明：
更新的imu状态 Ximu ，之前Nmax帧下的相机位姿
A 状态向量说明
imu状态向量为：
零偏建模为随机游走过程
imu误差状态向量为：
速度位置零偏采用加性噪声，姿态误差采用乘性噪声（带上三角的是估计值）
、
状态量包括：（前面有N帧相机位姿）
q是姿态，p是平移量
则误差状态量为：
算法的总流程为：
B 状态量和协方差的传播
（1）imu状态的传播为：
imu误差的传播方程（和惯导书一样）
imu更新采用的是五阶龙格库塔法
（2）协方差矩阵的更新
C 状态增广
当记录新一帧图像时，相机位姿可由IMU位姿给出：
imu系到相机系的旋转矩阵、相机系原点在i系的位置都是已知的（简单情况下可以约等于I和0？）
此时相机姿态扩展到状态向量中，对应的状态协方差矩阵增广为：
不明白协方差矩阵是怎么增广的？？？
D 测量模型（论文主要贡献点）
定义一个残差量，与误差状态量成线性关系，
动机： 多个帧下观察到的同一静态特征，会产生涉及所有这些相机位姿的约束
因此，相机观测量是按照观察到的特征量来分组的。同一特征点的所有观测量值用于定义约束方程，用于关联所有的相机位姿。
考虑一个特征点，由M帧图像观测到的情况
特征点在相机坐标系下的位置与相机位姿的关系表示为：
特征点在ECEF系下的三维坐标是未知的，因此在算法的第一步，需要用最小二乘法求得该特征点的三维位置。（三角测量的方法）
一旦获得了特征位置，残差可定义为：
线性化后为：
H矩阵为测量值z相对于误差状态量和特征点位置的雅可比矩阵
雅可比矩阵的值由参考文献【21】给出
根据附录可知，状态估计量会用于预测特征点位置，因此，特征点位置预测误差和误差状态量是相关的，因此该测量方程不能直接用于EKF测量更新。、
这里的V'论文里用的是A’
需要说明的是，A矩阵不需要计算，而是根据吉文斯旋转公式可以高效的将向量r和矩阵Hx投影到Hf的左零空间中。
此时噪声方差矩阵为：
E EKF 更新
以下两个条件满足之一，更新
（1）不再检测到之前多帧跟踪的特征点时；
（2）状态向量内的相机之前的位姿数目达到最大时，按照自己的理解，是丢弃（倒数第2个旧的值~倒数第2个旧的值+Nmax/3）个的位姿。
“我们选择始终将最古老的姿势保持在状态向量中，因为涉及姿势的几何约束对应于更大的基线，因此携带更有价值的定位信息。这种方法在实践中表现非常好”。
观测噪声是对角阵
实际中d可能会很大，为了减少计算复杂性，我们对Hx矩阵做QR分解
由此，观测方程可写为：
通过QR分解，可将残差r投影到Hx的基向量上，由此可以从测量值中仅保留有用的信息，Q2r0只和噪声相关，可以直接丢弃，即：
此时测量噪声方差也要改变：
之后利用EKF做更新就行了
之后还讨论了计算复杂性
2. 试验验证
特征提取方法：SIFT算法
滤波器状态向量中最多保持30个相机位姿
图像：3Hz
3. 其他公式推导
3.1 三角化求特征点在ECEF坐标系下的位置
三角化是通过多帧相机对同一个点的观测计算出特征点在世界坐标系下的绝对3D坐标,或者你可以认为是恢复出一个比较可靠的3D点。
这里应该需要知道各个相机之间的位姿变化？
3.2 观测矩阵H的推导
看代码的时候再补充
4 来自其他资料
“VIO算法的核心在后端，不管是优化的方法还是滤波的方法，都是靠后端的紧耦合融合获得较高的VIO精度，为了节省计算量一般前端都会做得很简单，基本都是FAST、Harris等角点加LK光流跟踪（S-MSCKF中使用的FAST角点，VINS_MONO中使用的goodFeatureToTrack）。如果计算资源充裕的话，也可以换成ORB、SURF、SIFT等描述子特征，精度应该会有所提升。”
996
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
