MSCKF公式推导

简介：
该算法基于多状态约束卡尔曼滤波技术（Multi-State Constraint Kalman Filter）设计开发，并旨在解决视觉与惯性数据融合问题。作为解决视觉与惯性数据融合问题的理想方案之一，MSCKF构建了一个紧耦合的SLAM框架。通过融合扩展卡尔曼滤波技术和视觉信息，该框架不仅实现了移动机器人的高精度定位，还显著提升了系统的鲁棒性。MSCKF的主要贡献在于其避免将路标点的位置直接纳入状态向量的策略。常规方法会通过将路标点位置作为状态向量的一部分来实现定位，这会导致状态空间维度不断增大，从而增加计算负担并影响算法效率。针对这一问题，MSCKF提出了一种创新性处理方式：当路标点不再可见或老化时，采用高斯-牛顿优化算法对这些路标点的三维位置进行估计，并将多个相机获取到的这些路标点的观测值作为约束条件整合进卡尔曼滤波框架中。这种处理方法能够有效避免状态向量维度增长的同时保留关键信息，从而显著提升了算法运行效率。 其工作流程可划分为预测阶段、图像处理环节以及状态更新模块三大组成部分。在预测阶段，通过IMU数据进行状态推演，在预测阶段，系统对后续时刻的状态作出预判。基于视觉信息的检测和追踪功能，在图像处理环节中，算法能够将当前帧的关键点与前帧的路标点建立对应关系。系统在状态更新模块中整合视觉信息、惯性测量结果以及卡尔曼滤波算法输出，持续优化状态估计精度。 在符号定义中，我们引入了一系列数学符号来表示状态向量。这些符号包括真实的状态向量（TrueState）、名义状态向量（NominalState）和误差状态向量（ErrorState）。真实的状态向量包含着噪声与偏差的影响，而名义状态向量则被假定为无噪声且理想的情况。误差的状态向量则是通过两者之差确定的。真实的状态向量是所有其他计算的基础，名义状态向量则被视为理想情况下的状态，误差状态向量则由两者之差确定。在该系统中，状态向量由真实态向量与误差态向量组成。其中，真实态向量基于多帧图像数据以及惯性导航系统的输出信息；而误差态向量则用于表征这些测量值的偏差特性。通过建立连续型误差运动模型来描述系统的动态行为，从而实现对误差态量的预先估计，同时在迭代过程中实时更新当前误差状态。该段落探讨了基于IMU测量结果来进行状态估计的方法。具体包括连续型式的误差运动方程和协方差传播模型。误差运动方程通过表征误差状态随时间的变化规律，而协方差传播则揭示了状态估计的不确定性（协方差）如何随着时间的推移而演变。在相机位姿的增加部分中，算法通过扩大状态向量来整合来自多台相机的数据，以便处理多个相机的观测数据。这种做法有助于更加有效地融合来自各台相机的视觉信息。视觉测量模型部分阐述了将视觉观测数据实现对滤波框架中的整合方法。这涉及定义视觉测量残差的过程，即观测数据与其预期结果间的差异。在卡尔曼滤波框架中应用这些残差时需要进行线性变换，从而实现对卡尔曼滤波方法的有效运用。在更新阶段，算法将视觉测量模型融合进来获取的观测数据作为约束条件完成对EKF的更新。这种处理方式旨在优化系统状态估计精度，并通过提高协方差矩阵的质量来增强整体估算效果。MSCKF方法通过有效利用EKF与边缘化策略，显著提升了计算效率。该算法成功应对了传统VIO中状态量快速增长的问题，并且保持了高精度定位能力。MSCKF已经被广泛应用于机器人定位与导航领域并获得了良好的反馈。