# RTK–IMU Engineering 6DoF 分支

该分支与 V3 `data_only` 严格链路并列，不改变其门禁结论：

```text
data_only_6dof_accepted = false
```

工程分支固定使用真实水平静止场地得到的 R2G 旋转，默认参考值为
`RPY=[0.4543066, -0.0026392, 0.0122384] deg`，输出始终记录：

```text
rotation_source = R2G_gravity_level_prior
translation_conditional_on_rotation = true
```

## 条件模型

估计量是 `l_I = p_ANT1^I`。每个连续质量段使用 HI13 设备时间、raw gyro/acc 动态预积分，融合 RTK Fixed 的 GGA/BESTNAVA 位置、BESTNAVA Doppler velocity 和 Q4 GNHPR 的 ANT1→ANT2 基线。逐段 nuisance state 包含初始姿态、IMU 原点初始位置、速度、gyro bias 和 acc bias。HI13 absolute quaternion/RPY（尤其 absolute yaw）不进入机械外参因子；R2V 仅作诊断。

静止相关因子分为两类：

- `gravity_candidate`：连续约 1.5 s 的低角速度、gyro/acc 方差稳定且加速度模长接近重力；它不能等价为静止。
- `zupt_static`：在 gravity candidate 基础上，必须同时有至少约 1 s 的连续 BESTNAVA Doppler 速度接近 0。高速或匀速直线不会加入 ZUPT。

连续段复用 R0 质量断点：checksum 无效、定位非 Fixed、GNHPR 非 Q4、设备时间回跳、HPR 测量间隙、baseline jump、位置测量间隙或 IMU gap 都会切段，禁止跨断点预积分。 断后片段还必须至少包含 6 个求解节点且持续不少于 5 s；更短的局部欠约束微段不会跨断点拼接，而是直接不进入杆臂优化。

## 高程与残差口径

- GGA 只约束 XY；GGA MSL altitude 不定义也不参与 ENU-Z。
- ENU-Z reference 只来自有效 Fixed BESTNAVA altitude；BESTNAVA 才约束 XYZ。
- 输出分别为 `gga_xy_residual`、`bestnava_xyz_residual` 和 `doppler_velocity_residual`。未进入 Z factor 的 GGA 高度不进入垂向残差统计。

## 可观性与验收

杆臂可观性不再使用全状态最小奇异向量。状态分为杆臂 `l` 与 nuisance state，对优化 Hessian 计算 Schur complement：

```text
H_l_marg = H_ll - H_ln pinv(H_nn) H_nl
```

只对该 3×3 marginal lever information 做 SVD，并输出：

- `l_I_marginal_covariance_m2`、`l_I_std_m`
- `lever_information_singular_values`
- `lever_information_condition_number`
- `lever_precision_rank`
- `weakest_lever_direction_I`

门禁分为两层：

- `solver_health_gates` 只判断优化是否收敛、数值是否有限且残差未发散。
- `engineering_acceptance_gates` 使用更严格的杆臂 marginal std/information/rank/condition、BESTNAVA XYZ、GGA XY、Doppler velocity、LOO、bootstrap、旋转敏感性和可选手量一致性。

任一核心门禁失败时，`engineering_6dof_accepted=false`。没有完整执行 bootstrap 和 18 组旋转敏感性时，两项门禁明确为 false，不会把阶段性 base/LOO 结果误标为正式放行。

Bootstrap 按 session 有放回抽样，并保留重复 session 的 multiplicity；重复抽中的 session 会重复贡献其全部连续段。

## 机械杆臂软先验与双解输出

`--manual-l-i-m` 仅在同时提供 `--manual-l-i-std-m` 或完整
`--manual-l-i-covariance-m2` 时才成为白化高斯软因子。求解器始终先运行无先验
`free_solution`，再运行 `prior_constrained_solution`；输出还包含机械参考及两者到
参考的差值。当前活动 engineering 解为 prior-constrained 解（若启用），但 acceptance
额外要求 free-solve 也与机械参考一致，软先验不能掩盖不可观或数据矛盾。

示例（数值需使用实际机械测量的 1σ，不可把示例值当作默认）：

```powershell
--manual-l-i-m -0.45072 -0.25682 0.73208 `
--manual-l-i-std-m <sigma_x_m> <sigma_y_m> <sigma_z_m>
```
## 坐标转换

```text
T_RTK_IMU: p_RTK = R_RTK_IMU p_IMU - R_RTK_IMU l_I
T_IMU_RTK: p_IMU = R_RTK_IMU^T p_RTK + l_I
```

RTK 原点是 ANT1，因此 `T_IMU_RTK.translation == l_I`，两矩阵必须互逆。

## 分阶段运行

第一阶段默认只运行 base fit、marginal observability、residual audit 和 LOO：

```powershell
python tools/run_rtk_imu_engineering_6dof.py `
  --manifest D:\data\rtk_imu_unified_v3\manifest.json `
  --output artifacts/rtk_imu_calibration_v3/engineering_6dof_base_loo.json `
  --session <session-a> `
  --session <session-b> `
  --session <session-c>
```

只有 base/LOO 合理后，才显式启动全量验证：

```powershell
python tools/run_rtk_imu_engineering_6dof.py `
  --manifest D:\data\rtk_imu_unified_v3\manifest.json `
  --output artifacts/rtk_imu_calibration_v3/engineering_6dof_full.json `
  --session <session-a> `
  --session <session-b> `
  --session <session-c> `
  --run-bootstrap --bootstrap-repetitions 40 --bootstrap-seed 0 `
  --run-rotation-sensitivity
```

正式会话应覆盖直行加减速、左右转、坡道和明确水平静止段。不得为了运行时间降低验收门禁或把未完成验证标为成功。