【MSCKF】MSCKF 物理量的含义：特征点雅可比矩阵、状态雅可比矩阵与残差向量的物理含义与关联-优快云博客

特征点雅可比矩阵、状态雅可比矩阵与残差向量的物理含义与关联

特征点雅可比矩阵、状态雅可比矩阵和残差向量的物理含义及它们之间关联

在MSCKF（Multi-State Constraint Kalman Filter）算法中，特征点雅可比矩阵（H_f）、状态雅可比矩阵（H_x）和残差向量（res）是构建观测方程和执行滤波器更新的核心数学组件。
下面详细解释它们的物理含义、数学定义以及相互关系。

1. 残差向量（res）的物理含义

残差向量（res）表示观测值与预测值之间的差异。在视觉惯性里程计中，残差计算如下：

res = z_{meas} - z_{pred}

其中：

$z_{meas}$ 是相机实际观测到的特征点图像坐标（像素值）
$z_{pred}$ 是根据当前状态估计和特征点位置预测的特征点图像坐标

在代码中，残差计算发生在 get_feature_jacobian_full 函数内部：

// 我们的残差
Eigen::Vector2d uv_m((double)feature.uvs[cam_id][m](0), (double)feature.uvs[cam_id][m](1));
res.block(2 * c, 0, 2, 1) = uv_m - uv_dist;

这里 uv_m 是实际观测到的图像坐标，uv_dist 是考虑了相机畸变后的预测坐标。每个特征点在不同时刻、不同相机中的观测会产生2个残差分量（u和v坐标）。

2. 特征点雅可比矩阵（H_f）的物理含义

特征点雅可比矩阵（H_f）表示观测残差相对于特征点状态变化的敏感度。数学上定义为：

$H_f = \frac{\partial res}{\partial f}$

其中：

$res$ 是残差向量
$f$ 是特征点的状态参数（位置或逆深度参数）

H_f的维度取决于：

行数：等于特征点所有观测产生的残差总数（每个观测贡献2行）
列数：等于特征点状态参数的数量（取决于特征点表示方式）

例如：

对于全局3D表示（GLOBAL_3D）： $H_f$ 是 (2n)×3 矩阵，其中n是观测次数
对于单目逆深度表示（ANCHORED_INVERSE_DEPTH_SINGLE）： $H_f$ 是 (2n)×1 矩阵

在MSCKF中， $H_f$ 的核心作用是后续通过零空间投影（nullspace projection）被消除，从而避免直接估计特征点位置。

3. 状态雅可比矩阵（ $H_x$ ）的物理含义

状态雅可比矩阵（ $H_x$ ）表示观测残差相对于系统状态变化的敏感度。数学上定义为：

$H_x = \frac{\partial res}{\partial x}$

其中：

$res$ 是残差向量
$x$ 是系统状态向量（包括IMU位姿、速度、偏置等）

$H_x$ 的维度：

行数：等于特征点所有观测产生的残差总数（每个观测贡献2行）
列数：等于系统状态向量的维度（pvqbabg 16）

状态雅可比矩阵的构建涉及复杂的链式求导，主要考虑：

从像素坐标到归一化平面坐标的导数
从归一化平面到相机坐标系的导数
从相机坐标系到IMU坐标系的导数
从IMU坐标系到世界坐标系的导数

4. 三者之间的关联关系

在MSCKF算法中，这三个组件通过观测方程联系起来：

$H_x \cdot (x - \hat{x}) + H_f \cdot (f - \hat{f}) + n$

其中：

$\hat{x}$ 是系统状态误差
$\hat{f}$ 是特征点状态误差
$n$ 是观测噪声

MSCKF的核心创新：零空间投影

MSCKF的核心优势在于不需要直接估计特征点位置。通过零空间投影操作，我们可以将上述方程转换为仅依赖系统状态的形式：

$Z^T \cdot res = Z^T \cdot H_x \cdot (x - \hat{x}) + Z^T \cdot n$

其中 $Z$ 是 $H_f$ 的左零空间矩阵，满足 $Z^T \cdot H_f = 0$ 。

这一过程在 nullspace_project_inplace 函数中实现，它通过Givens旋转将特征点雅可比矩阵 $H_f$ 变为上三角矩阵，然后仅保留零空间分量。

5. 代码中的实现细节

雅可比矩阵计算流程

特征点表示选择：根据配置选择特征点的表示方式（如3D位置、逆深度等）
特征点全局位置计算：

// 计算特征点在全局坐标系中的位置
Eigen::Vector3d p_FinG = feature.p_FinG;
if (LandmarkRepresentation::is_relative_representation(feature.feat_representation)) {
  // 对于锚定式表示，需要从锚定相机坐标系转换到全局坐标系
  Eigen::Matrix3d R_ItoC = state->_calib_IMUtoCAM.at(feature.anchor_cam_id)->Rot();
  Eigen::Vector3d p_IinC = state->_calib_IMUtoCAM.at(feature.anchor_cam_id)->pos();
  Eigen::Matrix3d R_GtoI = state->_clones_IMU.at(feature.anchor_clone_timestamp)->Rot();
  Eigen::Vector3d p_IinG = state->_clones_IMU.at(feature.anchor_clone_timestamp)->pos();
  p_FinG = R_GtoI.transpose() * R_ItoC.transpose() * (feature.p_FinA - p_IinC) + p_IinG;
}

雅可比矩阵链式求导：

// 预计算一些雅可比矩阵
Eigen::MatrixXd dz_dpfc = dz_dzn * dzn_dpfc;
Eigen::MatrixXd dz_dpfg = dz_dpfc * dpfc_dpfg;

// 链式法则：获取总的特征点雅可比矩阵
H_f.block(2 * c, 0, 2, H_f.cols()).noalias() = dz_dpfg * dpfg_dlambda;

// 链式法则：获取状态雅可比矩阵
H_x.block(2 * c, map_hx[clone_Ii], 2, clone_Ii->size()).noalias() = dz_dpfc * dpfc_dclone;

鲁棒核函数应用

代码中还应用了Huber核函数来提高系统对异常值的鲁棒性：

double huber_scale = 1.0;
if (use_Loss) {
    const double r_l2 = res.squaredNorm();
    if (r_l2 > huberB) {
        const double radius = sqrt(r_l2);
        double rho1 = std::max(std::numeric_limits<double>::min(), huberA / radius);
        huber_scale = sqrt(rho1);
        res *= huber_scale;
    }
}

// 雅可比矩阵也要相应缩放
if (use_Loss) {
    dzn_dpfc *= huber_scale;
}