VINS紧耦合优化公式

         前面求出了IMU测量误差及雅克比,还有视觉部分的残差及雅克比.那么最终的优化怎么处理的.             总的代价函数:                    主要分为三个部分,按照顺序一次是:边缘化的先验信息、IMU测量残差以及视觉的观测残差.         优化模型的状态量是滑动窗口中的状态量:                   第一个式子是滑动窗口内整个状态向量

>>阅读原文<<