108 lines
4.9 KiB
Markdown
108 lines
4.9 KiB
Markdown
# 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
|
||
```
|
||
|
||
正式会话应覆盖直行加减速、左右转、坡道和明确水平静止段。不得为了运行时间降低验收门禁或把未完成验证标为成功。 |