前面求出了IMU测量误差及雅克比,还有视觉部分的残差及雅克比.那么最终的优化怎么处理的.
总的代价函数:
主要分为三个部分,按照顺序一次是:边缘化的先验信息、IMU测量残差以及视觉的观测残差.
优化模型的状态量是滑动窗口中的状态量:
第一个式子是滑动窗口内整个状态向量，其中n是帧数，m是滑动窗口中的特征点总数，维度是15*n+6+m，第二个式子xk是在第k帧图像捕获到的IMU状态，包括位姿，速度，旋转，加速度计和陀螺仪偏置，第三个式子是相机外参(从相机坐标系下转到IMU的外参数)
前面IMU及视觉测量的已经推出来了,对于边缘先验怎么具体的进行还不是太了解.
边缘先验部分:
雅克比就是之前边缘化后的雅克比，误差就是之前的误差再加上新的误差。
这个滑动窗口残差的更新这块还是不大理解.
可以明确的是:
公式(24)是不太理解的
参考: