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