vins-mono:雅可比矩阵的推导
矩阵 推导 mono
2023-09-11 14:18:37 时间
imu残差公式
我们计算雅可比采用加扰动形式
以下是位置增量偏差对旋转qbk的雅可比推导
对应的雅可比矩阵的代码
jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt));
总结：
那么
参考：
[1]: Quaternion kinematics for the error-state Kalman filter