论文阅读：LIC-Fusion: LiDAR-Inertial-Camera Odometry

本文提出了一种紧耦合的多传感器（雷达-惯导-相机）融合算法，将IMU测量、稀疏视觉特征、提取的激光点融合。提出的算法在时间和空间上对三个异步传感器进行在线校准，补偿校准发生的变化。贡献在于将检测和追踪的激光surf/边特征和观测到的稀疏特征点以及IMU数据用MSCFK框架融合，仅仅采用单线程实现，实现了6自由度的位姿估计。雷达特征包括点和线面，在户外和室内环境都进行了实验，目前最好的VIOL里程计。

LIC-FUSION

A.状态向量

状态向量主要包括\(k\)时刻的IMU状态、IMU和相机之间的外参、IMU和激光雷达之间的外参、滑动窗口中过去\(m\)个相机帧对应的IMU状态、\(n\)个雷达帧对应的IMU状态。分别见公式（1）到（6）。当前时刻IMU状态包括全局坐标系到IMU的位姿四元数、IMU在全局坐标系中的速度、位置，重力和加速度的偏差。外参状态向量包括两个传感器之间的位姿变换。由于传感器延迟、时钟偏移或者数据传输延迟，作者估计了不同传感器与IMU之间的时间偏移（即以IMU为基准），假设存在一个时间偏移量，对其进行纠正，见公式（7）-（8）。参考文献[18]。状态估计值、偏差值、估计值的定义及更新见公式（10），注意旋转不能直接加法。

B.IMU传播

IMU运动学方程见公式（11）-（15）。

C.状态增强

当系统接收到新的一帧图像或者雷达数据，IMU将状态传递到当前时刻，同时也更新窗口中的IMU状态，为了标定不同传感器之间的时间偏移，将估计当前IMU时间，见公式（16），协方差矩阵见公式（17）。雅克比的推导见公式（18）（19）。

D.测量模型

1）雷达特征测量

雷达特征选取曲率比较低或者高的部分，分别对应着边和面。通过使用投影到最近的帧寻找最近的对应特征进行追踪，使用kd-tree快速查找，即将i+1帧的特征点投影到i帧上。而帧间位姿变换可以通过雷达i+1-IMU-Global-IMU-雷达i，具体推导见公式（21）-（23）。追踪完成后，在之前的两帧scan中找到两个边特征，这两个最近的特征应该在相邻的ring上，假设这两个边特征和当前帧的变特征对应物理世界中同一条边，则重投影的误差可以用当前帧的特征线到之前两个线的距离表示，见公式（24）。

对该距离进行线性化，加上高斯白噪声，需要求距离对状态向量的雅克比矩阵，为了实现扩展卡尔曼滤波更新，需要知道距离测量的协方差，由于该距离不是直接测量，因此需要用上文的相关帧上点的协方差进行传递，计算公式见公式（27）。使用基于马氏距离的概率方法将外点去除，见公式。

同理，对于投影的平面surf特征，在其他帧找到三个对应的surf特征，假设是在相同的物理平面上进行采样。测量残差为重投影的特征点和三个对应点形成平面的距离，协方差和马氏距离测试与边特征相似。

2）视觉特征测量

给定一个新的图像，提取FAST特征，使用KLT光流进行追踪，一旦视觉特征丢失或者追踪在滑动窗口之外，使用当前相机位姿进行三角化3D点。已知了3D视觉特征，残差可以用公式（28）和（29）表示，主要就是线性化。这里零空间和在线标定没看懂，二者有什么联系？

E.测量简练

在线性化雷达和视觉特征测量后，可以采用扩展卡尔曼滤波更新，而视觉和雷达的残差可以统一公式，由于测量量太多，采用Givens旋转对测量hessian矩阵进行QR分解后。线性的测量残差可以用标准EKF进行更新。

问题

MSCKF nullspace projection to remove this dependency 啥意思

滑动窗口为什么不维护相机位姿