IMU预积分-旋转残差求雅可比

目录

[1. 残差定义](#1. 残差定义)

[2. 雅可比推导](#2. 雅可比推导)

[3. 右乘BCH近似雅可比的逆的实现](#3. 右乘BCH近似雅可比的逆的实现)

---

1. 残差定义

根据IMU预积分（VINS）中的推导，帧间imu积分项的残差定义如下：

其中，旋转残差项的形式来源为：

中的q代表四元数。假设为旋转误差对应的旋转向量，则：

则等于虚部的2倍：

在IMU预积分（VINS）中以四元数 的形式对残差的雅可比进行了推导，本篇笔记该用以旋转矩阵R （李群）的形式推导该残差对下述扰动量的雅可比：

推导过程会用到李群的BCH公式与伴随性质：

（1）BCH公式：

其中，分别代表两个旋转向量（李代数），代表对李代数进行指数映射得到李群（旋转矩阵），分别代表左乘BCH近似雅可比的逆和右乘BCH近似雅可比的逆。

（2）伴随：

其中，代表旋转矩阵。

2. 雅可比推导

**求导过程采用右乘扰动模型，因为误差（扰动）定义在载体b系。**雅可比的推导过程如下：

1. 对 i 时刻无关量求导

2. 对 i 时刻旋转误差向量求导

3. 对 i 时刻陀螺仪bias误差求导

4. 对 j 时刻无关量求导

5. 对 j 时刻旋转误差向量求导

3. 右乘BCH近似雅可比的逆的实现

右雅可比：

其中，代表一个旋转向量，代表将旋转向量转为反对称矩阵，代表旋转角的大小（旋转向量的模长）。

右雅可比的逆：

代码实现：

#include <Eigen/Core>
#include <Eigen/Geometry>

/**
 * @brief 计算 SO(3) 右雅可比的逆 J_r^{-1}(phi)
 * @param phi 旋转向量 (李代数 so3)
 * @return 3x3 的右雅可比逆矩阵
 */
Eigen::Matrix3d RightJacobianInv(const Eigen::Vector3d& phi) {
    const double theta = phi.norm();
    Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
    
    // 1. 计算反对称矩阵 phi^
    Eigen::Matrix3d phi_hat;
    phi_hat << 0, -phi(2), phi(1),
               phi(2), 0, -phi(0),
               -phi(1), phi(0), 0;

    // 2. 根据角度大小选择计算方式
    if (theta < 1e-6) {
        // 小角度近似：J_r^{-1} ≈ I + 0.5 * phi^
        return I + 0.5 * phi_hat;
    } else {
        // 正常角度：使用闭式公式
        double theta2 = theta * theta;
        double sin_theta = sin(theta);
        double cos_theta = cos(theta);

        // 计算系数: [1/theta^2 - (1+cos(theta))/(2*theta*sin(theta))]
        double coeff = (1.0 / theta2) - (1.0 + cos_theta) / (2.0 * theta * sin_theta);

        // 组合公式: I + 0.5*phi^ + coeff * (phi^)^2
        return I + 0.5 * phi_hat + coeff * phi_hat * phi_hat;
    }
}

代码中对数值稳定性进行了处理。当旋转角很小，趋近于0时，直接使用上面的闭式公式会导致分母为 0 或数值不稳定。此时需要使用泰勒展开近似：