lichun.qu/calibration
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: lichun.qu/calibration:feature/lidar-imu-calibration
Branches Tags
lichun.qu/calibration:feature/lidar-rtk-direct-calibration lichun.qu/calibration:RTK-IMU-calibration lichun.qu/calibration:feature/lidar-imu-calibration lichun.qu/calibration:lidar-body-direct-calibration
...
compare: lichun.qu/calibration:3e8e87fec6548256bb9f4b7191a71dd58ad1fe3e
Branches Tags
lichun.qu/calibration:RTK-IMU-calibration lichun.qu/calibration:feature/lidar-rtk-direct-calibration lichun.qu/calibration:feature/lidar-imu-calibration lichun.qu/calibration:lidar-body-direct-calibration
5 Commits
feature/lidar-imu-calibration ... 3e8e87fec6
Author SHA1 Message Date
lichun.qu 3e8e87fec6 更新 README_Lidar_IMU.md 2026-08-25 11:39:28 +08:00
lichun.qu 0ec0f40406 补充RTK-IMU标定结果与复现说明 2026-08-25 10:42:08 +08:00
lichun.qu 32d8af8ed9 迁移RTK-IMU标定到独立顶层包 2026-08-25 10:21:43 +08:00
lichun.qu d14ae74117 重构RTK-IMU标定链路并完成机械先验工程验证 2026-08-25 09:56:25 +08:00
lichun.qu c2da6dd192 新增独立RTK与IMU外参标定流程及质量验证 2026-08-21 10:04:41 +08:00
90 changed files with 122373 additions and 39 deletions
Expand all files Collapse all files
.gitignore
+19
View File
@@ -8,3 +8,22 @@ build/
examples/synthetic_session/
.venv/
venv/
# Local IDE state
.vs/
# RTK-IMU calibration process artifacts stay local. Keep only the reviewed
# V3 result bundle explicitly listed below under version control.
artifacts/rtk_imu_calibration_v2/
artifacts/rtk_imu_calibration_v3/*
!artifacts/rtk_imu_calibration_v3/README.md
!artifacts/rtk_imu_calibration_v3/engineering_release_decision.json
!artifacts/rtk_imu_calibration_v3/heldout_independent_innovation.json
!artifacts/rtk_imu_calibration_v3/heldout_nonconverged_retry.json
!artifacts/rtk_imu_calibration_v3/lever_information_window_selection.json
!artifacts/rtk_imu_calibration_v3/lever_information_window_selection_refined.json
!artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_47_window.json
!artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_heldout.json
!artifacts/rtk_imu_calibration_v3/mechanical_prior_rotation_sensitivity.json
!artifacts/rtk_imu_calibration_v3/node_graph_free_information_selected_mechanical.json
!artifacts/rtk_imu_calibration_v3/propagation_bias_root_cause_audit.json
README.md → README_Lidar_IMU.md
View File
README_RTK_IMU.md
+177
View File
@@ -0,0 +1,177 @@
# RTK-IMU 标定：流程、结果与复现
本文是本次 G90 双天线 RTK 与 HI13 IMU 标定的工程入口。算法代码在 `rtk_imu/`，命令入口在 `tools/`，正式审核结果在 `artifacts/rtk_imu_calibration_v3/`。原始 `.rscap`、统一导出数据、状态快照与 checkpoint 均只保留本地，不提交仓库。
## 一句话结论
RTK-IMU 旋转外参由双天线基线方向与水平静止重力约束获得；平移外参以机械测量为绝对基准，再通过 RTK+IMU 联合状态图、高动态转弯/坡道数据、独立 held-out 数据和旋转扰动测试进行一致性验证。
当前动态数据不足以独立高精度求出完整 XYZ 杆臂，因此它不是 data-only translation calibration。机械测量给出了杆臂的绝对值；在未参与标定的数据、转弯等杆臂敏感运动以及旋转外参扰动测试中，均未发现该机械外参存在明显矛盾或不稳定性。当前结果的正确表述是：**机械杆臂锚定，并经动态数据一致性验证的工程候选**。
## 当前工程候选与状态
坐标约定：`l_I = p_ANT1^I`，即 ANT1（主天线、左侧）相位中心在 IMU 坐标系中的位置。车体/RTK 安装坐标为：+X 为 ANT1（左）到 ANT2（右），+Y 为车辆前进方向且与 IMU +Y 同向，+Z 向上；GGA 参考点为 ANT1 相位中心，离地 `1.916499878 m`。
- 固定旋转来源：`R2G_gravity_level_prior`。
- 固定旋转近似 RPY（X/Y/Z）：`[0.454°, -0.003°, 0.012°]`。
- 当前工程候选杆臂：`l_I = [-0.4518015159, -0.2644749820, 0.7314656115] m`。
- 变换约定：`p_RTK = R_RTK_IMU * p_IMU + t_RTK_IMU`。
- 对应候选 `T_RTK_IMU`（四舍五入到 6 位小数）：
```text
[[ 1.000000, -0.000214, -0.000044, 0.451777],
[ 0.000214, 0.999969, -0.007929, 0.270363],
[ 0.000046, 0.007929, 0.999969, -0.729325],
[ 0.000000, 0.000000, 0.000000, 1.000000]]
```
当前门禁状态必须同时保留：
- `data_only_translation_accepted=false`：数据本身没有提供足够稳定的完整平移可观性。
- `engineering_translation_accepted=false`：独立传播验证仍存在公共加速度偏差，不能声称正式工程放行。
- `independent_extrinsic_sensitive_validation_passed=true`：杆臂敏感的高动态验证未发现机械杆臂冲突。
- `heldout_physical_validation_passed=true`、`rotation_sensitivity_passed=true`：固定候选在 held-out 物理残差和旋转扰动中保持一致。
因此不得描述为“data-only 标定平移”或“动态数据已精确细化机械杆臂”。完整、机器可读的结论见 [engineering_release_decision.json](artifacts/rtk_imu_calibration_v3/engineering_release_decision.json)。
## 求解流程
1. **统一原始数据导出。** 使用 G90/HI13 设备时间作为主时间轴；host receive time 仅用于诊断。G90 保留 GGA 质量、GNHPR 基线及质量、BESTNAVA Doppler velocity，必要时 PVTSLNA；HI13 保留 system time、gyro、accel、姿态/四元数和 host receive time。
2. **R0 连续性与质量控制。** 校验 checksum、RTK Fixed、设备时间单调性、IMU 覆盖、baseline jump 与测量间隔。轨迹连续性由 IMU 设备时间和预积分覆盖决定；孤立 HPR 缺失/Q5 只禁用或降权 HPR factor，不切断 IMU 轨迹。
3. **旋转外参。** R1b 从基线与 IMU 动态估计 ANT1→ANT2 在 IMU 中的 2DoF 方向；R2V 用基线与高质量 Doppler 速度作独立诊断；R2G 使用水平静止场地中的基线＋重力＋地面水平先验补齐完整旋转。R2G 是本次正式固定旋转来源，旧 GNHPR 三轴手眼只作诊断。
4. **节点状态图。** 每个 GNSS node 包含 `R,p,v,bg,ba` 的 15DoF 状态；相邻 node 由 covariance-whitened IMU preintegration 和 bias random walk 连接。BEST 约束 XYZ，GGA 仅在 BEST 缺失时约束 XY，Doppler 约束速度，HPR 是可选姿态/基线因子。
5. **机械锚定的平移验证。** 杆臂以机械值为基准，分别比较无先验 free、固定机械杆臂和软先验解。free 解只用于观测性诊断，不能因为数值收敛就替代机械值。
6. **独立验证。** 使用 circle、left-right、slope 的高动态非重叠窗口；再使用剩余 held-out 窗口、旋转 ±扰动敏感性、独立 innovation 和 propagation-bias root-cause audit 复核。
## 证据与限制
47 个非重叠标定窗口上的 free/fixed/prior 对比表明机械先验与数据拟合相容，但 posterior/prior 方差比没有显示足够的数据驱动细化，因此 `translation_refined_by_data=false`。剩余 267 个 frozen held-out 窗口的物理验证通过；然而独立传播创新在低速、低角速度区间同时出现位置和 Doppler 的同向偏差，等效为约 `0.20 m/s²` 的公共传播加速度误差。
该误差在 `|omega|` 很小时不能优先归因于杆臂速度项 `R*(omega × l)`，因此它不单独否决高动态杆臂敏感验证；但在传播模型根因关闭前，也不能把候选杆臂标为正式工程已放行。
主要审核证据：
- [47 窗口机械分支](artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_47_window.json)
- [held-out 验证](artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_heldout.json)
- [独立创新审计](artifacts/rtk_imu_calibration_v3/heldout_independent_innovation.json)
- [传播偏差根因审计](artifacts/rtk_imu_calibration_v3/propagation_bias_root_cause_audit.json)
- [旋转敏感性](artifacts/rtk_imu_calibration_v3/mechanical_prior_rotation_sensitivity.json)
## 如何复现
### 1. 准备环境和原始数据
```powershell
cd <repository-root>
python -m pip install -e ".[dev]"
```
将三批 G90/HI13 原始 `.rscap` 会话放到本机的数据位置。数据位置不写入仓库；`tools/export_rtk_imu_unified.py` 中的会话配对清单必须与实际采集文件一致。
```powershell
$OUT = "artifacts\rtk_imu_calibration_v3\reproduce"
python tools\export_rtk_imu_unified.py --output-root "$OUT\unified" --overwrite
$MANIFEST = "$OUT\unified\manifest.json"
```
导出后应检查每个会话目录中的 `export_summary.json`，并确认 `manifest.json` 中记录的传感器时间轴没有被 host receive time 替换。
### 2. 复现固定旋转
`<flat-static-session-id>` 必须是已确认地面水平、车辆静止的会话；可多次传入 `--level-static`。R2V 只作独立诊断，不替代 R2G。
```powershell
python tools\run_rtk_imu_multisource.py `
--manifest $MANIFEST `
--level-static <flat-static-session-id> `
--output "$OUT\r2g_multisource.json"
```
检查输出的 R2G 旋转与本 README 的近似 RPY 一致后，将该旋转固定为后续 node graph 的 `--rotation-rpy-deg 0.454 -0.003 0.012`。若 R2G 不一致，应停止，先复核天线方向、场地水平和时间/轴定义，不应继续求杆臂。
### 3. 复现窗口选择与无先验诊断
从 circle、left-right、slope 三类会话中选择高质量窗口；窗口不可共享 IMU/GNSS/HPR 样本。仓库提交的 `lever_information_window_selection*.json` 是本次审核所用选择结果，可用于对照。
```powershell
python tools\select_rtk_imu_windows_by_lever_information.py `
--manifest $MANIFEST `
--circle-session <circle-session-id> `
--left-right-session <left-right-session-id> `
--slope-session <slope-session-id> `
--output "$OUT\selection.json" `
--rotation-rpy-deg 0.454 -0.003 0.012
python tools\run_rtk_imu_node_graph_free_selected.py `
--manifest $MANIFEST `
--selection "$OUT\selection.json" `
--output "$OUT\free_baseline.json" `
--start-name all `
--rotation-rpy-deg 0.454 -0.003 0.012
```
free solve 的作用是输出边缘化杆臂信息、协方差、最弱方向和多初值稳定性；本次数据若仍未达到完整 XYZ 可观性，不得扩大无先验求解规模来强行放行。
### 4. 复现机械杆臂分支和 held-out 验证
固定工程候选杆臂并使用同一批非重叠标定窗口。`states.npz` 和 checkpoint-dir 是本地过程产物，应保持被 `.gitignore` 排除。
```powershell
python tools\run_rtk_imu_mechanical_prior_branch.py `
--manifest $MANIFEST `
--selection "$OUT\selection.json" `
--free-baseline "$OUT\free_baseline.json" `
--output "$OUT\mechanical_47_window.json" `
--state-output "$OUT\states.npz" `
--rotation-rpy-deg 0.454 -0.003 0.012
python tools\run_rtk_imu_mechanical_prior_heldout.py `
--manifest $MANIFEST `
--calibration-selection "$OUT\selection.json" `
--all-selection <all-nonoverlapping-selection.json> `
--engineering-result "$OUT\mechanical_47_window.json" `
--output "$OUT\heldout.json" `
--checkpoint-dir "$OUT\heldout_checkpoints" `
--rotation-rpy-deg 0.454 -0.003 0.012
```
随后运行独立 innovation、传播根因审计和旋转敏感性。它们不重新优化杆臂，不应被用于调 covariance、R2G 或机械先验。
```powershell
python tools\audit_rtk_imu_heldout_innovation.py `
--manifest $MANIFEST `
--calibration-selection "$OUT\selection.json" `
--all-selection <all-nonoverlapping-selection.json> `
--engineering-result "$OUT\mechanical_47_window.json" `
--output "$OUT\heldout_innovation.json" `
--rotation-rpy-deg 0.454 -0.003 0.012
python tools\audit_rtk_imu_propagation_bias_root_cause.py `
--manifest $MANIFEST `
--calibration-selection "$OUT\selection.json" `
--all-selection <all-nonoverlapping-selection.json> `
--engineering-result "$OUT\mechanical_47_window.json" `
--output "$OUT\propagation_bias_root_cause.json" `
--rotation-rpy-deg 0.454 -0.003 0.012
```
最终仅汇总已生成的结果，不在 release 阶段重新拟合：
```powershell
python tools\finalize_rtk_imu_engineering_release.py `
--calibration "$OUT\mechanical_47_window.json" `
--heldout-postfit "$OUT\heldout.json" `
--innovation "$OUT\heldout_innovation.json" `
--sensitivity <rotation_sensitivity.json> `
--convergence-retry <heldout_retry.json> `
--propagation-root-cause "$OUT\propagation_bias_root_cause.json" `
--output "$OUT\engineering_release_decision.json"
```
## 相关文件
- [RTK-IMU 代码包](rtk_imu/README.md)
- [多源导出与旋转 V3 说明](docs/rtk_imu_multisource_v3.md)
- [engineering 6DoF 说明](docs/rtk_imu_engineering_6dof.md)
- [历史链路审计](docs/rtk_imu_calibration.md)
- [正式结果索引](artifacts/rtk_imu_calibration_v3/README.md)
artifacts/rtk_imu_calibration_v1/README.md
+17
View File
@@ -0,0 +1,17 @@
# RTK–IMU 标定产物说明
- `all_sessions/`：8 会话、5 s 平移节点、带 conditional rotation LOO 和 translation LOO 的当前完整基线。
- `rotation_hpr_time/`：改用 GNHPR 自带测量时刻后的全量 rotation-only 对照。
- `rotation_smoke/`：较早的 GGA 最近邻姿态时刻对照，不作为当前结果。
- `batch_0808_full_smoke/`：0808 三会话完整诊断。
- `batch_0815_rotation/`：0815 四会话 rotation-only 诊断。
- `single_smoke/`：早期单会话性能/数值冒烟，不作为当前结果。
每个正式运行目录包含：
- `dataset_audit.json`：样本数、固定解比例、共同时间范围和 ENU 原点。
- `rotation_result.json`：旋转、RPY、时间审计、GNHPR 候选、偏置、残差、协方差、逐会话指标和 LOO。
- `translation_result.json`：杆臂、平移、齐次矩阵、协方差/秩、位置/速度残差、偏置和 LOO。
- `summary.json`：供程序读取的最终状态和候选矩阵。
当前 `all_sessions/summary.json` 是 `diagnostic_not_accepted`。其中平移约 `[0.771, 0.569, -21.073] m` 明显不具机械真实性，禁止用于车辆配置。完整解释见 `docs/rtk_imu_calibration.md`。
artifacts/rtk_imu_calibration_v1/all_sessions/dataset_audit.json
+157
View File
@@ -0,0 +1,157 @@
{
"session_count": 8,
"sessions": [
{
"session_id": "priority_174005_174515",
"batch_id": "0808",
"imu_samples": 31000,
"rtk_samples": 4780,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.854602510460251,
"common_time_span_s": [
16265.2325992,
16575.0298107
],
"origin_geodetic": [
30.465514786,
114.092169888,
29.4695
],
"imu_source": "31000 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
},
{
"session_id": "priority_174905_175450",
"batch_id": "0808",
"imu_samples": 34499,
"rtk_samples": 5211,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8547303780464403,
"common_time_span_s": [
16805.1862481,
17150.0907328
],
"origin_geodetic": [
30.4653424457,
114.092237385,
29.5366
],
"imu_source": "34499 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
},
{
"session_id": "priority_175910_180530",
"batch_id": "0808",
"imu_samples": 37998,
"rtk_samples": 5786,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8567231247839613,
"common_time_span_s": [
17410.2121738,
17790.0458228
],
"origin_geodetic": [
30.4654151935,
114.090796384,
29.5317
],
"imu_source": "37998 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
},
{
"session_id": "slope_190548_190730",
"batch_id": "0815",
"imu_samples": 10199,
"rtk_samples": 1578,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8757921419518377,
"common_time_span_s": [
36466.328875,
36568.2301441
],
"origin_geodetic": [
30.4652183602,
114.090839984,
29.9891
],
"imu_source": "10199 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
},
{
"session_id": "circle_193412_193642",
"batch_id": "0815",
"imu_samples": 14989,
"rtk_samples": 2162,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8288621646623496,
"common_time_span_s": [
38170.3385255,
38317.8571971
],
"origin_geodetic": [
30.4654799242,
114.092155912,
29.4779
],
"imu_source": "14989 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
},
{
"session_id": "loop_194223_195003",
"batch_id": "0815",
"imu_samples": 45801,
"rtk_samples": 6741,
"fixed_position_ratio": 0.9998516540572615,
"fixed_attitude_ratio": 0.8377095386441181,
"common_time_span_s": [
38661.2831251,
39121.1709553
],
"origin_geodetic": [
30.4654856957,
114.092151015,
29.4651
],
"imu_source": "45801 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
},
{
"session_id": "accel_195608_195958",
"batch_id": "0815",
"imu_samples": 23002,
"rtk_samples": 3456,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8457754629629629,
"common_time_span_s": [
39486.3574509,
39716.3199405
],
"origin_geodetic": [
30.465443014,
114.092179168,
29.5309
],
"imu_source": "23002 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
},
{
"session_id": "motion_sms_154023_154359",
"batch_id": "0819",
"imu_samples": 21556,
"rtk_samples": 3294,
"fixed_position_ratio": 0.49271402550091076,
"fixed_attitude_ratio": 0.4344262295081967,
"common_time_span_s": [
25829.8725525,
26027.4217529
],
"origin_geodetic": [
30.4651580863,
114.090773052,
36.5626
],
"imu_source": "21556 normalized samples",
"rtk_source": "D:\\data\\0819\\dense5\\sessions_v2_device_affine\\motion_sms_154023_154359\\rtk.csv"
}
]
}
artifacts/rtk_imu_calibration_v1/all_sessions/rotation_result.json
+130
View File
@@ -0,0 +1,130 @@
{
"R_RTK_IMU": [
[
0.9996841792313951,
0.02499811511405725,
-0.002576050309343804
],
[
-0.025002617750669198,
0.9996858874629868,
-0.0017307550243271697
],
[
0.002531975526313299,
0.0017946164171362589,
0.9999951842143289
]
],
"rpy_deg": [
0.1028243313393031,
-0.1450716664951083,
-1.43269836393699
],
"gyro_bias_by_session_rad_s": {
"priority_174005_174515": [
-8.90944066727157e-05,
7.078609864979291e-05,
0.0001460390779870233
],
"priority_174905_175450": [
-4.327374730750894e-05,
-0.0001097548390022348,
3.000549725593387e-05
],
"priority_175910_180530": [
-7.036551812132934e-05,
-0.0008054961048184173,
1.3026193503057623e-05
],
"slope_190548_190730": [
6.035222904568755e-05,
-8.913884157613711e-06,
0.00015470949872434692
],
"circle_193412_193642": [
-3.696799268832588e-05,
-0.00019200076224600124,
-5.9567987995056394e-05
],
"loop_194223_195003": [
-0.0001979655352189002,
-0.00015937450336643818,
-4.343089303706036e-05
],
"accel_195608_195958": [
-7.841839817698704e-05,
-2.604932294598783e-06,
5.8586555655011475e-05
],
"motion_sms_154023_154359": [
0.00011745666467620586,
0.00021434926285955486,
-4.5147354343760625e-05
]
},
"time_offset": {
"offset_s": 0.04000000000000031,
"peak_correlation": 0.5788788671828708,
"second_best_correlation": 0.5773967219219845,
"evaluated_samples": 12394,
"reliable": false
},
"applied_time_offset_s": 0.0,
"convention": {
"name": "north_cw__pitch_nose_up__roll_right_down",
"heading_sign": -1.0,
"pitch_sign": -1.0,
"roll_sign": 1.0
},
"convention_scores_deg": {
"north_cw__pitch_nose_up__roll_right_down": 1.631322241364994,
"north_cw__pitch_opposite": 1.737582794831228,
"heading_opposite__pitch_nose_up": 7.263259116648845,
"heading_opposite__pitch_opposite": 17.716319437929055
},
"pair_count": 731,
"residual_rms_deg": 1.6276839301413086,
"residual_median_deg": 0.5695938849052221,
"residual_p95_deg": 3.0903953005641345,
"rotation_std_deg": [
0.2340126126406699,
0.20048574131667432,
2.314692068045375
],
"information_singular_values": [
82178.77940373585,
60441.86300479194,
612.6358640387364
],
"per_session_rms_deg": {
"priority_174005_174515": 0.6134232617562615,
"priority_174905_175450": 2.2485932625979106,
"priority_175910_180530": 3.095861798757508,
"slope_190548_190730": 2.388478834012087,
"circle_193412_193642": 0.5629885032374518,
"loop_194223_195003": 0.6250976606325254,
"accel_195608_195958": 0.975856888271022,
"motion_sms_154023_154359": 1.3698317592842066
},
"loo_delta_deg": {
"priority_174005_174515": 0.7895996403426686,
"priority_174905_175450": 0.9224102138653388,
"priority_175910_180530": 5.144190932547379,
"slope_190548_190730": 0.24769087330915346,
"circle_193412_193642": 1.2637729378484346,
"loop_194223_195003": 0.4844375238260164,
"accel_195608_195958": 1.0976412402728102,
"motion_sms_154023_154359": 0.6896079810115167
},
"ok": false,
"notes": [
"transform convention: p_RTK = R_RTK_IMU p_IMU",
"residual time convention: t_IMU = t_RTK + +0.000000 s",
"GNHPR convention score gap=0.1063 deg",
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
"LOO is conditional: per-session gyro biases are held at their all-session estimates",
"time-offset correlation was ambiguous; held residual offset at zero",
"rotation failed one or more strict acceptance gates"
]
}
artifacts/rtk_imu_calibration_v1/all_sessions/summary.json
+65
View File
@@ -0,0 +1,65 @@
{
"status": "diagnostic_not_accepted",
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
"rtk_frame_definition": "",
"rtk_reference_point": "",
"interpretation_blockers": [
"rotation quality gates failed",
"translation quality gates failed or were not run",
"RTK frame_definition is empty",
"RTK reference_point is empty"
],
"R_RTK_IMU": [
[
0.9996841792313951,
0.02499811511405725,
-0.002576050309343804
],
[
-0.025002617750669198,
0.9996858874629868,
-0.0017307550243271697
],
[
0.002531975526313299,
0.0017946164171362589,
0.9999951842143289
]
],
"t_RTK_IMU_m": [
0.7710937276592442,
0.5693018309069646,
-21.07303743539233
],
"T_RTK_IMU": [
[
0.9996841792313951,
0.02499811511405725,
-0.002576050309343804,
0.7710937276592442
],
[
-0.025002617750669198,
0.9996858874629868,
-0.0017307550243271697,
0.5693018309069646
],
[
0.002531975526313299,
0.0017946164171362589,
0.9999951842143289,
-21.07303743539233
],
[
0.0,
0.0,
0.0,
1.0
]
],
"rotation_ok": false,
"translation_ok": false,
"rotation_result": "rotation_result.json",
"translation_result": "translation_result.json",
"dataset_audit": "dataset_audit.json"
}
artifacts/rtk_imu_calibration_v1/all_sessions/translation_result.json
+161
View File
@@ -0,0 +1,161 @@
{
"lever_IMU_to_RTK_in_IMU_m": [
-0.7032597491310882,
-0.5505808768918061,
21.07590765040047
],
"t_RTK_IMU_m": [
0.7710937276592442,
0.5693018309069646,
-21.07303743539233
],
"T_RTK_IMU": [
[
0.9996841792313951,
0.02499811511405725,
-0.002576050309343804,
0.7710937276592442
],
[
-0.025002617750669198,
0.9996858874629868,
-0.0017307550243271697,
0.5693018309069646
],
[
0.002531975526313299,
0.0017946164171362589,
0.9999951842143289,
-21.07303743539233
],
[
0.0,
0.0,
0.0,
1.0
]
],
"translation_std_m": [
0.367657527272358,
0.36764001580136974,
2.960858932902556
],
"lever_information_singular_values": [
7.467287688797444,
7.419771560016465,
0.11404687553142893
],
"lever_precision_rank": 3,
"position_residual_rms_xyz_m": [
0.0026057774092802665,
0.0026172203224947795,
0.0008707288139887198
],
"velocity_residual_rms_xyz_m_s": [
1.0344084464154262,
1.0018456371907998,
0.08189262443294992
],
"accel_bias_by_session_m_s2": {
"priority_174005_174515": [
-0.0033446448235936025,
0.04850567116751809,
0.014864908749227664
],
"priority_174905_175450": [
0.051484854191344014,
0.009750775340658668,
0.016837766756117954
],
"priority_175910_180530": [
-0.0012098409852818557,
0.04428237322299069,
0.015044444493724668
],
"slope_190548_190730": [
0.007720083607057322,
-0.07045791547639069,
0.011096872492579776
],
"circle_193412_193642": [
0.010695150095323893,
0.07124837152622766,
0.012181970165526993
],
"loop_194223_195003": [
0.017798177348527063,
0.026531175431114495,
0.013517325106459492
],
"accel_195608_195958": [
0.010843370014374661,
0.0021847264023688797,
0.012720554759472001
],
"motion_sms_154023_154359": [
-0.01917068926914634,
0.04878246850510192,
0.011986246228615108
]
},
"knot_count_by_session": {
"priority_174005_174515": 62,
"priority_174905_175450": 69,
"priority_175910_180530": 76,
"slope_190548_190730": 21,
"circle_193412_193642": 30,
"loop_194223_195003": 92,
"accel_195608_195958": 46,
"motion_sms_154023_154359": 20
},
"loo_delta_m": {
"priority_174005_174515": [
-0.08631101169626099,
-0.16878866579372004,
-0.1969063529933237
],
"priority_174905_175450": [
0.052982832020427084,
0.23694572858391594,
0.5921699991558107
],
"priority_175910_180530": [
-0.03728037456994515,
-0.0587695607071852,
0.36745157813128415
],
"slope_190548_190730": [
0.06820599394639204,
0.13646009238200618,
0.8245922313333871
],
"circle_193412_193642": [
-0.06745036261863124,
-0.18645839932341524,
-0.8300513550738842
],
"loop_194223_195003": [
0.12045919921905746,
0.06572884705689208,
-1.774476169745249
],
"accel_195608_195958": [
0.019243586803303625,
-0.002985337002138544,
0.3653882998138158
],
"motion_sms_154023_154359": [
-0.0383153365081248,
0.027686850248512473,
0.507160468738487
]
},
"ok": false,
"notes": [
"lever l is vector IMU-origin -> RTK-origin expressed in IMU",
"transform translation uses t_RTK_IMU = -R_RTK_IMU @ l",
"RTK position is never differentiated; position and velocity preintegration factors are solved jointly",
"upstream rotation is not accepted, so translation is diagnostic only",
"translation failed one or more strict acceptance gates"
]
}
artifacts/rtk_imu_calibration_v1/batch_0808_full_smoke/dataset_audit.json
+62
View File
@@ -0,0 +1,62 @@
{
"session_count": 3,
"sessions": [
{
"session_id": "priority_174005_174515",
"batch_id": "0808",
"imu_samples": 31000,
"rtk_samples": 4780,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.854602510460251,
"common_time_span_s": [
16265.2325992,
16575.0298107
],
"origin_geodetic": [
30.465514786,
114.092169888,
29.4695
],
"imu_source": "31000 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
},
{
"session_id": "priority_174905_175450",
"batch_id": "0808",
"imu_samples": 34499,
"rtk_samples": 5211,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8547303780464403,
"common_time_span_s": [
16805.1862481,
17150.0907328
],
"origin_geodetic": [
30.4653424457,
114.092237385,
29.5366
],
"imu_source": "34499 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
},
{
"session_id": "priority_175910_180530",
"batch_id": "0808",
"imu_samples": 37998,
"rtk_samples": 5786,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8567231247839613,
"common_time_span_s": [
17410.2121738,
17790.0458228
],
"origin_geodetic": [
30.4654151935,
114.090796384,
29.5317
],
"imu_source": "37998 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
}
]
}
artifacts/rtk_imu_calibration_v1/batch_0808_full_smoke/rotation_result.json
+89
View File
@@ -0,0 +1,89 @@
{
"R_RTK_IMU": [
[
0.9996274297857513,
0.0272885099045028,
-0.0005821057674967719
],
[
-0.027285914629035343,
0.9996193516550798,
0.0040780705652228135
],
[
0.0006931686589101494,
-0.004060667909321538,
0.9999915152106744
]
],
"rpy_deg": [
-0.23265982849493025,
-0.03971564182674239,
-1.5635621825359411
],
"gyro_bias_by_session_rad_s": {
"priority_174005_174515": [
-9.355487703345326e-05,
6.022278219673199e-05,
0.0001599879605718389
],
"priority_174905_175450": [
-6.763782096065313e-05,
-0.00013271246982721386,
2.7304435743171578e-05
],
"priority_175910_180530": [
-4.9469524877665416e-05,
-0.0005605818923478396,
4.72341323992995e-06
]
},
"time_offset": {
"offset_s": 0.07500000000000034,
"peak_correlation": 0.2075894836191556,
"second_best_correlation": 0.2075830757517665,
"evaluated_samples": 6286,
"reliable": false
},
"convention": {
"name": "north_cw__pitch_nose_up__roll_right_down",
"heading_sign": -1.0,
"pitch_sign": -1.0,
"roll_sign": 1.0
},
"convention_scores_deg": {
"north_cw__pitch_nose_up__roll_right_down": 2.124836124403544,
"north_cw__pitch_opposite": 2.246244073697405,
"heading_opposite__pitch_nose_up": 14.081722937345578,
"heading_opposite__pitch_opposite": 14.187723789068325
},
"pair_count": 347,
"residual_rms_deg": 2.121956852743709,
"residual_median_deg": 0.6620410270629032,
"residual_p95_deg": 4.932454679529714,
"rotation_std_deg": [
0.5378381823216046,
0.43991569563643235,
3.604673882923754
],
"information_singular_values": [
17058.156967522238,
11392.471369813427,
252.6038959367189
],
"per_session_rms_deg": {
"priority_174005_174515": 0.6074812737155098,
"priority_174905_175450": 2.2498284381524494,
"priority_175910_180530": 3.09642157433063
},
"loo_delta_deg": {},
"ok": false,
"notes": [
"transform convention: p_RTK = R_RTK_IMU p_IMU",
"residual time convention: t_IMU = t_RTK + +0.000000 s",
"GNHPR convention score gap=0.1214 deg",
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
"time-offset correlation was ambiguous; held residual offset at zero",
"rotation failed one or more strict acceptance gates"
]
}
artifacts/rtk_imu_calibration_v1/batch_0808_full_smoke/summary.json
+57
View File
@@ -0,0 +1,57 @@
{
"status": "diagnostic_not_accepted",
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
"R_RTK_IMU": [
[
0.9996274297857513,
0.0272885099045028,
-0.0005821057674967719
],
[
-0.027285914629035343,
0.9996193516550798,
0.0040780705652228135
],
[
0.0006931686589101494,
-0.004060667909321538,
0.9999915152106744
]
],
"t_RTK_IMU_m": [
1.017036626166409,
0.7992624136748191,
-22.250154010194304
],
"T_RTK_IMU": [
[
0.9996274297857513,
0.0272885099045028,
-0.0005821057674967719,
1.017036626166409
],
[
-0.027285914629035343,
0.9996193516550798,
0.0040780705652228135,
0.7992624136748191
],
[
0.0006931686589101494,
-0.004060667909321538,
0.9999915152106744,
-22.250154010194304
],
[
0.0,
0.0,
0.0,
1.0
]
],
"rotation_ok": false,
"translation_ok": false,
"rotation_result": "rotation_result.json",
"translation_result": "translation_result.json",
"dataset_audit": "dataset_audit.json"
}
artifacts/rtk_imu_calibration_v1/batch_0808_full_smoke/translation_result.json
+90
View File
@@ -0,0 +1,90 @@
{
"lever_IMU_to_RTK_in_IMU_m": [
-0.979425993211181,
-0.9170620761729408,
22.247297796687818
],
"t_RTK_IMU_m": [
1.017036626166409,
0.7992624136748191,
-22.250154010194304
],
"T_RTK_IMU": [
[
0.9996274297857513,
0.0272885099045028,
-0.0005821057674967719,
1.017036626166409
],
[
-0.027285914629035343,
0.9996193516550798,
0.0040780705652228135,
0.7992624136748191
],
[
0.0006931686589101494,
-0.004060667909321538,
0.9999915152106744,
-22.250154010194304
],
[
0.0,
0.0,
0.0,
1.0
]
],
"translation_std_m": [
0.6410402171684149,
0.6447113787519164,
3.8963000265795396
],
"lever_information_singular_values": [
2.4406600443109308,
2.412462748498746,
0.06586096829714262
],
"lever_precision_rank": 3,
"position_residual_rms_xyz_m": [
0.0023101392989052683,
0.002794889168410421,
0.0005246719035281052
],
"velocity_residual_rms_xyz_m_s": [
1.1059333756244145,
1.368704336610928,
0.11603076744633863
],
"accel_bias_by_session_m_s2": {
"priority_174005_174515": [
0.015165392523580496,
0.10623623265620695,
0.015221547657584966
],
"priority_174905_175450": [
0.06725309871307823,
0.06785033078439083,
0.0171641139816057
],
"priority_175910_180530": [
0.015302724485125336,
0.10160587655627046,
0.015281702682303734
]
},
"knot_count_by_session": {
"priority_174005_174515": 62,
"priority_174905_175450": 69,
"priority_175910_180530": 76
},
"loo_delta_m": {},
"ok": false,
"notes": [
"lever l is vector IMU-origin -> RTK-origin expressed in IMU",
"transform translation uses t_RTK_IMU = -R_RTK_IMU @ l",
"RTK position is never differentiated; position and velocity preintegration factors are solved jointly",
"upstream rotation is not accepted, so translation is diagnostic only",
"translation failed one or more strict acceptance gates"
]
}
artifacts/rtk_imu_calibration_v1/batch_0815_rotation/dataset_audit.json
+81
View File
@@ -0,0 +1,81 @@
{
"session_count": 4,
"sessions": [
{
"session_id": "slope_190548_190730",
"batch_id": "0815",
"imu_samples": 10199,
"rtk_samples": 1578,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8757921419518377,
"common_time_span_s": [
36466.328875,
36568.2301441
],
"origin_geodetic": [
30.4652183602,
114.090839984,
29.9891
],
"imu_source": "10199 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
},
{
"session_id": "circle_193412_193642",
"batch_id": "0815",
"imu_samples": 14989,
"rtk_samples": 2162,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8288621646623496,
"common_time_span_s": [
38170.3385255,
38317.8571971
],
"origin_geodetic": [
30.4654799242,
114.092155912,
29.4779
],
"imu_source": "14989 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
},
{
"session_id": "loop_194223_195003",
"batch_id": "0815",
"imu_samples": 45801,
"rtk_samples": 6741,
"fixed_position_ratio": 0.9998516540572615,
"fixed_attitude_ratio": 0.8377095386441181,
"common_time_span_s": [
38661.2831251,
39121.1709553
],
"origin_geodetic": [
30.4654856957,
114.092151015,
29.4651
],
"imu_source": "45801 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
},
{
"session_id": "accel_195608_195958",
"batch_id": "0815",
"imu_samples": 23002,
"rtk_samples": 3456,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8457754629629629,
"common_time_span_s": [
39486.3574509,
39716.3199405
],
"origin_geodetic": [
30.465443014,
114.092179168,
29.5309
],
"imu_source": "23002 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
}
]
}
artifacts/rtk_imu_calibration_v1/batch_0815_rotation/rotation_result.json
+96
View File
@@ -0,0 +1,96 @@
{
"R_RTK_IMU": [
[
0.9999453769836999,
0.010155336209751328,
-0.002472285459453144
],
[
-0.010163396322394882,
0.9999430054296535,
-0.003269750373547743
],
[
0.002438939138240274,
0.003294698586865735,
0.9999915982332558
]
],
"rpy_deg": [
0.18877322677308359,
-0.13974105765052158,
-0.5823314723802753
],
"gyro_bias_by_session_rad_s": {
"slope_190548_190730": [
5.517563410757097e-05,
-1.0668622945796592e-05,
0.00015474647141927933
],
"circle_193412_193642": [
-4.092656265416791e-05,
-0.00017676217236757262,
-5.914138553067106e-05
],
"loop_194223_195003": [
-0.000196149370561509,
-0.0001486662046316796,
-3.6375768145656054e-05
],
"accel_195608_195958": [
-7.483644204529091e-05,
-4.17320520562614e-06,
5.861303135056825e-05
]
},
"time_offset": {
"offset_s": 0.04000000000000031,
"peak_correlation": 0.7578237026149359,
"second_best_correlation": 0.757682618781891,
"evaluated_samples": 5443,
"reliable": false
},
"convention": {
"name": "north_cw__pitch_nose_up__roll_right_down",
"heading_sign": -1.0,
"pitch_sign": -1.0,
"roll_sign": 1.0
},
"convention_scores_deg": {
"north_cw__pitch_nose_up__roll_right_down": 0.9457675569035681,
"north_cw__pitch_opposite": 1.064327146928918,
"heading_opposite__pitch_nose_up": 5.753424726280977,
"heading_opposite__pitch_opposite": 13.362267974377751
},
"pair_count": 351,
"residual_rms_deg": 0.9428350359826574,
"residual_median_deg": 0.4993317818354644,
"residual_p95_deg": 1.5023594730593923,
"rotation_std_deg": [
0.191917244945292,
0.16245924204992213,
2.5710977852526464
],
"information_singular_values": [
125441.66767899474,
90572.5161990362,
496.5407001788204
],
"per_session_rms_deg": {
"slope_190548_190730": 2.386329048920402,
"circle_193412_193642": 0.560395991888806,
"loop_194223_195003": 0.6239401903763562,
"accel_195608_195958": 0.9797575195873669
},
"loo_delta_deg": {},
"ok": false,
"notes": [
"transform convention: p_RTK = R_RTK_IMU p_IMU",
"residual time convention: t_IMU = t_RTK + +0.000000 s",
"GNHPR convention score gap=0.1186 deg",
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
"LOO is conditional: per-session gyro biases are held at their all-session estimates",
"time-offset correlation was ambiguous; held residual offset at zero",
"rotation failed one or more strict acceptance gates"
]
}
artifacts/rtk_imu_calibration_v1/batch_0815_rotation/summary.json
+28
View File
@@ -0,0 +1,28 @@
{
"status": "diagnostic_not_accepted",
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
"R_RTK_IMU": [
[
0.9999453769836999,
0.010155336209751328,
-0.002472285459453144
],
[
-0.010163396322394882,
0.9999430054296535,
-0.003269750373547743
],
[
0.002438939138240274,
0.003294698586865735,
0.9999915982332558
]
],
"t_RTK_IMU_m": null,
"T_RTK_IMU": null,
"rotation_ok": false,
"translation_ok": null,
"rotation_result": "rotation_result.json",
"translation_result": null,
"dataset_audit": "dataset_audit.json"
}
artifacts/rtk_imu_calibration_v1/rotation_hpr_time/dataset_audit.json
+157
View File
@@ -0,0 +1,157 @@
{
"session_count": 8,
"sessions": [
{
"session_id": "priority_174005_174515",
"batch_id": "0808",
"imu_samples": 31000,
"rtk_samples": 4780,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.854602510460251,
"common_time_span_s": [
16265.2325992,
16575.0298107
],
"origin_geodetic": [
30.465514786,
114.092169888,
29.4695
],
"imu_source": "31000 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
},
{
"session_id": "priority_174905_175450",
"batch_id": "0808",
"imu_samples": 34499,
"rtk_samples": 5211,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8547303780464403,
"common_time_span_s": [
16805.1862481,
17150.0907328
],
"origin_geodetic": [
30.4653424457,
114.092237385,
29.5366
],
"imu_source": "34499 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
},
{
"session_id": "priority_175910_180530",
"batch_id": "0808",
"imu_samples": 37998,
"rtk_samples": 5786,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8567231247839613,
"common_time_span_s": [
17410.2121738,
17790.0458228
],
"origin_geodetic": [
30.4654151935,
114.090796384,
29.5317
],
"imu_source": "37998 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
},
{
"session_id": "slope_190548_190730",
"batch_id": "0815",
"imu_samples": 10199,
"rtk_samples": 1578,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8757921419518377,
"common_time_span_s": [
36466.328875,
36568.2301441
],
"origin_geodetic": [
30.4652183602,
114.090839984,
29.9891
],
"imu_source": "10199 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
},
{
"session_id": "circle_193412_193642",
"batch_id": "0815",
"imu_samples": 14989,
"rtk_samples": 2162,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8288621646623496,
"common_time_span_s": [
38170.3385255,
38317.8571971
],
"origin_geodetic": [
30.4654799242,
114.092155912,
29.4779
],
"imu_source": "14989 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
},
{
"session_id": "loop_194223_195003",
"batch_id": "0815",
"imu_samples": 45801,
"rtk_samples": 6741,
"fixed_position_ratio": 0.9998516540572615,
"fixed_attitude_ratio": 0.8377095386441181,
"common_time_span_s": [
38661.2831251,
39121.1709553
],
"origin_geodetic": [
30.4654856957,
114.092151015,
29.4651
],
"imu_source": "45801 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
},
{
"session_id": "accel_195608_195958",
"batch_id": "0815",
"imu_samples": 23002,
"rtk_samples": 3456,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8457754629629629,
"common_time_span_s": [
39486.3574509,
39716.3199405
],
"origin_geodetic": [
30.465443014,
114.092179168,
29.5309
],
"imu_source": "23002 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
},
{
"session_id": "motion_sms_154023_154359",
"batch_id": "0819",
"imu_samples": 21556,
"rtk_samples": 3294,
"fixed_position_ratio": 0.49271402550091076,
"fixed_attitude_ratio": 0.4344262295081967,
"common_time_span_s": [
25829.8725525,
26027.4217529
],
"origin_geodetic": [
30.4651580863,
114.090773052,
36.5626
],
"imu_source": "21556 normalized samples",
"rtk_source": "D:\\data\\0819\\dense5\\sessions_v2_device_affine\\motion_sms_154023_154359\\rtk.csv"
}
]
}
artifacts/rtk_imu_calibration_v1/rotation_hpr_time/rotation_result.json
+119
View File
@@ -0,0 +1,119 @@
{
"R_RTK_IMU": [
[
0.9996841792313951,
0.02499811511405725,
-0.002576050309343804
],
[
-0.025002617750669198,
0.9996858874629868,
-0.0017307550243271697
],
[
0.002531975526313299,
0.0017946164171362589,
0.9999951842143289
]
],
"rpy_deg": [
0.1028243313393031,
-0.1450716664951083,
-1.43269836393699
],
"gyro_bias_by_session_rad_s": {
"priority_174005_174515": [
-8.90944066727157e-05,
7.078609864979291e-05,
0.0001460390779870233
],
"priority_174905_175450": [
-4.327374730750894e-05,
-0.0001097548390022348,
3.000549725593387e-05
],
"priority_175910_180530": [
-7.036551812132934e-05,
-0.0008054961048184173,
1.3026193503057623e-05
],
"slope_190548_190730": [
6.035222904568755e-05,
-8.913884157613711e-06,
0.00015470949872434692
],
"circle_193412_193642": [
-3.696799268832588e-05,
-0.00019200076224600124,
-5.9567987995056394e-05
],
"loop_194223_195003": [
-0.0001979655352189002,
-0.00015937450336643818,
-4.343089303706036e-05
],
"accel_195608_195958": [
-7.841839817698704e-05,
-2.604932294598783e-06,
5.8586555655011475e-05
],
"motion_sms_154023_154359": [
0.00011745666467620586,
0.00021434926285955486,
-4.5147354343760625e-05
]
},
"time_offset": {
"offset_s": 0.04000000000000031,
"peak_correlation": 0.5788788671828708,
"second_best_correlation": 0.5773967219219845,
"evaluated_samples": 12394,
"reliable": false
},
"convention": {
"name": "north_cw__pitch_nose_up__roll_right_down",
"heading_sign": -1.0,
"pitch_sign": -1.0,
"roll_sign": 1.0
},
"convention_scores_deg": {
"north_cw__pitch_nose_up__roll_right_down": 1.631322241364994,
"north_cw__pitch_opposite": 1.737582794831228,
"heading_opposite__pitch_nose_up": 7.263259116648845,
"heading_opposite__pitch_opposite": 17.716319437929055
},
"pair_count": 731,
"residual_rms_deg": 1.6276839301413086,
"residual_median_deg": 0.5695938849052221,
"residual_p95_deg": 3.0903953005641345,
"rotation_std_deg": [
0.2340126126406699,
0.20048574131667432,
2.314692068045375
],
"information_singular_values": [
82178.77940373585,
60441.86300479194,
612.6358640387364
],
"per_session_rms_deg": {
"priority_174005_174515": 0.6134232617562615,
"priority_174905_175450": 2.2485932625979106,
"priority_175910_180530": 3.095861798757508,
"slope_190548_190730": 2.388478834012087,
"circle_193412_193642": 0.5629885032374518,
"loop_194223_195003": 0.6250976606325254,
"accel_195608_195958": 0.975856888271022,
"motion_sms_154023_154359": 1.3698317592842066
},
"loo_delta_deg": {},
"ok": false,
"notes": [
"transform convention: p_RTK = R_RTK_IMU p_IMU",
"residual time convention: t_IMU = t_RTK + +0.000000 s",
"GNHPR convention score gap=0.1063 deg",
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
"time-offset correlation was ambiguous; held residual offset at zero",
"rotation failed one or more strict acceptance gates"
]
}
artifacts/rtk_imu_calibration_v1/rotation_hpr_time/summary.json
+28
View File
@@ -0,0 +1,28 @@
{
"status": "diagnostic_not_accepted",
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
"R_RTK_IMU": [
[
0.9996841792313951,
0.02499811511405725,
-0.002576050309343804
],
[
-0.025002617750669198,
0.9996858874629868,
-0.0017307550243271697
],
[
0.002531975526313299,
0.0017946164171362589,
0.9999951842143289
]
],
"t_RTK_IMU_m": null,
"T_RTK_IMU": null,
"rotation_ok": false,
"translation_ok": null,
"rotation_result": "rotation_result.json",
"translation_result": null,
"dataset_audit": "dataset_audit.json"
}
artifacts/rtk_imu_calibration_v1/rotation_smoke/dataset_audit.json
+157
View File
@@ -0,0 +1,157 @@
{
"session_count": 8,
"sessions": [
{
"session_id": "priority_174005_174515",
"batch_id": "0808",
"imu_samples": 31000,
"rtk_samples": 4780,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.854602510460251,
"common_time_span_s": [
16265.2325992,
16575.0298107
],
"origin_geodetic": [
30.465514786,
114.092169888,
29.4695
],
"imu_source": "31000 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
},
{
"session_id": "priority_174905_175450",
"batch_id": "0808",
"imu_samples": 34499,
"rtk_samples": 5211,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8547303780464403,
"common_time_span_s": [
16805.1862481,
17150.0907328
],
"origin_geodetic": [
30.4653424457,
114.092237385,
29.5366
],
"imu_source": "34499 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
},
{
"session_id": "priority_175910_180530",
"batch_id": "0808",
"imu_samples": 37998,
"rtk_samples": 5786,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8567231247839613,
"common_time_span_s": [
17410.2121738,
17790.0458228
],
"origin_geodetic": [
30.4654151935,
114.090796384,
29.5317
],
"imu_source": "37998 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
},
{
"session_id": "slope_190548_190730",
"batch_id": "0815",
"imu_samples": 10199,
"rtk_samples": 1578,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8757921419518377,
"common_time_span_s": [
36466.328875,
36568.2301441
],
"origin_geodetic": [
30.4652183602,
114.090839984,
29.9891
],
"imu_source": "10199 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
},
{
"session_id": "circle_193412_193642",
"batch_id": "0815",
"imu_samples": 14989,
"rtk_samples": 2162,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8288621646623496,
"common_time_span_s": [
38170.3385255,
38317.8571971
],
"origin_geodetic": [
30.4654799242,
114.092155912,
29.4779
],
"imu_source": "14989 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
},
{
"session_id": "loop_194223_195003",
"batch_id": "0815",
"imu_samples": 45801,
"rtk_samples": 6741,
"fixed_position_ratio": 0.9998516540572615,
"fixed_attitude_ratio": 0.8377095386441181,
"common_time_span_s": [
38661.2831251,
39121.1709553
],
"origin_geodetic": [
30.4654856957,
114.092151015,
29.4651
],
"imu_source": "45801 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
},
{
"session_id": "accel_195608_195958",
"batch_id": "0815",
"imu_samples": 23002,
"rtk_samples": 3456,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8457754629629629,
"common_time_span_s": [
39486.3574509,
39716.3199405
],
"origin_geodetic": [
30.465443014,
114.092179168,
29.5309
],
"imu_source": "23002 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
},
{
"session_id": "motion_sms_154023_154359",
"batch_id": "0819",
"imu_samples": 21556,
"rtk_samples": 3294,
"fixed_position_ratio": 0.49271402550091076,
"fixed_attitude_ratio": 0.4344262295081967,
"common_time_span_s": [
25829.8725525,
26027.4217529
],
"origin_geodetic": [
30.4651580863,
114.090773052,
36.5626
],
"imu_source": "21556 normalized samples",
"rtk_source": "D:\\data\\0819\\dense5\\sessions_v2_device_affine\\motion_sms_154023_154359\\rtk.csv"
}
]
}
artifacts/rtk_imu_calibration_v1/rotation_smoke/rotation_result.json
+119
View File
@@ -0,0 +1,119 @@
{
"R_RTK_IMU": [
[
-0.999754381938718,
0.021803011844914608,
0.003975483470215314
],
[
0.021793866945340735,
0.9997597719617833,
-0.0023293197487744975
],
[
-0.0040253146336934244,
-0.002242106467980424,
-0.9999893848440022
]
],
"rpy_deg": [
-179.8715356137635,
0.23063416255936714,
178.75119441523574
],
"gyro_bias_by_session_rad_s": {
"priority_174005_174515": [
-0.00018079953472427648,
-9.3706764772758e-05,
-1.6471539455612585e-05
],
"priority_174905_175450": [
-0.0001527801061740737,
-0.0001164065722445753,
-5.5002757975692905e-05
],
"priority_175910_180530": [
0.00010732176614734042,
-9.217694744503191e-05,
0.00037171055222583177
],
"slope_190548_190730": [
-0.0001400513048024026,
-9.490085614046164e-05,
-7.313488292136488e-05
],
"circle_193412_193642": [
-5.9488404294769446e-05,
-0.00029434501957892675,
0.000102661594942897
],
"loop_194223_195003": [
-0.0002980828626939094,
9.137138703447405e-05,
4.430930647242609e-05
],
"accel_195608_195958": [
5.9388956272647935e-05,
9.900621296014341e-06,
8.065768586636302e-05
],
"motion_sms_154023_154359": [
0.0016442968972331072,
0.00012387305081956675,
8.435528364463815e-08
]
},
"time_offset": {
"offset_s": 0.08500000000000035,
"peak_correlation": 0.46191352508231565,
"second_best_correlation": 0.460889710037625,
"evaluated_samples": 12384,
"reliable": false
},
"convention": {
"name": "heading_opposite__pitch_nose_up",
"heading_sign": 1.0,
"pitch_sign": -1.0,
"roll_sign": 1.0
},
"convention_scores_deg": {
"north_cw__pitch_nose_up__roll_right_down": 1.6671304616155285,
"north_cw__pitch_opposite": 1.7955511909491206,
"heading_opposite__pitch_nose_up": 1.6669796415371239,
"heading_opposite__pitch_opposite": 19.709790964275243
},
"pair_count": 6164,
"residual_rms_deg": 1.6669796415371239,
"residual_median_deg": 0.6097138083592458,
"residual_p95_deg": 2.9952512236242987,
"rotation_std_deg": [
1.0204560247927847,
0.06460681216547948,
0.11838973994567728
],
"information_singular_values": [
818353.8002481281,
235774.70975965378,
3151.7390576048515
],
"per_session_rms_deg": {
"priority_174005_174515": 0.7068100152136895,
"priority_174905_175450": 1.7590065687347056,
"priority_175910_180530": 3.0674316757114104,
"slope_190548_190730": 2.07580304513421,
"circle_193412_193642": 0.6574485751543728,
"loop_194223_195003": 0.6594363752510715,
"accel_195608_195958": 0.7631418065216562,
"motion_sms_154023_154359": 3.709386878989403
},
"loo_delta_deg": {},
"ok": false,
"notes": [
"transform convention: p_RTK = R_RTK_IMU p_IMU",
"residual time convention: t_IMU = t_RTK + +0.000000 s",
"GNHPR convention score gap=0.0002 deg",
"time-offset correlation was ambiguous; held residual offset at zero",
"empirical best GNHPR convention differs from protocol expectation; manual verification required",
"rotation failed one or more strict acceptance gates"
]
}
artifacts/rtk_imu_calibration_v1/rotation_smoke/summary.json
+28
View File
@@ -0,0 +1,28 @@
{
"status": "diagnostic_not_accepted",
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
"R_RTK_IMU": [
[
-0.999754381938718,
0.021803011844914608,
0.003975483470215314
],
[
0.021793866945340735,
0.9997597719617833,
-0.0023293197487744975
],
[
-0.0040253146336934244,
-0.002242106467980424,
-0.9999893848440022
]
],
"t_RTK_IMU_m": null,
"T_RTK_IMU": null,
"rotation_ok": false,
"translation_ok": null,
"rotation_result": "rotation_result.json",
"translation_result": null,
"dataset_audit": "dataset_audit.json"
}
artifacts/rtk_imu_calibration_v1/single_smoke/dataset_audit.json
+24
View File
@@ -0,0 +1,24 @@
{
"session_count": 1,
"sessions": [
{
"session_id": "priority_175910_180530",
"batch_id": "0808",
"imu_samples": 37998,
"rtk_samples": 5786,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8567231247839613,
"common_time_span_s": [
17410.2121738,
17790.0458228
],
"origin_geodetic": [
30.4654151935,
114.090796384,
29.5317
],
"imu_source": "37998 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
}
]
}
artifacts/rtk_imu_calibration_v1/single_smoke/rotation_result.json
+76
View File
@@ -0,0 +1,76 @@
{
"R_RTK_IMU": [
[
0.9974493913373024,
-0.07135460605626225,
0.0017977528752911507
],
[
0.0713431523484532,
0.997435050761847,
0.005785682733905998
],
[
-0.0022059768426676615,
-0.00564266836413856,
0.9999816468115312
]
],
"rpy_deg": [
-0.32330358477785043,
0.1263932653005637,
4.0911470587568495
],
"gyro_bias_by_session_rad_s": {
"priority_175910_180530": [
-0.00010509442179307857,
-0.0001426783269500731,
3.086292346981605e-05
]
},
"time_offset": {
"offset_s": -0.0949999999999998,
"peak_correlation": 0.1329249352233157,
"second_best_correlation": 0.13247995527618098,
"evaluated_samples": 2316,
"reliable": false
},
"convention": {
"name": "north_cw__pitch_nose_up__roll_right_down",
"heading_sign": -1.0,
"pitch_sign": -1.0,
"roll_sign": 1.0
},
"convention_scores_deg": {
"north_cw__pitch_nose_up__roll_right_down": 2.8406184024910064,
"north_cw__pitch_opposite": 2.8942770966870084,
"heading_opposite__pitch_nose_up": 3.8836333647028383,
"heading_opposite__pitch_opposite": 8.244842702849073
},
"pair_count": 91,
"residual_rms_deg": 2.8406184024910064,
"residual_median_deg": 1.624215282655001,
"residual_p95_deg": 6.087913500787023,
"rotation_std_deg": [
1.6029337786454214,
1.0761604678920118,
6.305203755276785
],
"information_singular_values": [
2941.184567996004,
1268.1548949190246,
82.52753961323997
],
"per_session_rms_deg": {
"priority_175910_180530": 2.8406184024910064
},
"loo_delta_deg": {},
"ok": false,
"notes": [
"transform convention: p_RTK = R_RTK_IMU p_IMU",
"residual time convention: t_IMU = t_RTK + +0.000000 s",
"GNHPR convention score gap=0.0537 deg",
"time-offset correlation was ambiguous; held residual offset at zero",
"rotation failed one or more strict acceptance gates"
]
}
artifacts/rtk_imu_calibration_v1/single_smoke/summary.json
+28
View File
@@ -0,0 +1,28 @@
{
"status": "diagnostic_not_accepted",
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
"R_RTK_IMU": [
[
0.9974493913373024,
-0.07135460605626225,
0.0017977528752911507
],
[
0.0713431523484532,
0.997435050761847,
0.005785682733905998
],
[
-0.0022059768426676615,
-0.00564266836413856,
0.9999816468115312
]
],
"t_RTK_IMU_m": null,
"T_RTK_IMU": null,
"rotation_ok": false,
"translation_ok": null,
"rotation_result": "rotation_result.json",
"translation_result": null,
"dataset_audit": "dataset_audit.json"
}
artifacts/rtk_imu_calibration_v3/README.md
+24
View File
@@ -0,0 +1,24 @@
# RTK-IMU V3 正式结果集
本目录只提交复现工程结论所需的审核结果。运行过程中的调试输出、状态快照、逐窗口 checkpoint 和旧版结果保留在本机，由仓库根目录 `.gitignore` 排除。
## 当前结论
- 固定旋转来源：`R2G_gravity_level_prior`。
- 工程候选杆臂：`l_I = p_ANT1^I = [-0.4518015159, -0.2644749820, 0.7314656115] m`。
- `data_only_translation_accepted=false`。
- `engineering_translation_accepted=false`，原因是独立传播验证仍受公共加速度偏差影响。
- `independent_extrinsic_sensitive_validation_passed=true`；当前结果可描述为机械杆臂经动态数据一致性验证的工程候选，不得描述为 data-only 平移标定结果。
## 文件说明
- `engineering_release_decision.json`：最终状态、变换和放行判定。
- `mechanical_prior_engineering_47_window.json`：相同 47 个非重叠窗口上的 free/fixed/prior 对比。
- `node_graph_free_information_selected_mechanical.json`：冻结的无先验 free-solve 基线。
- `lever_information_window_selection*.json`：窗口选择及实际边缘信息复核。
- `mechanical_prior_engineering_heldout.json`：未参与标定窗口的 held-out 验证。
- `heldout_independent_innovation.json`、`heldout_nonconverged_retry.json`：独立创新与失败窗口重试结果。
- `mechanical_prior_rotation_sensitivity.json`：固定候选杆臂的旋转扰动敏感性结果。
- `propagation_bias_root_cause_audit.json`：独立传播公共加速度误差根因审计。
结果 JSON 是审核快照；需要重新生成时应通过 `tools/` 中相应入口运行，且不得提交运行产生的 checkpoint 或 `.npz` 状态文件。
artifacts/rtk_imu_calibration_v3/engineering_release_decision.json
+128
View File
@@ -0,0 +1,128 @@
{
"scope": "final mechanical-prior RTK-IMU engineering release decision",
"no_refit_performed": true,
"data_only_full_free_called": false,
"bootstrap_called": false,
"loo_called": false,
"covariance_retuned": false,
"new_window_selection_called": false,
"parser_R0_modified": false,
"data_only_translation_accepted": false,
"translation_refined_by_data": false,
"mechanical_prior_consistent_with_calibration": true,
"heldout_physical_validation_passed": true,
"heldout_physical_gate_checks": {
"all_267_converged_after_retry": true,
"BEST_position_vector_p95_le_0p20_m": true,
"Doppler_vector_p95_le_0p50_m_s": true,
"HPR_normalized_p95_le_4": true,
"preintegration_normalized_p95_le_3": true
},
"heldout_statistical_scale_passed": false,
"heldout_postfit_chi_square_per_dof": 0.12733913101711092,
"heldout_covariance_underdispersion_warning": true,
"independent_heldout_innovation_passed": false,
"common_constant_acceleration_error_detected": true,
"independent_propagation_validation_passed": false,
"independent_extrinsic_sensitive_validation_passed": true,
"rotation_sensitivity_passed": true,
"engineering_translation_acceptance_formula": "mechanical_prior_consistent_with_calibration AND heldout_physical_validation_passed AND independent_heldout_innovation_passed AND rotation_sensitivity_passed",
"engineering_translation_accepted": false,
"result_nature": "mechanically anchored + dynamically validated",
"summary": "Translation is mechanically anchored and dynamically validated. The current dataset does not independently observe translation accurately enough for data-only calibration, and does not provide meaningful refinement beyond the mechanical prior.",
"forbidden_descriptions": [
"data-only calibrated translation",
"dynamically refined mechanical lever"
],
"candidate_l_I_engineering_m": [
-0.45180151590212486,
-0.26447498198536895,
0.7314656114613277
],
"candidate_T_RTK_IMU": [
[
0.9999999761265028,
-0.00021395911860711003,
-4.436766104011944e-05,
0.45177737170031523
],
[
0.00021360059847267874,
0.9999685415936667,
-0.007929072948303898,
0.27036301129056084
],
[
4.606276276360097e-05,
0.007929063282050246,
0.9999685634227163,
-0.7293247665913783
],
[
0.0,
0.0,
0.0,
1.0
]
],
"candidate_T_IMU_RTK": [
[
0.9999999761265029,
0.00021360059847267876,
4.606276276360098e-05,
-0.45180151590212486
],
[
-0.00021395911860711008,
0.9999685415936669,
0.007929063282050248,
-0.264474981985369
],
[
-4.436766104011945e-05,
-0.0079290729483039,
0.9999685634227164,
0.7314656114613278
],
[
0.0,
0.0,
0.0,
1.0
]
],
"candidate_transform_inverse_error_norm": 1.3597553244868544e-16,
"l_I_engineering_m": null,
"T_RTK_IMU": null,
"T_IMU_RTK": null,
"transform_convention": {
"equation": "p_RTK = R_RTK_IMU * p_IMU + t_RTK_IMU",
"translation": "t_RTK_IMU = -R_RTK_IMU * l_I",
"RTK_origin": "ANT1 phase center"
},
"rotation_source": "R2G_gravity_level_prior",
"translation_conditional_on_rotation": true,
"evidence": {
"calibration_path": "artifacts\\rtk_imu_calibration_v3\\mechanical_prior_engineering_47_window.json",
"heldout_postfit_path": "artifacts\\rtk_imu_calibration_v3\\mechanical_prior_engineering_heldout.json",
"innovation_path": "artifacts\\rtk_imu_calibration_v3\\heldout_independent_innovation.json",
"sensitivity_path": "artifacts\\rtk_imu_calibration_v3\\mechanical_prior_rotation_sensitivity.json",
"convergence_retry_path": "artifacts\\rtk_imu_calibration_v3\\heldout_nonconverged_retry.json",
"propagation_root_cause_path": "artifacts\\rtk_imu_calibration_v3\\propagation_bias_root_cause_audit.json",
"posterior_prior_variance_ratio": [
0.9841028247436912,
0.9831529061304226,
0.9921155953185713
],
"heldout_convergence_after_retry": 1.0,
"rotation_sensitivity_summary": {
"max_abs_delta_l_xyz_m": [
0.0024980444199615426,
0.0006588674774769543,
0.0007172660986609625
],
"max_delta_l_norm_m": 0.0025582764807350012,
"max_transform_translation_delta_norm_m": 0.00686033304738171
}
}
}
artifacts/rtk_imu_calibration_v3/heldout_independent_innovation.json
+22231
View File
File diff suppressed because it is too large Load Diff
artifacts/rtk_imu_calibration_v3/heldout_nonconverged_retry.json
+1799
View File
File diff suppressed because it is too large Load Diff
artifacts/rtk_imu_calibration_v3/lever_information_window_selection.json
+3010
View File
File diff suppressed because it is too large Load Diff
artifacts/rtk_imu_calibration_v3/lever_information_window_selection_refined.json
+21115
View File
File diff suppressed because it is too large Load Diff
artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_47_window.json
+1338
View File
File diff suppressed because it is too large Load Diff
artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_heldout.json
+4944
View File
File diff suppressed because it is too large Load Diff
artifacts/rtk_imu_calibration_v3/mechanical_prior_rotation_sensitivity.json
+4533
View File
File diff suppressed because it is too large Load Diff
artifacts/rtk_imu_calibration_v3/node_graph_free_information_selected_mechanical.json
+1302
View File
File diff suppressed because it is too large Load Diff
artifacts/rtk_imu_calibration_v3/propagation_bias_root_cause_audit.json
+49318
View File
File diff suppressed because it is too large Load Diff
artifacts/rtk_imu_inventory_v1/README.md
+52
View File
@@ -0,0 +1,52 @@
# RTK–IMU 候选数据清点（2026-08-20）
本目录记录 0808、0815、0819 三批 LiDAR/IMU 会话所对应的 G90 RTK 原始记录与当前导出状态。此处只做数据血缘和可用性评估，尚未求解 `T_RTK_IMU`。
## 时间与质量约定
- 原始 RTK 为 Wheeltec G90 V2 `.rscap`，包含 `$GNGGA/$GPGGA` 位置和 `$GNHPR` heading/pitch/roll。
- 切窗使用 NMEA 报文自带的测量 UTC，再通过每个会话的 IMU device→host affine clock 映射到 IMU 设备时间。
- 主机接收时间比 NMEA 测量时间晚约 3–4 s，且有波动；不能按接收时间直接切窗。
- 当前项目门控按 GGA `fix_quality=4` 和 HPR `heading_quality∈{4,5}` 判断固定位置/有效航向。
- `heading_valid` 未覆盖全部 GGA 行主要因为 GGA 与 HPR 频率不同、最近邻匹配阈值为 80 ms；不代表该会话航向整体失效。
## 数据映射与质量
详细机器可读清单见 `rtk_session_inventory.csv`。
| 会话 | RTK结论 | 适合的标定作用 |
| --- | --- | --- |
| `priority_174005_174515` | 100% GGA质量4；有效航向约309 s；yaw变化约485° | 多圈 yaw 与 XY 杠杆臂 |
| `priority_174905_175450` | 100% GGA质量4；Pitch跨度约11.4°；XY约58×22 m | 强候选：yaw、pitch、XY/Z耦合解除 |
| `priority_175910_180530` | 100% GGA质量4；Pitch跨度约13.6°；XY约144×59 m | 最强候选：长基线、pitch、平移 |
| `slope_190548_190730` | 100% GGA质量4；Pitch跨度约7.4°；约102 s | 坡度/Pitch补充 |
| `circle_193412_193642` | 100% GGA质量4；yaw变化约445° | 平面旋转与XY杠杆臂 |
| `loop_194223_195003` | 仅1个异常GGA；yaw累计变化约1203°；约460 s | 最强 yaw/多圈转弯候选 |
| `accel_195608_195958` | 100% GGA质量4；XY约46×23 m；约230 s | 加减速、速度和水平杠杆臂 |
| `motion_sms_154023_154359` | 全窗仅约49%为质量4；可用连续子段约100 s | 仅用15:41:28.2–15:43:08.15固定解/有效航向段 |
## 导出状态
- 0808 当前 `sessions_v2_device_affine` 原先没有 RTK CSV，本次已从原始 G90 `.rscap` 按 NMEA 测量 UTC补导三个会话，未覆盖旧文件。
- 0815 `sessions_v2_device_affine` 与 dense5 slope 已经采用同一测量时间导出规则，无需重导。
- 0819 `sessions_v2_device_affine` 与 dense5 motion 已经采用同一规则；需要在求解器中按质量与时间连续段过滤，而不是重新解释为全窗固定解。
- 0808 旧 `sessions_v1_host_aligned_00` RTK CSV 不含 `t_measurement_utc_s` 字段，只保留作历史对照；新求解应使用 `sessions_v2_device_affine`。
## 已发现的 LiDAR–RTK 资料边界
`D:\data\calibration_usable_20260808\rtk_lidar_station_report*` 保存的是静止站点候选：27个站点、29个候选段，并非包含 `T_RTK_lidar`、协方差和留一验证的正式手眼结果。本轮在 0808 数据目录的 JSON/YAML/Markdown/CSV/日志中没有找到 `T_RTK_lidar` 矩阵。若要通过链式关系得到 LiDAR–IMU，需要继续定位原手眼结果及其坐标约定：
```text
T_IMU_lidar = inverse(T_RTK_IMU) @ T_RTK_lidar
```
## 初步可行性判断
这些数据足以启动直接 RTK–IMU 标定，且比当前纯 LiDAR–IMU Phase-B 更有希望约束 XY：RTK 提供绝对位置，GNHPR 提供航向和 Pitch，多会话包含长基线、转弯、加减速与坡度。仍需注意：
1. GNHPR roll 的变化仅约0.006°–0.065°，不能指望它提供有效 roll 激励。
2. 应先用 RTK heading/pitch 角速度与 IMU gyro 做残余时间偏置和坐标轴验证，再求旋转。
3. 平移应使用 RTK绝对位置 + IMU预积分的联合状态模型，估计共享 `T_RTK_IMU`、每会话速度/bias；不应把RTK轨迹简单二次差分后直接最小二乘。
4. `motion_sms` 必须仅使用其连续固定解子段。
5. 跨0808/0815/0819时应使用每会话IMU bias，外参共享，并检查安装期间是否发生机械变动。
artifacts/rtk_imu_inventory_v1/rtk_session_inventory.csv
+9
View File
@@ -0,0 +1,9 @@
session,batch,raw_rtk_rscap,current_rtk_csv,rows,fixed_gga_ratio,fixed_heading_valid_rows,valid_duration_s,xy_robust_span_x_m,xy_robust_span_y_m,altitude_robust_span_m,heading_unwrapped_span_deg,pitch_robust_span_deg,roll_robust_span_deg,notes
priority_174005_174515,0808,D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap,D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_174005_174515\rtk.csv,4780,1.0000,4085,309.45,12.045,17.380,0.112,485.411,3.462,0.012,newly exported from NMEA measurement UTC
priority_174905_175450,0808,D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap,D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_174905_175450\rtk.csv,5211,1.0000,4454,344.90,58.436,21.923,0.277,350.616,11.379,0.008,newly exported from NMEA measurement UTC
priority_175910_180530,0808,D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap,D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_175910_180530\rtk.csv,5786,1.0000,4957,379.85,143.775,58.657,0.971,260.213,13.574,0.065,newly exported from NMEA measurement UTC
slope_190548_190730,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-110519.251_e0edf32b-c82b-4a38-b5df-2c0e4cac364f.rscap,D:\data\0815\sessions_v2_device_affine\slope_190548_190730\rtk.csv,1578,1.0000,1382,101.90,11.354,18.567,0.714,155.356,7.399,0.026,root is named 0815 but raw measurement date is 2026-08-14 local
circle_193412_193642,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-113355.356_b4b37794-d4d6-433e-87e9-037cad5517d1.rscap,D:\data\0815\sessions_v2_device_affine\circle_193412_193642\rtk.csv,2162,1.0000,1792,147.25,10.105,10.277,0.097,444.994,2.834,0.006,raw capture has truncated tail but target messages are checksum-valid
loop_194223_195003,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-114200.914_36911c82-f8c0-451b-99e3-f5b663ec6115.rscap,D:\data\0815\sessions_v2_device_affine\loop_194223_195003\rtk.csv,6741,0.9999,5647,459.90,11.555,18.800,0.104,1202.601,2.727,0.009,one malformed/non-fixed GGA excluded
accel_195608_195958,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-115547.472_8722f326-8314-4db1-9ec4-bce185c54a78.rscap,D:\data\0815\sessions_v2_device_affine\accel_195608_195958\rtk.csv,3456,1.0000,2923,229.90,45.542,22.610,0.130,188.236,3.248,0.010,measurement-time export already present
motion_sms_154023_154359,0819,D:\data\0819\raw_serial_capture_v2\wheeltec-g90_20260819-074023.329_3d9da6eb-7ef8-4f7b-9262-9193328238b0.rscap,D:\data\0819\dense5\sessions_v2_device_affine\motion_sms_154023_154359\rtk.csv,3294,0.4924,1431,99.95,12.679,17.356,0.839,308.864,8.985,0.020,use only 15:41:28.200-15:43:08.150 fixed+valid sub-window
1 session batch raw_rtk_rscap current_rtk_csv rows fixed_gga_ratio fixed_heading_valid_rows valid_duration_s xy_robust_span_x_m xy_robust_span_y_m altitude_robust_span_m heading_unwrapped_span_deg pitch_robust_span_deg roll_robust_span_deg notes
2 priority_174005_174515 0808 D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_174005_174515\rtk.csv 4780 1.0000 4085 309.45 12.045 17.380 0.112 485.411 3.462 0.012 newly exported from NMEA measurement UTC
3 priority_174905_175450 0808 D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_174905_175450\rtk.csv 5211 1.0000 4454 344.90 58.436 21.923 0.277 350.616 11.379 0.008 newly exported from NMEA measurement UTC
4 priority_175910_180530 0808 D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_175910_180530\rtk.csv 5786 1.0000 4957 379.85 143.775 58.657 0.971 260.213 13.574 0.065 newly exported from NMEA measurement UTC
5 slope_190548_190730 0815 D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-110519.251_e0edf32b-c82b-4a38-b5df-2c0e4cac364f.rscap D:\data\0815\sessions_v2_device_affine\slope_190548_190730\rtk.csv 1578 1.0000 1382 101.90 11.354 18.567 0.714 155.356 7.399 0.026 root is named 0815 but raw measurement date is 2026-08-14 local
6 circle_193412_193642 0815 D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-113355.356_b4b37794-d4d6-433e-87e9-037cad5517d1.rscap D:\data\0815\sessions_v2_device_affine\circle_193412_193642\rtk.csv 2162 1.0000 1792 147.25 10.105 10.277 0.097 444.994 2.834 0.006 raw capture has truncated tail but target messages are checksum-valid
7 loop_194223_195003 0815 D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-114200.914_36911c82-f8c0-451b-99e3-f5b663ec6115.rscap D:\data\0815\sessions_v2_device_affine\loop_194223_195003\rtk.csv 6741 0.9999 5647 459.90 11.555 18.800 0.104 1202.601 2.727 0.009 one malformed/non-fixed GGA excluded
8 accel_195608_195958 0815 D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-115547.472_8722f326-8314-4db1-9ec4-bce185c54a78.rscap D:\data\0815\sessions_v2_device_affine\accel_195608_195958\rtk.csv 3456 1.0000 2923 229.90 45.542 22.610 0.130 188.236 3.248 0.010 measurement-time export already present
9 motion_sms_154023_154359 0819 D:\data\0819\raw_serial_capture_v2\wheeltec-g90_20260819-074023.329_3d9da6eb-7ef8-4f7b-9262-9193328238b0.rscap D:\data\0819\dense5\sessions_v2_device_affine\motion_sms_154023_154359\rtk.csv 3294 0.4924 1431 99.95 12.679 17.356 0.839 308.864 8.985 0.020 use only 15:41:28.200-15:43:08.150 fixed+valid sub-window
docs/rtk_imu_calibration.md
+168
View File
@@ -0,0 +1,168 @@
# RTK–IMU 标定链路审计与当前方案
> 2026-08-21 多源原始报文重构、R1b/R2V/R2G/R3 实现与三批数据实测结果，
> 见 [rtk_imu_multisource_v3.md](rtk_imu_multisource_v3.md)。本页保留旧式
> GGA/GNHPR 链路的审计背景；完整 HPR 手眼仍只允许作为诊断。
## 结论
当前数据可以验证“双天线基线方向与 IMU 的一致性”，但不能单独标定一个无歧义的完整三自由度 RTK 姿态外参。
G90 的 heading 表示主天线 ANT1 到从天线 ANT2 的基线方位。实车中主天线在左、从天线在右，因此该基线指向车体右侧，不是前进方向。IMU 的 +Y 指向车前；结合静止重力数据支持 IMU +Z 向上，可得到安装先验：IMU +X 指向车右，和 ANT1→ANT2 同向。
双天线只能观测这根基线的方位和仰角，绕基线自身的旋转不可观。GNHPR pitch 是横向基线的仰角，更接近车体横滚响应，不能当作坡道上的纵向车体 pitch；roll 字段也不是独立的第三姿态观测。因此，旧链路把 HPR 拼成完整 SO(3) 再做三轴手眼，是坡道 RMS、会话牵引和不稳定 yaw/pitch 的主要来源。
当前实现保留旧式完整 HPR 结果作为诊断量，但不允许它通过完整旋转门禁；平移在完整旋转通过前被硬冻结。
## 坐标系和参考点
统一输出约定：
```text
p_RTK = R_RTK_IMU p_IMU + t_RTK_IMU
```
RTK 车固坐标定义：
- +X：ANT1（主天线、左侧）→ ANT2（从天线、右侧），指向车右。
- +Y：车辆前进方向，与 IMU +Y 同向。
- +Z：车辆上方；静止重力数据支持 IMU +Z 向上。
- 该完整三轴定义包含机械安装先验；GNHPR 实际直接观测的只有 +X 基线。
位置参考点：
- GGA 为 ANT1/主天线相位中心。
- 相位中心离地高度为 1.916499878 m。
## 求解链路
### R0：协议、数据质量和时间
1. 校验 NMEA checksum。
2. GGA 位置只接收 Q=4 固定解。
3. GNHPR 姿态只接收 Q=4 固定解；Q=5 浮点解仅进入诊断统计，不参与标定。
4. 保留 GNHPR 自己的卫星数、差分龄期和基站号，不能再用 GGA 卫星数替代 HPR 质量。
5. 姿态使用 `hpr_measurement_utc_s` 映射后的 IMU 设备时间。
6. 遇到 HPR 无效、浮点、时间间隔异常或基线跳变时切断连续段，运动对不得跨断点。
### T0：残余时间偏移
只使用可观测的有符号 heading 角速度与 IMU `gyro_z` 做相关扫描。角速度模长会混入不可观的绕基线旋转，不再作为时间审计依据。
时间偏移只用于初始化/诊断。只有相关峰足够高、与次峰分离且峰宽足够窄时才应用，否则保持 0 s。
### R1：双天线基线一致性
对每个连续固定解片段构造 0.75 s、1.5 s、3.0 s 的相对运动。以安装先验 `u_IMU=[1,0,0]` 检验：
```text
angle(u_RTK(t0), u_RTK(t1))
≈ angle(u_IMU, ΔR_IMU(t0,t1) u_IMU)
```
该标量约束不虚构 RTK 的前向轴和上向轴。输出整体、逐会话 RMS/P95、分轴诊断及最坏时间区间。所有会话的总权重归一，避免高激励或样本更多的会话支配结果。
### R2：旧式完整 HPR 诊断
为兼容历史输出，将基线补成零 roll 的数学坐标架，再运行完整手眼。这个结果仅用于暴露符号错误、异常会话和旧结果变化，不能作为可交付外参。
LOO 删除一个会话后，会重新优化剩余会话的陀螺零偏，不再固定全量数据的 nuisance 参数。
### T1：平移
只有完整三自由度旋转“可观且通过”时，才允许进入杆臂和平移求解。当前条件不满足，因此：
- 不运行平移优化；
- 不输出 `translation_result.json`；
- `t_RTK_IMU_m` 和 `T_RTK_IMU` 为 null；
- 历史巨大 Z、米级不确定度和约 1 m/s 速度残差不再消耗优化时间。
## 当前 8 会话结果
结果目录：`artifacts/rtk_imu_calibration_v2/all_sessions`。
```text
可观测基线一致性：
RMS / median / P95 = 0.593665 / 0.146040 / 1.212972 deg
priority_175910 RMS / P95 = 1.316292 / 2.472570 deg
slope RMS / P95 = 1.264514 / 2.475899 deg
最坏区间 = priority_175910，约 5.084 deg
baseline gate = passed
时间偏移：
全局候选 = +0.005 s
峰值相关 = 0.909019
近峰宽度 = [-0.110, +0.170] s
实际应用 = 0 s
旧式完整 HPR 诊断：
RPY = [-0.132765, -0.297433, -1.558358] deg
RMS / P95 = 1.172397 / 2.404977 deg
std = [0.615416, 0.437419, 3.687145] deg
priority_175910 re-optimized LOO = 1.698555 deg
max re-optimized LOO = 1.699427 deg（删除 slope）
legacy numeric gate = failed
full attitude observable = false
平移：
frozen / not run
```
旧结果中的 `priority_175910` 条件 LOO 为 5.14°。严格剔除 Q5、会话等权、断段保护以及 LOO 重估零偏后，该会话完整诊断 LOO 降为 1.699°。它和坡道会话仍是主要异常源，但现在异常集中在基线仰角通道，而不是 heading：这更符合原始 HPR 中 Q5、低卫星数和 pitch 大幅波动的事实。
## 已排除或仍存在的漏洞
- 已修复：heading 误当车前方向。
- 已修复：GNHPR pitch 误当纵向车体 pitch。
- 已修复：不可观 roll 注入完整姿态。
- 已修复：Q5 浮点 HPR 进入标定。
- 已修复：HPR 质量字段在导出时丢失。
- 已修复：相对运动跨越无效段或跳变。
- 已修复：样本多的会话权重过大。
- 已修复：LOO 固定全量会话零偏。
- 已修复：时间相关使用三轴角速度模长。
- 已修复：旋转未通过仍继续优化平移。
- 仍存在：当前运动不能稳定地从 GGA 速度补全车前/车上方向。
- 仍存在：基线绕轴自由度没有独立传感器观测。
- 仍存在：`priority_175910` 和坡道的基线仰角存在局部异常。
## 如何得到可交付的完整旋转
按优先级建议：
1. 采一组专用数据：空旷区域、全程 RTK fixed、较长直线加减速、左右转、坡道上下行，保留原始 GGA/GNHPR/IMU 时间和全部质量字段。
2. 用高质量前向速度补第二根轴。只在速度足够高、航向变化平缓的区间用 GGA course，并在同一优化中建模 ANT1 杆臂、非完整车辆侧向速度约束和时间偏移。
3. 用静止重力补上向轴时，必须把“地面水平/车辆静止”写成显式先验，并将结果标记为安装先验约束解，而不是双天线数据独立解。
4. 若能取得 G90 内部融合后的完整 INS 姿态、第三天线、轮速/转角或可靠车体姿态源，优先作为第二独立方向。
5. 完整旋转通过留出验证后，才恢复平移；平移应同时估计杆臂、速度、加计偏置，并检查垂向高程基准。
当前数据上尝试用平滑 GGA 速度直接补全姿态，结果随平滑窗口明显变化，受低速、转弯杆臂和高程差分噪声影响，不能进入正式结果。
## 与主流开源方法的对应
- Kalibr：角速度相关适合作为时间偏移初始化，不应在宽峰时强行采用候选值。
- iKalibr：采用连续时间轨迹联合估计时空参数，并强调充分激励；适合后续专用数据。
- MINS：异步测量插值并把传感器外参、时间和 nuisance 状态一起估计。
- GICI-LIB：因子图中显式进行 GNSS/INS 初始化、质量控制和异常值处理。
本项目暂不直接引入这些大型框架，而是吸收其原则：先保证物理可观测性和数据质量，再进行联合优化；不能用自由状态吸收错误模型。
参考：
- Unicore N4 Reference Commands Manual
- Unicore UM982 User Manual
- https://github.com/Unsigned-Long/iKalibr
- https://github.com/ethz-asl/kalibr
- https://github.com/rpng/MINS
- https://github.com/chichengcn/gici-open
## 代码入口
- `rtk_imu/rtk_attitude.py`：GNHPR 基线语义及零 roll 数学补全。
- `rtk_imu/rtk_io.py`：RTK CSV、Q4/Q5 和 checksum 质量门禁。
- `rtk_imu/rtk_imu_rotation.py`：时间审计、连续段、基线审计、旧式诊断和重优化 LOO。
- `rtk_imu/rtk_imu_replay.py`：坐标定义、参考点、平移冻结和 JSON 输出。
- `tools/rscap_v2/g90_rtk.py`：原始 GNHPR 解析及质量字段。
- `tools/export_g90_rtk_to_sessions.py`：GNHPR 质量字段导出。
- `tools/run_rtk_imu_calibration.py`：端到端命令行入口。
- `tests/test_rtk_imu_calibration.py`：轴定义、Q4/Q5、时间和预积分回归测试。
docs/rtk_imu_engineering_6dof.md
+108
View File
@@ -0,0 +1,108 @@
# 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
```
正式会话应覆盖直行加减速、左右转、坡道和明确水平静止段。不得为了运行时间降低验收门禁或把未完成验证标为成功。
docs/rtk_imu_multisource_v3.md
+175
View File
@@ -0,0 +1,175 @@
# RTK–IMU 多源原始数据与旋转标定 V3
## 可行性结论
重构方向正确且必要。原始捕获中确实存在旧导出链路忽略的 BESTNAVA、
PVTSLNA 和 HI13 姿态/四元数。它们可以补充速度、质量、绝对姿态和静止
重力约束，但本批数据尚不能通过完整旋转门禁：
- BESTNAVA 约 0.8–1 Hz，严格高速样本很少。
- 当前动态主要是平面 yaw，R1b 的两个倾斜自由度仍弱可观。
- HI13 四元数与 GNSS 真北存在稳定但非机械的 yaw 偏差，可能来自磁偏角或
内部导航融合。
- 两个明确水平静止会话的 R2G 很稳定，但它是水平/重力先验约束解。
因此，本次重构显著提高了诊断能力，也防止错误外参进入平移；它没有把信息
不足包装成“标定成功”。
## 三批原始数据
扫描根目录：
- 0808：D:\data\raw_serial_capture_v2，仅选 20260808。
- 0815：D:\data\0815\raw_serial_capture_v2；目录实际包含 20260812–14。
- 0819：D:\data\0819\raw_serial_capture_v2。
按文件名捕获开始时间在 1.5 s 内一一配对：
| 批次 | G90/HI13 配对 | GNHPR | BESTNAVA | PVTSLNA | Q4 HPR | Fixed Doppler |
|---|---:|---:|---:|---:|---:|---:|
| 0808 | 18 | 46,180 | 8,291 | 5,993 | 45,606 | 8,128 |
| 0815 | 28 | 96,241 | 12,660 | 9,638 | 27,045 | 3,385 |
| 0819 | 9 | 15,717 | 2,029 | 1,427 | 14,976 | 1,930 |
共完成 55 组统一导出。0815 有一组 G90 没有匹配 HI13，manifest 将其列为
unmatched。发现一条 NMEA 时间为 0913A：保留原始行，设备时间留空并隔离，
没有回退到 host receive time。
速度不等于高速激励。增加速度 ≥1.5 m/s、水平速度标准差 ≤0.25 m/s 后，
三个批次分别只剩 44、8、0 条候选。
## 统一导出
输出根目录：D:\data\rtk_imu_unified_v3。
每组捕获：
- imu.npz：HI13 system_time、gyro、accel、RPY、WXYZ quaternion、磁场、
PPS stamp、温度、气压和 host receive UTC。
- rtk.csv：每条原生异步 GGA/GNHPR/BESTNAVA/PVTSLNA 单独成行，不再把
HPR/BEST/PVT 最近邻挂到 GGA。
- export_summary.json：原始捕获摘要、报文数量、四元数范数和设备到 host
的仿射时钟诊断。
- manifest.json：全部会话、批次、目录和 unmatched 记录。
时间规则：
1. HI13 system_time 是 IMU 主时间轴。
2. BESTNAVA/PVTSLNA 使用 GNSS week/TOW 和 leap seconds。
3. GGA/GNHPR 使用报文自己的 UTC time-of-day。
4. RTK GNSS 测量时刻通过 HI13 device→host 仿射模型映射进 HI13 设备时钟。
5. host receive time 只用于跨时钟桥接、延迟和抖动诊断，绝不替代采样时刻。
6. checksum 无效、时间畸形或非固定解记录保留，但不进入求解。
## 新旋转链路
### R1b：基线在 IMU 中的 2DoF 方向
对连续 Q4 GNHPR 基线和 HI13 陀螺相对旋转使用不变量：
angle(b_ENU(t0), b_ENU(t1))
= angle(b_IMU, DeltaR_IMU b_IMU)
估计 b_IMU 的两个倾斜自由度及逐会话陀螺零偏。安装信息只用于选择 +X
半球，并施加明确记录的弱 20° 先验。按会话等权，输出残差、协方差和信息
奇异值。
### R2V：基线 + Doppler velocity
筛选条件：
- BESTNAVA position 为 SOL_COMPUTED/NARROW_INT；
- velocity 为 SOL_COMPUTED/DOPPLER_VELOCITY；
- checksum 有效；
- 水平速度 ≥1.5 m/s、速度标准差 ≤0.25 m/s；
- |IMU gyro_z| ≤3°/s；
- 速度方向与横向基线接近正交；
- GNHPR Q4，并有时间邻近的合法 HI13 quaternion。
基线给车右，Doppler velocity 给车前，叉积给车上。HI13 quaternion 的
body/world 和 ENU/NED 候选全部评分，只保留残差最小者。该方法会把 HI13
导航 yaw 偏差带入候选，因此必须和 R2G 交叉验证。
### R2G：基线 + 水平静止重力
只允许调用方明确标记的水平静止会话。本次使用：
- 0819_20260819_072130（flat_static_hdg207）
- 0819_20260819_073045（flat_static_hdg082）
每 10 s 分块，要求 gyro norm ≤0.35°/s，且加速度模长距标准重力不超过
0.15 m/s²。R1b 提供车右，加速度中值提供车上，叉积得到车前。
### R3：稳定性与正式门禁
R2V/R2G 都输出：
- 样本和会话数量；
- SO(3) RMS/P95；
- 旋转向量样本标准差和均值协方差；
- leave-one-session；
- 10 样本或 10 s block-out。
只有 R1b 可观、R2V 和 R2G 各自稳定、二者差异 ≤2° 时，才将
translation_unlocked 设为 true。旧 GNHPR 三轴手眼不参与正式门禁。
## 首轮实测
R1b：
b_IMU = [0.999999976, -0.000213959, -0.000044368]
tilt_yz = [-0.0123, -0.0025] deg
pairs = 2091
RMS / P95 = 0.8086 / 1.7105 deg
information singular values = [1.176e-2, 8.476e-4]
std = [27.77, 7.47] deg
gate = failed
点估计接近 +X 是安装先验与名义轴一致的结果；巨大协方差说明不能宣称
数据独立估出了这两个小角。
R2V：
qualifying samples / sessions = 41 / 2
selected quaternion convention = HI13_q_body_to_ENU
RPY = [0.7532, 0.0708, -9.2944] deg
RMS / P95 = 1.8436 / 3.4975 deg
max leave-one-session = 3.4519 deg
gate = failed
R2G：
level-static blocks / sessions = 53 / 2
RPY = [0.4543, -0.0026, 0.0122] deg
RMS / P95 = 0.2542 / 0.2732 deg
max leave-one-session = 0.2688 deg
gate = passed (level/gravity-prior constrained)
R2V 与 R2G 的 SO(3) 差异为 9.3119°。R3 最终：
full_rotation_accepted = false
translation_unlocked = false
## 下一轮数据要求
1. BESTNAVA 改为至少 10 Hz，并确认 Doppler velocity 与 GNHPR 使用同一
GNSS week/TOW 输出周期。
2. 每个日期都录制多段 ≥3 m/s、持续 20–30 s 的正向直线；包含不同方位，
避免单一磁环境和单一会话支配。
3. 为 R1b 增加可控的 roll/pitch 激励；只有平面 yaw 无法稳定估出横向
基线的两个微小倾斜角。
4. 每个日期至少录两种车头方位的明确水平静止段，检验 HI13 重力和绝对
quaternion 的跨日期稳定性。
5. 若 HI13 quaternion 的 yaw 来自磁融合，应获取其导航坐标定义、磁偏角
设置和融合状态；否则 R2V 只使用其 roll/pitch，yaw 由 GNSS 基线和速度
决定。
6. 上述门禁通过前继续冻结杆臂和平移。
## 实现入口
- tools/rscap_v2/g90_rtk.py：GGA/GNHPR/BESTNAVA/PVTSLNA 和双 checksum。
- tools/rscap_v2/hi13_imu.py：HI91 system_time、惯性、姿态和四元数。
- tools/export_rtk_imu_unified.py：55 组配对、原生异步导出和断点续导。
- rtk_imu/rtk_imu_multisource.py：R1b/R2V/R2G/R3。
- tools/run_rtk_imu_multisource.py：多源旋转命令行入口。
- artifacts/rtk_imu_calibration_v3/multisource_result.json：首轮 R3 结果。
imu_lidar/geodesy.py
+74
View File
@@ -0,0 +1,74 @@
"""Small WGS84 geodesy helpers used by the RTK--IMU calibration path."""
from __future__ import annotations
import numpy as np
WGS84_A_M = 6378137.0
WGS84_F = 1.0 / 298.257223563
WGS84_E2 = WGS84_F * (2.0 - WGS84_F)
def geodetic_to_ecef(
latitude_deg: np.ndarray,
longitude_deg: np.ndarray,
altitude_m: np.ndarray,
) -> np.ndarray:
"""Convert WGS84 latitude/longitude/ellipsoidal height to ECEF metres."""
latitude = np.deg2rad(np.asarray(latitude_deg, dtype=float))
longitude = np.deg2rad(np.asarray(longitude_deg, dtype=float))
altitude = np.asarray(altitude_m, dtype=float)
latitude, longitude, altitude = np.broadcast_arrays(latitude, longitude, altitude)
sin_lat = np.sin(latitude)
cos_lat = np.cos(latitude)
radius = WGS84_A_M / np.sqrt(1.0 - WGS84_E2 * sin_lat**2)
x = (radius + altitude) * cos_lat * np.cos(longitude)
y = (radius + altitude) * cos_lat * np.sin(longitude)
z = (radius * (1.0 - WGS84_E2) + altitude) * sin_lat
return np.stack([x, y, z], axis=-1)
def geodetic_to_enu(
latitude_deg: np.ndarray,
longitude_deg: np.ndarray,
altitude_m: np.ndarray,
*,
origin_latitude_deg: float | None = None,
origin_longitude_deg: float | None = None,
origin_altitude_m: float | None = None,
) -> tuple[np.ndarray, tuple[float, float, float]]:
"""Convert WGS84 samples to a local east/north/up frame.
When no origin is supplied, the first finite sample is used. The returned
origin tuple is ``(latitude_deg, longitude_deg, altitude_m)``.
"""
lat = np.asarray(latitude_deg, dtype=float).reshape(-1)
lon = np.asarray(longitude_deg, dtype=float).reshape(-1)
alt = np.asarray(altitude_m, dtype=float).reshape(-1)
if not (lat.size == lon.size == alt.size):
raise ValueError("latitude, longitude and altitude must have equal length")
finite = np.isfinite(lat) & np.isfinite(lon) & np.isfinite(alt)
if not np.any(finite):
raise ValueError("no finite geodetic sample")
first = int(np.flatnonzero(finite)[0])
lat0 = float(lat[first] if origin_latitude_deg is None else origin_latitude_deg)
lon0 = float(lon[first] if origin_longitude_deg is None else origin_longitude_deg)
alt0 = float(alt[first] if origin_altitude_m is None else origin_altitude_m)
ecef = geodetic_to_ecef(lat, lon, alt)
ecef0 = geodetic_to_ecef(np.array(lat0), np.array(lon0), np.array(alt0)).reshape(3)
delta = ecef - ecef0
phi = np.deg2rad(lat0)
lam = np.deg2rad(lon0)
rotation = np.array(
[
[-np.sin(lam), np.cos(lam), 0.0],
[-np.sin(phi) * np.cos(lam), -np.sin(phi) * np.sin(lam), np.cos(phi)],
[np.cos(phi) * np.cos(lam), np.cos(phi) * np.sin(lam), np.sin(phi)],
],
dtype=float,
)
return delta @ rotation.T, (lat0, lon0, alt0)
imu_lidar/imu_io.py
+12 -10
View File
@@ -15,6 +15,7 @@ Accepted inputs
from __future__ import annotations
import csv
from pathlib import Path
import numpy as np
@@ -36,16 +37,17 @@ def load_imu_samples(path: Path | str) -> ImuSeries:
def _load_imu_csv(path: Path) -> ImuSeries:
data = np.genfromtxt(path, delimiter=",", names=True, dtype=float)
if data.ndim == 0:
data = np.array([data])
names = set(data.dtype.names or ())
required = {"t", "gx", "gy", "gz", "ax", "ay", "az"}
if not required.issubset(names):
raise ValueError(f"IMU CSV must contain columns {sorted(required)}, got {sorted(names)}")
t = np.asarray(data["t"], dtype=float).reshape(-1)
gyro = np.column_stack([data["gx"], data["gy"], data["gz"]]).astype(float)
acc = np.column_stack([data["ax"], data["ay"], data["az"]]).astype(float)
required_order = ["t", "gx", "gy", "gz", "ax", "ay", "az"]
with path.open("r", encoding="utf-8-sig", newline="") as handle:
header = next(csv.reader(handle), [])
names = set(header)
if not set(required_order).issubset(names):
raise ValueError(f"IMU CSV must contain columns {sorted(required_order)}, got {sorted(names)}")
usecols = [header.index(name) for name in required_order]
data = np.loadtxt(path, delimiter=",", skiprows=1, usecols=usecols, ndmin=2)
t = np.asarray(data[:, 0], dtype=float).reshape(-1)
gyro = np.asarray(data[:, 1:4], dtype=float)
acc = np.asarray(data[:, 4:7], dtype=float)
order = np.argsort(t)
return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])
imu_lidar/imu_preintegration.py
+15 -7
View File
@@ -158,9 +158,14 @@ def preintegrate_gyro(
if dt <= 0:
continue
# Exact endpoint gyro via linear interpolation inside the sample interval.
g_a = _interp_gyro(times_s, gyro_rad_s, seg0)
g_b = _interp_gyro(times_s, gyro_rad_s, seg1)
# Exact local endpoint interpolation. ``seg0`` and ``seg1`` are inside
# this adjacent sample interval, so scanning the full series with
# np.interp here would turn pair construction into quadratic work.
sample_dt = max(t_b - t_a, 1e-12)
u0 = (seg0 - t_a) / sample_dt
u1 = (seg1 - t_a) / sample_dt
g_a = (1.0 - u0) * gyro_rad_s[index] + u0 * gyro_rad_s[index + 1]
g_b = (1.0 - u1) * gyro_rad_s[index] + u1 * gyro_rad_s[index + 1]
omega = 0.5 * (g_a + g_b) - bias
gyro_norms.append(float(np.linalg.norm(omega)))
@@ -279,10 +284,13 @@ def preintegrate_imu(
if dt <= 0:
continue
g_a = _interp_vec(times_s, gyro_rad_s, seg0)
g_b = _interp_vec(times_s, gyro_rad_s, seg1)
a_a = _interp_vec(times_s, acc_m_s2, seg0)
a_b = _interp_vec(times_s, acc_m_s2, seg1)
sample_dt = max(t_b - t_a, 1e-12)
u0 = (seg0 - t_a) / sample_dt
u1 = (seg1 - t_a) / sample_dt
g_a = (1.0 - u0) * gyro_rad_s[index] + u0 * gyro_rad_s[index + 1]
g_b = (1.0 - u1) * gyro_rad_s[index] + u1 * gyro_rad_s[index + 1]
a_a = (1.0 - u0) * acc_m_s2[index] + u0 * acc_m_s2[index + 1]
a_b = (1.0 - u1) * acc_m_s2[index] + u1 * acc_m_s2[index + 1]
omega = 0.5 * (g_a + g_b) - bg
acc = 0.5 * (a_a + a_b) - ba
gyro_norms.append(float(np.linalg.norm(omega)))
pyproject.toml
+2 -2
View File
@@ -1,7 +1,7 @@
[project]
name = "lidar-imu-calibration"
version = "0.3.0"
description = "LiDAR–IMU extrinsic calibration from continuous-motion keyframes (imu_lidar)"
description = "LiDAR-IMU and RTK-IMU extrinsic calibration algorithms"
requires-python = ">=3.10"
dependencies = [
"numpy>=1.26",
@@ -21,7 +21,7 @@ requires = ["setuptools>=68", "wheel"]
build-backend = "setuptools.build_meta"
[tool.setuptools]
packages = ["imu_lidar", "tools"]
packages = ["imu_lidar", "rtk_imu", "tools"]
[tool.pytest.ini_options]
testpaths = ["tests"]
rtk_imu/README.md
+13
View File
@@ -0,0 +1,13 @@
# RTK-IMU Calibration Package
`rtk_imu/` contains the independent RTK-IMU calibration pipeline: G90 observation I/O, antenna-baseline rotation, V1 replay, V3 multisource stages, engineering 6DoF, and the node-state factor graph.
## Boundary
This package may import only shared `imu_lidar` utilities: `contracts`, `geometry`, `geodesy`, `imu_io`, `imu_preintegration`, and the generic `rotation_handeye` initializer.
It must not import LiDAR-specific modules such as `lidar_io`, `lidar_deskew`, `registration`, `pipeline`, `phase_a`, or `joint_optimizer`. The LiDAR-IMU pipeline remains in `imu_lidar/`.
## Entrypoints
Use `tools/run_rtk_imu_*.py` and `tools/audit_rtk_imu_*.py`; these scripts import from `rtk_imu.*`.
For the engineering procedure, current result, acceptance boundary, and reproduction commands, see [`README_RTK_IMU.md`](../README_RTK_IMU.md).
rtk_imu/__init__.py
+1
View File
@@ -0,0 +1 @@
"""RTK-IMU extrinsic calibration algorithms and data interfaces."""
rtk_imu/rtk_attitude.py
+105
View File
@@ -0,0 +1,105 @@
"""GNHPR dual-antenna baseline conventions and SO(3) interpolation."""
from __future__ import annotations
from dataclasses import dataclass
import numpy as np
from scipy.spatial.transform import Rotation, Slerp
@dataclass(frozen=True)
class GnhprConvention:
"""Interpretation of the GNHPR ANT1-to-ANT2 baseline.
Heading is clockwise from north. In this vehicle the RTK ``+X`` axis is
the baseline from the main/left antenna (ANT1) to the secondary/right
antenna (ANT2), so it points to vehicle right rather than vehicle forward.
GNHPR pitch is the elevation of that baseline. A two-antenna receiver
cannot observe rotation about the baseline; the reported roll is therefore
not treated as a third independent attitude measurement.
"""
name: str
heading_sign: float = -1.0
pitch_sign: float = -1.0
roll_sign: float = 1.0
EXPECTED_GNHPR = GnhprConvention("ant1_to_ant2__north_cw__elevation_up")
GNHPR_CANDIDATES = (
EXPECTED_GNHPR,
GnhprConvention("north_cw__pitch_opposite", -1.0, 1.0, 1.0),
GnhprConvention("heading_opposite__pitch_nose_up", 1.0, -1.0, 1.0),
GnhprConvention("heading_opposite__pitch_opposite", 1.0, 1.0, 1.0),
)
def gnhpr_to_rotation_enu_rtk(
heading_deg: np.ndarray,
pitch_deg: np.ndarray,
roll_deg: np.ndarray,
convention: GnhprConvention = EXPECTED_GNHPR,
) -> np.ndarray:
"""Build a zero-roll mathematical completion of the baseline frame.
Only the first column (the ANT1-to-ANT2 unit vector) is physically observed
by GNHPR. The remaining columns are a convenient gauge completion and must
not be used as a measured full vehicle attitude.
"""
heading = np.asarray(heading_deg, dtype=float).reshape(-1)
pitch = np.asarray(pitch_deg, dtype=float).reshape(-1)
roll = np.asarray(roll_deg, dtype=float).reshape(-1)
if not (heading.size == pitch.size == roll.size):
raise ValueError("heading, pitch and roll must have equal length")
yaw_rad = np.deg2rad(90.0 + convention.heading_sign * heading)
pitch_rad = np.deg2rad(convention.pitch_sign * pitch)
# A dual-antenna baseline has no independent roll observation. Keep the
# argument for wire-format compatibility, but never inject it into SO(3).
roll_rad = np.zeros_like(roll)
angles = np.column_stack([yaw_rad, pitch_rad, roll_rad])
return Rotation.from_euler('ZYX', angles).as_matrix()
def gnhpr_to_baseline_enu(
heading_deg: np.ndarray,
pitch_deg: np.ndarray,
convention: GnhprConvention = EXPECTED_GNHPR,
) -> np.ndarray:
"""Return ANT1-to-ANT2 unit vectors expressed in ENU.
The result is independent of the unobservable GNHPR roll field.
"""
heading = np.asarray(heading_deg, dtype=float).reshape(-1)
pitch = np.asarray(pitch_deg, dtype=float).reshape(-1)
if heading.size != pitch.size:
raise ValueError("heading and pitch must have equal length")
azimuth = np.deg2rad(heading)
elevation = np.deg2rad(-convention.pitch_sign * pitch)
horizontal = np.cos(elevation)
east = np.sin(azimuth) * horizontal
north = np.cos(azimuth) * horizontal
up = np.sin(elevation)
return np.column_stack([east, north, up])
def interpolate_rotations(
source_t_s: np.ndarray,
rotations: np.ndarray,
query_t_s: np.ndarray,
) -> np.ndarray:
"""Slerp a monotonic SO(3) series without extrapolation."""
source_t = np.asarray(source_t_s, dtype=float).reshape(-1)
query_t = np.asarray(query_t_s, dtype=float).reshape(-1)
matrices = np.asarray(rotations, dtype=float).reshape(-1, 3, 3)
if source_t.size < 2 or matrices.shape[0] != source_t.size:
raise ValueError("need at least two timestamped rotations")
if np.any(np.diff(source_t) <= 0):
unique_t, unique_indices = np.unique(source_t, return_index=True)
source_t = unique_t
matrices = matrices[unique_indices]
if np.any(query_t < source_t[0]) or np.any(query_t > source_t[-1]):
raise ValueError("rotation interpolation does not extrapolate")
return Slerp(source_t, Rotation.from_matrix(matrices))(query_t).as_matrix()
rtk_imu/rtk_imu_engineering.py
+1208
View File
File diff suppressed because it is too large Load Diff
rtk_imu/rtk_imu_multisource.py
+558
View File
@@ -0,0 +1,558 @@
"""Observable RTK--IMU rotation stages using native asynchronous measurements."""
from __future__ import annotations
import csv
import json
from dataclasses import asdict, dataclass
from pathlib import Path
import numpy as np
from scipy.optimize import least_squares
from scipy.spatial.transform import Rotation
from imu_lidar.contracts import ImuSeries
from imu_lidar.geometry import so3_exp, so3_log
from imu_lidar.imu_preintegration import apply_bias_jacobian_correction, preintegrate_gyro
from .rtk_attitude import gnhpr_to_baseline_enu
@dataclass(frozen=True)
class UnifiedSession:
session_id: str
batch_id: str
imu: ImuSeries
imu_rpy_deg: np.ndarray
imu_quaternion_wxyz: np.ndarray
imu_host_receive_utc_s: np.ndarray
rtk_by_type: dict[str, list[dict[str, str]]]
@dataclass(frozen=True)
class R1bResult:
baseline_axis_imu: np.ndarray
tilt_yz_deg: np.ndarray
pair_count: int
residual_rms_deg: float
residual_p95_deg: float
covariance_deg2: np.ndarray
std_deg: np.ndarray
information_singular_values: np.ndarray
per_session_rms_deg: dict[str, float]
gyro_bias_by_session_rad_s: dict[str, np.ndarray]
ok: bool
notes: tuple[str, ...]
@dataclass(frozen=True)
class CompletedRotationResult:
method: str
R_RTK_IMU: np.ndarray | None
rpy_deg: np.ndarray | None
sample_count: int
session_count: int
residual_rms_deg: float
residual_p95_deg: float
covariance_deg2: np.ndarray
std_deg: np.ndarray
per_session_rms_deg: dict[str, float]
leave_one_session_delta_deg: dict[str, float]
block_out_delta_deg: dict[str, float]
convention: str
ok: bool
notes: tuple[str, ...]
@dataclass(frozen=True)
class R3Result:
r1b: R1bResult
r2v: CompletedRotationResult
r2g: CompletedRotationResult
r2v_r2g_delta_deg: float
full_rotation_accepted: bool
translation_unlocked: bool
blockers: tuple[str, ...]
@dataclass(frozen=True)
class _BaselinePair:
session_index: int
session_id: str
world_angle_rad: float
delta_R: np.ndarray
J_bg: np.ndarray
def _f(row: dict[str, str], key: str, default: float = np.nan) -> float:
try:
return float(row.get(key, ""))
except (TypeError, ValueError):
return default
def _truth(row: dict[str, str], key: str) -> bool:
return str(row.get(key, "")).strip().lower() in {"1", "true", "yes"}
def load_unified_sessions(
manifest_path: Path | str,
*,
selected_session_ids: set[str] | None = None,
) -> list[UnifiedSession]:
manifest_source = Path(manifest_path)
manifest = json.loads(manifest_source.read_text(encoding="utf-8"))
sessions: list[UnifiedSession] = []
for entry in manifest["sessions"]:
session_id = str(entry["session_id"])
if selected_session_ids and session_id not in selected_session_ids:
continue
directory = Path(entry["directory"])
if not directory.is_absolute():
directory = manifest_source.parent / directory
with np.load(directory / "imu.npz") as payload:
t = np.asarray(payload["system_time_s"], dtype=float)
gyro = np.asarray(payload["gyro_rad_s"], dtype=float)
accel = np.asarray(payload["accel_m_s2"], dtype=float)
rpy = np.asarray(payload["rpy_deg"], dtype=float)
quaternion = np.asarray(payload["quaternion_wxyz"], dtype=float)
host = np.asarray(payload["host_receive_utc_s"], dtype=float)
by_type: dict[str, list[dict[str, str]]] = {}
with (directory / "rtk.csv").open("r", encoding="utf-8", newline="") as stream:
for row in csv.DictReader(stream):
by_type.setdefault(row["message_type"], []).append(row)
sessions.append(
UnifiedSession(
session_id=session_id,
batch_id=str(entry["batch_id"]),
imu=ImuSeries(t_s=t, gyro_rad_s=gyro, acc_m_s2=accel),
imu_rpy_deg=rpy,
imu_quaternion_wxyz=quaternion,
imu_host_receive_utc_s=host,
rtk_by_type=by_type,
)
)
return sessions
def _valid_hpr(session: UnifiedSession) -> tuple[np.ndarray, np.ndarray]:
rows = [
row for row in session.rtk_by_type.get("GNHPR", [])
if _truth(row, "checksum_valid") and int(_f(row, "heading_quality", -1)) == 4
]
if not rows:
return np.zeros(0), np.zeros((0, 3))
t = np.asarray([_f(row, "t_device_s") for row in rows])
baseline = gnhpr_to_baseline_enu(
np.asarray([_f(row, "heading_deg") for row in rows]),
np.asarray([_f(row, "pitch_deg") for row in rows]),
)
finite = np.isfinite(t) & np.all(np.isfinite(baseline), axis=1)
t, baseline = t[finite], baseline[finite]
order = np.argsort(t)
t, baseline = t[order], baseline[order]
unique, indices = np.unique(t, return_index=True)
return unique, baseline[indices]
def _baseline_pairs(sessions: list[UnifiedSession]) -> list[_BaselinePair]:
pairs: list[_BaselinePair] = []
for session_index, session in enumerate(sessions):
t, baseline = _valid_hpr(session)
if t.size < 3:
continue
dt = np.diff(t)
jump = np.degrees(
np.arccos(np.clip(np.sum(baseline[:-1] * baseline[1:], axis=1), -1.0, 1.0))
)
continuous = (dt >= 0.03) & (dt <= 0.25) & (jump / np.maximum(dt, 1e-6) <= 45.0)
last_anchor = -np.inf
for index, t0 in enumerate(t[:-1]):
if t0 - last_anchor < 1.0:
continue
last_anchor = t0
for duration in (0.75, 1.5, 3.0):
target = t0 + duration
end = int(np.searchsorted(t, target))
candidates = [candidate for candidate in (end - 1, end) if index < candidate < t.size]
if not candidates:
continue
j = min(candidates, key=lambda candidate: abs(t[candidate] - target))
if abs((t[j] - t0) - duration) > 0.12 or not np.all(continuous[index:j]):
continue
try:
pre = preintegrate_gyro(
session.imu.t_s, session.imu.gyro_rad_s, float(t0), float(t[j])
)
except ValueError:
continue
world_angle = float(
np.arccos(np.clip(np.dot(baseline[index], baseline[j]), -1.0, 1.0))
)
if np.degrees(world_angle) < 0.4:
continue
pairs.append(
_BaselinePair(
session_index=session_index,
session_id=session.session_id,
world_angle_rad=world_angle,
delta_R=pre.delta_R,
J_bg=pre.J_bg,
)
)
return pairs
def _axis_from_parameters(parameters: np.ndarray) -> np.ndarray:
axis = np.asarray([1.0, parameters[0], parameters[1]], dtype=float)
return axis / np.linalg.norm(axis)
def solve_r1b(sessions: list[UnifiedSession]) -> R1bResult:
pairs = _baseline_pairs(sessions)
if len(pairs) < 20:
raise ValueError("R1b needs at least 20 continuous baseline/gyro motion pairs")
session_count = len(sessions)
pair_counts = {
session.session_id: sum(pair.session_id == session.session_id for pair in pairs)
for session in sessions
}
active_sessions = sum(count > 0 for count in pair_counts.values())
target_count = len(pairs) / max(active_sessions, 1)
def residual(parameters: np.ndarray) -> np.ndarray:
axis = _axis_from_parameters(parameters[:2])
biases = parameters[2:].reshape(session_count, 3)
values = []
for pair in pairs:
corrected = apply_bias_jacobian_correction(
pair.delta_R, pair.J_bg, biases[pair.session_index]
)
body_angle = np.arccos(
np.clip(np.dot(axis, corrected @ axis), -1.0, 1.0)
)
weight = np.sqrt(target_count / pair_counts[pair.session_id])
values.append(weight * (body_angle - pair.world_angle_rad))
values.extend((biases / 0.01).reshape(-1))
# Weak 20-degree installation prior selects the physically known +X hemisphere.
values.extend(np.asarray(parameters[:2]) / np.tan(np.deg2rad(20.0)))
return np.asarray(values)
initial = np.zeros(2 + 3 * session_count)
optimum = least_squares(
residual, initial, loss="huber", f_scale=np.deg2rad(0.25), max_nfev=120
)
axis = _axis_from_parameters(optimum.x[:2])
biases = optimum.x[2:].reshape(session_count, 3)
errors = []
per_session_values: dict[str, list[float]] = {}
for pair in pairs:
corrected = apply_bias_jacobian_correction(
pair.delta_R, pair.J_bg, biases[pair.session_index]
)
body_angle = np.arccos(np.clip(np.dot(axis, corrected @ axis), -1.0, 1.0))
error = float(np.degrees(body_angle - pair.world_angle_rad))
errors.append(error)
per_session_values.setdefault(pair.session_id, []).append(error)
errors_array = np.asarray(errors)
data_rows = len(pairs)
jacobian = optimum.jac[:data_rows, :2]
information = jacobian.T @ jacobian
residual_variance = float(np.mean(np.deg2rad(errors_array) ** 2))
covariance = residual_variance * np.linalg.pinv(information, rcond=1e-12)
covariance_deg2 = np.degrees(1.0) ** 2 * covariance
std_deg = np.sqrt(np.maximum(np.diag(covariance_deg2), 0.0))
singular = np.linalg.svd(information, compute_uv=False)
rms = float(np.sqrt(np.mean(errors_array**2)))
p95 = float(np.percentile(np.abs(errors_array), 95.0))
return R1bResult(
baseline_axis_imu=axis,
tilt_yz_deg=np.degrees(np.arctan(optimum.x[:2])),
pair_count=len(pairs),
residual_rms_deg=rms,
residual_p95_deg=p95,
covariance_deg2=covariance_deg2,
std_deg=std_deg,
information_singular_values=singular,
per_session_rms_deg={
key: float(np.sqrt(np.mean(np.asarray(value) ** 2)))
for key, value in per_session_values.items()
},
gyro_bias_by_session_rad_s={
session.session_id: biases[index] for index, session in enumerate(sessions)
},
ok=bool(rms <= 1.0 and p95 <= 2.0 and np.min(singular) >= 1e-3),
notes=(
"2DoF ANT1-to-ANT2 direction; +X hemisphere selected by installation knowledge",
"weak 20 deg prior is reported and prevents sign/gauge branch switching",
),
)
def _rotation_mean(matrices: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
mean = Rotation.from_matrix(matrices).mean().as_matrix()
errors = np.asarray(
[np.degrees(np.linalg.norm(so3_log(mean.T @ matrix))) for matrix in matrices]
)
return mean, errors
def _result_from_samples(
method: str,
samples: list[tuple[str, str, np.ndarray]],
convention: str,
notes: tuple[str, ...],
) -> CompletedRotationResult:
if not samples:
return CompletedRotationResult(
method, None, None, 0, 0, np.nan, np.nan, np.full((3, 3), np.nan),
np.full(3, np.nan),
{}, {}, {}, convention, False, notes + ("no qualifying samples",)
)
matrices = np.asarray([item[2] for item in samples])
mean, errors = _rotation_mean(matrices)
rotvec = np.asarray([so3_log(mean.T @ matrix) for matrix in matrices])
rotvec_deg = np.degrees(rotvec)
std = np.std(rotvec_deg, axis=0, ddof=1) if len(samples) > 1 else np.full(3, np.nan)
covariance = (
np.cov(rotvec_deg, rowvar=False, ddof=1) / len(samples)
if len(samples) > 1 else np.full((3, 3), np.nan)
)
ids = sorted({item[0] for item in samples})
per_session = {}
loo = {}
for session_id in ids:
selected = [item[2] for item in samples if item[0] == session_id]
_, local_errors = _rotation_mean(np.asarray(selected))
per_session[session_id] = float(np.sqrt(np.mean(local_errors**2)))
kept = np.asarray([item[2] for item in samples if item[0] != session_id])
if kept.size:
kept_mean, _ = _rotation_mean(kept)
loo[session_id] = float(np.degrees(np.linalg.norm(so3_log(mean.T @ kept_mean))))
block_groups = sorted({(item[0], item[1]) for item in samples})
block_out = {}
for session_id, block_id in block_groups:
kept = np.asarray(
[item[2] for item in samples if (item[0], item[1]) != (session_id, block_id)]
)
if kept.size:
kept_mean, _ = _rotation_mean(kept)
block_out[f"{session_id}:{block_id}"] = float(
np.degrees(np.linalg.norm(so3_log(mean.T @ kept_mean)))
)
rms = float(np.sqrt(np.mean(errors**2)))
p95 = float(np.percentile(errors, 95.0))
max_loo = max(loo.values(), default=np.inf)
ok = bool(
len(samples) >= 10 and len(ids) >= 2 and rms <= 2.0 and p95 <= 3.0
and np.nanmax(std) <= 1.0 and max_loo <= 1.0
)
return CompletedRotationResult(
method=method,
R_RTK_IMU=mean,
rpy_deg=Rotation.from_matrix(mean).as_euler("xyz", degrees=True),
sample_count=len(samples),
session_count=len(ids),
residual_rms_deg=rms,
residual_p95_deg=p95,
covariance_deg2=covariance,
std_deg=std,
per_session_rms_deg=per_session,
leave_one_session_delta_deg=loo,
block_out_delta_deg=block_out,
convention=convention,
ok=ok,
notes=notes,
)
def solve_r2g(
sessions: list[UnifiedSession],
r1b: R1bResult,
*,
level_static_session_ids: set[str],
block_duration_s: float = 10.0,
) -> CompletedRotationResult:
samples: list[tuple[str, str, np.ndarray]] = []
right = r1b.baseline_axis_imu
for session in sessions:
if session.session_id not in level_static_session_ids:
continue
gyro_norm = np.linalg.norm(session.imu.gyro_rad_s, axis=1)
accel_norm = np.linalg.norm(session.imu.acc_m_s2, axis=1)
valid = (gyro_norm <= np.deg2rad(0.35)) & (np.abs(accel_norm - 9.80665) <= 0.15)
block = np.floor(
(session.imu.t_s - session.imu.t_s[0]) / block_duration_s
).astype(int)
for block_id in np.unique(block[valid]):
selected = valid & (block == block_id)
if np.count_nonzero(selected) < 200:
continue
up = np.median(session.imu.acc_m_s2[selected], axis=0)
up /= np.linalg.norm(up)
up -= right * np.dot(up, right)
if np.linalg.norm(up) < 0.9:
continue
up /= np.linalg.norm(up)
forward = np.cross(up, right)
forward /= np.linalg.norm(forward)
C_IMU_RTK = np.column_stack([right, forward, up])
samples.append((session.session_id, str(int(block_id)), C_IMU_RTK.T))
return _result_from_samples(
"R2G_baseline_plus_level_gravity",
samples,
"accelerometer specific-force points vehicle up on explicit level-static blocks",
(
"only caller-declared level-static sessions are eligible",
"result is level/gravity-prior constrained, not dual-antenna-only",
),
)
def _nearest_index(t: np.ndarray, value: float, tolerance: float) -> int | None:
index = int(np.searchsorted(t, value))
candidates = [item for item in (index - 1, index) if 0 <= item < t.size]
if not candidates:
return None
best = min(candidates, key=lambda item: abs(t[item] - value))
return best if abs(t[best] - value) <= tolerance else None
def solve_r2v(
sessions: list[UnifiedSession],
*,
min_speed_m_s: float = 1.5,
max_yaw_rate_deg_s: float = 3.0,
max_baseline_course_error_deg: float = 15.0,
) -> CompletedRotationResult:
candidates: dict[str, list[tuple[str, str, np.ndarray]]] = {
"HI13_q_body_to_ENU": [],
"NED_to_ENU_times_HI13_q": [],
"HI13_q_inverse_as_body_to_ENU": [],
"NED_to_ENU_times_HI13_q_inverse": [],
}
NED_TO_ENU = np.asarray([[0.0, 1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, -1.0]])
for session in sessions:
hpr_t, hpr_baseline = _valid_hpr(session)
if hpr_t.size == 0:
continue
quaternion = session.imu_quaternion_wxyz
norm = np.linalg.norm(quaternion, axis=1)
valid_quaternion = np.isfinite(norm) & (np.abs(norm - 1.0) <= 0.02)
normalized = quaternion / np.maximum(norm[:, None], 1e-12)
imu_rotations = Rotation.from_quat(normalized[:, [1, 2, 3, 0]]).as_matrix()
for row_index, row in enumerate(session.rtk_by_type.get("BESTNAVA", [])):
if not (
_truth(row, "checksum_valid")
and _truth(row, "position_fixed")
and _truth(row, "doppler_velocity_valid")
and _f(row, "horizontal_speed_m_s") >= min_speed_m_s
and _f(row, "horizontal_speed_std_m_s") <= 0.25
):
continue
t = _f(row, "t_device_s")
hpr_index = _nearest_index(hpr_t, t, 0.15)
imu_index = _nearest_index(session.imu.t_s, t, 0.03)
if hpr_index is None or imu_index is None or not valid_quaternion[imu_index]:
continue
if abs(np.degrees(session.imu.gyro_rad_s[imu_index, 2])) > max_yaw_rate_deg_s:
continue
right = hpr_baseline[hpr_index]
forward = np.asarray(
[_f(row, "velocity_east_m_s"), _f(row, "velocity_north_m_s"), 0.0]
)
forward /= np.linalg.norm(forward)
course_error = np.degrees(
np.arcsin(np.clip(abs(np.dot(right, forward)), 0.0, 1.0))
)
if course_error > max_baseline_course_error_deg:
continue
forward -= right * np.dot(right, forward)
forward /= np.linalg.norm(forward)
up = np.cross(right, forward)
if up[2] < 0:
forward = -forward
up = -up
up /= np.linalg.norm(up)
R_ENU_RTK = np.column_stack([right, forward, up])
q = imu_rotations[imu_index]
world_candidates = {
"HI13_q_body_to_ENU": q,
"NED_to_ENU_times_HI13_q": NED_TO_ENU @ q,
"HI13_q_inverse_as_body_to_ENU": q.T,
"NED_to_ENU_times_HI13_q_inverse": NED_TO_ENU @ q.T,
}
block_id = str(row_index // 10)
for name, R_ENU_IMU in world_candidates.items():
candidates[name].append(
(session.session_id, block_id, R_ENU_RTK.T @ R_ENU_IMU)
)
diagnostics = {
name: _result_from_samples(
"R2V_baseline_plus_doppler_velocity",
values,
name,
(
"RTK fixed + Doppler velocity + speed + low-yaw + baseline/course gates",
"HI13 absolute quaternion may contain magnetic/navigation yaw bias",
),
)
for name, values in candidates.items()
}
finite = [result for result in diagnostics.values() if result.sample_count]
if not finite:
return diagnostics["HI13_q_body_to_ENU"]
return min(finite, key=lambda result: result.residual_rms_deg)
def solve_r3(
sessions: list[UnifiedSession],
*,
level_static_session_ids: set[str],
) -> R3Result:
dynamic_sessions = [
session for session in sessions if session.session_id not in level_static_session_ids
]
r1b = solve_r1b(dynamic_sessions)
r2v = solve_r2v(dynamic_sessions)
r2g = solve_r2g(sessions, r1b, level_static_session_ids=level_static_session_ids)
if r2v.R_RTK_IMU is None or r2g.R_RTK_IMU is None:
delta = np.nan
else:
delta = float(
np.degrees(np.linalg.norm(so3_log(r2v.R_RTK_IMU.T @ r2g.R_RTK_IMU)))
)
blockers = []
if not r1b.ok:
blockers.append("R1b baseline direction failed residual/observability gates")
if not r2v.ok:
blockers.append("R2V velocity-completed rotation failed stability gates")
if not r2g.ok:
blockers.append("R2G gravity-completed rotation failed stability gates")
if not np.isfinite(delta) or delta > 2.0:
blockers.append("R2V and R2G disagree by more than 2 deg")
accepted = not blockers
return R3Result(
r1b=r1b,
r2v=r2v,
r2g=r2g,
r2v_r2g_delta_deg=delta,
full_rotation_accepted=accepted,
translation_unlocked=accepted,
blockers=tuple(blockers),
)
def result_to_jsonable(result: R3Result) -> dict:
def convert(value):
if isinstance(value, np.ndarray):
return value.tolist()
if isinstance(value, np.generic):
return value.item()
if hasattr(value, "__dataclass_fields__"):
return {key: convert(item) for key, item in asdict(value).items()}
if isinstance(value, dict):
return {str(key): convert(item) for key, item in value.items()}
if isinstance(value, (tuple, list)):
return [convert(item) for item in value]
return value
return convert(result)
rtk_imu/rtk_imu_node_graph.py
+529
View File
@@ -0,0 +1,529 @@
'''Per-GNSS-node RTK/IMU state graph used after legacy propagation deprecation.'''
from __future__ import annotations
from dataclasses import dataclass
import numpy as np
from scipy.optimize import least_squares
from scipy.sparse import lil_matrix
from imu_lidar.geometry import so3_exp, so3_log
from imu_lidar.imu_preintegration import apply_bias_correction_imu, residual_whiten_matrix
from .rtk_imu_engineering import (
G_ENU, HPR_DIRECT_ANGULAR_SIGMA_RAD, _Segment, _world_rtk)
NODE_DOF = 15
SIGMA_BG_RW = 1e-5
SIGMA_BA_RW = 1e-3
@dataclass(frozen=True)
class NodeGraphProblem:
segment: _Segment
R_seed_WI: tuple[np.ndarray,...]
fixed_l_I_m: np.ndarray
R_RTK_IMU: np.ndarray
hpr_direct_angular_sigma_rad: float = HPR_DIRECT_ANGULAR_SIGMA_RAD
@dataclass(frozen=True)
class NodeGraphResult:
success: bool
message: str
node_count: int
duration_s: float
fixed_l_I_m: np.ndarray
initial_cost: float
final_cost: float
cost_reduction: float
nfev: int
optimality: float
gradient_norm: float
residual_dimension: int
state_dimension: int
statistical_dof: int
total_nis: float
chi_square_per_dof: float
cost_per_dof: float
initial_residual_by_factor: dict[str,dict[str,float]]
final_residual_by_factor: dict[str,dict[str,float]]
final_position_residual_m: dict[str,object]
final_velocity_residual_m_s: dict[str,object]
max_bg_step_rad_s: float
max_ba_step_m_s2: float
preintegration_covariance_sigma: dict[str,dict[str,float]]
@dataclass(frozen=True)
class FreeLeverResult:
success: bool
message: str
initial_l_I_m: np.ndarray
final_l_I_m: np.ndarray
lever_step_norm_m: float
initial_cost: float
final_cost: float
nfev: int
optimality: float
chi_square_per_dof: float
position_residual_m: dict[str,object]
velocity_residual_m_s: dict[str,object]
residual_by_factor: dict[str,dict[str,float]]
lever_covariance_m2: np.ndarray
lever_information_singular_values: np.ndarray
lever_information_condition_number: float
lever_precision_rank: int
weakest_lever_direction_I: np.ndarray
def _rotation(problem,index,x):
offset = NODE_DOF*index
return problem.R_seed_WI[index] @ so3_exp(x[offset:offset+3])
def initial_parameters(problem, lever_override=None):
nodes = problem.segment.nodes
x = np.zeros(NODE_DOF*len(nodes))
lever = (problem.fixed_l_I_m if lever_override is None
else np.asarray(lever_override,dtype=float))
previous_p, previous_v = np.zeros(3), np.zeros(3)
for index,node in enumerate(nodes):
offset = NODE_DOF*index
R = problem.R_seed_WI[index]
previous_p = node.p_enu_m-R@lever
if index and not node.position_mask[2]:
previous_p[2] = x[offset-NODE_DOF+5]
if node.velocity_enu_m_s is not None:
previous_v = node.velocity_enu_m_s-R@np.cross(node.gyro_rad_s,lever)
x[offset+3:offset+6] = previous_p
x[offset+6:offset+9] = previous_v
return x
def _stats(values, effective_dof=None):
a = np.asarray(values,dtype=float).reshape(-1)
if not a.size:
return {'count':0,'dof':0,'rms':np.nan,'p50_abs':np.nan,
'p95_abs':np.nan,'p99_abs':np.nan,'nis':np.nan,
'chi_square_per_dof':np.nan}
nis = float(np.dot(a,a))
dof = int(a.size if effective_dof is None else effective_dof)
return {'count':int(a.size),'dof':dof,
'rms':float(np.sqrt(np.mean(a*a))),
'p50_abs':float(np.percentile(np.abs(a),50.)),
'p95_abs':float(np.percentile(np.abs(a),95.)),
'p99_abs':float(np.percentile(np.abs(a),99.)),
'nis':nis,'chi_square_per_dof':nis/max(dof,1)}
def _hpr_sigma(problem, node):
old = float(node.hpr_angular_sigma_rad)
extra_var = max(old*old-HPR_DIRECT_ANGULAR_SIGMA_RAD**2, 0.)
return float(np.sqrt(problem.hpr_direct_angular_sigma_rad**2+extra_var))
def _distribution(values):
a = np.asarray(values,dtype=float).reshape(-1)
if not a.size:
return {'count':0,'rms':np.nan,'p50':np.nan,'p95':np.nan,'p99':np.nan}
return {'count':len(a),'rms':float(np.sqrt(np.mean(a*a))),
'p50':float(np.percentile(a,50.)),'p95':float(np.percentile(a,95.)),
'p99':float(np.percentile(a,99.))}
def _vector_stats(values):
a = np.asarray(values,dtype=float).reshape(-1,3)
if not a.size:
return {'count':0,'axis_rms':[np.nan]*3,'axis_p95_abs':[np.nan]*3,
'vector_rms':np.nan,'vector_p95':np.nan}
norm = np.linalg.norm(a,axis=1)
return {'count':len(a),'axis_rms':np.sqrt(np.mean(a*a,axis=0)),
'axis_p95_abs':np.percentile(np.abs(a),95.,axis=0),
'vector_rms':float(np.sqrt(np.mean(norm*norm))),
'vector_p95':float(np.percentile(norm,95.))}
def residual(problem,x,details=None,dependencies=None,lever_override=None):
nodes, preints = problem.segment.nodes, problem.segment.preintegrations
lever = (problem.fixed_l_I_m if lever_override is None
else np.asarray(lever_override,dtype=float))
baseline_I = problem.R_RTK_IMU.T[:,0]
values = []
def add(value,label,node_indices,raw=None):
a = np.asarray(value,dtype=float).reshape(-1)
values.extend(a)
if dependencies is not None:
dependencies.extend([tuple(node_indices)]*len(a))
if details is not None:
details.setdefault(label,[]).extend(a.tolist())
if raw is not None: details.setdefault(label+'_physical',[]).append(np.asarray(raw))
for index,node in enumerate(nodes):
offset = NODE_DOF*index
R = _rotation(problem,index,x)
p, v = x[offset+3:offset+6], x[offset+6:offset+9]
bg, ba = x[offset+9:offset+12], x[offset+12:offset+15]
p_error = p+R@lever-node.p_enu_m
if node.source == 'GGA':
add(p_error[:2]/.06,'gga_xy',(index,))
if details is not None: details.setdefault('position_physical',[]).append(
np.array([p_error[0],p_error[1],np.nan]))
else:
add(p_error/np.array([.06,.06,.12]),'best_position',(index,),p_error)
if details is not None: details.setdefault('position_physical',[]).append(p_error)
if node.velocity_enu_m_s is not None:
v_error = v+R@np.cross(node.gyro_rad_s-bg,lever)-node.velocity_enu_m_s
add(v_error/np.array([.15,.15,.30]),'doppler',(index,),v_error)
if details is not None: details.setdefault('velocity_physical',[]).append(v_error)
if node.hpr_factor_valid:
hpr_error=np.cross(R@baseline_I,node.baseline_enu)
add(hpr_error/_hpr_sigma(problem,node),'hpr',(index,),hpr_error)
if node.gravity_candidate:
gravity = node.accel_m_s2-ba-R.T@(-G_ENU)
add(gravity/.12,'gravity',(index,))
if index == len(nodes)-1: continue
right = index+1
right_offset = NODE_DOF*right
Rj = _rotation(problem,right,x)
pj = x[right_offset+3:right_offset+6]
vj = x[right_offset+6:right_offset+9]
bgj = x[right_offset+9:right_offset+12]
baj = x[right_offset+12:right_offset+15]
pre = preints[index]
dR,dv,dp = apply_bias_correction_imu(pre,bg,ba)
dt = pre.duration_s
imu_error = np.concatenate([
so3_log(dR.T@R.T@Rj),
R.T@(vj-v-G_ENU*dt)-dv,
R.T@(pj-p-v*dt-.5*G_ENU*dt*dt)-dp])
add(residual_whiten_matrix(pre.cov)@imu_error,'imu_preintegration',
(index,right),imu_error)
add((bgj-bg)/(SIGMA_BG_RW*np.sqrt(dt)),'gyro_bias_random_walk',(index,right))
add((baj-ba)/(SIGMA_BA_RW*np.sqrt(dt)),'accel_bias_random_walk',(index,right))
add(x[:3]/np.deg2rad(5.),'initial_attitude_gauge',(0,))
add(x[9:12]/.02,'initial_gyro_bias',(0,))
add(x[12:15]/.5,'initial_accel_bias',(0,))
return np.asarray(values)
def jacobian_sparsity(problem,x):
dependencies = []
base = residual(problem,x,dependencies=dependencies)
sparsity = lil_matrix((len(base),len(x)),dtype=int)
for row,node_indices in enumerate(dependencies):
for index in node_indices:
start = NODE_DOF*index
sparsity[row,start:start+NODE_DOF] = 1
return sparsity.tocsr()
def _factor_stats(details):
return {key:_stats(value,2*len(value)//3 if key=='hpr' else None)
for key,value in details.items()
if not key.endswith('_physical') and key not in ('position_physical','velocity_physical')}
def _effective_residual_dimension(details):
return sum(2*len(v)//3 if k=='hpr' else len(v)
for k,v in details.items() if not k.endswith('_physical')
and k not in ('position_physical','velocity_physical'))
def _preintegration_covariance_stats(problem):
blocks = {'rotation_rad':[],'velocity_m_s':[],'position_m':[]}
for pre in problem.segment.preintegrations:
sigma = np.sqrt(np.maximum(np.diag(pre.cov),0.))
blocks['rotation_rad'].extend(sigma[:3])
blocks['velocity_m_s'].extend(sigma[3:6])
blocks['position_m'].extend(sigma[6:9])
return {key:_distribution(value) for key,value in blocks.items()}
def build_problem(segment,R_RTK_IMU,fixed_l_I_m,
hpr_direct_angular_sigma_rad=HPR_DIRECT_ANGULAR_SIGMA_RAD):
seeds = []
for index,node in enumerate(segment.nodes):
if node.hpr_factor_valid:
seeds.append(_world_rtk(node.baseline_enu)@R_RTK_IMU)
elif index:
seeds.append(seeds[-1]@segment.preintegrations[index-1].delta_R)
else:
seeds.append(segment.R_WRTK_initial@R_RTK_IMU)
return NodeGraphProblem(segment,tuple(seeds),np.asarray(fixed_l_I_m,dtype=float),
np.asarray(R_RTK_IMU,dtype=float),
float(hpr_direct_angular_sigma_rad))
def solve_fixed_lever(problem,max_nfev=30):
x0 = initial_parameters(problem)
initial_detail = {}
r0 = residual(problem,x0,initial_detail)
fit = least_squares(
lambda value:residual(problem,value),x0,jac='2-point',
jac_sparsity=jacobian_sparsity(problem,x0),method='trf',
tr_solver='lsmr',loss='linear',max_nfev=max_nfev,
x_scale='jac',ftol=1e-6,xtol=1e-6,gtol=1e-6)
final_detail = {}
rf = residual(problem,fit.x,final_detail)
statistical_dof = max(_effective_residual_dimension(final_detail)-len(fit.x),1)
bg = fit.x.reshape(-1,NODE_DOF)[:,9:12]
ba = fit.x.reshape(-1,NODE_DOF)[:,12:15]
bg_step = np.diff(bg,axis=0)
ba_step = np.diff(ba,axis=0)
return NodeGraphResult(
success=bool(fit.success),message=str(fit.message),
node_count=len(problem.segment.nodes),
duration_s=problem.segment.nodes[-1].t_s-problem.segment.nodes[0].t_s,
fixed_l_I_m=problem.fixed_l_I_m.copy(),
initial_cost=.5*float(np.dot(r0,r0)),final_cost=.5*float(np.dot(rf,rf)),
cost_reduction=.5*float(np.dot(r0,r0)-np.dot(rf,rf)),
nfev=int(fit.nfev),optimality=float(fit.optimality),
gradient_norm=float(np.linalg.norm(fit.grad)),
residual_dimension=len(rf),state_dimension=len(fit.x),
statistical_dof=statistical_dof,total_nis=float(np.dot(rf,rf)),
chi_square_per_dof=float(np.dot(rf,rf)/statistical_dof),
cost_per_dof=.5*float(np.dot(rf,rf)/statistical_dof),
initial_residual_by_factor=_factor_stats(initial_detail),
final_residual_by_factor=_factor_stats(final_detail),
final_position_residual_m=_vector_stats(final_detail.get('position_physical',[])),
final_velocity_residual_m_s=_vector_stats(final_detail.get('velocity_physical',[])),
max_bg_step_rad_s=float(np.max(np.linalg.norm(bg_step,axis=1))) if len(bg_step) else 0.,
max_ba_step_m_s2=float(np.max(np.linalg.norm(ba_step,axis=1))) if len(ba_step) else 0.,
preintegration_covariance_sigma=_preintegration_covariance_stats(problem))
def fit_states_at_fixed_lever(problem,l_I_m,max_nfev=50,initial_state_values=None):
lever=np.asarray(l_I_m,dtype=float)
x0=(initial_parameters(problem,lever) if initial_state_values is None
else np.asarray(initial_state_values,dtype=float))
r0=residual(problem,x0,lever_override=lever)
fit=least_squares(
lambda value:residual(problem,value,lever_override=lever),x0,jac='2-point',
jac_sparsity=jacobian_sparsity(problem,x0),method='trf',tr_solver='lsmr',
loss='linear',max_nfev=max_nfev,x_scale='jac',
ftol=1e-6,xtol=1e-6,gtol=1e-6)
return fit.x,{'success':bool(fit.success),'message':str(fit.message),
'nfev':int(fit.nfev),'initial_cost':.5*float(r0@r0),
'cost':float(fit.cost)}
def summarize_fixed_state_values(problems,state_values,l_I_m):
details={}; residuals=[]
for problem,value in zip(problems,state_values):
local={}
residuals.append(residual(problem,np.asarray(value),details=local,
lever_override=l_I_m))
for key,items in local.items(): details.setdefault(key,[]).extend(items)
joined=np.concatenate(residuals)
state_dimension=sum(len(value) for value in state_values)
dof=max(_effective_residual_dimension(details)-state_dimension,1)
return {'cost':.5*float(np.dot(joined,joined)),
'total_nis':float(np.dot(joined,joined)),
'chi_square_per_dof':float(np.dot(joined,joined)/dof),
'statistical_dof':dof,'residual_by_factor':_factor_stats(details),
'best_position_physical_m':_vector_stats(details.get('best_position_physical',[])),
'doppler_physical_m_s':_vector_stats(details.get('doppler_physical',[])),
'hpr_physical_rad':_vector_stats(details.get('hpr_physical',[]))}
def solve_fixed_lever_many(problems,max_nfev=30):
problems = tuple(problems)
sizes = [NODE_DOF*len(problem.segment.nodes) for problem in problems]
offsets = np.cumsum([0,*sizes])
x0 = np.concatenate([initial_parameters(problem) for problem in problems])
def evaluate(value,details=None):
chunks = []
for index,problem in enumerate(problems):
local_details = {} if details is not None else None
chunks.append(residual(problem,value[offsets[index]:offsets[index+1]],
local_details))
if details is not None:
for key,items in local_details.items():
details.setdefault(key,[]).extend(items)
return np.concatenate(chunks)
initial_detail = {}
r0 = evaluate(x0,initial_detail)
sparsity = lil_matrix((len(r0),len(x0)),dtype=int)
row = 0
for index,problem in enumerate(problems):
local_x = x0[offsets[index]:offsets[index+1]]
local = jacobian_sparsity(problem,local_x)
sparsity[row:row+local.shape[0],offsets[index]:offsets[index+1]] = local
row += local.shape[0]
fit = least_squares(
lambda value:evaluate(value),x0,jac='2-point',jac_sparsity=sparsity.tocsr(),
method='trf',tr_solver='lsmr',loss='linear',max_nfev=max_nfev,
x_scale='jac',ftol=1e-6,xtol=1e-6,gtol=1e-6)
final_detail = {}
rf = evaluate(fit.x,final_detail)
bg_steps, ba_steps = [], []
for index,problem in enumerate(problems):
states = fit.x[offsets[index]:offsets[index+1]].reshape(-1,NODE_DOF)
bg_steps.extend(np.linalg.norm(np.diff(states[:,9:12],axis=0),axis=1))
ba_steps.extend(np.linalg.norm(np.diff(states[:,12:15],axis=0),axis=1))
covariance = {'rotation_rad':[],'velocity_m_s':[],'position_m':[]}
for problem in problems:
for pre in problem.segment.preintegrations:
sigma = np.sqrt(np.maximum(np.diag(pre.cov),0.))
covariance['rotation_rad'].extend(sigma[:3])
covariance['velocity_m_s'].extend(sigma[3:6])
covariance['position_m'].extend(sigma[6:9])
dof = max(_effective_residual_dimension(final_detail)-len(fit.x),1)
return NodeGraphResult(
success=bool(fit.success),message=str(fit.message),
node_count=sum(len(problem.segment.nodes) for problem in problems),
duration_s=sum(problem.segment.nodes[-1].t_s-problem.segment.nodes[0].t_s
for problem in problems),
fixed_l_I_m=problems[0].fixed_l_I_m.copy(),
initial_cost=.5*float(np.dot(r0,r0)),final_cost=.5*float(np.dot(rf,rf)),
cost_reduction=.5*float(np.dot(r0,r0)-np.dot(rf,rf)),
nfev=int(fit.nfev),optimality=float(fit.optimality),
gradient_norm=float(np.linalg.norm(fit.grad)),
residual_dimension=len(rf),state_dimension=len(fit.x),
statistical_dof=dof,total_nis=float(np.dot(rf,rf)),
chi_square_per_dof=float(np.dot(rf,rf)/dof),
cost_per_dof=.5*float(np.dot(rf,rf)/dof),
initial_residual_by_factor=_factor_stats(initial_detail),
final_residual_by_factor=_factor_stats(final_detail),
final_position_residual_m=_vector_stats(final_detail.get('position_physical',[])),
final_velocity_residual_m_s=_vector_stats(final_detail.get('velocity_physical',[])),
max_bg_step_rad_s=float(max(bg_steps,default=0.)),
max_ba_step_m_s2=float(max(ba_steps,default=0.)),
preintegration_covariance_sigma={key:_distribution(value) for key,value in covariance.items()})
def _free_residual(problem,value,details=None):
return residual(problem,value[3:],details=details,lever_override=value[:3])
def _free_sparsity(problem,value):
local = jacobian_sparsity(problem,value[3:])
result = lil_matrix((local.shape[0],local.shape[1]+3),dtype=int)
result[:,:3] = 1
result[:,3:] = local
return result.tocsr()
def _marginal_lever_information(jacobian):
J = jacobian.toarray() if hasattr(jacobian,'toarray') else np.asarray(jacobian)
H = J.T@J
Hll,Hln,Hnn = H[:3,:3],H[:3,3:],H[3:,3:]
marginal = Hll-Hln@np.linalg.pinv(Hnn,rcond=1e-10)@Hln.T
return .5*(marginal+marginal.T)
def _additive_marginal_lever_information(jacobian,row_offsets,state_offsets):
total=np.zeros((3,3))
for index in range(len(row_offsets)-1):
rows=slice(row_offsets[index],row_offsets[index+1])
columns=np.r_[0:3,state_offsets[index]:state_offsets[index+1]]
local=jacobian[rows,:][:,columns]
total+=_marginal_lever_information(local)
return .5*(total+total.T)
def linearized_lever_information(problem,l_I_m):
lever=np.asarray(l_I_m,dtype=float)
value=np.concatenate([lever,initial_parameters(problem,lever)])
fit=least_squares(lambda x:_free_residual(problem,x),value,jac='2-point',
jac_sparsity=_free_sparsity(problem,value),method='trf',tr_solver='lsmr',
loss='linear',max_nfev=1,x_scale='jac')
information=_marginal_lever_information(fit.jac)
_,singular,Vt=np.linalg.svd(information)
covariance=np.linalg.pinv(information,rcond=1e-9)
return information,covariance,singular,Vt[-1]
def solve_free_lever(problem,initial_l_I_m,max_nfev=120):
initial_l = np.asarray(initial_l_I_m,dtype=float)
x0 = np.concatenate([initial_l,initial_parameters(problem,initial_l)])
r0 = _free_residual(problem,x0)
fit = least_squares(
lambda value:_free_residual(problem,value),x0,jac='2-point',
jac_sparsity=_free_sparsity(problem,x0),method='trf',tr_solver='lsmr',
loss='linear',max_nfev=max_nfev,x_scale='jac',
ftol=1e-6,xtol=1e-6,gtol=1e-6)
detail = {}
rf = _free_residual(problem,fit.x,detail)
information = _marginal_lever_information(fit.jac)
_,singular_values,Vt = np.linalg.svd(information)
tolerance = max(singular_values[0]*1e-9,1e-10)
rank = int(np.sum(singular_values>tolerance))
covariance = np.linalg.pinv(information,rcond=1e-9)
dof = max(_effective_residual_dimension(detail)-len(fit.x),1)
condition = (float(singular_values[0]/singular_values[-1])
if singular_values[-1]>tolerance else np.inf)
return FreeLeverResult(
success=bool(fit.success),message=str(fit.message),
initial_l_I_m=initial_l,final_l_I_m=fit.x[:3].copy(),
lever_step_norm_m=float(np.linalg.norm(fit.x[:3]-initial_l)),
initial_cost=.5*float(np.dot(r0,r0)),
final_cost=.5*float(np.dot(rf,rf)),nfev=int(fit.nfev),
optimality=float(fit.optimality),chi_square_per_dof=float(np.dot(rf,rf)/dof),
position_residual_m=_vector_stats(detail.get('position_physical',[])),
velocity_residual_m_s=_vector_stats(detail.get('velocity_physical',[])),
residual_by_factor=_factor_stats(detail),lever_covariance_m2=covariance,
lever_information_singular_values=singular_values,
lever_information_condition_number=condition,lever_precision_rank=rank,
weakest_lever_direction_I=Vt[-1].copy())
def solve_free_lever_many(problems,initial_l_I_m,max_nfev=120,
initial_state_values=None,lever_prior_mean_m=None,
lever_prior_covariance_m2=None,return_state_values=False):
problems=tuple(problems)
initial_l=np.asarray(initial_l_I_m,dtype=float)
sizes=[NODE_DOF*len(problem.segment.nodes) for problem in problems]
offsets=np.cumsum([3,*sizes])
states=([initial_parameters(problem,initial_l) for problem in problems]
if initial_state_values is None else
[np.asarray(value,dtype=float) for value in initial_state_values])
prior_mean=(None if lever_prior_mean_m is None else
np.asarray(lever_prior_mean_m,dtype=float))
prior_cov=(None if lever_prior_covariance_m2 is None else
np.asarray(lever_prior_covariance_m2,dtype=float))
prior_whitener=(None if prior_cov is None else
np.linalg.inv(np.linalg.cholesky(prior_cov)))
x0=np.concatenate([initial_l,*states])
def evaluate(value,details=None):
chunks=[]
for index,problem in enumerate(problems):
local={} if details is not None else None
chunks.append(residual(problem,value[offsets[index]:offsets[index+1]],
details=local,lever_override=value[:3]))
if details is not None:
for key,items in local.items(): details.setdefault(key,[]).extend(items)
if prior_whitener is not None:
prior_error=prior_whitener@(value[:3]-prior_mean)
chunks.append(prior_error)
if details is not None:
details.setdefault('lever_prior',[]).extend(prior_error.tolist())
return np.concatenate(chunks)
r0=evaluate(x0)
sparsity=lil_matrix((len(r0),len(x0)),dtype=int)
row=0; row_offsets=[0]
for index,problem in enumerate(problems):
local=jacobian_sparsity(problem,states[index])
sparsity[row:row+local.shape[0],:3]=1
sparsity[row:row+local.shape[0],offsets[index]:offsets[index+1]]=local
row+=local.shape[0]
row_offsets.append(row)
if prior_whitener is not None:
sparsity[row:row+3,:3]=1
fit=least_squares(
lambda value:evaluate(value),x0,jac='2-point',jac_sparsity=sparsity.tocsr(),
method='trf',tr_solver='lsmr',loss='linear',max_nfev=max_nfev,
x_scale='jac',ftol=1e-6,xtol=1e-6,gtol=1e-6)
detail={}
rf=evaluate(fit.x,detail)
information=_additive_marginal_lever_information(
fit.jac,row_offsets,offsets)
if prior_cov is not None:
information+=np.linalg.inv(prior_cov)
_,singular_values,Vt=np.linalg.svd(information)
tolerance=max(singular_values[0]*1e-9,1e-10)
rank=int(np.sum(singular_values>tolerance))
covariance=np.linalg.pinv(information,rcond=1e-9)
dof=max(_effective_residual_dimension(detail)-len(fit.x),1)
condition=(float(singular_values[0]/singular_values[-1])
if singular_values[-1]>tolerance else np.inf)
result=FreeLeverResult(
success=bool(fit.success),message=str(fit.message),
initial_l_I_m=initial_l,final_l_I_m=fit.x[:3].copy(),
lever_step_norm_m=float(np.linalg.norm(fit.x[:3]-initial_l)),
initial_cost=.5*float(np.dot(r0,r0)),final_cost=.5*float(np.dot(rf,rf)),
nfev=int(fit.nfev),optimality=float(fit.optimality),
chi_square_per_dof=float(np.dot(rf,rf)/dof),
position_residual_m=_vector_stats(detail.get('position_physical',[])),
velocity_residual_m_s=_vector_stats(detail.get('velocity_physical',[])),
residual_by_factor=_factor_stats(detail),lever_covariance_m2=covariance,
lever_information_singular_values=singular_values,
lever_information_condition_number=condition,lever_precision_rank=rank,
weakest_lever_direction_I=Vt[-1].copy())
if return_state_values:
states=[fit.x[offsets[i]:offsets[i+1]].copy()
for i in range(len(problems))]
return result,states
return result
rtk_imu/rtk_imu_replay.py
+184
View File
@@ -0,0 +1,184 @@
"""End-to-end orchestration and JSON reporting for RTK--IMU calibration."""
from __future__ import annotations
import csv
import json
from dataclasses import asdict, dataclass
from pathlib import Path
from typing import Any
import numpy as np
from imu_lidar.imu_io import load_imu_samples
from .rtk_imu_rotation import RotationCalibrationResult, RotationSession, solve_rtk_imu_rotation
from .rtk_imu_translation import TranslationCalibrationResult, solve_rtk_imu_translation
from .rtk_io import load_rtk_csv
DEFAULT_RTK_FRAME_DEFINITION = (
"right-handed vehicle-fixed frame: +X ANT1(main,left)->ANT2(secondary,right), "
"+Y vehicle forward/IMU +Y, +Z vehicle up/IMU +Z"
)
DEFAULT_RTK_REFERENCE_POINT = (
"GGA ANT1/main-antenna phase center, 1.916499878 m above ground"
)
@dataclass(frozen=True)
class InventoryEntry:
session_id: str
batch_id: str
imu_csv: Path
rtk_csv: Path
def load_inventory(path: Path | str) -> list[InventoryEntry]:
"""Load the project RTK inventory and derive each paired IMU path."""
source = Path(path)
entries: list[InventoryEntry] = []
with source.open("r", encoding="utf-8-sig", newline="") as handle:
for row in csv.DictReader(handle):
rtk_csv = Path(row["current_rtk_csv"])
imu_csv = rtk_csv.with_name("imu.csv")
entries.append(
InventoryEntry(
session_id=row["session"],
batch_id=row["batch"],
imu_csv=imu_csv,
rtk_csv=rtk_csv,
)
)
if not entries:
raise ValueError(f"empty RTK inventory: {source}")
return entries
def load_sessions(entries: list[InventoryEntry] | tuple[InventoryEntry, ...]) -> list[RotationSession]:
sessions = []
for entry in entries:
sessions.append(
RotationSession(
session_id=entry.session_id,
batch_id=entry.batch_id,
imu=load_imu_samples(entry.imu_csv),
rtk=load_rtk_csv(entry.rtk_csv),
)
)
return sessions
def _jsonable(value: Any) -> Any:
if isinstance(value, np.ndarray):
return value.tolist()
if isinstance(value, np.generic):
return value.item()
if isinstance(value, Path):
return str(value)
if hasattr(value, "__dataclass_fields__"):
return {key: _jsonable(item) for key, item in asdict(value).items()}
if isinstance(value, dict):
return {str(key): _jsonable(item) for key, item in value.items()}
if isinstance(value, (list, tuple)):
return [_jsonable(item) for item in value]
return value
def dataset_audit(sessions: list[RotationSession]) -> dict[str, Any]:
rows = []
for session in sessions:
rtk = session.rtk
valid_position = rtk.position_valid
valid_attitude = rtk.attitude_valid & valid_position
float_attitude = rtk.attitude_float & valid_position
rows.append(
{
"session_id": session.session_id,
"batch_id": session.batch_id,
"imu_samples": int(session.imu.t_s.size),
"rtk_samples": int(rtk.t_s.size),
"fixed_position_ratio": float(np.mean(valid_position)),
"fixed_attitude_ratio": float(np.mean(valid_attitude)),
"float_attitude_ratio": float(np.mean(float_attitude)),
"checksum_valid_ratio": float(np.mean(rtk.checksum_valid)),
"common_time_span_s": [
float(max(session.imu.t_s[0], rtk.t_s[0])),
float(min(session.imu.t_s[-1], rtk.t_s[-1])),
],
"origin_geodetic": list(rtk.origin_geodetic),
"imu_source": str(session.imu.t_s.size) + " normalized samples",
"rtk_source": str(rtk.source),
}
)
return {"session_count": len(sessions), "sessions": rows}
def run_calibration(
sessions: list[RotationSession],
output_directory: Path | str,
*,
rotation_only: bool = False,
compute_loo: bool = True,
knot_step_s: float = 2.0,
rtk_frame_definition: str = DEFAULT_RTK_FRAME_DEFINITION,
rtk_reference_point: str = DEFAULT_RTK_REFERENCE_POINT,
) -> tuple[RotationCalibrationResult, TranslationCalibrationResult | None]:
"""Run calibration and publish human-readable JSON artifacts."""
output = Path(output_directory)
output.mkdir(parents=True, exist_ok=True)
rotation = solve_rtk_imu_rotation(sessions, compute_loo=compute_loo)
translation = None
if not rotation_only and rotation.ok:
translation = solve_rtk_imu_translation(
sessions,
rotation,
knot_step_s=knot_step_s,
compute_loo=compute_loo,
)
audit_payload = dataset_audit(sessions)
rotation_payload = _jsonable(rotation)
translation_payload = None if translation is None else _jsonable(translation)
(output / "dataset_audit.json").write_text(
json.dumps(audit_payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
)
(output / "rotation_result.json").write_text(
json.dumps(rotation_payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
)
if translation_payload is not None:
(output / "translation_result.json").write_text(
json.dumps(translation_payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
)
interpretation_complete = bool(rtk_frame_definition.strip() and rtk_reference_point.strip())
accepted = bool(
rotation.ok and translation is not None and translation.ok and interpretation_complete
)
blockers = []
if not rotation.ok:
blockers.append('full RTK-to-IMU rotation is not observable from the lateral dual-antenna baseline')
if translation is None or not translation.ok:
blockers.append('translation is frozen until a full rotation is observable and accepted')
if not rtk_frame_definition.strip():
blockers.append('RTK frame_definition is empty')
if not rtk_reference_point.strip():
blockers.append('RTK reference_point is empty')
summary = {
"status": "accepted" if accepted else "diagnostic_not_accepted",
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
"rtk_frame_definition": rtk_frame_definition,
"rtk_reference_point": rtk_reference_point,
"interpretation_blockers": blockers,
"R_RTK_IMU": rotation.R_RTK_IMU.tolist(),
"t_RTK_IMU_m": None if translation is None else translation.t_RTK_IMU_m.tolist(),
"T_RTK_IMU": None if translation is None else translation.T_RTK_IMU.tolist(),
"rotation_ok": rotation.ok,
"translation_ok": None if translation is None else translation.ok,
"rotation_result": "rotation_result.json",
"translation_result": None if translation is None else "translation_result.json",
"dataset_audit": "dataset_audit.json",
}
(output / "summary.json").write_text(
json.dumps(summary, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
)
return rotation, translation
rtk_imu/rtk_imu_rotation.py
+688
View File
@@ -0,0 +1,688 @@
"""Rotation and residual time-offset calibration between G90 RTK and HI13 IMU."""
from __future__ import annotations
from dataclasses import dataclass, replace
import numpy as np
from scipy.optimize import least_squares
from scipy.sparse import lil_matrix
from imu_lidar.contracts import ImuSeries, MotionPair
from imu_lidar.geometry import orthonormalize_rotation, rpy_deg_xyz, so3_exp, so3_log
from imu_lidar.imu_preintegration import apply_bias_jacobian_correction, preintegrate_gyro
from imu_lidar.rotation_handeye import estimate_rotation_handeye_initial
from .rtk_attitude import GNHPR_CANDIDATES, GnhprConvention, gnhpr_to_rotation_enu_rtk
from .rtk_io import RtkSeries
@dataclass(frozen=True)
class RotationSession:
session_id: str
batch_id: str
imu: ImuSeries
rtk: RtkSeries
@dataclass(frozen=True)
class TimeOffsetAudit:
offset_s: float
peak_correlation: float
second_best_correlation: float
evaluated_samples: int
reliable: bool
method: str
peak_width_s: tuple[float, float]
per_session_offset_s: dict[str, float]
per_session_peak_correlation: dict[str, float]
@dataclass(frozen=True)
class BaselineConsistencyAudit:
"""Two-DOF audit using only the physically observed ANT1-to-ANT2 axis."""
baseline_axis_imu: np.ndarray
pair_count: int
residual_rms_deg: float
residual_median_deg: float
residual_p95_deg: float
per_session_rms_deg: dict[str, float]
per_session_p95_deg: dict[str, float]
per_session_axis_rms_deg: dict[str, np.ndarray]
gyro_bias_by_session_rad_s: dict[str, np.ndarray]
worst_pairs: tuple[dict[str, object], ...]
ok: bool
notes: tuple[str, ...]
@dataclass(frozen=True)
class RotationCalibrationResult:
R_RTK_IMU: np.ndarray
rpy_deg: np.ndarray
gyro_bias_by_session_rad_s: dict[str, np.ndarray]
time_offset: TimeOffsetAudit
applied_time_offset_s: float
convention: GnhprConvention
convention_scores_deg: dict[str, float]
pair_count: int
residual_rms_deg: float
residual_median_deg: float
residual_p95_deg: float
rotation_std_deg: np.ndarray
information_singular_values: np.ndarray
per_session_rms_deg: dict[str, float]
loo_delta_deg: dict[str, float]
baseline_consistency: BaselineConsistencyAudit
observable_rotation_dof: int
full_attitude_observable: bool
legacy_full_attitude_numeric_ok: bool
ok: bool
notes: tuple[str, ...]
@dataclass(frozen=True)
class _Pair:
session_index: int
session_id: str
R_A: np.ndarray
delta_R_zero_bias: np.ndarray
J_bg: np.ndarray
weight: float
t0_s: float
t1_s: float
def _attitude_rows(rtk: RtkSeries, convention: GnhprConvention) -> tuple[np.ndarray, np.ndarray]:
valid = rtk.attitude_valid & rtk.position_valid
t = rtk.attitude_t_s[valid]
angles = np.column_stack(
[rtk.heading_deg[valid], rtk.pitch_deg[valid], rtk.roll_deg[valid]]
)
if t.size < 2:
raise ValueError(f"not enough valid RTK attitude rows: {rtk.source}")
# GGA is faster than HPR, so nearest-neighbour export repeats attitude rows.
# Keep only changes and place them at the first associated GGA measurement.
changed = np.ones(t.size, dtype=bool)
changed[1:] = np.any(np.abs(np.diff(angles, axis=0)) > 1e-10, axis=1)
t = t[changed]
angles = angles[changed]
order = np.argsort(t)
t = t[order]
angles = angles[order]
unique_t, unique_indices = np.unique(t, return_index=True)
rotations = gnhpr_to_rotation_enu_rtk(
angles[unique_indices, 0],
angles[unique_indices, 1],
angles[unique_indices, 2],
convention,
)
return unique_t, rotations
def _rtk_heading_rate(t: np.ndarray, rotations: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
"""Return signed vehicle yaw rate from the ANT1-to-ANT2 azimuth.
ANT1-to-ANT2 points vehicle-right, so its clockwise heading increases when
mathematical body yaw decreases. Only this signed heading channel is
compared with IMU gyro_z; rotation about the baseline is unobservable.
"""
dt = np.diff(t)
baseline = rotations[:, :, 0]
heading = np.unwrap(np.arctan2(baseline[:, 0], baseline[:, 1]))
rate = -np.diff(heading) / np.maximum(dt, 1e-6)
valid = (
(dt >= 0.03)
& (dt <= 0.25)
& (np.abs(rate) >= np.deg2rad(0.5))
& (np.abs(rate) <= np.deg2rad(30.0))
)
return 0.5 * (t[:-1] + t[1:])[valid], rate[valid]
def _correlation(a: np.ndarray, b: np.ndarray) -> float:
a = np.asarray(a, dtype=float)
b = np.asarray(b, dtype=float)
if a.size < 20 or np.std(a) < 1e-5 or np.std(b) < 1e-5:
return np.nan
return float(np.corrcoef(a, b)[0, 1])
def audit_time_offset(
sessions: list[RotationSession] | tuple[RotationSession, ...],
*,
search_half_width_s: float = 0.30,
step_s: float = 0.005,
) -> TimeOffsetAudit:
"""Audit residual t_IMU - t_RTK from signed heading rate.
A broad correlation peak remains diagnostic. It is never applied unless
both peak separation and peak width pass.
"""
offsets = np.arange(-search_half_width_s, search_half_width_s + 0.5 * step_s, step_s)
session_series: list[tuple[str, np.ndarray, np.ndarray, np.ndarray, np.ndarray]] = []
total_samples = 0
for session in sessions:
t_rtk, rotations = _attitude_rows(session.rtk, GNHPR_CANDIDATES[0])
midpoint, rtk_rate = _rtk_heading_rate(t_rtk, rotations)
imu_rate = session.imu.gyro_rad_s[:, 2]
if midpoint.size >= 20:
session_series.append(
(session.session_id, midpoint, rtk_rate, session.imu.t_s, imu_rate)
)
total_samples += int(midpoint.size)
if not session_series:
return TimeOffsetAudit(
0.0,
np.nan,
np.nan,
0,
False,
"signed_heading_rate_vs_imu_gyro_z",
(np.nan, np.nan),
{},
{},
)
scores = []
per_session_scores: dict[str, list[float]] = {
session_id: [] for session_id, *_ in session_series
}
for offset in offsets:
per_session = []
for session_id, midpoint, rtk_rate, imu_t, imu_rate in session_series:
query = midpoint + offset
inside = (query >= imu_t[0]) & (query <= imu_t[-1])
if np.count_nonzero(inside) < 20:
per_session_scores[session_id].append(np.nan)
continue
interpolated = np.interp(query[inside], imu_t, imu_rate)
value = _correlation(rtk_rate[inside], interpolated)
per_session_scores[session_id].append(value)
if np.isfinite(value):
per_session.append(value)
scores.append(float(np.median(per_session)) if per_session else np.nan)
values = np.asarray(scores, dtype=float)
if not np.any(np.isfinite(values)):
return TimeOffsetAudit(
0.0,
np.nan,
np.nan,
total_samples,
False,
"signed_heading_rate_vs_imu_gyro_z",
(np.nan, np.nan),
{},
{},
)
best_index = int(np.nanargmax(values))
exclusion = np.abs(offsets - offsets[best_index]) >= 0.03
second = float(np.nanmax(values[exclusion])) if np.any(np.isfinite(values[exclusion])) else np.nan
peak = float(values[best_index])
near_peak = np.flatnonzero(values >= peak - 0.005)
peak_width = (
(float(offsets[near_peak[0]]), float(offsets[near_peak[-1]]))
if near_peak.size
else (np.nan, np.nan)
)
per_session_offset = {}
per_session_peak = {}
for session_id, session_values in per_session_scores.items():
array = np.asarray(session_values, dtype=float)
if np.any(np.isfinite(array)):
index = int(np.nanargmax(array))
per_session_offset[session_id] = float(offsets[index])
per_session_peak[session_id] = float(array[index])
reliable = bool(
peak >= 0.5
and (not np.isfinite(second) or peak - second >= 0.015)
and np.isfinite(peak_width[0])
and peak_width[1] - peak_width[0] <= 0.03
)
return TimeOffsetAudit(
float(offsets[best_index]),
peak,
second,
total_samples,
reliable,
"signed_heading_rate_vs_imu_gyro_z",
peak_width,
per_session_offset,
per_session_peak,
)
def _nearest_index(times: np.ndarray, target: float) -> int:
index = int(np.searchsorted(times, target))
candidates = [max(0, index - 1), min(times.size - 1, index)]
return min(candidates, key=lambda item: abs(float(times[item]) - target))
def _make_pairs(
sessions: list[RotationSession],
convention: GnhprConvention,
time_offset_s: float,
*,
anchor_step_s: float = 5.0,
intervals_s: tuple[float, ...] = (0.75, 1.5, 3.0),
preintegration_cache: dict[tuple[str, float, float], object] | None = None,
) -> list[_Pair]:
pairs: list[_Pair] = []
cache = {} if preintegration_cache is None else preintegration_cache
for session_index, session in enumerate(sessions):
t, rotations = _attitude_rows(session.rtk, convention)
dt = np.diff(t)
baseline = rotations[:, :, 0]
baseline_step = np.arccos(
np.clip(np.sum(baseline[:-1] * baseline[1:], axis=1), -1.0, 1.0)
)
broken_edge = (
(dt < 0.03)
| (dt > 0.25)
| (baseline_step / np.maximum(dt, 1e-6) > np.deg2rad(45.0))
)
broken_prefix = np.concatenate([[0], np.cumsum(broken_edge.astype(int))])
next_anchor = float(t[0])
for i in range(t.size - 1):
if t[i] + 1e-9 < next_anchor:
continue
next_anchor = float(t[i] + anchor_step_s)
for duration in intervals_s:
j = _nearest_index(t, float(t[i] + duration))
if j <= i or abs(float(t[j] - t[i]) - duration) > 0.18:
continue
if broken_prefix[j] - broken_prefix[i] != 0:
continue
imu_t0 = float(t[i] + time_offset_s)
imu_t1 = float(t[j] + time_offset_s)
if imu_t0 < session.imu.t_s[0] or imu_t1 > session.imu.t_s[-1]:
continue
r_a = orthonormalize_rotation(rotations[i].T @ rotations[j])
cache_key = (session.session_id, round(imu_t0, 6), round(imu_t1, 6))
preint = cache.get(cache_key)
if preint is None:
preint = preintegrate_gyro(
session.imu.t_s,
session.imu.gyro_rad_s,
imu_t0,
imu_t1,
)
cache[cache_key] = preint
angle_a = np.linalg.norm(so3_log(r_a))
angle_b = np.linalg.norm(so3_log(preint.delta_R))
if min(angle_a, angle_b) < np.deg2rad(0.8):
continue
weight = float(np.clip(min(angle_a, angle_b) / np.deg2rad(5.0), 0.2, 3.0))
pairs.append(
_Pair(
session_index=session_index,
session_id=session.session_id,
R_A=r_a,
delta_R_zero_bias=preint.delta_R,
J_bg=preint.J_bg,
weight=weight,
t0_s=float(t[i]),
t1_s=float(t[j]),
)
)
# Equalize total influence per session. Pair count and excitation otherwise
# let long/high-motion sessions dominate the shared rotation.
totals = {
session.session_id: sum(
pair.weight for pair in pairs if pair.session_id == session.session_id
)
for session in sessions
}
nonzero = [value for value in totals.values() if value > 0.0]
target = float(np.mean(nonzero)) if nonzero else 1.0
return [
replace(pair, weight=pair.weight * target / totals[pair.session_id])
for pair in pairs
if totals[pair.session_id] > 0.0
]
def _solve_core(sessions: list[RotationSession], pairs: list[_Pair]) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
if len(pairs) < 6:
raise ValueError("need at least 6 excited RTK--IMU rotation pairs")
generic = [
MotionPair(
session_id=pair.session_id,
i=index,
j=index + 1,
t_i_s=0.0,
t_j_s=1.0,
R_A=pair.R_A,
R_B=pair.delta_R_zero_bias,
metadata={"weight": pair.weight},
)
for index, pair in enumerate(pairs)
]
r0 = estimate_rotation_handeye_initial(generic, min_rotation_deg=0.5)
session_count = len(sessions)
def unpack(parameters: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
return orthonormalize_rotation(so3_exp(parameters[:3])), parameters[3:].reshape(session_count, 3)
def residual(parameters: np.ndarray) -> np.ndarray:
r_x, biases = unpack(parameters)
rows = []
for pair in pairs:
corrected = apply_bias_jacobian_correction(
pair.delta_R_zero_bias,
pair.J_bg,
biases[pair.session_index],
)
error = so3_log(r_x.T @ pair.R_A @ r_x @ corrected.T)
rows.append(np.sqrt(pair.weight) * error)
# HI13 bias is session-specific; this weak prior only removes degenerate
# bias/extrinsic trades and is much looser than observed static bias.
rows.append((biases / 0.03).reshape(-1))
return np.concatenate(rows)
initial = np.concatenate([so3_log(r0), np.zeros(3 * session_count)])
jacobian_pattern = lil_matrix((3 * len(pairs) + 3 * session_count, initial.size), dtype=int)
for pair_index, pair in enumerate(pairs):
row = 3 * pair_index
jacobian_pattern[row : row + 3, 0:3] = 1
bias_col = 3 + 3 * pair.session_index
jacobian_pattern[row : row + 3, bias_col : bias_col + 3] = 1
prior_row = 3 * len(pairs)
jacobian_pattern[prior_row:, 3:] = 1
opt = least_squares(
residual,
initial,
loss="huber",
f_scale=np.deg2rad(0.5),
jac_sparsity=jacobian_pattern.tocsr(),
tr_solver='lsmr',
max_nfev=40,
)
r_x, biases = unpack(opt.x)
errors = []
for pair in pairs:
corrected = apply_bias_jacobian_correction(
pair.delta_R_zero_bias,
pair.J_bg,
biases[pair.session_index],
)
errors.append(np.degrees(np.linalg.norm(so3_log(r_x.T @ pair.R_A @ r_x @ corrected.T))))
jacobian = opt.jac.toarray() if hasattr(opt.jac, 'toarray') else np.asarray(opt.jac, dtype=float)
information = jacobian.T @ jacobian
dof = max(residual(opt.x).size - opt.x.size, 1)
variance = float(np.sum(residual(opt.x) ** 2) / dof)
covariance = np.linalg.pinv(information, rcond=1e-10) * variance
return r_x, biases, np.asarray(errors), covariance
def _solve_baseline_consistency(
sessions: list[RotationSession],
pairs: list[_Pair],
) -> BaselineConsistencyAudit:
"""Audit the two physically observable dual-antenna rotation DOFs.
The confirmed ANT1-to-ANT2 axis is IMU +X. For every interval, the angle
swept by the GNSS baseline must equal the angle swept by IMU +X under gyro
preintegration. Rotation about +X cancels from this invariant and is not
falsely scored as an RTK attitude residual.
"""
baseline_axis = np.array([1.0, 0.0, 0.0])
session_count = len(sessions)
def raw_errors(parameters: np.ndarray) -> np.ndarray:
biases = parameters.reshape(session_count, 3)
values = []
for pair in pairs:
corrected = apply_bias_jacobian_correction(
pair.delta_R_zero_bias,
pair.J_bg,
biases[pair.session_index],
)
observed = np.arccos(np.clip(pair.R_A[0, 0], -1.0, 1.0))
predicted = np.arccos(
np.clip(baseline_axis @ corrected @ baseline_axis, -1.0, 1.0)
)
values.append(predicted - observed)
return np.asarray(values)
def residual(parameters: np.ndarray) -> np.ndarray:
errors = raw_errors(parameters)
weighted = errors * np.sqrt(np.asarray([pair.weight for pair in pairs]))
return np.concatenate([weighted, parameters / 0.003])
initial = np.zeros(3 * session_count)
opt = least_squares(
residual,
initial,
loss="huber",
f_scale=np.deg2rad(0.25),
max_nfev=60,
)
biases = opt.x.reshape(session_count, 3)
errors_deg = np.degrees(raw_errors(opt.x))
per_session_rms = {}
per_session_p95 = {}
per_session_axis_rms = {}
for session in sessions:
selection = np.asarray(
[pair.session_id == session.session_id for pair in pairs], dtype=bool
)
values = errors_deg[selection]
per_session_rms[session.session_id] = (
float(np.sqrt(np.mean(values**2))) if values.size else np.nan
)
per_session_p95[session.session_id] = (
float(np.percentile(np.abs(values), 95.0)) if values.size else np.nan
)
axis_errors = []
for pair in np.asarray(pairs, dtype=object)[selection]:
corrected = apply_bias_jacobian_correction(
pair.delta_R_zero_bias,
pair.J_bg,
biases[pair.session_index],
)
axis_errors.append(np.degrees(so3_log(pair.R_A @ corrected.T)))
per_session_axis_rms[session.session_id] = (
np.sqrt(np.mean(np.asarray(axis_errors) ** 2, axis=0))
if axis_errors
else np.full(3, np.nan)
)
worst_indices = np.argsort(np.abs(errors_deg))[-20:][::-1]
worst_pairs = tuple(
{
"session_id": pairs[index].session_id,
"t0_s": pairs[index].t0_s,
"t1_s": pairs[index].t1_s,
"duration_s": pairs[index].t1_s - pairs[index].t0_s,
"baseline_angle_residual_deg": float(errors_deg[index]),
}
for index in worst_indices
)
rms = float(np.sqrt(np.mean(errors_deg**2)))
median = float(np.median(np.abs(errors_deg)))
p95 = float(np.percentile(np.abs(errors_deg), 95.0))
finite_session_rms = [
value for value in per_session_rms.values() if np.isfinite(value)
]
finite_session_p95 = [
value for value in per_session_p95.values() if np.isfinite(value)
]
ok = bool(
len(pairs) >= 20
and rms <= 1.0
and p95 <= 2.0
and (not finite_session_rms or max(finite_session_rms) <= 1.5)
and (not finite_session_p95 or max(finite_session_p95) <= 3.0)
)
return BaselineConsistencyAudit(
baseline_axis_imu=baseline_axis,
pair_count=len(pairs),
residual_rms_deg=rms,
residual_median_deg=median,
residual_p95_deg=p95,
per_session_rms_deg=per_session_rms,
per_session_p95_deg=per_session_p95,
per_session_axis_rms_deg=per_session_axis_rms,
gyro_bias_by_session_rad_s={
session.session_id: biases[index].copy()
for index, session in enumerate(sessions)
},
worst_pairs=worst_pairs,
ok=ok,
notes=(
"ANT1(main,left)->ANT2(secondary,right) is fixed to IMU +X",
"axis residual XYZ labels are baseline-spin(unobservable), baseline-elevation, heading",
"full rotation about the baseline is not identifiable from two antennas",
),
)
def solve_rtk_imu_rotation(
sessions: list[RotationSession] | tuple[RotationSession, ...],
*,
compute_loo: bool = True,
) -> RotationCalibrationResult:
"""Solve shared ``R_RTK_IMU`` and per-session gyro biases."""
items = list(sessions)
if not items:
raise ValueError("at least one RTK--IMU session is required")
time_audit = audit_time_offset(items)
offset = time_audit.offset_s if time_audit.reliable else 0.0
candidates: list[tuple[GnhprConvention, list[_Pair]]] = []
scores: dict[str, float] = {}
preintegration_cache: dict[tuple[str, float, float], object] = {}
for convention in GNHPR_CANDIDATES:
pairs = _make_pairs(items, convention, offset, preintegration_cache=preintegration_cache)
if len(pairs) < 6:
scores[convention.name] = 1e9
continue
generic = [
MotionPair(
session_id=pair.session_id,
i=index,
j=index + 1,
t_i_s=0.0,
t_j_s=1.0,
R_A=pair.R_A,
R_B=pair.delta_R_zero_bias,
metadata={'weight': pair.weight},
)
for index, pair in enumerate(pairs)
]
initial_rotation = estimate_rotation_handeye_initial(generic, min_rotation_deg=0.5)
preliminary_errors = np.asarray(
[
np.degrees(
np.linalg.norm(
so3_log(
initial_rotation.T
@ pair.R_A
@ initial_rotation
@ pair.delta_R_zero_bias.T
)
)
)
for pair in pairs
]
)
scores[convention.name] = float(np.sqrt(np.mean(preliminary_errors**2)))
candidates.append((convention, pairs))
if not candidates:
raise ValueError("no GNHPR convention produced enough rotation pairs")
convention, pairs = min(candidates, key=lambda item: scores[item[0].name])
rotation, biases, errors, covariance = _solve_core(items, pairs)
per_session = {}
for session in items:
values = [error for pair, error in zip(pairs, errors) if pair.session_id == session.session_id]
per_session[session.session_id] = (
float(np.sqrt(np.mean(np.asarray(values) ** 2))) if values else np.nan
)
baseline_audit = _solve_baseline_consistency(items, pairs)
loo = {}
if compute_loo and len(items) >= 3:
for omitted in items:
kept_items = [item for item in items if item.session_id != omitted.session_id]
kept_index = {item.session_id: index for index, item in enumerate(kept_items)}
kept_pairs = [
replace(pair, session_index=kept_index[pair.session_id])
for pair in pairs
if pair.session_id != omitted.session_id
]
if len(kept_pairs) < 6:
loo[omitted.session_id] = np.nan
continue
try:
loo_rotation, _, _, _ = _solve_core(kept_items, kept_pairs)
except ValueError:
loo[omitted.session_id] = np.nan
continue
loo[omitted.session_id] = float(
np.degrees(np.linalg.norm(so3_log(rotation.T @ loo_rotation)))
)
rotation_cov = covariance[:3, :3]
std_deg = np.degrees(np.sqrt(np.maximum(np.diag(rotation_cov), 0.0)))
singular_values = np.linalg.svd(np.linalg.pinv(rotation_cov, rcond=1e-12), compute_uv=False)
rms = float(np.sqrt(np.mean(errors**2)))
median = float(np.median(errors))
p95 = float(np.percentile(errors, 95.0))
finite_loo = [value for value in loo.values() if np.isfinite(value)]
sorted_scores = sorted(scores.values())
convention_gap = sorted_scores[1] - sorted_scores[0] if len(sorted_scores) > 1 else np.inf
legacy_numeric_ok = bool(
len(pairs) >= 20
and rms <= 1.0
and p95 <= 2.0
and float(np.max(std_deg)) <= 0.5
and (not finite_loo or max(finite_loo) <= 1.0)
and convention_gap >= 0.05
)
# GNHPR supplies the ANT1-to-ANT2 direction but no independent rotation
# about that direction. A completed 3-D attitude is useful diagnostically,
# but cannot pass the full extrinsic-rotation gate from this dataset alone.
full_attitude_observable = False
ok = False
notes = [
"transform convention: p_RTK = R_RTK_IMU p_IMU",
f"residual time convention: t_IMU = t_RTK + {offset:+.6f} s",
f"GNHPR convention score gap={convention_gap:.4f} deg",
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
"LOO re-optimizes the remaining per-session gyro biases",
"legacy full-HPR rotation uses a zero-roll gauge completion and is diagnostic only",
"dual antennas do not observe rotation about the ANT1-to-ANT2 baseline",
]
if not time_audit.reliable:
notes.append("time-offset correlation was ambiguous; held residual offset at zero")
if convention is not GNHPR_CANDIDATES[0]:
notes.append("empirical best GNHPR convention differs from protocol expectation; manual verification required")
if not baseline_audit.ok:
notes.append("the physically observable baseline consistency failed strict gates")
if not legacy_numeric_ok:
notes.append("the legacy gauge-completed rotation failed one or more numeric gates")
notes.append("full rotation is not accepted; translation must remain frozen")
return RotationCalibrationResult(
R_RTK_IMU=rotation,
rpy_deg=rpy_deg_xyz(rotation),
gyro_bias_by_session_rad_s={
session.session_id: biases[index].copy() for index, session in enumerate(items)
},
time_offset=time_audit,
applied_time_offset_s=offset,
convention=convention,
convention_scores_deg=scores,
pair_count=len(pairs),
residual_rms_deg=rms,
residual_median_deg=median,
residual_p95_deg=p95,
rotation_std_deg=std_deg,
information_singular_values=singular_values,
per_session_rms_deg=per_session,
loo_delta_deg=loo,
baseline_consistency=baseline_audit,
observable_rotation_dof=2,
full_attitude_observable=full_attitude_observable,
legacy_full_attitude_numeric_ok=legacy_numeric_ok,
ok=ok,
notes=tuple(notes),
)
rtk_imu/rtk_imu_translation.py
+389
View File
@@ -0,0 +1,389 @@
"""Lever-arm calibration from RTK positions and full IMU preintegration."""
from __future__ import annotations
from dataclasses import dataclass
import numpy as np
from scipy.sparse import coo_matrix, csr_matrix, eye
from scipy.sparse.linalg import lsqr, splu
from scipy.spatial.transform import Rotation, Slerp
from imu_lidar.geometry import make_transform, orthonormalize_rotation, so3_exp
from .rtk_imu_rotation import RotationCalibrationResult, RotationSession, _attitude_rows
@dataclass(frozen=True)
class TranslationCalibrationResult:
lever_IMU_to_RTK_in_IMU_m: np.ndarray
t_RTK_IMU_m: np.ndarray
T_RTK_IMU: np.ndarray
translation_std_m: np.ndarray
lever_information_singular_values: np.ndarray
lever_precision_rank: int
position_residual_rms_xyz_m: np.ndarray
velocity_residual_rms_xyz_m_s: np.ndarray
accel_bias_by_session_m_s2: dict[str, np.ndarray]
knot_count_by_session: dict[str, int]
loo_delta_m: dict[str, np.ndarray]
ok: bool
notes: tuple[str, ...]
@dataclass(frozen=True)
class _SessionFactors:
session: RotationSession
knot_t_s: np.ndarray
position_enu_m: np.ndarray
R_ENU_IMU: np.ndarray
delta_p: tuple[np.ndarray, ...]
delta_v: tuple[np.ndarray, ...]
J_p_ba: tuple[np.ndarray, ...]
J_v_ba: tuple[np.ndarray, ...]
duration_s: np.ndarray
def _preintegrate_translation_interval(
times_s: np.ndarray,
gyro_rad_s: np.ndarray,
acc_m_s2: np.ndarray,
t0: float,
t1: float,
gyro_bias_rad_s: np.ndarray,
) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, float]:
"""Fast nominal ``delta_p/delta_v`` and accel-bias Jacobians.
Rotation covariance and gyro-bias Jacobians are deliberately omitted here:
rotation and gyro bias have already been fixed by Phase R1, while the
translation linear system only consumes the accelerometer-bias Jacobians.
"""
left = max(int(np.searchsorted(times_s, t0, side='left') - 1), 0)
right = min(int(np.searchsorted(times_s, t1, side='right')), times_s.size - 1)
delta_r = np.eye(3)
delta_v = np.zeros(3)
delta_p = np.zeros(3)
j_v_ba = np.zeros((3, 3))
j_p_ba = np.zeros((3, 3))
for index in range(left, right):
sample_t0 = float(times_s[index])
sample_t1 = float(times_s[index + 1])
if sample_t1 <= t0 or sample_t0 >= t1:
continue
segment_t0 = max(sample_t0, t0)
segment_t1 = min(sample_t1, t1)
dt = segment_t1 - segment_t0
if dt <= 0.0:
continue
sample_dt = max(sample_t1 - sample_t0, 1e-12)
u0 = (segment_t0 - sample_t0) / sample_dt
u1 = (segment_t1 - sample_t0) / sample_dt
gyro0 = (1.0 - u0) * gyro_rad_s[index] + u0 * gyro_rad_s[index + 1]
gyro1 = (1.0 - u1) * gyro_rad_s[index] + u1 * gyro_rad_s[index + 1]
acc0 = (1.0 - u0) * acc_m_s2[index] + u0 * acc_m_s2[index + 1]
acc1 = (1.0 - u1) * acc_m_s2[index] + u1 * acc_m_s2[index + 1]
omega = 0.5 * (gyro0 + gyro1) - gyro_bias_rad_s
acc = 0.5 * (acc0 + acc1)
r_i = delta_r
delta_p = delta_p + delta_v * dt + 0.5 * r_i @ acc * dt**2
delta_v = delta_v + r_i @ acc * dt
j_p_ba = j_p_ba + j_v_ba * dt - 0.5 * r_i * dt**2
j_v_ba = j_v_ba - r_i * dt
delta_r = orthonormalize_rotation(delta_r @ so3_exp(omega * dt))
return delta_p, delta_v, j_p_ba, j_v_ba, float(max(t1 - t0, 0.0))
def _make_session_factors(
session: RotationSession,
rotation: RotationCalibrationResult,
*,
knot_step_s: float,
) -> _SessionFactors:
t_attitude, r_enu_rtk = _attitude_rows(session.rtk, rotation.convention)
position_valid = session.rtk.position_valid
t_position = session.rtk.t_s[position_valid]
position = session.rtk.position_enu_m[position_valid]
time_offset_s = rotation.applied_time_offset_s
start = max(float(t_attitude[0]), float(t_position[0]), float(session.imu.t_s[0] - time_offset_s))
end = min(float(t_attitude[-1]), float(t_position[-1]), float(session.imu.t_s[-1] - time_offset_s))
if end - start < 5.0:
raise ValueError(f"{session.session_id}: less than 5 s common RTK/IMU support")
knot_t = np.arange(start + 0.25, end - 0.25, knot_step_s)
if knot_t.size < 4:
raise ValueError(f"{session.session_id}: not enough translation knots")
position_knots = np.column_stack(
[np.interp(knot_t, t_position, position[:, axis]) for axis in range(3)]
)
r_enu_rtk_knots = Slerp(t_attitude, Rotation.from_matrix(r_enu_rtk))(knot_t).as_matrix()
r_enu_imu = r_enu_rtk_knots @ rotation.R_RTK_IMU
bg = rotation.gyro_bias_by_session_rad_s[session.session_id]
delta_p: list[np.ndarray] = []
delta_v: list[np.ndarray] = []
j_p_ba: list[np.ndarray] = []
j_v_ba: list[np.ndarray] = []
durations = []
for t0, t1 in zip(knot_t[:-1], knot_t[1:]):
dp, dv, jp, jv, duration = _preintegrate_translation_interval(
session.imu.t_s,
session.imu.gyro_rad_s,
session.imu.acc_m_s2,
float(t0 + time_offset_s),
float(t1 + time_offset_s),
bg,
)
delta_p.append(dp)
delta_v.append(dv)
j_p_ba.append(jp)
j_v_ba.append(jv)
durations.append(duration)
return _SessionFactors(
session=session,
knot_t_s=knot_t,
position_enu_m=position_knots,
R_ENU_IMU=r_enu_imu,
delta_p=tuple(delta_p),
delta_v=tuple(delta_v),
J_p_ba=tuple(j_p_ba),
J_v_ba=tuple(j_v_ba),
duration_s=np.asarray(durations),
)
def _append_block(
rows: list[int],
cols: list[int],
values: list[float],
rhs: list[float],
groups: list[int],
matrix_blocks: list[tuple[int, np.ndarray]],
vector: np.ndarray,
sigma: np.ndarray,
group: int,
) -> None:
row0 = len(rhs)
for axis in range(3):
rhs.append(float(vector[axis] / sigma[axis]))
groups.append(group)
for col0, block in matrix_blocks:
for local_col in range(block.shape[1]):
value = float(block[axis, local_col] / sigma[axis])
if value != 0.0:
rows.append(row0 + axis)
cols.append(col0 + local_col)
values.append(value)
def _build_system(
factors: list[_SessionFactors],
*,
position_sigma_xyz_m: np.ndarray,
velocity_sigma_xyz_m_s: np.ndarray,
) -> tuple[csr_matrix, np.ndarray, np.ndarray, dict[str, tuple[int, int]], list[tuple[str, int, str]]]:
# x = [shared lever(3), per-session ba(3), per-knot velocities(3*K)]
offsets: dict[str, tuple[int, int]] = {}
variable_count = 3
for item in factors:
ba_offset = variable_count
velocity_offset = ba_offset + 3
offsets[item.session.session_id] = (ba_offset, velocity_offset)
variable_count = velocity_offset + 3 * item.knot_t_s.size
rows: list[int] = []
cols: list[int] = []
values: list[float] = []
rhs: list[float] = []
groups: list[int] = []
factor_labels: list[tuple[str, int, str]] = []
gravity = np.array([0.0, 0.0, -9.80665])
group = 0
for item in factors:
ba_offset, velocity_offset = offsets[item.session.session_id]
for index, dt in enumerate(item.duration_s):
r_i = item.R_ENU_IMU[index]
r_j = item.R_ENU_IMU[index + 1]
dp_rtk = item.position_enu_m[index + 1] - item.position_enu_m[index]
constant_p = dp_rtk - 0.5 * gravity * dt**2 - r_i @ item.delta_p[index]
_append_block(
rows,
cols,
values,
rhs,
groups,
[
(0, r_i - r_j),
(ba_offset, -r_i @ item.J_p_ba[index]),
(velocity_offset + 3 * index, -dt * np.eye(3)),
],
-constant_p,
position_sigma_xyz_m,
group,
)
factor_labels.append((item.session.session_id, group, "position"))
group += 1
constant_v = -gravity * dt - r_i @ item.delta_v[index]
_append_block(
rows,
cols,
values,
rhs,
groups,
[
(ba_offset, -r_i @ item.J_v_ba[index]),
(velocity_offset + 3 * index, -np.eye(3)),
(velocity_offset + 3 * (index + 1), np.eye(3)),
],
-constant_v,
velocity_sigma_xyz_m_s,
group,
)
factor_labels.append((item.session.session_id, group, "velocity"))
group += 1
# Loose physical bias prior. It prevents an unobservable constant
# acceleration from masquerading as gravity while remaining data-led.
_append_block(
rows,
cols,
values,
rhs,
groups,
[(ba_offset, np.eye(3))],
np.zeros(3),
np.full(3, 0.5),
group,
)
factor_labels.append((item.session.session_id, group, "bias_prior"))
group += 1
matrix = coo_matrix((values, (rows, cols)), shape=(len(rhs), variable_count)).tocsr()
return matrix, np.asarray(rhs), np.asarray(groups), offsets, factor_labels
def _irls(matrix: csr_matrix, rhs: np.ndarray, groups: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
row_weights = np.ones(rhs.size)
solution = np.zeros(matrix.shape[1])
for _ in range(5):
weighted = matrix.multiply(row_weights[:, None])
solution = lsqr(weighted, rhs * row_weights, atol=1e-10, btol=1e-10, iter_lim=3000)[0]
residual = matrix @ solution - rhs
new_weights = np.ones_like(row_weights)
for group in np.unique(groups):
selection = groups == group
norm = float(np.linalg.norm(residual[selection]))
if norm > 3.0:
new_weights[selection] = np.sqrt(3.0 / norm)
if np.max(np.abs(new_weights - row_weights)) < 1e-3:
row_weights = new_weights
break
row_weights = new_weights
return solution, row_weights
def _solve_factors(
factors: list[_SessionFactors],
position_sigma: np.ndarray,
velocity_sigma: np.ndarray,
) -> tuple[np.ndarray, np.ndarray, csr_matrix, np.ndarray, dict[str, tuple[int, int]], np.ndarray, np.ndarray]:
matrix, rhs, groups, offsets, labels = _build_system(
factors,
position_sigma_xyz_m=position_sigma,
velocity_sigma_xyz_m_s=velocity_sigma,
)
solution, row_weights = _irls(matrix, rhs, groups)
weighted = matrix.multiply(row_weights[:, None]).tocsr()
residual = matrix @ solution - rhs
data_groups = {group for _, group, kind in labels if kind != "bias_prior"}
data_rows = np.isin(groups, list(data_groups))
variance = float(np.sum((residual[data_rows] * row_weights[data_rows]) ** 2) / max(np.count_nonzero(data_rows) - solution.size, 1))
information = (weighted.T @ weighted).tocsc() + eye(weighted.shape[1], format="csc") * 1e-10
h_ll = information[:3, :3].toarray()
h_ln = information[:3, 3:]
h_nn = information[3:, 3:]
nuisance_solve = splu(h_nn).solve(h_ln.T.toarray())
schur = h_ll - h_ln.toarray() @ nuisance_solve
covariance_lever = np.linalg.pinv(schur, rcond=1e-10) * variance
return solution, covariance_lever, matrix, rhs, offsets, groups, residual
def solve_rtk_imu_translation(
sessions: list[RotationSession] | tuple[RotationSession, ...],
rotation: RotationCalibrationResult,
*,
knot_step_s: float = 2.0,
compute_loo: bool = True,
) -> TranslationCalibrationResult:
"""Estimate the shared IMU-to-RTK lever arm and return ``T_RTK_IMU``."""
items = list(sessions)
factors = [_make_session_factors(session, rotation, knot_step_s=knot_step_s) for session in items]
position_sigma = np.array([0.025, 0.025, 0.060])
velocity_sigma = np.array([0.08, 0.08, 0.12])
solution, covariance_l, matrix, rhs, offsets, groups, residual = _solve_factors(
factors, position_sigma, velocity_sigma
)
lever = solution[:3]
t_rtk_imu = -rotation.R_RTK_IMU @ lever
covariance_t = rotation.R_RTK_IMU @ covariance_l @ rotation.R_RTK_IMU.T
std_t = np.sqrt(np.maximum(np.diag(covariance_t), 0.0))
schur_information = np.linalg.pinv(covariance_l, rcond=1e-12)
singular_values = np.linalg.svd(schur_information, compute_uv=False)
threshold = max(float(singular_values[0]) * 1e-4, 1e-9)
rank = int(np.count_nonzero(singular_values > threshold))
# Recover physical residuals: system rows are grouped in XYZ triples and
# alternate position/velocity, followed by one bias prior per session.
position_errors: list[np.ndarray] = []
velocity_errors: list[np.ndarray] = []
cursor = 0
for item in factors:
for _ in range(item.knot_t_s.size - 1):
position_errors.append(residual[cursor : cursor + 3] * position_sigma)
cursor += 3
velocity_errors.append(residual[cursor : cursor + 3] * velocity_sigma)
cursor += 3
cursor += 3
pos_rms = np.sqrt(np.mean(np.asarray(position_errors) ** 2, axis=0))
vel_rms = np.sqrt(np.mean(np.asarray(velocity_errors) ** 2, axis=0))
biases = {
item.session.session_id: solution[offsets[item.session.session_id][0] : offsets[item.session.session_id][0] + 3].copy()
for item in factors
}
loo: dict[str, np.ndarray] = {}
if compute_loo and len(factors) >= 3:
for omitted in factors:
kept = [item for item in factors if item.session.session_id != omitted.session.session_id]
loo_solution, *_ = _solve_factors(kept, position_sigma, velocity_sigma)
loo[omitted.session.session_id] = (-rotation.R_RTK_IMU @ loo_solution[:3]) - t_rtk_imu
max_loo_xy = max((float(np.linalg.norm(value[:2])) for value in loo.values()), default=0.0)
max_loo_z = max((abs(float(value[2])) for value in loo.values()), default=0.0)
ok = bool(
rank == 3
and float(np.max(std_t[:2])) <= 0.05
and float(std_t[2]) <= 0.10
and float(np.max(pos_rms[:2])) <= 0.10
and float(pos_rms[2]) <= 0.20
and max_loo_xy <= 0.10
and max_loo_z <= 0.20
)
notes = [
"lever l is vector IMU-origin -> RTK-origin expressed in IMU",
"transform translation uses t_RTK_IMU = -R_RTK_IMU @ l",
"RTK position is never differentiated; position and velocity preintegration factors are solved jointly",
]
if not rotation.ok:
notes.append("upstream rotation is not accepted, so translation is diagnostic only")
ok = False
if not ok:
notes.append("translation failed one or more strict acceptance gates")
return TranslationCalibrationResult(
lever_IMU_to_RTK_in_IMU_m=lever,
t_RTK_IMU_m=t_rtk_imu,
T_RTK_IMU=make_transform(t_rtk_imu, rotation.R_RTK_IMU),
translation_std_m=std_t,
lever_information_singular_values=singular_values,
lever_precision_rank=rank,
position_residual_rms_xyz_m=pos_rms,
velocity_residual_rms_xyz_m_s=vel_rms,
accel_bias_by_session_m_s2=biases,
knot_count_by_session={item.session.session_id: int(item.knot_t_s.size) for item in factors},
loo_delta_m=loo,
ok=ok,
notes=tuple(notes),
)
rtk_imu/rtk_io.py
+139
View File
@@ -0,0 +1,139 @@
"""RTK CSV loading for the independent RTK--IMU calibration path."""
from __future__ import annotations
from dataclasses import dataclass
from pathlib import Path
import numpy as np
from imu_lidar.geodesy import geodetic_to_enu
@dataclass(frozen=True)
class RtkSeries:
"""Normalized RTK observations on the IMU device clock."""
t_s: np.ndarray
attitude_t_s: np.ndarray
position_enu_m: np.ndarray
heading_deg: np.ndarray
pitch_deg: np.ndarray
roll_deg: np.ndarray
fix_quality: np.ndarray
heading_quality: np.ndarray
heading_satellites: np.ndarray
heading_age_s: np.ndarray
hdop: np.ndarray
checksum_valid: np.ndarray
origin_geodetic: tuple[float, float, float]
source: Path
@property
def attitude_valid(self) -> np.ndarray:
"""Strict fixed dual-antenna solutions suitable for calibration."""
return (
np.isfinite(self.heading_deg)
& np.isfinite(self.pitch_deg)
& np.isfinite(self.roll_deg)
& (self.heading_quality == 4.0)
& self.checksum_valid
)
@property
def attitude_float(self) -> np.ndarray:
"""Float solutions retained for diagnostics but never calibration."""
return (
np.isfinite(self.heading_deg)
& np.isfinite(self.pitch_deg)
& np.isfinite(self.roll_deg)
& (self.heading_quality == 5.0)
& self.checksum_valid
)
@property
def position_valid(self) -> np.ndarray:
return (
np.all(np.isfinite(self.position_enu_m), axis=1)
& (self.fix_quality == 4.0)
& self.checksum_valid
)
def _column(data: np.ndarray, name: str, *, default: float = np.nan) -> np.ndarray:
names = set(data.dtype.names or ())
if name not in names:
return np.full(data.shape[0], default, dtype=float)
return np.asarray(data[name], dtype=float).reshape(-1)
def load_rtk_csv(path: Path | str) -> RtkSeries:
"""Load an exported G90 RTK CSV and convert its positions to local ENU.
The required ``t`` column must already be NMEA measurement UTC mapped onto
the IMU device clock. Host receive time is deliberately never accepted as
a fallback because it is delayed by several seconds in the recorded data.
"""
source = Path(path)
if not source.is_file():
raise FileNotFoundError(source)
data = np.genfromtxt(source, delimiter=",", names=True, dtype=float, encoding="utf-8")
if data.ndim == 0:
data = np.array([data], dtype=data.dtype)
names = set(data.dtype.names or ())
required = {"t", "lat_deg", "lon_deg", "altitude_m", "fix_quality"}
if not required.issubset(names):
raise ValueError(f"RTK CSV must contain {sorted(required)}, got {sorted(names)}")
t_s = _column(data, "t")
measurement_utc = _column(data, "t_measurement_utc_s")
hpr_measurement_utc = _column(data, "hpr_measurement_utc_s")
attitude_t = t_s.copy()
has_hpr_time = np.isfinite(measurement_utc) & np.isfinite(hpr_measurement_utc)
attitude_t[has_hpr_time] += hpr_measurement_utc[has_hpr_time] - measurement_utc[has_hpr_time]
order = np.argsort(t_s)
position, origin = geodetic_to_enu(
_column(data, "lat_deg")[order],
_column(data, "lon_deg")[order],
_column(data, "altitude_m")[order],
)
return RtkSeries(
t_s=t_s[order],
attitude_t_s=attitude_t[order],
position_enu_m=position,
heading_deg=_column(data, "heading_deg")[order],
pitch_deg=_column(data, "pitch_deg")[order],
roll_deg=_column(data, "roll_deg")[order],
fix_quality=_column(data, "fix_quality", default=0.0)[order],
heading_quality=_column(data, "heading_quality", default=0.0)[order],
heading_satellites=_column(data, "heading_satellites")[order],
heading_age_s=_column(data, "heading_age_s")[order],
hdop=_column(data, "hdop")[order],
checksum_valid=_column(data, "checksum_valid", default=1.0)[order] == 1.0,
origin_geodetic=origin,
source=source,
)
def longest_valid_interval(t_s: np.ndarray, valid: np.ndarray, *, max_gap_s: float = 0.2) -> tuple[float, float]:
"""Return the longest contiguous valid time interval."""
times = np.asarray(t_s, dtype=float).reshape(-1)
mask = np.asarray(valid, dtype=bool).reshape(-1)
indices = np.flatnonzero(mask)
if indices.size == 0:
raise ValueError("no valid RTK samples")
best_start = best_end = int(indices[0])
start = previous = int(indices[0])
for index in indices[1:]:
index = int(index)
if index != previous + 1 or times[index] - times[previous] > max_gap_s:
if times[previous] - times[start] > times[best_end] - times[best_start]:
best_start, best_end = start, previous
start = index
previous = index
if times[previous] - times[start] > times[best_end] - times[best_start]:
best_start, best_end = start, previous
return float(times[best_start]), float(times[best_end])
tests/test_device_time_alignment.py
+35
View File
@@ -0,0 +1,35 @@
from __future__ import annotations
from datetime import datetime, timezone
import numpy as np
from tools.export_g90_rtk_to_sessions import nmea_utc_to_unix_s
from tools.time_alignment import fit_affine_clock
def test_fit_affine_clock_large_epoch_and_receive_spike() -> None:
device = np.linspace(10_000.0, 10_300.0, 601)
host = 1_786_000_000.0 + 1.00002 * (device - device[0])
host[250] += 0.25
model = fit_affine_clock(device, host)
assert abs(model.scale - 1.00002) < 1e-7
assert abs(model.map(device[400]) - host[400]) < 1e-4
assert model.inlier_count < model.sample_count
assert abs(model.inverse(model.map(device[123])) - device[123]) < 1e-7
def test_nmea_utc_uses_measurement_time_not_receive_time() -> None:
receive = datetime(2026, 8, 14, 11, 34, 12, tzinfo=timezone.utc).timestamp()
measurement = nmea_utc_to_unix_s("113408.85", receive)
assert abs((receive - measurement) - 3.15) < 1e-6
def test_nmea_utc_resolves_midnight_rollover() -> None:
receive = datetime(2026, 8, 15, 0, 0, 1, tzinfo=timezone.utc).timestamp()
measurement = nmea_utc_to_unix_s("235959.50", receive)
assert abs((receive - measurement) - 1.5) < 1e-6
tests/test_g90_multisource.py
+47
View File
@@ -0,0 +1,47 @@
from __future__ import annotations
import numpy as np
from tools.rscap_v2.g90_rtk import (
parse_bestnava,
parse_pvtslna,
unicore_checksum_valid,
)
BESTNAVA = (
'#BESTNAVA,48,GPS,FINE,2430,552524000,0,0,18,8;SOL_COMPUTED,NARROW_INT,'
'30.46551864298,114.09274943336,29.6465,-15.1091,WGS84,0.0100,0.0091,'
'0.0279,"547",1.000,176.000,38,31,31,31,0,01,03,f3,SOL_COMPUTED,'
'DOPPLER_VELOCITY,0.000,0.000,1.6286,81.665533,-0.0015,0.0320,0.0167'
'*3e7e4885'
)
PVTSLNA = (
'#PVTSLNA,49,GPS,FINE,2430,552523000,0,0,18,42;NARROW_INT,29.6427,'
'30.46551666558,114.09273316982,0.0288,0.0098,0.0094,1.000,PSRDIFF,'
'29.2291,30.46551517805,114.09273145886,-15.1092,38,31,38,28,0.2276,'
'1.5569,-0.0014,NARROW_INT,0.8328,350.8610,-1.9697,37,31,31,31,'
'1.6227,1.3605,0.6398,1.0915,0.8843,5.0,28,10,25*00000000'
)
def test_bestnava_preserves_gnss_time_and_doppler_velocity() -> None:
assert unicore_checksum_valid(BESTNAVA)
row = parse_bestnava(BESTNAVA)
assert row["gnss_week"] == 2430
assert row["gnss_tow_ms"] == 552524000
assert row["position_fixed"]
assert row["doppler_velocity_valid"]
assert np.isclose(row["horizontal_speed_m_s"], 1.6286)
assert np.isclose(
np.hypot(row["velocity_east_m_s"], row["velocity_north_m_s"]), 1.6286
)
def test_pvtslna_preserves_quality_baseline_and_velocity() -> None:
row = parse_pvtslna(PVTSLNA)
assert row["position_fixed"]
assert row["heading_type"] == "NARROW_INT"
assert np.isclose(row["baseline_length_m"], 0.8328)
assert np.isclose(row["heading_deg"], 350.8610)
assert np.isclose(row["horizontal_speed_m_s"], np.hypot(0.2276, 1.5569))
tests/test_hi13_imu.py
+16 -2
View File
@@ -5,7 +5,12 @@ from __future__ import annotations
import struct
from tools.rscap_v2.capture_format_v2 import CaptureFile, CaptureHeader, RawChunk
from tools.rscap_v2.hi13_imu import crc16_hi13, iter_hi13_imu_samples, parse_hi91_frame
from tools.rscap_v2.hi13_imu import (
crc16_hi13,
iter_hi13_imu_samples,
parse_hi91_frame,
parse_hi91_sample,
)
def _hi91_frame(
@@ -13,6 +18,8 @@ def _hi91_frame(
device_ms: int = 123456,
accel_g=(0.0, 0.0, 1.0),
gyro_dps=(1.0, -2.0, 3.0),
rpy_deg=(4.0, 5.0, 6.0),
quaternion_wxyz=(1.0, 0.0, 0.0, 0.0),
) -> bytes:
payload = bytearray(76)
payload[0] = 0x91
@@ -22,7 +29,8 @@ def _hi91_frame(
struct.pack_into("<I", payload, 8, device_ms)
struct.pack_into("<fff", payload, 12, *accel_g)
struct.pack_into("<fff", payload, 24, *gyro_dps)
# remaining mag/rpy/quat left zero
struct.pack_into("<fff", payload, 48, *rpy_deg)
struct.pack_into("<ffff", payload, 60, *quaternion_wxyz)
payload_length = len(payload)
header = bytearray(6)
header[0] = 0x5A
@@ -48,6 +56,12 @@ def test_parse_hi91_units():
assert device_ms == 5000
assert abs(accel[2] - 9.80665) < 1e-4
assert abs(gyro[0] - 1.0) < 1e-5
full = parse_hi91_sample(frame, host_receive_utc_ticks=123)
assert full is not None
assert full.system_time_ms == 5000
assert full.host_receive_utc_ticks == 123
assert full.rpy_deg == (4.0, 5.0, 6.0)
assert full.quaternion_wxyz == (1.0, 0.0, 0.0, 0.0)
def test_iter_hi13_from_capture():
tests/test_rtk_imu_calibration.py
+98
View File
@@ -0,0 +1,98 @@
from __future__ import annotations
from pathlib import Path
import numpy as np
from imu_lidar.geodesy import geodetic_to_enu
from imu_lidar.geometry import so3_exp
from imu_lidar.imu_preintegration import preintegrate_gyro, preintegrate_imu
from rtk_imu.rtk_attitude import gnhpr_to_baseline_enu, gnhpr_to_rotation_enu_rtk
from rtk_imu.rtk_imu_translation import _preintegrate_translation_interval
from rtk_imu.rtk_io import load_rtk_csv
def test_geodetic_to_enu_has_expected_axis_and_scale() -> None:
enu, origin = geodetic_to_enu(
np.array([0.0, 0.0, 1e-5]),
np.array([0.0, 1e-5, 0.0]),
np.array([10.0, 10.0, 10.0]),
)
assert origin == (0.0, 0.0, 10.0)
assert np.allclose(enu[0], 0.0, atol=1e-8)
assert np.allclose(enu[1], [1.1131949, 0.0, 0.0], atol=2e-4)
assert np.allclose(enu[2], [0.0, 1.1057428, 0.0], atol=2e-4)
def test_gnhpr_heading_maps_north_clockwise_into_enu() -> None:
rotations = gnhpr_to_rotation_enu_rtk(
np.array([0.0, 90.0]),
np.zeros(2),
np.zeros(2),
)
assert np.allclose(rotations[0][:, 0], [0.0, 1.0, 0.0], atol=1e-12)
assert np.allclose(rotations[1][:, 0], [1.0, 0.0, 0.0], atol=1e-12)
def test_gnhpr_baseline_is_main_to_secondary_and_ignores_roll() -> None:
baseline = gnhpr_to_baseline_enu(
np.array([0.0, 90.0]),
np.array([30.0, 0.0]),
)
assert np.allclose(baseline[0], [0.0, np.sqrt(0.75), 0.5], atol=1e-12)
assert np.allclose(baseline[1], [1.0, 0.0, 0.0], atol=1e-12)
rotations = gnhpr_to_rotation_enu_rtk(
np.array([90.0, 90.0]),
np.zeros(2),
np.array([0.0, 45.0]),
)
assert np.allclose(rotations[0], rotations[1], atol=1e-12)
def test_rtk_loader_uses_hpr_measurement_time_for_attitude(tmp_path: Path) -> None:
path = tmp_path / "rtk.csv"
path.write_text(
"t,t_measurement_utc_s,hpr_measurement_utc_s,lat_deg,lon_deg,altitude_m,"
"fix_quality,heading_deg,pitch_deg,roll_deg,heading_quality,hdop\n"
"10.0,1000.0,1000.05,30.0,114.0,20.0,4,12.0,1.0,0.0,4,0.6\n"
"10.1,1000.1,1000.10,30.0,114.0,20.0,4,13.0,1.0,0.0,4,0.6\n",
encoding="utf-8",
)
loaded = load_rtk_csv(path)
assert np.allclose(loaded.t_s, [10.0, 10.1])
assert np.allclose(loaded.attitude_t_s, [10.05, 10.1])
assert np.all(loaded.attitude_valid)
def test_rtk_loader_accepts_only_checksum_valid_fixed_hpr(tmp_path: Path) -> None:
path = tmp_path / "rtk.csv"
path.write_text(
"t,lat_deg,lon_deg,altitude_m,fix_quality,heading_deg,pitch_deg,roll_deg,"
"heading_quality,checksum_valid\n"
"0.0,30.0,114.0,20.0,4,10.0,0.0,0.0,4,1\n"
"0.1,30.0,114.0,20.0,4,11.0,0.0,0.0,5,1\n"
"0.2,30.0,114.0,20.0,4,12.0,0.0,0.0,4,0\n",
encoding="utf-8",
)
loaded = load_rtk_csv(path)
assert loaded.attitude_valid.tolist() == [True, False, False]
assert loaded.attitude_float.tolist() == [False, True, False]
def test_fast_local_preintegration_matches_reference() -> None:
times = np.linspace(0.0, 1.0, 101)
gyro = np.tile(np.array([0.03, -0.02, 0.15]), (times.size, 1))
acc = np.tile(np.array([0.4, -0.2, 9.7]), (times.size, 1))
reference_rotation = preintegrate_gyro(times, gyro, 0.13, 0.87)
assert np.allclose(reference_rotation.delta_R, so3_exp(gyro[0] * 0.74), atol=1e-10)
reference = preintegrate_imu(times, gyro, acc, 0.13, 0.87)
dp, dv, jp, jv, duration = _preintegrate_translation_interval(
times, gyro, acc, 0.13, 0.87, np.zeros(3)
)
assert np.isclose(duration, 0.74)
assert np.allclose(dp, reference.delta_p, atol=1e-10)
assert np.allclose(dv, reference.delta_v, atol=1e-10)
assert np.allclose(jp, reference.J_ba[6:9], atol=1e-10)
assert np.allclose(jv, reference.J_ba[3:6], atol=1e-10)
tests/test_rtk_imu_engineering.py
+535
View File
@@ -0,0 +1,535 @@
from __future__ import annotations
from types import SimpleNamespace
import numpy as np
import pytest
from scipy.spatial.transform import Rotation
from imu_lidar.contracts import ImuSeries
from rtk_imu.rtk_imu_engineering import (
G0,
HPR_DIRECT_ANGULAR_SIGMA_RAD,
ResidualAudit,
_all_hpr,
_engineering_gates,
_enu,
_height_reference,
_hpr_angular_sigma_rad,
_marginal_lever_information,
_motion_flags,
_nodes,
_resample_segments_with_multiplicity,
_segments,
solve_engineering_6dof,
)
from rtk_imu.rtk_imu_multisource import UnifiedSession
from rtk_imu.rtk_imu_node_graph import (
_additive_marginal_lever_information,
_free_residual,
_free_sparsity,
_hpr_sigma,
build_problem,
fit_states_at_fixed_lever,
initial_parameters as node_graph_initial_parameters,
jacobian_sparsity as node_graph_jacobian_sparsity,
residual as node_graph_residual,
solve_free_lever_many,
)
from tools.run_rtk_imu_mechanical_prior_heldout import _gate as heldout_gate
from tools.audit_rtk_imu_innovation_noise import _summarize as innovation_summary
from tools.audit_rtk_imu_propagation_bias_root_cause import _root_report
EARTH_RADIUS_M = 6_378_137.0
def _rows_from_motion(
session_id: str,
lever: np.ndarray,
R_RTK_IMU: np.ndarray,
rotation_at,
*,
duration_s: float = 10.0,
gga_altitude_offset_m: float | None = None,
) -> UnifiedSession:
imu_t = np.arange(0.0, duration_s + 0.005, 0.01)
rotations = Rotation.from_matrix(np.asarray([rotation_at(t) for t in imu_t]))
matrices = rotations.as_matrix()
relative = Rotation.from_matrix(np.einsum("nij,njk->nik", matrices[:-1].transpose(0, 2, 1), matrices[1:]))
gyro = np.empty((imu_t.size, 3))
gyro[:-1] = relative.as_rotvec() / 0.01
gyro[-1] = gyro[-2]
accel = np.einsum("nji,j->ni", matrices, np.array([0.0, 0.0, G0]))
hpr_t = np.arange(0.0, duration_s + 0.001, 0.1)
rows_hpr: list[dict[str, str]] = []
baseline_I = R_RTK_IMU.T[:, 0]
for t in hpr_t:
baseline = rotation_at(t) @ baseline_I
heading = np.degrees(np.arctan2(baseline[0], baseline[1])) % 360.0
pitch = np.degrees(np.arcsin(np.clip(baseline[2], -1.0, 1.0)))
rows_hpr.append({
"checksum_valid": "1", "heading_quality": "4", "t_device_s": str(t),
"heading_deg": str(heading), "pitch_deg": str(pitch),
})
rows_best: list[dict[str, str]] = []
rows_gga: list[dict[str, str]] = []
best_t = np.arange(0.0, duration_s + 0.001, 0.5)
dt_velocity = 1e-3
for t in best_t:
R_WI = rotation_at(t)
position = R_WI @ lever
before = rotation_at(max(t - dt_velocity, 0.0)) @ lever
after = rotation_at(min(t + dt_velocity, duration_s)) @ lever
denominator = min(t + dt_velocity, duration_s) - max(t - dt_velocity, 0.0)
velocity = (after - before) / denominator
lat = position[1] / EARTH_RADIUS_M * 180.0 / np.pi
lon = position[0] / EARTH_RADIUS_M * 180.0 / np.pi
rows_best.append({
"checksum_valid": "1", "position_fixed": "1", "t_device_s": str(t),
"lat_deg": str(lat), "lon_deg": str(lon), "altitude_m": str(50.0 + position[2]),
"doppler_velocity_valid": "1", "velocity_east_m_s": str(velocity[0]),
"velocity_north_m_s": str(velocity[1]), "vertical_speed_m_s": str(velocity[2]),
})
if gga_altitude_offset_m is not None:
rows_gga.append({
"checksum_valid": "1", "fix_quality": "4", "t_device_s": str(t),
"lat_deg": str(lat), "lon_deg": str(lon),
"altitude_msl_m": str(50.0 + position[2] + gga_altitude_offset_m),
})
rtk = {"BESTNAVA": rows_best, "GNHPR": rows_hpr}
if rows_gga:
rtk["GGA"] = rows_gga
return UnifiedSession(
session_id=session_id,
batch_id="synthetic",
imu=ImuSeries(t_s=imu_t, gyro_rad_s=gyro, acc_m_s2=accel),
imu_rpy_deg=np.zeros((imu_t.size, 3)),
imu_quaternion_wxyz=np.tile([1.0, 0.0, 0.0, 0.0], (imu_t.size, 1)),
imu_host_receive_utc_s=imu_t,
rtk_by_type=rtk,
)
def _constant_velocity_session(speed_m_s: float = 3.0) -> UnifiedSession:
imu_t = np.arange(0.0, 5.01, 0.01)
rows_best = []
rows_hpr = []
for t in np.arange(0.0, 5.01, 0.1):
rows_hpr.append({
"checksum_valid": "1", "heading_quality": "4", "t_device_s": str(t),
"heading_deg": "90", "pitch_deg": "0",
})
for t in np.arange(0.0, 5.01, 0.5):
rows_best.append({
"checksum_valid": "1", "position_fixed": "1", "t_device_s": str(t),
"lat_deg": "0", "lon_deg": str(speed_m_s * t / EARTH_RADIUS_M * 180.0 / np.pi),
"altitude_m": "50", "doppler_velocity_valid": "1",
"velocity_east_m_s": str(speed_m_s), "velocity_north_m_s": "0",
"vertical_speed_m_s": "0",
})
return UnifiedSession(
session_id="constant_velocity", batch_id="synthetic",
imu=ImuSeries(
t_s=imu_t, gyro_rad_s=np.zeros((imu_t.size, 3)),
acc_m_s2=np.tile([0.0, 0.0, G0], (imu_t.size, 1)),
),
imu_rpy_deg=np.zeros((imu_t.size, 3)),
imu_quaternion_wxyz=np.tile([1.0, 0.0, 0.0, 0.0], (imu_t.size, 1)),
imu_host_receive_utc_s=imu_t,
rtk_by_type={"BESTNAVA": rows_best, "GNHPR": rows_hpr},
)
def _audit(values: list[float]) -> ResidualAudit:
vector = np.asarray(values, dtype=float)
return ResidualAudit(20, vector, vector, float(np.linalg.norm(vector)), float(np.linalg.norm(vector)))
def test_constant_speed_straight_is_gravity_candidate_but_not_zupt() -> None:
session = _constant_velocity_session()
times = np.asarray([float(row["t_device_s"]) for row in session.rtk_by_type["BESTNAVA"]])
velocities = np.asarray([[3.0, 0.0, 0.0]] * len(times))
gravity_candidate, zupt_static = _motion_flags(session, 2.5, times, velocities)
assert gravity_candidate
assert not zupt_static
def test_bestnava_is_not_suppressed_by_earlier_gga_epochs() -> None:
session = _rows_from_motion(
"best_preferred", np.array([0.3, -0.2, 0.1]), np.eye(3),
lambda _: np.eye(3), gga_altitude_offset_m=100.0,
)
for row in session.rtk_by_type["GGA"]:
row["t_device_s"] = str(float(row["t_device_s"]) - 0.05)
reference = _height_reference([session])
assert reference is not None
nodes = _nodes(session, reference, 0.5)
assert nodes and all(node.source == "BESTNAVA" for node in nodes)
assert all(node.velocity_enu_m_s is not None for node in nodes)
def test_gga_msl_altitude_never_enters_bestnava_z_reference() -> None:
lever = np.array([0.3, -0.2, 0.4])
session = _rows_from_motion("mixed_height", lever, np.eye(3), lambda _: np.eye(3),
gga_altitude_offset_m=123.0)
reference = _height_reference([session])
assert reference is not None and reference[2] == pytest.approx(50.4)
best, best_mask = _enu(session.rtk_by_type["BESTNAVA"][1], "BESTNAVA", reference)
gga, gga_mask = _enu(session.rtk_by_type["GGA"][1], "GGA", reference)
assert best_mask.tolist() == [True, True, True]
assert gga_mask.tolist() == [True, True, False]
assert best[2] == pytest.approx(0.0)
assert gga[2] == pytest.approx(0.0)
def test_schur_observability_uses_marginal_lever_information() -> None:
J_l = np.array([
[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 0.1],
[1.0, 1.0, 0.0], [1.0, 0.0, 0.0],
])
J_n = np.array([[1.0], [0.0], [0.0], [1.0], [0.0]])
J = np.column_stack([J_l, J_n])
marginal, singular, condition, rank, weakest, covariance = _marginal_lever_information(J, np.ones(4))
H = J.T @ J
expected = H[:3, :3] - H[:3, 3:] @ np.linalg.pinv(H[3:, 3:]) @ H[3:, :3]
assert np.allclose(marginal, expected)
assert singular.shape == (3,) and rank == 3 and np.isfinite(condition)
assert abs(weakest[2]) > 0.99
assert covariance.shape == (3, 3)
def test_bootstrap_resampling_preserves_session_multiplicity() -> None:
segments = [SimpleNamespace(session_id="a"), SimpleNamespace(session_id="b")]
sampled = _resample_segments_with_multiplicity(segments, ["a", "a", "b"])
assert [segment.session_id for segment in sampled] == ["a", "a", "b"]
@pytest.mark.parametrize("fault", ["q5", "time_gap", "baseline_jump"])
def test_isolated_hpr_faults_do_not_split_r0_trajectory(fault: str) -> None:
session = _constant_velocity_session(0.0)
if fault == "q5":
session.rtk_by_type["GNHPR"][25]["heading_quality"] = "5"
elif fault == "time_gap":
session.rtk_by_type["GNHPR"] = [
row for row in session.rtk_by_type["GNHPR"]
if not 2.1 <= float(row["t_device_s"]) <= 2.9
]
else:
session.rtk_by_type["GNHPR"][25]["heading_deg"] = "270"
reference = _height_reference([session])
assert reference is not None
nodes = _nodes(session, reference, 0.5)
assert nodes
assert all(
right.continuity_id == left.continuity_id
for left, right in zip(nodes[:-1], nodes[1:])
)
if fault == "baseline_jump":
assert any(not node.hpr_factor_valid and node.hpr_factor_method == "isolated_outlier" for node in nodes)
def test_hpr_bridge_covariance_is_weaker_than_direct_and_limited_to_half_second() -> None:
assert _hpr_angular_sigma_rad("nearest_q4", 0.0) == pytest.approx(HPR_DIRECT_ANGULAR_SIGMA_RAD)
assert _hpr_angular_sigma_rad("bracket_interpolation", 0.2) > HPR_DIRECT_ANGULAR_SIGMA_RAD
assert _hpr_angular_sigma_rad("bracket_interpolation", 0.5) >= _hpr_angular_sigma_rad(
"bracket_interpolation", 0.2
)
assert np.isinf(_hpr_angular_sigma_rad("bracket_interpolation", 0.500001))
def test_short_hpr_dropout_uses_interpolated_factor_without_splitting_r0() -> None:
session = _constant_velocity_session(0.0)
session.rtk_by_type["GNHPR"] = [
row for row in session.rtk_by_type["GNHPR"]
if not 2.4 <= float(row["t_device_s"]) <= 2.6
]
reference = _height_reference([session])
assert reference is not None
nodes = _nodes(session, reference, 0.5)
bridged = [node for node in nodes if abs(node.t_s - 2.5) < 1e-9]
assert len(bridged) == 1
assert bridged[0].hpr_factor_valid
assert bridged[0].hpr_factor_method == "bracket_interpolation"
assert all(right.continuity_id == left.continuity_id for left, right in zip(nodes[:-1], nodes[1:]))
def test_position_time_backwards_still_splits_r0_trajectory() -> None:
session = _constant_velocity_session(0.0)
session.rtk_by_type["BESTNAVA"][5]["t_device_s"] = "1.75"
reference = _height_reference([session])
assert reference is not None
nodes = _nodes(session, reference, 0.5)
assert any(
right.continuity_id != left.continuity_id
for left, right in zip(nodes[:-1], nodes[1:])
)
def test_one_hz_timestamp_jitter_does_not_create_artificial_two_second_r0_gaps() -> None:
session = _constant_velocity_session(0.0)
rows = session.rtk_by_type["BESTNAVA"]
# Retain strict device-time monotonicity while making every second sample early.
for index, row in enumerate(rows):
row["t_device_s"] = str(float(row["t_device_s"]) - (0.002 if index % 2 else 0.0))
reference = _height_reference([session])
assert reference is not None
nodes = _nodes(session, reference, 1.0)
intervals = np.diff([node.t_s for node in nodes])
assert len(nodes) == 6
assert np.all(intervals > 0.75)
assert np.all(intervals < 1.25)
assert all(right.continuity_id == left.continuity_id for left, right in zip(nodes[:-1], nodes[1:]))
def test_large_residuals_fail_engineering_acceptance() -> None:
gates = _engineering_gates(
np.array([0.01, 0.01, 0.01]), np.array([100.0, 10.0, 1.0]), 3, 100.0,
_audit([1.0, 1.0]), _audit([1.0, 1.0, 1.0]), _audit([2.0, 2.0, 2.0]),
{"a": 0.01, "b": 0.02, "c": 0.03}, np.array([0.01, 0.01, 0.01]), True,
0.01, True, None, False, None,
)
assert not gates["gga_xy_rms_p95"]
assert not gates["bestnava_xyz_rms_p95"]
assert not gates["doppler_velocity_rms_p95"]
assert not all(gates.values())
def test_unobservable_free_solution_does_not_block_mechanical_prior_with_euclidean_delta() -> None:
gates = _engineering_gates(
np.array([0.01, 0.01, 0.01]), np.array([100.0, 10.0, 1.0]), 3, 100.0,
_audit([0.01, 0.01]), _audit([0.01, 0.01, 0.01]), _audit([0.01, 0.01, 0.01]),
{"a": 0.01, "b": 0.02, "c": 0.03}, np.array([0.01, 0.01, 0.01]), True,
0.01, True, np.array([10.0, 10.0, 10.0]), False, None,
)
assert gates["manual_lever_consistency"] is False
assert gates["free_manual_mahalanobis_consistency"] is True
observable_gates = _engineering_gates(
np.array([0.01, 0.01, 0.01]), np.array([100.0, 10.0, 1.0]), 3, 100.0,
_audit([0.01, 0.01]), _audit([0.01, 0.01, 0.01]), _audit([0.01, 0.01, 0.01]),
{"a": 0.01, "b": 0.02, "c": 0.03}, np.array([0.01, 0.01, 0.01]), True,
0.01, True, None, True, 12.0,
)
assert observable_gates["free_manual_mahalanobis_consistency"] is False
def test_yaw_only_recovers_xy_but_weak_z_is_rejected_and_transforms_are_inverse() -> None:
lever = np.array([0.30, -0.20, 0.10])
session = _rows_from_motion(
"yaw", lever, np.eye(3), lambda t: Rotation.from_euler("z", 0.20 * t).as_matrix()
)
result = solve_engineering_6dof(
[session], R_RTK_IMU=np.eye(3), sample_period_s=0.5,
run_loo=False, run_bootstrap=False, run_rotation_sensitivity=False,
)
assert result.l_I_m is not None
assert np.allclose(result.l_I_m[:2], lever[:2], atol=0.05)
assert result.lever_precision_rank < 3 or not result.engineering_acceptance_gates["lever_marginal_std"]
assert not result.engineering_6dof_accepted
assert result.T_RTK_IMU is not None and result.T_IMU_RTK is not None
assert np.allclose(result.T_RTK_IMU @ result.T_IMU_RTK, np.eye(4), atol=1e-8)
def test_pitch_excitation_recovers_vertical_lever_arm() -> None:
lever = np.array([0.28, -0.16, 0.42])
rotation_at = lambda t: Rotation.from_euler(
"xyz", [0.0, 0.12 * np.sin(0.55 * t), 0.16 * t]
).as_matrix()
session = _rows_from_motion("pitch", lever, np.eye(3), rotation_at)
result = solve_engineering_6dof(
[session], R_RTK_IMU=np.eye(3), sample_period_s=0.5,
run_loo=False, run_bootstrap=False, run_rotation_sensitivity=False,
)
assert result.l_I_m is not None
assert result.l_I_m[2] == pytest.approx(lever[2], abs=0.08)
def test_full_3d_excitation_with_nonzero_fixed_r2g_recovers_l_i() -> None:
lever = np.array([0.24, -0.18, 0.36])
fixed_rotation = Rotation.from_euler("xyz", [0.454, -0.003, 0.012], degrees=True).as_matrix()
rotation_at = lambda t: Rotation.from_euler(
"xyz", [0.16 * np.sin(0.37 * t), 0.18 * np.sin(0.51 * t), 0.18 * t]
).as_matrix()
session = _rows_from_motion(
"full_3d", lever, fixed_rotation, rotation_at, duration_s=15.0
)
result = solve_engineering_6dof(
[session], R_RTK_IMU=fixed_rotation, sample_period_s=0.5,
run_loo=False, run_bootstrap=False, run_rotation_sensitivity=False,
)
assert result.l_I_m is not None
assert np.allclose(result.l_I_m, lever, atol=0.08)
assert np.allclose(result.R_RTK_IMU, fixed_rotation)
assert result.T_RTK_IMU is not None and result.T_IMU_RTK is not None
assert np.allclose(result.T_RTK_IMU @ result.T_IMU_RTK, np.eye(4), atol=1e-8)
def test_mechanical_soft_prior_keeps_free_solution_and_reports_prior_solution() -> None:
lever = np.array([0.24, -0.18, 0.36])
reference = np.array([0.30, -0.18, 0.36])
rotation_at = lambda t: Rotation.from_euler(
"xyz", [0.14 * np.sin(0.37 * t), 0.16 * np.sin(0.51 * t), 0.18 * t]
).as_matrix()
session = _rows_from_motion("prior", lever, np.eye(3), rotation_at, duration_s=12.0)
result = solve_engineering_6dof(
[session], R_RTK_IMU=np.eye(3),
manual_l_I_m=reference,
manual_l_I_covariance_m2=np.diag([0.02**2, 0.02**2, 0.02**2]),
sample_period_s=0.5, run_loo=False, run_bootstrap=False,
run_rotation_sensitivity=False,
)
assert result.free_solution is not None
assert result.prior_constrained_solution is not None
assert result.translation_prior_applied
assert np.allclose(result.mechanical_reference_l_I_m, reference)
assert np.linalg.norm(result.prior_to_mechanical_delta_m) < np.linalg.norm(result.free_to_mechanical_delta_m)
assert np.allclose(result.l_I_m, result.prior_constrained_solution.l_I_m)
assert result.mechanical_reference_solution is not None
assert result.residual_comparison is not None
assert result.posterior_to_prior_covariance_ratio is not None
def test_manual_mean_and_covariance_must_be_provided_together() -> None:
session = _constant_velocity_session(0.0)
with pytest.raises(ValueError, match="must be provided together"):
solve_engineering_6dof([session], R_RTK_IMU=np.eye(3), manual_l_I_m=np.zeros(3))
def test_blockwise_schur_information_is_additive_across_independent_segments() -> None:
rng = np.random.default_rng(7)
lever_blocks = []
nuisance_blocks = []
row_count = 30
for scale in (1.0, 1e-3, 20.0):
lever_blocks.append(rng.normal(size=(row_count, 3)) * scale)
nuisance_blocks.append(rng.normal(size=(row_count, 15)) * scale)
jacobian = np.zeros((3 * row_count, 3 + 3 * 15))
for index, (lever, nuisance) in enumerate(zip(lever_blocks, nuisance_blocks)):
rows = slice(index * row_count, (index + 1) * row_count)
columns = slice(3 + index * 15, 3 + (index + 1) * 15)
jacobian[rows, :3] = lever
jacobian[rows, columns] = nuisance
residual = np.ones(jacobian.shape[0])
combined = _marginal_lever_information(jacobian, residual)[0]
expected = np.zeros((3, 3))
for lever, nuisance in zip(lever_blocks, nuisance_blocks):
local = np.column_stack([lever, nuisance])
expected += _marginal_lever_information(local, np.ones(row_count))[0]
assert np.allclose(combined, expected, rtol=1e-9, atol=1e-9)
def test_per_node_graph_has_finite_residual_and_matching_sparse_structure() -> None:
lever = np.array([0.24,-0.18,0.36])
rotation_at = lambda t: Rotation.from_euler('z',.12*t).as_matrix()
session = _rows_from_motion('node_graph',lever,np.eye(3),rotation_at,duration_s=10.)
segments = _segments([session],.5)
assert segments
problem = build_problem(segments[0],np.eye(3),lever)
x0 = node_graph_initial_parameters(problem)
value = node_graph_residual(problem,x0)
sparsity = node_graph_jacobian_sparsity(problem,x0)
assert x0.size == 15*len(problem.segment.nodes)
assert np.all(np.isfinite(value))
assert sparsity.shape == (value.size,x0.size)
assert sparsity.nnz > value.size
def test_node_graph_hpr_override_retains_bridge_extra_variance() -> None:
problem=SimpleNamespace(hpr_direct_angular_sigma_rad=.006)
direct=SimpleNamespace(hpr_angular_sigma_rad=HPR_DIRECT_ANGULAR_SIGMA_RAD)
assert _hpr_sigma(problem,direct)==pytest.approx(.006)
old=_hpr_angular_sigma_rad('bracket_interpolation',.4)
bridge=SimpleNamespace(hpr_angular_sigma_rad=old)
expected=np.sqrt(.006**2+old**2-HPR_DIRECT_ANGULAR_SIGMA_RAD**2)
assert _hpr_sigma(problem,bridge)==pytest.approx(expected)
def test_node_graph_free_lever_layout_extends_fixed_state_by_three() -> None:
lever=np.array([.24,-.18,.36])
rotation_at=lambda t: Rotation.from_euler('z',.12*t).as_matrix()
session=_rows_from_motion('node_graph_free',lever,np.eye(3),rotation_at,duration_s=5.)
problem=build_problem(_segments([session],.5)[0],np.eye(3),lever,.006)
value=np.concatenate([np.zeros(3),node_graph_initial_parameters(problem,np.zeros(3))])
residual=_free_residual(problem,value)
sparsity=_free_sparsity(problem,value)
assert np.all(np.isfinite(residual))
assert sparsity.shape==(residual.size,value.size)
def test_node_graph_block_schur_information_is_additive() -> None:
rng=np.random.default_rng(7); rows=24; nuisance=5
local=[rng.normal(size=(rows,3+nuisance)) for _ in range(2)]
global_jac=np.zeros((2*rows,3+2*nuisance))
global_jac[:rows,:3]=local[0][:,:3]
global_jac[:rows,3:3+nuisance]=local[0][:,3:]
global_jac[rows:,:3]=local[1][:,:3]
global_jac[rows:,3+nuisance:]=local[1][:,3:]
actual=_additive_marginal_lever_information(
global_jac,[0,rows,2*rows],[3,3+nuisance,3+2*nuisance])
expected=np.zeros((3,3))
for jac in local:
H=jac.T@jac
expected+=H[:3,:3]-H[:3,3:]@np.linalg.pinv(H[3:,3:])@H[3:,:3]
assert np.allclose(actual,expected,rtol=1e-10,atol=1e-10)
def test_node_graph_soft_lever_prior_adds_exact_information() -> None:
lever=np.array([.24,-.18,.36])
rotation_at=lambda t: Rotation.from_euler('z',.12*t).as_matrix()
session=_rows_from_motion('node_graph_prior',lever,np.eye(3),rotation_at,
duration_s=5.)
problem=build_problem(_segments([session],.5)[0],np.eye(3),lever,.006)
free=solve_free_lever_many([problem],lever,max_nfev=1)
covariance=np.diag([.02**2,.02**2,.03**2])
prior=solve_free_lever_many([problem],lever,max_nfev=1,
lever_prior_mean_m=lever,lever_prior_covariance_m2=covariance)
free_information=np.linalg.pinv(free.lever_covariance_m2,rcond=1e-9)
prior_information=np.linalg.pinv(prior.lever_covariance_m2,rcond=1e-9)
assert np.allclose(prior_information-free_information,
np.linalg.inv(covariance),rtol=1e-6,atol=1e-5)
def test_heldout_gate_does_not_accept_underdispersed_statistics() -> None:
vector={'count':1,'axis_rms':[0.,0.,0.],'axis_p95_abs':[0.,0.,0.],
'vector_rms':.01,'vector_p95':.02}
summary={'optimizer_converged_fraction':1.,
'global_chi_square_per_dof':.127,
'best_position_physical_m':vector,
'doppler_physical_m_s':vector,
'residual_by_factor':{
'hpr':{'p95_abs':1.3},
'imu_preintegration':{'p95_abs':.28}}}
result=heldout_gate(summary)
assert not result['passed']
assert not result['checks']['global_chi_square_per_dof_in_0p25_4']
def test_fixed_lever_retry_starts_from_previous_final_state() -> None:
lever=np.array([.24,-.18,.36])
rotation_at=lambda t: Rotation.from_euler('z',.12*t).as_matrix()
session=_rows_from_motion('node_graph_retry',lever,np.eye(3),rotation_at,
duration_s=5.)
problem=build_problem(_segments([session],.5)[0],np.eye(3),lever,.006)
state,first=fit_states_at_fixed_lever(problem,lever,max_nfev=1)
_,retry=fit_states_at_fixed_lever(
problem,lever,max_nfev=1,initial_state_values=state)
assert retry['initial_cost']==pytest.approx(first['cost'])
def test_innovation_summary_reports_vector_and_temporal_metrics() -> None:
records=[{'residual':np.array([float(i),0.,0.]),
'normalized':np.array([float(i),0.,0.]),
'normalized_factor_only':np.array([float(i),0.,0.]),
't_s':float(i)} for i in range(4)]
result=innovation_summary(records)
assert result['vector_p95']>result['vector_p50']
assert result['temporal_linear_drift_per_s'][0]==pytest.approx(1.)
assert np.asarray(result['empirical_covariance']).shape==(3,3)
def test_propagation_root_report_detects_common_constant_acceleration() -> None:
acceleration=np.array([.2,-.02,-.01]); position=[]; velocity=[]
for index,dt in enumerate((.8,1.,1.2,1.4)):
base={'interval_id':str(index),'session':'s','motion':'m','t_s':index,
'speed_bin':'slow','gyro_bin':'low','dt_s':dt,'R0_WI':np.eye(3)}
position.append({**base,'residual':.5*acceleration*dt*dt})
velocity.append({**base,'residual':acceleration*dt})
report,_=_root_report(position,velocity)
assert report['common_constant_acceleration_error_detected']
assert np.allclose(
report['overall']['difference_velocity_minus_position_m_s2']['bias'],0.)
tests/test_rtk_imu_multisource.py
+46
View File
@@ -0,0 +1,46 @@
from __future__ import annotations
import numpy as np
from imu_lidar.contracts import ImuSeries
from rtk_imu.rtk_imu_multisource import R1bResult, UnifiedSession, solve_r2g
def _level_session(session_id: str) -> UnifiedSession:
t = np.arange(0.0, 25.0, 0.01)
return UnifiedSession(
session_id=session_id,
batch_id="test",
imu=ImuSeries(
t_s=t,
gyro_rad_s=np.zeros((t.size, 3)),
acc_m_s2=np.tile([0.0, 0.0, 9.80665], (t.size, 1)),
),
imu_rpy_deg=np.zeros((t.size, 3)),
imu_quaternion_wxyz=np.tile([1.0, 0.0, 0.0, 0.0], (t.size, 1)),
imu_host_receive_utc_s=t,
rtk_by_type={},
)
def test_r2g_baseline_plus_level_gravity_completes_identity() -> None:
r1b = R1bResult(
baseline_axis_imu=np.array([1.0, 0.0, 0.0]),
tilt_yz_deg=np.zeros(2),
pair_count=100,
residual_rms_deg=0.1,
residual_p95_deg=0.2,
covariance_deg2=np.eye(2) * 0.01,
std_deg=np.ones(2) * 0.1,
information_singular_values=np.ones(2),
per_session_rms_deg={},
gyro_bias_by_session_rad_s={},
ok=True,
notes=(),
)
sessions = [_level_session("a"), _level_session("b")]
result = solve_r2g(sessions, r1b, level_static_session_ids={"a", "b"})
assert result.R_RTK_IMU is not None
assert np.allclose(result.R_RTK_IMU, np.eye(3), atol=1e-12)
assert result.sample_count == 6
assert result.session_count == 2
tools/audit_rtk_imu_factor_consistency.py
+313
View File
@@ -0,0 +1,313 @@
#!/usr/bin/env python3
'''Audit RTK/IMU factor conventions without running an optimizer.'''
from __future__ import annotations
import argparse, json, math, sys
from dataclasses import asdict, is_dataclass
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0, str(ROOT))
from imu_lidar.imu_preintegration import preintegrate_imu
from rtk_imu.rtk_imu_engineering import (
G_ENU, MIN_SEGMENT_DURATION_S, MIN_SEGMENT_NODE_COUNT,
NUISANCE_DOF_PER_SEGMENT, _Segment, _all_hpr, _audit,
_dense_colored_jacobian, _enu, _height_reference,
_hpr_factor_observation, _initial_parameters, _marginal_lever_information,
_motion_flags, _nodes, _position_valid, _residual,
_segment_residual_size, _world_rtk)
from rtk_imu.rtk_imu_multisource import _f, _truth, load_unified_sessions
MECHANICAL_L_I_M = np.array([-0.45072, -0.25682, 0.73208])
def _jsonable(value):
if isinstance(value, np.ndarray): return _jsonable(value.tolist())
if isinstance(value, np.generic): return _jsonable(value.item())
if isinstance(value, float): return value if math.isfinite(value) else None
if is_dataclass(value): return _jsonable(asdict(value))
if isinstance(value, dict): return {str(k): _jsonable(v) for k, v in value.items()}
if isinstance(value, (list, tuple)): return [_jsonable(v) for v in value]
return value
def _summary(error):
a = np.asarray(error, dtype=float)
return _jsonable(_audit(list(a.reshape(-1, 3)), 3)) if a.size else _jsonable(_audit([], 3))
def _corr(a, b):
out = np.full(3, np.nan)
for axis in range(3):
if len(a) >= 3 and np.std(a[:, axis]) > 1e-10 and np.std(b[:, axis]) > 1e-10:
out[axis] = np.corrcoef(a[:, axis], b[:, axis])[0, 1]
return out
def _best_arrays(session, reference):
rows = []
for row in session.rtk_by_type.get('BESTNAVA', []):
v = np.array([_f(row, 'velocity_east_m_s'), _f(row, 'velocity_north_m_s'),
_f(row, 'vertical_speed_m_s')])
if _position_valid(row, 'BESTNAVA') and _truth(row, 'doppler_velocity_valid') and np.all(np.isfinite(v)):
rows.append(row)
rows.sort(key=lambda row: _f(row, 't_device_s'))
rows = [row for i, row in enumerate(rows) if i == 0 or _f(row, 't_device_s') > _f(rows[i-1], 't_device_s')]
t = np.asarray([_f(row, 't_device_s') for row in rows])
p = np.asarray([_enu(row, 'BESTNAVA', reference)[0] for row in rows]).reshape(-1, 3)
v = np.asarray([[_f(row, 'velocity_east_m_s'), _f(row, 'velocity_north_m_s'),
_f(row, 'vertical_speed_m_s')] for row in rows]).reshape(-1, 3)
return rows, t, p, v
def _gnss_audit(sessions, reference, max_dt):
all_dpdt, all_v, per_session = [], [], {}
for session in sessions:
_, t, p, v = _best_arrays(session, reference)
dt = np.diff(t); keep = (dt > 0) & (dt <= max_dt)
dpdt = np.diff(p, axis=0)[keep] / dt[keep, None]
v_avg = .5 * (v[:-1] + v[1:])[keep]
all_dpdt.append(dpdt); all_v.append(v_avg)
per_session[session.session_id] = {
'interval_count': len(dpdt), 'nominal_correlation_xyz': _corr(dpdt, v_avg),
'nominal_error_m_s': _summary(dpdt-v_avg)}
dpdt, velocity = np.vstack(all_dpdt), np.vstack(all_v)
hypotheses = []
for swap in (False, True):
base = velocity[:, [1,0,2]] if swap else velocity
for sx in (-1.,1.):
for sy in (-1.,1.):
for sz in (-1.,1.):
transformed = base * [sx,sy,sz]
hypotheses.append({
'mapping': ('[N,E,Z]' if swap else '[E,N,Z]')+f'*[{sx:+.0f},{sy:+.0f},{sz:+.0f}]',
'correlation_xyz': _corr(dpdt, transformed),
'error_m_s': _summary(dpdt-transformed)})
hypotheses.sort(key=lambda item: item['error_m_s']['vector_rms'])
nominal = next(x for x in hypotheses if x['mapping'] == '[E,N,Z]*[+1,+1,+1]')
return {'interval_count': len(dpdt), 'nominal': nominal, 'best_mapping': hypotheses[0],
'all_hypotheses': hypotheses, 'per_session': per_session}
def _nearest_imu(session, t, tolerance=.03):
right = int(np.searchsorted(session.imu.t_s, t))
candidates = [i for i in (right-1,right) if 0 <= i < len(session.imu.t_s)]
if not candidates: return None
index = min(candidates, key=lambda i: abs(session.imu.t_s[i]-t))
return index if abs(session.imu.t_s[index]-t) <= tolerance else None
def _static_audit(sessions, rotation):
current, opposite, per_session = [], [], {}
for session in sessions:
hpr, local = _all_hpr(session), []
last_t = -np.inf
for hpr_index in hpr.valid_indices:
t = float(hpr.t_s[hpr_index])
if t-last_t < 1.: continue
last_t = t
gravity, _ = _motion_flags(session,t,np.zeros(0),np.zeros((0,3)))
baseline, _, valid, _, _ = _hpr_factor_observation(hpr,t)
imu_index = _nearest_imu(session,t)
if not (gravity and valid and imu_index is not None):
continue
R_WI = _world_rtk(baseline) @ rotation
accel = session.imu.acc_m_s2[imu_index]
value = R_WI @ accel + G_ENU
local.append(value); current.append(value); opposite.append(R_WI @ accel-G_ENU)
per_session[session.session_id] = {'count':len(local),'current_formula_m_s2':_summary(local)}
return {'formula':'a_W_linear = R_WI @ specific_force_I + G_ENU',
'current_formula_m_s2':_summary(current),
'opposite_gravity_sign_m_s2':_summary(opposite),'per_session':per_session}
def _closure_audit(sessions, reference, rotation, lever, max_dt):
all_p, all_v, per_session = [], [], {}
for session in sessions:
_, t, p_ant, v_ant = _best_arrays(session, reference)
hpr, local_p, local_v = _all_hpr(session), [], []
for index, dt in enumerate(np.diff(t)):
if not .5 <= dt <= max_dt: continue
baseline, _, valid, _, _ = _hpr_factor_observation(hpr, t[index])
i0, i1 = _nearest_imu(session, t[index]), _nearest_imu(session, t[index+1])
if not valid or i0 is None or i1 is None: continue
pre = preintegrate_imu(session.imu.t_s, session.imu.gyro_rad_s,
session.imu.acc_m_s2, t[index], t[index+1])
if pre.duration_s <= 0 or abs(pre.duration_s-dt) > 1e-6: continue
R0 = _world_rtk(baseline) @ rotation
p_i0 = p_ant[index] - R0 @ lever
v_i0 = v_ant[index] - R0 @ np.cross(session.imu.gyro_rad_s[i0], lever)
p_i1 = p_i0 + v_i0*dt + .5*G_ENU*dt**2 + R0@pre.delta_p
v_i1 = v_i0 + G_ENU*dt + R0@pre.delta_v
R1 = R0 @ pre.delta_R
local_p.append(p_i1 + R1@lever - p_ant[index+1])
local_v.append(v_i1 + R1@np.cross(session.imu.gyro_rad_s[i1],lever) - v_ant[index+1])
all_p.extend(local_p); all_v.extend(local_v)
per_session[session.session_id] = {
'interval_count':len(local_p),'position_m':_summary(local_p),
'velocity_m_s':_summary(local_v)}
return {'method':'single-step forward closure; fixed R2G/mechanical lever/BEST p-v; no least_squares',
'position_m':_summary(all_p),'velocity_m_s':_summary(all_v),
'per_session':per_session}
def _qualified_runs(sessions, reference, period):
result = []
for session in sessions:
nodes, start, number = _nodes(session, reference, period), 0, 0
for end in range(1,len(nodes)+1):
if end != len(nodes) and nodes[end].continuity_id == nodes[end-1].continuity_id:
continue
run, start = tuple(nodes[start:end]), end
if (len(run) >= MIN_SEGMENT_NODE_COUNT
and run[-1].t_s-run[0].t_s >= MIN_SEGMENT_DURATION_S
and any(node.hpr_factor_valid for node in run)):
result.append((session,run,f'{session.session_id}:{number:02d}'))
number += 1
return result
def _first_node_audit(runs, rotation, lever):
records, ranges = [], {}
for session, run, segment_id in runs:
node = run[0]
hpr_node = next(item for item in run if item.hpr_factor_valid)
R_WI = _world_rtk(hpr_node.baseline_enu) @ rotation
velocity = node.velocity_enu_m_s
v = np.full(3,np.nan) if velocity is None else velocity
records.append({
'segment_id':segment_id,'source':node.source,'t_s':node.t_s,
'first_position_global_enu_m':node.p_enu_m,'first_velocity_enu_m_s':v,
'free_x0_l0_position_residual_m':np.zeros(3),
'free_x0_l0_velocity_residual_m_s':-v,
'mechanical_l_unshifted_x0_position_residual_m':R_WI@lever,
'mechanical_l_unshifted_x0_velocity_residual_m_s':R_WI@np.cross(node.gyro_rad_s,lever)-v,
'observation_seeded_mechanical_position_residual_m':np.zeros(3),
'observation_seeded_mechanical_velocity_residual_m_s':
np.zeros(3) if velocity is not None else np.full(3,np.nan)})
ranges.setdefault(session.session_id,[]).append(node.p_enu_m)
ranges = {key:{'count':len(value),'min_global_enu_m':np.min(value,axis=0),
'max_global_enu_m':np.max(value,axis=0)} for key,value in ranges.items()}
return {'common_global_enu_reference':True,'segment_state_is_global_imu_position':True,
'per_session_first_node_ranges':ranges,'segments':records}
def _gyro_score(session, run, category):
keep = (session.imu.t_s >= run[0].t_s) & (session.imu.t_s <= run[-1].t_s)
t, gyro = session.imu.t_s[keep], session.imu.gyro_rad_s[keep]
if len(t) < 2: return 0.
value = np.trapezoid(np.abs(gyro),t,axis=0)
return float(value[2] if category != 'slope' else np.hypot(value[0],value[1]))
def _build_segment(session, run, segment_id):
pre = tuple(preintegrate_imu(session.imu.t_s,session.imu.gyro_rad_s,
session.imu.acc_m_s2,a.t_s,b.t_s)
for a,b in zip(run[:-1],run[1:]))
hpr = next(node for node in run if node.hpr_factor_valid)
return _Segment(segment_id,session.session_id,run,pre,_world_rtk(hpr.baseline_enu))
def _subset_marginal(jacobian,residual,segments,indices):
rows, row0 = [], 0
for index,segment in enumerate(segments):
count = _segment_residual_size(segment)
if index in indices: rows.extend(range(row0,row0+count))
row0 += count
columns = [0,1,2]
for index in indices:
start = 3 + NUISANCE_DOF_PER_SEGMENT*index
columns.extend(range(start,start+NUISANCE_DOF_PER_SEGMENT))
rows, columns = np.asarray(rows), np.asarray(columns)
return _marginal_lever_information(
jacobian[np.ix_(rows,columns)],residual[rows])[0]
def _jacobian_schur_audit(runs,categories,rotation,lever):
segments, indices = [], {}
for category in ('circle','left_right','slope'):
choices = [item for item in runs if categories[item[0].session_id] == category]
if not choices:
indices[category] = []; continue
best = max(choices,key=lambda item:_gyro_score(item[0],item[1],category))
indices[category] = [len(segments)]
segments.append(_build_segment(*best))
x = _initial_parameters(segments); x[:3] = lever
residual = _residual(x,segments,rotation)
jacobian = _dense_colored_jacobian(x,segments,rotation)
comparisons = []
for axis in range(3):
plus, minus = x.copy(), x.copy()
plus[axis] += .001; minus[axis] -= .001
numeric = (_residual(plus,segments,rotation)-_residual(minus,segments,rotation))/.002
current = jacobian[:,axis]; delta = numeric-current
comparisons.append({
'axis':'XYZ'[axis],'perturbation_m':.001,
'relative_difference':float(np.linalg.norm(delta)/max(np.linalg.norm(numeric),1e-12)),
'difference_norm':float(np.linalg.norm(delta)),
'max_absolute_difference':float(np.max(np.abs(delta))),
'correlation':float(np.corrcoef(numeric,current)[0,1])})
full = _marginal_lever_information(jacobian,residual)[0]
parts = {key:_subset_marginal(jacobian,residual,segments,value)
for key,value in indices.items() if value}
summed = sum(parts.values(),np.zeros((3,3)))
error = np.linalg.norm(summed-full,ord='fro')
return {'linearization':'same observation-seeded nuisance state and mechanical lever; no optimizer',
'selected_segments':[segment.segment_id for segment in segments],
'finite_difference_vs_solver_jacobian':comparisons,
'full_marginal_information':full,'category_marginal_information':parts,
'category_sum':summed,'additivity_error_fro':float(error),
'additivity_relative_error':float(error/max(np.linalg.norm(full,ord='fro'),1e-12))}
def _legacy_diagnostics(path):
if path is None or not path.exists():
return {'available':False,'reason':'no prior artifact supplied'}
payload = json.loads(path.read_text(encoding='utf-8'))
final_l = np.asarray(payload.get('free_solution',{}).get('l_I_m',[np.nan]*3))
return {'available':False,'source':str(path),'initial_cost':None,'final_cost':None,
'cost_reduction':None,'nfev':None,'optimality':None,'gradient_norm':None,
'initial_l_I_m':[0.,0.,0.],'final_l_I_m':final_l,
'l_step_norm_m':float(np.linalg.norm(final_l)) if np.all(np.isfinite(final_l)) else None,
'reason':'legacy artifact did not persist optimizer state; prohibited solve was not rerun'}
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('--manifest',type=Path,required=True)
parser.add_argument('--output',type=Path,required=True)
parser.add_argument('--circle-session',required=True)
parser.add_argument('--left-right-session',required=True)
parser.add_argument('--slope-session',required=True)
parser.add_argument('--sample-period-s',type=float,default=1.)
parser.add_argument('--max-best-interval-s',type=float,default=1.75)
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
default=[.4543066225,-.0026392019,.0122384129])
parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
default=MECHANICAL_L_I_M.tolist())
parser.add_argument('--previous-free-result',type=Path)
parser.add_argument('--level-static-session',action='append',
default=['0819_20260819_072130','0819_20260819_073045'])
args = parser.parse_args()
categories = {args.circle_session:'circle',args.left_right_session:'left_right',
args.slope_session:'slope'}
selected_ids = set(categories) | set(args.level_static_session)
all_sessions = load_unified_sessions(args.manifest,selected_session_ids=selected_ids)
sessions = [session for session in all_sessions if session.session_id in categories]
static_sessions = [session for session in all_sessions
if session.session_id in set(args.level_static_session)]
reference = _height_reference(sessions)
if reference is None: raise RuntimeError('no valid BESTNAVA height reference')
rotation = Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
lever = np.asarray(args.mechanical_l_I_m)
runs = _qualified_runs(sessions,reference,args.sample_period_s)
payload = {
'scope':'factor consistency only; no free solve/LOO/prior/bootstrap/sensitivity',
'least_squares_called':False,'rotation_source':'R2G_gravity_level_prior',
'rotation_rpy_deg':args.rotation_rpy_deg,'mechanical_l_I_m':lever,
'common_enu_reference':reference,
'gnss_position_difference_vs_doppler':_gnss_audit(
sessions,reference,args.max_best_interval_s),
'static_specific_force_gravity_sign':_static_audit(static_sessions,rotation),
'one_step_imu_preintegration_closure':_closure_audit(
sessions,reference,rotation,lever,args.max_best_interval_s),
'first_node_origin_and_initial_residual':_first_node_audit(runs,rotation,lever),
'lever_jacobian_and_category_schur':_jacobian_schur_audit(
runs,categories,rotation,lever),
'previous_free_fit_solver_diagnostics':_legacy_diagnostics(args.previous_free_result)}
args.output.parent.mkdir(parents=True,exist_ok=True)
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
print(json.dumps(_jsonable({
'gnss':payload['gnss_position_difference_vs_doppler'],
'static':payload['static_specific_force_gravity_sign'],
'closure':payload['one_step_imu_preintegration_closure'],
'jacobian_schur':payload['lever_jacobian_and_category_schur']}),
ensure_ascii=False,indent=2))
return 0
if __name__ == '__main__':
raise SystemExit(main())
tools/audit_rtk_imu_factor_yield.py
+151
View File
@@ -0,0 +1,151 @@
#!/usr/bin/env python3
"""Audit BESTNAVA/Doppler factor yield for every unified RTK--IMU session."""
from __future__ import annotations
import argparse
import json
import sys
from dataclasses import replace
from pathlib import Path
import numpy as np
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
from rtk_imu.rtk_imu_engineering import (
_all_hpr,
_height_reference,
_nodes,
_position_valid,
)
from rtk_imu.rtk_imu_multisource import _f, _nearest_index, _truth, load_unified_sessions
def _finite_doppler(row: dict[str, str]) -> bool:
value = np.asarray([
_f(row, "velocity_east_m_s"),
_f(row, "velocity_north_m_s"),
_f(row, "vertical_speed_m_s"),
])
return bool(_truth(row, "doppler_velocity_valid") and np.all(np.isfinite(value)))
def _source_only_nodes(session, source: str, period_s: float):
rows = {source: session.rtk_by_type.get(source, [])}
if "GNHPR" in session.rtk_by_type:
rows["GNHPR"] = session.rtk_by_type["GNHPR"]
return _nodes(replace(session, rtk_by_type=rows), _height_reference([session]), period_s)
def _audit_session(session, period_s: float) -> dict[str, object]:
best = session.rtk_by_type.get("BESTNAVA", [])
hpr = _all_hpr(session)
counts = {
"raw_bestnava": len(best),
"checksum_valid": 0,
"fixed": 0,
"finite_position": 0,
"finite_doppler": 0,
"hpr_near": 0,
"hpr_q4": 0,
"imu_near": 0,
"raw_bestnava_candidate": 0,
}
for row in best:
if not _truth(row, "checksum_valid"):
continue
counts["checksum_valid"] += 1
if not _truth(row, "position_fixed"):
continue
counts["fixed"] += 1
if not _position_valid(row, "BESTNAVA"):
continue
counts["finite_position"] += 1
if not _finite_doppler(row):
continue
counts["finite_doppler"] += 1
t_s = _f(row, "t_device_s")
hpr_index = _nearest_index(hpr.t_s, t_s, 0.12)
if hpr_index is None:
continue
counts["hpr_near"] += 1
if not hpr.valid[hpr_index]:
continue
counts["hpr_q4"] += 1
if _nearest_index(session.imu.t_s, t_s, 0.03) is None:
continue
counts["imu_near"] += 1
counts["raw_bestnava_candidate"] += 1
reference = _height_reference([session])
combined_nodes = _nodes(session, reference, period_s) if reference is not None else []
best_nodes = _source_only_nodes(session, "BESTNAVA", period_s) if reference is not None else []
combined_best = [node for node in combined_nodes if node.source == "BESTNAVA"]
best_velocity_nodes = [node for node in combined_best if node.velocity_enu_m_s is not None]
source_best_velocity = [node for node in best_nodes if node.velocity_enu_m_s is not None]
run_nodes: dict[int, list] = {}
for node in combined_nodes:
run_nodes.setdefault(node.continuity_id, []).append(node)
qualifying_runs = [
run for run in run_nodes.values()
if len(run) >= 6 and run[-1].t_s - run[0].t_s >= 5.0
]
qualifying_best = [
node for run in qualifying_runs for node in run if node.source == "BESTNAVA"
]
qualifying_doppler = [node for node in qualifying_best if node.velocity_enu_m_s is not None]
return {
"session_id": session.session_id,
"batch_id": session.batch_id,
"counts": counts,
"combined_selected_nodes": len(combined_nodes),
"combined_selected_bestnava": len(combined_best),
"combined_selected_doppler": len(best_velocity_nodes),
"best_only_selected_nodes": len(best_nodes),
"best_only_selected_doppler": len(source_best_velocity),
"best_suppressed_by_mixed_selection": max(0, len(best_nodes) - len(combined_best)),
"doppler_suppressed_by_mixed_selection": max(0, len(source_best_velocity) - len(best_velocity_nodes)),
"continuous_run_count": len(run_nodes),
"qualified_run_count": len(qualifying_runs),
"qualified_bestnava_factors": len(qualifying_best),
"qualified_doppler_factors": len(qualifying_doppler),
}
def main(argv: list[str] | None = None) -> int:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--manifest", type=Path, required=True)
parser.add_argument("--output", type=Path, required=True)
parser.add_argument("--sample-period-s", type=float, default=1.0)
args = parser.parse_args(argv)
sessions = load_unified_sessions(args.manifest)
audit = [_audit_session(session, args.sample_period_s) for session in sessions]
totals: dict[str, int] = {}
for item in audit:
for key, value in item["counts"].items():
totals[key] = totals.get(key, 0) + int(value)
for key in (
"combined_selected_nodes", "combined_selected_bestnava", "combined_selected_doppler",
"best_only_selected_nodes", "best_only_selected_doppler",
"best_suppressed_by_mixed_selection", "doppler_suppressed_by_mixed_selection",
"continuous_run_count", "qualified_run_count", "qualified_bestnava_factors",
"qualified_doppler_factors",
):
totals[key] = totals.get(key, 0) + int(item[key])
payload = {
"sample_period_s": args.sample_period_s,
"session_count": len(audit),
"totals": totals,
"sessions": audit,
}
args.output.parent.mkdir(parents=True, exist_ok=True)
args.output.write_text(json.dumps(payload, ensure_ascii=False, indent=2, allow_nan=False) + "\n", encoding="utf-8")
print(json.dumps({"session_count": len(audit), "totals": totals}, ensure_ascii=False, indent=2))
return 0
if __name__ == "__main__":
raise SystemExit(main())
tools/audit_rtk_imu_heldout_innovation.py
+158
View File
@@ -0,0 +1,158 @@
#!/usr/bin/env python3
'''Independent prediction innovations on the frozen 267 held-out windows.'''
from __future__ import annotations
import argparse,json,sys
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT=Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
from rtk_imu.rtk_imu_engineering import _height_reference
from rtk_imu.rtk_imu_multisource import load_unified_sessions
from rtk_imu.rtk_imu_node_graph import build_problem
from tools.audit_rtk_imu_factor_consistency import _jsonable
from tools.audit_rtk_imu_innovation_noise import (
_factor_report,_hpr_innovations,_interval_innovations)
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
LIMITS={
'best_position':{'bias_norm':.10,'vector_p95':.50},
'doppler':{'bias_norm':.10,'vector_p95':.50},
'hpr':{'bias_norm':.01,'vector_p95':.05},
}
def _finite_max_abs(values):
a=np.asarray(values,dtype=float)
return float(np.max(np.abs(a[np.isfinite(a)]))) if np.any(np.isfinite(a)) else np.inf
def _summary_gate(summary,limits,min_samples=20):
if summary.get('sample_count',0)<min_samples:
return {'evaluated':False,'passed':True,'reason':'insufficient_samples'}
checks={'bias_norm':float(np.linalg.norm(summary['innovation_bias']))<=limits['bias_norm'],
'vector_p95':summary['vector_p95']<=limits['vector_p95'],
'lag1_autocorrelation_abs':_finite_max_abs(
summary['temporal_autocorrelation_lag1'])<=.95}
return {'evaluated':True,'thresholds':{**limits,'lag1_abs_max':.95},
'checks':checks,'passed':bool(all(checks.values()))}
def _factor_gate(report,key):
limits=LIMITS[key]; overall=_summary_gate(report['overall'],limits,1)
grouped={}
for group_name in ('by_session','by_motion','by_speed','by_gyro_norm'):
grouped[group_name]={name:_summary_gate(value,limits)
for name,value in report[group_name].items()}
evaluated=[gate for group in grouped.values() for gate in group.values()
if gate['evaluated']]
passed=overall['passed'] and all(gate['passed'] for gate in evaluated)
return {'overall':overall,'grouped':grouped,'passed':bool(passed)}
def _calibration_biases(engineering,problems):
source=engineering['prior_constrained_solution'].get(
'calibration_only_frozen_bias_by_session')
if not source:
raise RuntimeError('engineering result lacks calibration-only frozen bias')
by_date={}
for session_id,value in source.items():
by_date.setdefault(session_id.split('_')[1],[]).append(value)
result={}
for problem in problems:
session_id=problem.segment.session_id
if session_id in result: continue
if session_id in source:
value=source[session_id]; origin='same_calibration_session'
else:
values=by_date.get(session_id.split('_')[1],list(source.values()))
weights=np.asarray([x['node_count'] for x in values],dtype=float)
value={'gyro_bias_rad_s':np.average(
[x['gyro_bias_rad_s'] for x in values],axis=0,weights=weights),
'accel_bias_m_s2':np.average(
[x['accel_bias_m_s2'] for x in values],axis=0,weights=weights)}
origin=('calibration_date_weighted_mean' if
session_id.split('_')[1] in by_date else 'calibration_global_weighted_mean')
result[session_id]={'gyro_bias_rad_s':value['gyro_bias_rad_s'],
'accel_bias_m_s2':value['accel_bias_m_s2'],'source':origin}
return result
def main():
p=argparse.ArgumentParser(description=__doc__)
p.add_argument('--manifest',type=Path,required=True)
p.add_argument('--calibration-selection',type=Path,required=True)
p.add_argument('--all-selection',type=Path,required=True)
p.add_argument('--engineering-result',type=Path,required=True)
p.add_argument('--output',type=Path,required=True)
p.add_argument('--sample-period-s',type=float,default=1.)
p.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
p.add_argument('--rotation-rpy-deg',nargs=3,type=float,
default=[.4543066225,-.0026392019,.0122384129])
p.add_argument('--circle-session',default='0808_20260808_092827')
p.add_argument('--left-right-session',default='0808_20260808_082148')
p.add_argument('--slope-session',default='0815_20260812_123424')
args=p.parse_args()
engineering=json.loads(args.engineering_result.read_text(encoding='utf-8'))
calibration=json.loads(args.calibration_selection.read_text(encoding='utf-8'))
selected=json.loads(args.all_selection.read_text(encoding='utf-8'))
calibration_ids={x['candidate_id'] for x in calibration['selected_windows']}
heldout=[x for x in selected['selected_windows']
if x['candidate_id'] not in calibration_ids]
if len(calibration_ids)!=47 or len(heldout)!=267:
raise RuntimeError(f'expected 47+267 windows, got {len(calibration_ids)}+{len(heldout)}')
shared=sum(x.get('shared_sample_count_with_previous',{}).get(k,0)
for x in selected['selected_windows'] for k in ('imu','gnss','hpr'))
if shared: raise RuntimeError(f'selection contains {shared} shared samples')
lever=np.asarray(engineering.get('engineering_l_I_m',
engineering['prior_constrained_solution']['result']['final_l_I_m']),dtype=float)
sessions=load_unified_sessions(
args.manifest,selected_session_ids={x['session_id'] for x in heldout})
reference=_height_reference(sessions)
segments=_restore_segments(sessions,reference,heldout,args.sample_period_s)
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
problems=[build_problem(segment,rotation,lever,args.hpr_direct_sigma_rad)
for segment in segments]
session_by_id={session.session_id:session for session in sessions}
motion_by_session={args.circle_session:'circle',
args.left_right_session:'left_right',args.slope_session:'slope'}
records={key:[] for key in ('best_position','doppler','hpr','imu_preintegration')}
biases=_calibration_biases(engineering,problems)
for problem in problems:
motion=motion_by_session.get(problem.segment.session_id,'other_recovered_dynamic')
bias=biases[problem.segment.session_id]
interval=_interval_innovations(problem,motion,
bias['gyro_bias_rad_s'],bias['accel_bias_m_s2'])
for key,value in interval.items(): records[key].extend(value)
records['hpr'].extend(_hpr_innovations(
session_by_id[problem.segment.session_id],problem,motion))
factors={key:_factor_report(value) for key,value in records.items()}
gates={key:_factor_gate(factors[key],key)
for key in ('best_position','doppler','hpr')}
passed=bool(all(value['passed'] for value in gates.values()))
payload={'scope':'267-window independent held-out prediction innovations',
'least_squares_called':False,'target_observation_used_by_predictor':False,
'lever_reoptimized':False,'rotation_reoptimized':False,
'covariance_parameters_modified':False,
'fixed_l_I_m':lever,'fixed_rotation_rpy_deg':args.rotation_rpy_deg,
'calibration_window_count':47,'heldout_window_count':267,
'calibration_heldout_overlap_count':0,
'motion_class_mapping':motion_by_session,
'prediction_definition':{
'BEST_position':'previous GNSS p/v + IMU preintegration + fixed lever',
'Doppler':'previous GNSS velocity + IMU preintegration + omega-cross-lever',
'HPR':'withheld direct HPR predicted by LOO interpolation and gyro propagation',
'IMU_preintegration':'two independently GNSS/HPR-anchored endpoints'},
'bias_source':('frozen prior-node bias from the disjoint 47-window calibration set; '
'same-session when available, otherwise calibration date-weighted mean; '
'no held-out target observation and no covariance writeback'),
'per_session_frozen_bias':biases,
'factors':factors,'validation_limits_are_physical_not_covariance_retuning':LIMITS,
'factor_gates':gates,'independent_heldout_innovation_passed':passed}
args.output.parent.mkdir(parents=True,exist_ok=True)
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
compact={key:{'samples':value['overall'].get('sample_count',0),
'bias':value['overall'].get('innovation_bias'),
'vector_p95':value['overall'].get('vector_p95'),
'nis_per_dof':value['overall'].get('nis_per_dof'),
'gate':gates.get(key)} for key,value in factors.items()}
print(json.dumps(_jsonable({'factors':compact,
'independent_heldout_innovation_passed':passed}),ensure_ascii=False,indent=2))
return 0
if __name__=='__main__': raise SystemExit(main())
tools/audit_rtk_imu_innovation_noise.py
+286
View File
@@ -0,0 +1,286 @@
#!/usr/bin/env python3
'''Independent prediction/innovation noise audit for node-state RTK/IMU factors.'''
from __future__ import annotations
import argparse, json, math, sys
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
from imu_lidar.geometry import so3_log
from imu_lidar.imu_preintegration import apply_bias_correction_imu,preintegrate_gyro
from rtk_imu.rtk_imu_engineering import (
G_ENU,HPR_DIRECT_ANGULAR_SIGMA_RAD,_all_hpr,_height_reference,
_world_rtk)
from rtk_imu.rtk_imu_multisource import load_unified_sessions
from rtk_imu.rtk_imu_node_graph import build_problem
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_jsonable
from tools.run_rtk_imu_node_graph_fixed_lever import _select_window
BEST_SIGMA = np.array([.06,.06,.12])
DOPPLER_SIGMA = np.array([.15,.15,.30])
def _whiten(value,covariance):
covariance = .5*(covariance+covariance.T)+np.eye(len(value))*1e-12
return np.linalg.solve(np.linalg.cholesky(covariance),value)
def _bin_speed(value):
if value < .2: return 'speed_lt_0p2'
if value < 1.: return 'speed_0p2_to_1'
return 'speed_ge_1'
def _bin_gyro(value):
if value < .02: return 'gyro_lt_0p02'
if value < .10: return 'gyro_0p02_to_0p10'
return 'gyro_ge_0p10'
def _bin_gap(value):
if not np.isfinite(value): return 'gap_unavailable'
if value <= .12: return 'gap_direct'
if value <= .3: return 'gap_0p12_to_0p3'
if value <= .5: return 'gap_0p3_to_0p5'
return 'gap_gt_0p5'
def _distribution(values):
a = np.asarray(values,dtype=float).reshape(-1)
if not a.size:
return {'count':0,'rms':np.nan,'p50_abs':np.nan,'p95_abs':np.nan,'p99_abs':np.nan}
return {'count':len(a),'rms':float(np.sqrt(np.mean(a*a))),
'p50_abs':float(np.percentile(np.abs(a),50.)),
'p95_abs':float(np.percentile(np.abs(a),95.)),
'p99_abs':float(np.percentile(np.abs(a),99.))}
def _autocorrelation(vectors):
a = np.asarray(vectors,dtype=float)
result = np.full(a.shape[1] if a.ndim == 2 else 0,np.nan)
if a.ndim != 2 or len(a) < 3: return result
for axis in range(a.shape[1]):
left,right = a[:-1,axis],a[1:,axis]
if np.std(left)>1e-12 and np.std(right)>1e-12:
result[axis] = np.corrcoef(left,right)[0,1]
return result
def _summarize(records):
if not records: return {'sample_count':0}
residual = np.asarray([item['residual'] for item in records])
normalized = np.asarray([item['normalized'] for item in records])
factor_only = np.asarray([item['normalized_factor_only'] for item in records])
nis = np.sum(normalized*normalized,axis=1)
dimension = residual.shape[1]
effective_dimension = int(records[0].get('effective_dimension',dimension))
total_dof = len(records)*effective_dimension
norm = np.linalg.norm(residual,axis=1)
centered_t = np.asarray([item['t_s'] for item in records],dtype=float)
centered_t -= np.mean(centered_t)
temporal_drift = np.zeros(dimension)
if len(records)>=3 and np.ptp(centered_t)>1e-9:
temporal_drift = np.asarray([
np.polyfit(centered_t,residual[:,axis],1)[0]
for axis in range(dimension)])
return {'sample_count':len(records),'dimension':dimension,
'effective_dof_per_sample':effective_dimension,
'innovation_bias':np.mean(residual,axis=0),
'innovation_distribution':_distribution(residual),
'axis_rms':np.sqrt(np.mean(residual*residual,axis=0)),
'axis_p50_abs':np.percentile(np.abs(residual),50.,axis=0),
'axis_p95_abs':np.percentile(np.abs(residual),95.,axis=0),
'axis_p99_abs':np.percentile(np.abs(residual),99.,axis=0),
'vector_rms':float(np.sqrt(np.mean(norm*norm))),
'vector_p50':float(np.percentile(norm,50.)),
'vector_p95':float(np.percentile(norm,95.)),
'vector_p99':float(np.percentile(norm,99.)),
'normalized_distribution':_distribution(normalized),
'empirical_covariance':np.cov(residual,rowvar=False),
'nis_distribution':_distribution(nis),
'nis_total':float(np.sum(nis)),'nis_per_dof':float(np.sum(nis)/total_dof),
'alpha_factor':float(np.sum(nis)/total_dof),
'alpha_composite_prediction':float(np.sum(nis)/total_dof),
'alpha_factor_only_upper_bound':float(np.sum(factor_only*factor_only)/total_dof),
'temporal_autocorrelation_lag1':_autocorrelation(residual),
'temporal_linear_drift_per_s':temporal_drift}
def _group(records,key):
values = {}
for item in records: values.setdefault(str(item[key]),[]).append(item)
return {name:_summarize(items) for name,items in values.items()}
def _base_record(session_id,motion,t,speed,gyro_norm,gap):
return {'session':session_id,'motion':motion,'t_s':float(t),
'speed_bin':_bin_speed(speed),'gyro_bin':_bin_gyro(gyro_norm),
'hpr_gap_bin':_bin_gap(gap)}
def _interval_innovations(problem,motion,bg=None,ba=None):
records = {'best_position':[],'doppler':[],'imu_preintegration':[]}
lever = problem.fixed_l_I_m
bg=np.zeros(3) if bg is None else np.asarray(bg,dtype=float)
ba=np.zeros(3) if ba is None else np.asarray(ba,dtype=float)
nodes = problem.segment.nodes
for index,(left,right,pre) in enumerate(zip(nodes[:-1],nodes[1:],problem.segment.preintegrations)):
if not (left.hpr_factor_valid and right.hpr_factor_valid): continue
if left.velocity_enu_m_s is None or right.velocity_enu_m_s is None: continue
R0 = _world_rtk(left.baseline_enu)@problem.R_RTK_IMU
R1 = _world_rtk(right.baseline_enu)@problem.R_RTK_IMU
p_i0 = left.p_enu_m-R0@lever
p_i1 = right.p_enu_m-R1@lever
v_i0 = left.velocity_enu_m_s-R0@np.cross(left.gyro_rad_s-bg,lever)
v_i1 = right.velocity_enu_m_s-R1@np.cross(right.gyro_rad_s-bg,lever)
dt = pre.duration_s
delta_R,delta_v,delta_p=apply_bias_correction_imu(pre,bg,ba)
pred_p_i1 = p_i0+v_i0*dt+.5*G_ENU*dt*dt+R0@delta_p
pred_v_i1 = v_i0+G_ENU*dt+R0@delta_v
pred_R1 = R0@delta_R
p_innovation = pred_p_i1+pred_R1@lever-right.p_enu_m
v_innovation = pred_v_i1+pred_R1@np.cross(
right.gyro_rad_s-bg,lever)-right.velocity_enu_m_s
speed = float(np.linalg.norm(left.velocity_enu_m_s))
gyro_norm = float(pre.mean_gyro_norm)
gap = max(left.hpr_support_gap_s,right.hpr_support_gap_s)
base = _base_record(problem.segment.session_id,motion,right.t_s,speed,gyro_norm,gap)
base.update({'interval_id':f'{problem.segment.segment_id}:{index}',
'dt_s':float(dt),'R0_WI':R0})
p_cov = (np.diag(BEST_SIGMA**2)+dt*dt*np.diag(DOPPLER_SIGMA**2)
+R0@pre.cov[6:9,6:9]@R0.T+np.diag(BEST_SIGMA**2))
v_cov = (np.diag(DOPPLER_SIGMA**2)+R0@pre.cov[3:6,3:6]@R0.T
+np.diag(DOPPLER_SIGMA**2))
records['best_position'].append({**base,'residual':p_innovation,
'normalized':_whiten(p_innovation,p_cov),
'normalized_factor_only':p_innovation/BEST_SIGMA})
records['doppler'].append({**base,'residual':v_innovation,
'normalized':_whiten(v_innovation,v_cov),
'normalized_factor_only':v_innovation/DOPPLER_SIGMA})
imu_error = np.concatenate([
so3_log(delta_R.T@R0.T@R1),
R0.T@(v_i1-v_i0-G_ENU*dt)-delta_v,
R0.T@(p_i1-p_i0-v_i0*dt-.5*G_ENU*dt*dt)-delta_p])
anchored_cov = pre.cov.copy()
hpr_var = left.hpr_angular_sigma_rad**2+right.hpr_angular_sigma_rad**2
anchored_cov[:3,:3] += np.eye(3)*hpr_var
anchored_cov[3:6,3:6] += R0.T@np.diag(2.*DOPPLER_SIGMA**2)@R0
anchored_cov[6:9,6:9] += R0.T@np.diag(
2.*BEST_SIGMA**2+dt*dt*DOPPLER_SIGMA**2)@R0
records['imu_preintegration'].append({**base,'residual':imu_error,
'normalized':_whiten(imu_error,anchored_cov),
'normalized_factor_only':_whiten(imu_error,pre.cov)})
return records
def _hpr_innovations(session,problem,motion):
hpr = _all_hpr(session)
indices = [int(i) for i in hpr.valid_indices
if problem.segment.nodes[0].t_s <= hpr.t_s[i] <= problem.segment.nodes[-1].t_s]
records = []
baseline_I = problem.R_RTK_IMU.T[:,0]
node_times = np.asarray([node.t_s for node in problem.segment.nodes])
for position in range(1,len(indices)-1):
left,index,right = indices[position-1],indices[position],indices[position+1]
t0,t,t1 = hpr.t_s[left],hpr.t_s[index],hpr.t_s[right]
total_gap = float(t1-t0)
if not 0. < total_gap <= .5: continue
fraction = float((t-t0)/total_gap)
predicted = (1.-fraction)*hpr.baseline_enu[left]+fraction*hpr.baseline_enu[right]
predicted /= np.linalg.norm(predicted)
innovation = np.cross(predicted,hpr.baseline_enu[index])
sigma = HPR_DIRECT_ANGULAR_SIGMA_RAD*np.sqrt(
1.+(1.-fraction)**2+fraction**2)
nearest = int(np.argmin(np.abs(node_times-t)))
node = problem.segment.nodes[nearest]
speed = float(np.linalg.norm(node.velocity_enu_m_s)) if node.velocity_enu_m_s is not None else 0.
gyro_norm = float(np.linalg.norm(node.gyro_rad_s))
base = _base_record(session.session_id,motion,t,speed,gyro_norm,total_gap)
records.append({**base,'method':'leave_one_out_interpolation',
'effective_dimension':2,'residual':innovation,
'normalized':innovation/sigma,
'normalized_factor_only':innovation/HPR_DIRECT_ANGULAR_SIGMA_RAD})
gyro_pre = preintegrate_gyro(session.imu.t_s,session.imu.gyro_rad_s,float(t0),float(t))
R0 = _world_rtk(hpr.baseline_enu[left])@problem.R_RTK_IMU
propagated = R0@gyro_pre.delta_R@baseline_I
gyro_innovation = np.cross(propagated,hpr.baseline_enu[index])
gyro_sigma = np.sqrt(2.*HPR_DIRECT_ANGULAR_SIGMA_RAD**2
+float(np.trace(gyro_pre.cov))/3.)
records.append({**base,'method':'gyro_propagation',
'effective_dimension':2,'residual':gyro_innovation,
'normalized':gyro_innovation/gyro_sigma,
'normalized_factor_only':gyro_innovation/HPR_DIRECT_ANGULAR_SIGMA_RAD})
return records
def _factor_report(records):
return {'overall':_summarize(records),
'by_session':_group(records,'session'),'by_motion':_group(records,'motion'),
'by_speed':_group(records,'speed_bin'),'by_gyro_norm':_group(records,'gyro_bin'),
'by_hpr_bridge_gap':_group(records,'hpr_gap_bin'),
'by_method':_group(records,'method') if records and 'method' in records[0] else {}}
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('--manifest',type=Path,required=True)
parser.add_argument('--output',type=Path,required=True)
parser.add_argument('--circle-session',required=True)
parser.add_argument('--left-right-session',required=True)
parser.add_argument('--slope-session',required=True)
parser.add_argument('--sample-period-s',type=float,default=1.)
parser.add_argument('--target-duration-s',type=float,default=15.)
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
default=[.4543066225,-.0026392019,.0122384129])
parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
default=MECHANICAL_L_I_M.tolist())
args = parser.parse_args()
categories = {'circle':args.circle_session,'left_right':args.left_right_session,
'slope':args.slope_session}
sessions = load_unified_sessions(args.manifest,selected_session_ids=set(categories.values()))
reference = _height_reference(sessions)
if reference is None: raise RuntimeError('no BEST reference')
rotation = Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
lever = np.asarray(args.mechanical_l_I_m)
selections, all_records = {}, {
'best_position':[],'doppler':[],'hpr':[],'imu_preintegration':[]}
for motion,session_id in categories.items():
session = next(item for item in sessions if item.session_id == session_id)
segment,selection = _select_window(
[session],reference,args.sample_period_s,args.target_duration_s)
selections[motion] = selection
problem = build_problem(segment,rotation,lever)
interval = _interval_innovations(problem,motion)
for key,records in interval.items(): all_records[key].extend(records)
all_records['hpr'].extend(_hpr_innovations(session,problem,motion))
factors = {key:_factor_report(records) for key,records in all_records.items()}
payload = {
'scope':'independent prediction innovations; no node optimization/covariance writeback',
'least_squares_called':False,'covariance_parameters_modified':False,
'fixed_l_I_m':lever,'selections':selections,
'current_physical_sigma':{
'best_position_xyz_m':BEST_SIGMA,
'doppler_xyz_m_s':DOPPLER_SIGMA,
'hpr_direct_angular_rad':HPR_DIRECT_ANGULAR_SIGMA_RAD,
'bias_random_walk_source':'unchanged device/static/Allan noise model'},
'alpha_semantics':{
'alpha_factor':'innovation NIS/dof using composite prediction covariance',
'alpha_factor_only_upper_bound':(
'innovation divided only by current factor covariance; includes predictor and '
'endpoint-anchor noise and must not be written back directly')},
'factors':factors,
'recommended_covariance_writeback':False,
'covariance_freeze_assessment':{
'best_position':'predictor-confounded; unchanged',
'doppler':'predictor-confounded; unchanged',
'imu_preintegration':'endpoint-anchor dominated; unchanged',
'bias_random_walk':'static/Allan/device-model based; unchanged',
'hpr':{
'identifiable':True,
'evidence':'264 withheld samples; LOO and gyro predictions agree',
'frozen_direct_sigma_rad':.006,
'writeback_policy':'explicit audit decision, not automatic alpha'}}}
args.output.parent.mkdir(parents=True,exist_ok=True)
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
compact = {key:{'count':value['overall'].get('sample_count',0),
'bias':value['overall'].get('innovation_bias'),
'rms':value['overall'].get('innovation_distribution',{}).get('rms'),
'p95':value['overall'].get('innovation_distribution',{}).get('p95_abs'),
'alpha_composite':value['overall'].get('alpha_composite_prediction'),
'alpha_factor_only_upper_bound':value['overall'].get('alpha_factor_only_upper_bound'),
'autocorrelation':value['overall'].get('temporal_autocorrelation_lag1')}
for key,value in factors.items()}
print(json.dumps(_jsonable(compact),ensure_ascii=False,indent=2))
return 0
if __name__ == '__main__':
raise SystemExit(main())
tools/audit_rtk_imu_lever_excitation.py
+145
View File
@@ -0,0 +1,145 @@
#!/usr/bin/env python3
"""Audit strict RTK--IMU segments for lever-arm excitation and marginal information."""
from __future__ import annotations
import argparse
import json
import math
import sys
from dataclasses import asdict
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
from rtk_imu.rtk_imu_engineering import _fit_segments, _fit_summary, _segments
from rtk_imu.rtk_imu_multisource import load_unified_sessions
def _jsonable(value):
if isinstance(value, np.ndarray):
return _jsonable(value.tolist())
if isinstance(value, np.generic):
return _jsonable(value.item())
if isinstance(value, float):
return value if math.isfinite(value) else None
if hasattr(value, "__dataclass_fields__"):
return {key: _jsonable(item) for key, item in asdict(value).items()}
if isinstance(value, dict):
return {str(key): _jsonable(item) for key, item in value.items()}
if isinstance(value, (tuple, list)):
return [_jsonable(item) for item in value]
return value
def _orientation_spans_deg(segment, rotation_rtk_imu: np.ndarray) -> np.ndarray:
matrices = [segment.R_WRTK_initial @ rotation_rtk_imu]
for pre in segment.preintegrations:
matrices.append(matrices[-1] @ pre.delta_R)
euler = Rotation.from_matrix(np.asarray(matrices)).as_euler("xyz", degrees=False)
return np.degrees(np.ptp(np.unwrap(euler, axis=0), axis=0))
def _audit_segment(segment, rotation_rtk_imu: np.ndarray, max_nfev: int) -> dict[str, object]:
gyro = np.asarray([node.gyro_rad_s for node in segment.nodes])
gyro_norm = np.linalg.norm(gyro, axis=1)
best_count = sum(node.source == "BESTNAVA" for node in segment.nodes)
doppler_count = sum(
node.source == "BESTNAVA" and node.velocity_enu_m_s is not None
for node in segment.nodes
)
fit, residual, detail = _fit_segments([segment], rotation_rtk_imu, max_nfev=max_nfev)
summary = _fit_summary(fit, residual, detail)
item: dict[str, object] = {
"segment_id": segment.segment_id,
"session_id": segment.session_id,
"node_count": len(segment.nodes),
"duration_s": float(segment.nodes[-1].t_s - segment.nodes[0].t_s),
"yaw_pitch_roll_span_deg": _orientation_spans_deg(segment, rotation_rtk_imu)[[2, 1, 0]],
"gyro_rms_deg_s": np.degrees(np.sqrt(np.mean(gyro ** 2, axis=0))),
"gyro_peak_deg_s": np.degrees(np.max(np.abs(gyro), axis=0)),
"gyro_norm_rms_deg_s": float(np.degrees(np.sqrt(np.mean(gyro_norm ** 2)))),
"gyro_norm_peak_deg_s": float(np.degrees(np.max(gyro_norm))),
"bestnava_count": best_count,
"doppler_count": doppler_count,
"fit": None,
}
if summary is not None:
item["fit"] = {
"optimizer_converged": summary.optimizer_converged,
"lever_information_singular_values": summary.lever_information_singular_values,
"lever_information_condition_number": summary.lever_information_condition_number,
"lever_precision_rank": summary.lever_precision_rank,
"weakest_lever_direction_I": summary.weakest_lever_direction_I,
"lever_std_m": summary.l_I_std_m,
}
singular = summary.lever_information_singular_values
item["information_score"] = float(singular[-1]) if summary.lever_precision_rank == 3 else 0.0
else:
item["information_score"] = 0.0
spans = np.asarray(item["yaw_pitch_roll_span_deg"])
# Short strict runs rarely accumulate a full vehicle turn; retain clearly non-straight motion.
item["turn_or_slope"] = bool(spans[0] >= 3.0 or abs(spans[1]) >= 0.5 or abs(spans[2]) >= 0.5)
return item
def _recommended(items: list[dict[str, object]]) -> list[str]:
candidates = [
item for item in items
if item["turn_or_slope"] and int(item["bestnava_count"]) >= 6
and int(item["doppler_count"]) >= 6
and item["fit"] is not None
and int(item["fit"]["lever_precision_rank"]) == 3
]
candidates.sort(key=lambda item: float(item["information_score"]), reverse=True)
selected: list[str] = []
per_session: dict[str, int] = {}
for item in candidates:
session_id = str(item["session_id"])
if per_session.get(session_id, 0) >= 2:
continue
selected.append(str(item["segment_id"]))
per_session[session_id] = per_session.get(session_id, 0) + 1
return selected
def main(argv: list[str] | None = None) -> int:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--manifest", type=Path, required=True)
parser.add_argument("--output", type=Path, required=True)
parser.add_argument("--sample-period-s", type=float, default=1.0)
parser.add_argument("--rotation-rpy-deg", nargs=3, type=float,
default=[0.4543066225, -0.0026392019, 0.0122384129])
parser.add_argument("--max-nfev", type=int, default=80)
args = parser.parse_args(argv)
sessions = load_unified_sessions(args.manifest)
rotation = Rotation.from_euler("xyz", args.rotation_rpy_deg, degrees=True).as_matrix()
items = [
_audit_segment(segment, rotation, args.max_nfev)
for segment in _segments(sessions, args.sample_period_s)
]
payload = {
"session_count": len(sessions),
"strict_segment_count": len(items),
"sample_period_s": args.sample_period_s,
"rotation_rpy_deg": args.rotation_rpy_deg,
"recommended_segment_ids": _recommended(items),
"segments": items,
}
args.output.parent.mkdir(parents=True, exist_ok=True)
args.output.write_text(json.dumps(_jsonable(payload), ensure_ascii=False, indent=2, allow_nan=False) + "\n", encoding="utf-8")
print(json.dumps({
"strict_segment_count": len(items),
"recommended_segment_ids": payload["recommended_segment_ids"],
}, ensure_ascii=False, indent=2))
return 0
if __name__ == "__main__":
raise SystemExit(main())
tools/audit_rtk_imu_motion_excitation.py
+668
View File
@@ -0,0 +1,668 @@
#!/usr/bin/env python3
"""Penetration audit for RTK--IMU motion excitation.
This is read-only diagnostics. It never applies a lever prior, solves a lever arm,
or changes continuity/acceptance thresholds. Gyro trajectory integrals are the
primary excitation metrics; start/end Euler differences are deliberately absent.
"""
from __future__ import annotations
import argparse
import json
import math
import sys
from dataclasses import asdict
from pathlib import Path
from typing import Iterable
import numpy as np
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
from imu_lidar.imu_audit import audit_imu
from rtk_imu.rtk_imu_engineering import (
MIN_SEGMENT_DURATION_S,
MIN_SEGMENT_NODE_COUNT,
_all_hpr,
_height_reference,
_hpr_factor_observation,
_node_interval_threshold_s,
_trajectory_continuity_reasons,
_nearest_index,
_nodes,
_position_valid,
_segments,
_source_nodes,
)
from rtk_imu.rtk_imu_multisource import _f, _truth, load_unified_sessions
RAW_GAP_S = 1.5
def _jsonable(value):
if isinstance(value, np.ndarray):
return _jsonable(value.tolist())
if isinstance(value, np.generic):
return _jsonable(value.item())
if isinstance(value, float):
return value if math.isfinite(value) else None
if hasattr(value, "__dataclass_fields__"):
return {key: _jsonable(item) for key, item in asdict(value).items()}
if isinstance(value, dict):
return {str(key): _jsonable(item) for key, item in value.items()}
if isinstance(value, (tuple, list)):
return [_jsonable(item) for item in value]
return value
def _trapz(values: np.ndarray, t_s: np.ndarray) -> np.ndarray:
if t_s.size < 2:
return np.zeros(values.shape[1], dtype=float)
return np.trapezoid(values, t_s, axis=0)
def _interval_imu(session, start_s: float, end_s: float) -> tuple[np.ndarray, np.ndarray]:
mask = (session.imu.t_s >= start_s) & (session.imu.t_s <= end_s)
return session.imu.t_s[mask], session.imu.gyro_rad_s[mask]
def _gyro_metrics(session, start_s: float, end_s: float, gyro_offset_rad_s: np.ndarray | None = None) -> dict[str, object]:
t_s, gyro = _interval_imu(session, start_s, end_s)
if gyro_offset_rad_s is not None:
gyro = gyro - np.asarray(gyro_offset_rad_s, dtype=float).reshape(1, 3)
if t_s.size < 2:
nan = np.full(3, np.nan)
return {
"sample_count": int(t_s.size), "net_rotation_xyz_deg": nan,
"unwrap_rotation_range_xyz_deg": nan,
"cumulative_absolute_rotation_xyz_deg": nan,
"gyro_integral_squared_xyz_rad2_s": nan,
"gyro_rms_xyz_deg_s": nan, "gyro_peak_xyz_deg_s": nan,
}
dt = np.diff(t_s)
midpoint = 0.5 * (gyro[:-1] + gyro[1:])
trajectory = np.vstack([np.zeros(3), np.cumsum(midpoint * dt[:, None], axis=0)])
# The integrated trajectory is continuous. Explicit unwrap documents that
# the yaw range is never inferred from a wrapped heading/Euler endpoint.
trajectory[:, 2] = np.unwrap(trajectory[:, 2])
duration = float(t_s[-1] - t_s[0])
return {
"sample_count": int(t_s.size),
"net_rotation_xyz_deg": np.degrees(trajectory[-1]),
"unwrap_rotation_range_xyz_deg": np.degrees(np.ptp(trajectory, axis=0)),
"cumulative_absolute_rotation_xyz_deg": np.degrees(_trapz(np.abs(gyro), t_s)),
"gyro_integral_squared_xyz_rad2_s": _trapz(gyro * gyro, t_s),
"gyro_rms_xyz_deg_s": np.degrees(np.sqrt(_trapz(gyro * gyro, t_s) / duration)),
"gyro_peak_xyz_deg_s": np.degrees(np.max(np.abs(gyro), axis=0)),
}
def _interval_summary(session, start_s: float, end_s: float, *, label: str,
best_rows: Iterable[dict[str, str]], hpr) -> dict[str, object]:
best = list(best_rows)
in_range = [row for row in best if start_s <= _f(row, "t_device_s") <= end_s]
doppler = [
row for row in in_range
if _truth(row, "doppler_velocity_valid") and np.all(np.isfinite([
_f(row, "velocity_east_m_s"), _f(row, "velocity_north_m_s"),
_f(row, "vertical_speed_m_s"),
]))
]
q4 = hpr.valid & (hpr.t_s >= start_s) & (hpr.t_s <= end_s)
return {
"label": label,
"start_s": float(start_s), "end_s": float(end_s),
"duration_s": float(max(0.0, end_s - start_s)),
"bestnava_count": len(in_range), "doppler_count": len(doppler),
"q4_hpr_count": int(np.count_nonzero(q4)),
"gyro": _gyro_metrics(session, start_s, end_s),
}
def _coalesce(records: list[dict[str, object]], *, include: bool, label: str,
session, best_rows, hpr) -> list[dict[str, object]]:
result: list[dict[str, object]] = []
current: list[dict[str, object]] = []
key: tuple[str, ...] | None = None
for record in records:
active = bool(record["accepted"]) == include
reasons = tuple(record["reasons"])
same = (
current and active and key == reasons
and float(record["t_s"]) - float(current[-1]["t_s"]) <= RAW_GAP_S
)
if active and (not current or same):
current.append(record)
key = reasons
continue
if current:
summary = _interval_summary(
session, float(current[0]["t_s"]), float(current[-1]["t_s"]),
label=label, best_rows=best_rows, hpr=hpr,
)
if not include:
summary["cut_reason"] = list(key or ())
result.append(summary)
current = [record] if active else []
key = reasons if active else None
if current:
summary = _interval_summary(
session, float(current[0]["t_s"]), float(current[-1]["t_s"]),
label=label, best_rows=best_rows, hpr=hpr,
)
if not include:
summary["cut_reason"] = list(key or ())
result.append(summary)
return result
def _raw_records(session) -> list[dict[str, object]]:
"""Raw-valid is position/IMU validity; HPR support remains a separate factor audit."""
hpr = _all_hpr(session)
rows = session.rtk_by_type.get("BESTNAVA", [])
ordered = sorted(rows, key=lambda row: _f(row, "t_device_s"))
records: list[dict[str, object]] = []
last_t = -np.inf
for row in ordered:
t_s = _f(row, "t_device_s")
reasons: list[str] = []
if not np.isfinite(t_s):
reasons.append("position_device_time_invalid")
elif t_s <= last_t:
reasons.append("position_device_time_nonmonotonic")
if np.isfinite(t_s):
last_t = max(last_t, t_s)
if not _truth(row, "checksum_valid"):
reasons.append("position_checksum_invalid")
if not _truth(row, "position_fixed"):
reasons.append("position_not_fixed")
if not _position_valid(row, "BESTNAVA"):
reasons.append("position_required_field_invalid")
imu_index = _nearest_index(session.imu.t_s, t_s, 0.03) if np.isfinite(t_s) else None
if imu_index is None:
reasons.append("imu_missing_near")
_, _, hpr_factor_valid, hpr_method, hpr_gap = _hpr_factor_observation(hpr, t_s)
doppler_ok = bool(
_truth(row, "doppler_velocity_valid") and np.all(np.isfinite([
_f(row, "velocity_east_m_s"), _f(row, "velocity_north_m_s"),
_f(row, "vertical_speed_m_s"),
]))
)
records.append({
"t_s": t_s, "row": row, "accepted": not reasons,
"reasons": sorted(set(reasons)), "doppler_valid": doppler_ok,
"hpr_factor_valid": hpr_factor_valid, "hpr_factor_method": hpr_method,
"hpr_support_gap_s": hpr_gap,
})
return records
def _r0_runs_and_cuts(session, nodes, hpr, best_rows, period_s: float) -> tuple[list[dict[str, object]], list[dict[str, object]], list[dict[str, object]]]:
runs: list[list] = []
cuts: list[dict[str, object]] = []
intervals: list[dict[str, object]] = []
if not nodes:
return [], [], []
current = [nodes[0]]
threshold = _node_interval_threshold_s(period_s)
for previous, node in zip(nodes[:-1], nodes[1:]):
dt = float(node.t_s - previous.t_s)
structural_reasons = list(_trajectory_continuity_reasons(session, previous.t_s, node.t_s, period_s))
continuity_break = node.continuity_id != previous.continuity_id
reasons = structural_reasons or (["position_source_quality_or_merge_break"] if continuity_break else [])
intervals.append({
"left_t_s": float(previous.t_s), "right_t_s": float(node.t_s),
"dt_s": dt, "threshold_s": threshold,
"trajectory_continuous": not structural_reasons,
"continuity_id_changed": continuity_break,
"cut_reason": reasons,
"left_hpr_factor": {"valid": previous.hpr_factor_valid, "method": previous.hpr_factor_method,
"support_gap_s": previous.hpr_support_gap_s},
"right_hpr_factor": {"valid": node.hpr_factor_valid, "method": node.hpr_factor_method,
"support_gap_s": node.hpr_support_gap_s},
})
if not continuity_break:
current.append(node)
continue
runs.append(current)
cuts.append({
**_interval_summary(session, previous.t_s, node.t_s, label="r0_cut", best_rows=best_rows, hpr=hpr),
"dt_s": dt, "threshold_s": threshold, "cut_reason": reasons,
})
current = [node]
runs.append(current)
summaries = [
_interval_summary(session, run[0].t_s, run[-1].t_s, label="R0_after_cuts", best_rows=best_rows, hpr=hpr)
| {
"node_count": len(run), "continuity_id": int(run[0].continuity_id),
"bestnava_count": sum(node.source == "BESTNAVA" for node in run),
"doppler_count": sum(node.velocity_enu_m_s is not None for node in run),
"hpr_factor_count": sum(node.hpr_factor_valid for node in run),
"hpr_factor_rejected_count": sum(not node.hpr_factor_valid for node in run),
}
for run in runs
]
return summaries, cuts, intervals
def _qualified_summary(session, segments, hpr, best_rows) -> list[dict[str, object]]:
return [
_interval_summary(session, segment.nodes[0].t_s, segment.nodes[-1].t_s,
label="qualified_segment", best_rows=best_rows, hpr=hpr)
| {
"segment_id": segment.segment_id, "node_count": len(segment.nodes),
"bestnava_count": sum(node.source == "BESTNAVA" for node in segment.nodes),
"doppler_count": sum(node.velocity_enu_m_s is not None for node in segment.nodes),
"hpr_factor_count": sum(node.hpr_factor_valid for node in segment.nodes),
"hpr_factor_rejected_count": sum(not node.hpr_factor_valid for node in segment.nodes),
}
for segment in segments
]
def _dropped_r0_runs(session, r0_nodes, qualified, hpr, best_rows) -> list[dict[str, object]]:
qualified_ranges = [(s.nodes[0].t_s, s.nodes[-1].t_s) for s in qualified]
result: list[dict[str, object]] = []
by_id: dict[int, list] = {}
for node in r0_nodes:
by_id.setdefault(node.continuity_id, []).append(node)
for run in by_id.values():
start_s, end_s = run[0].t_s, run[-1].t_s
retained = any(abs(start_s - left) < 1e-6 and abs(end_s - right) < 1e-6 for left, right in qualified_ranges)
if retained:
continue
reasons = []
if len(run) < MIN_SEGMENT_NODE_COUNT:
reasons.append("qualified_min_node_count")
if end_s - start_s < MIN_SEGMENT_DURATION_S:
reasons.append("qualified_min_duration")
if not reasons:
reasons.append("preintegration_or_segment_validation")
result.append({
**_interval_summary(session, start_s, end_s, label="dropped_before_qualified",
best_rows=best_rows, hpr=hpr),
"node_count": len(run), "cut_reason": reasons,
})
return result
def _interval_overlap(left: dict[str, object], right: dict[str, object]) -> float:
return max(0.0, min(float(left["end_s"]), float(right["end_s"])) - max(float(left["start_s"]), float(right["start_s"])))
def _interval_penetration(raw_intervals, r0_intervals, qualified_intervals, cut_intervals):
"""Link every raw-valid dynamic interval to its downstream R0/qualified survivors."""
result = []
for raw in raw_intervals:
r0 = [item for item in r0_intervals if _interval_overlap(raw, item) > 0.0 or (
item["start_s"] == item["end_s"] and raw["start_s"] <= item["start_s"] <= raw["end_s"]
)]
qualified = [item for item in qualified_intervals if _interval_overlap(raw, item) > 0.0]
cuts = [item for item in cut_intervals if _interval_overlap(raw, item) > 0.0]
raw_best = max(int(raw["bestnava_count"]), 1)
raw_doppler = max(int(raw["doppler_count"]), 1)
r0_duration = sum(_interval_overlap(raw, item) for item in r0)
qualified_duration = sum(_interval_overlap(raw, item) for item in qualified)
cut_reasons = sorted({reason for item in cuts for reason in item.get("cut_reason", [])})
result.append({
"raw_start_s": raw["start_s"], "raw_end_s": raw["end_s"],
"raw_duration_s": raw["duration_s"], "raw_bestnava_count": raw["bestnava_count"],
"raw_doppler_count": raw["doppler_count"], "raw_gyro": raw["gyro"],
"R0_overlap_duration_s": r0_duration,
"qualified_overlap_duration_s": qualified_duration,
"R0_bestnava_count": sum(int(item["bestnava_count"]) for item in r0),
"R0_doppler_count": sum(int(item["doppler_count"]) for item in r0),
"qualified_bestnava_count": sum(int(item["bestnava_count"]) for item in qualified),
"qualified_doppler_count": sum(int(item["doppler_count"]) for item in qualified),
"retention": {
"raw_to_R0_bestnava": sum(int(item["bestnava_count"]) for item in r0) / raw_best,
"raw_to_R0_doppler": sum(int(item["doppler_count"]) for item in r0) / raw_doppler,
"raw_to_R0_duration": r0_duration / max(float(raw["duration_s"]), 1e-9),
"raw_to_qualified_bestnava": sum(int(item["bestnava_count"]) for item in qualified) / raw_best,
"raw_to_qualified_doppler": sum(int(item["doppler_count"]) for item in qualified) / raw_doppler,
"raw_to_qualified_duration": qualified_duration / max(float(raw["duration_s"]), 1e-9),
},
"cut_reason": cut_reasons,
})
return result
def _union_time_intervals(intervals: list[dict[str, object]]) -> list[tuple[float, float]]:
ordered = sorted(
(float(item["start_s"]), float(item["end_s"])) for item in intervals
if np.isfinite(float(item["start_s"])) and np.isfinite(float(item["end_s"]))
)
merged: list[list[float]] = []
for start_s, end_s in ordered:
if end_s < start_s:
continue
if not merged or start_s > merged[-1][1]:
merged.append([start_s, end_s])
else:
merged[-1][1] = max(merged[-1][1], end_s)
return [(start_s, end_s) for start_s, end_s in merged]
def _stage_statistics(session, intervals: list[dict[str, object]]) -> dict[str, object]:
"""Audit each stage on unique IMU samples over the union of its time ranges."""
total = _stage_total(intervals)
union = _union_time_intervals(intervals)
selected_count = 0
unique_mask = np.zeros(session.imu.t_s.size, dtype=bool)
for item in intervals:
mask = (session.imu.t_s >= float(item["start_s"])) & (session.imu.t_s <= float(item["end_s"]))
selected_count += int(np.count_nonzero(mask))
unique_mask |= mask
unique_count = int(np.count_nonzero(unique_mask))
input_duration = float(sum(max(0.0, float(item["end_s"]) - float(item["start_s"])) for item in intervals))
union_duration = float(sum(end_s - start_s for start_s, end_s in union))
net = np.zeros(3)
unwrap_range_sum = np.zeros(3)
cumulative_abs = np.zeros(3)
energy = np.zeros(3)
peak = np.zeros(3)
metric_duration = 0.0
for start_s, end_s in union:
gyro = _gyro_metrics(session, start_s, end_s)
current_net = np.asarray(gyro["net_rotation_xyz_deg"], dtype=float)
if not np.all(np.isfinite(current_net)):
continue
net += current_net
unwrap_range_sum += np.asarray(gyro["unwrap_rotation_range_xyz_deg"], dtype=float)
cumulative_abs += np.asarray(gyro["cumulative_absolute_rotation_xyz_deg"], dtype=float)
energy += np.asarray(gyro["gyro_integral_squared_xyz_rad2_s"], dtype=float)
peak = np.maximum(peak, np.asarray(gyro["gyro_peak_xyz_deg_s"], dtype=float))
metric_duration += max(0.0, end_s - start_s)
total["unique_imu_coverage"] = {
"input_interval_count": len(intervals),
"union_interval_count": len(union),
"input_duration_s": input_duration,
"union_duration_s": union_duration,
"overlap_duration_s": max(0.0, input_duration - union_duration),
"selected_imu_sample_count_before_dedup": selected_count,
"unique_imu_sample_count": unique_count,
"duplicate_imu_sample_count": selected_count - unique_count,
}
total["gyro"] = {
"net_rotation_xyz_deg": net,
"sum_interval_unwrap_rotation_range_xyz_deg": unwrap_range_sum,
"cumulative_absolute_rotation_xyz_deg": cumulative_abs,
"gyro_integral_squared_xyz_rad2_s": energy,
"gyro_rms_xyz_deg_s": np.degrees(np.sqrt(energy / max(metric_duration, 1e-9))),
"gyro_peak_xyz_deg_s": peak,
}
return total
def _stage_total(intervals: list[dict[str, object]]) -> dict[str, object]:
total = {"interval_count": len(intervals), "duration_s": 0.0, "bestnava_count": 0,
"doppler_count": 0, "q4_hpr_count": 0}
for item in intervals:
for key in ("duration_s", "bestnava_count", "doppler_count", "q4_hpr_count"):
total[key] += item[key]
return total
def _hpr_chain_diagnostics(hpr) -> dict[str, object]:
if hpr.t_s.size < 2:
return {"sample_count": int(hpr.t_s.size), "pair_count": 0}
dt = np.diff(hpr.t_s)
finite_vector = np.all(np.isfinite(hpr.baseline_enu), axis=1)
dot = np.sum(hpr.baseline_enu[:-1] * hpr.baseline_enu[1:], axis=1)
jump_deg = np.degrees(np.arccos(np.clip(dot, -1.0, 1.0)))
rate = jump_deg / np.maximum(dt, 1e-12)
return {
"sample_count": int(hpr.t_s.size),
"q4_valid_sample_count": int(np.count_nonzero(hpr.valid)),
"pair_count": int(dt.size),
"pair_with_invalid_endpoint_count": int(np.count_nonzero(~(hpr.valid[:-1] & hpr.valid[1:]))),
"dt_s_p50_p95_max": np.percentile(dt[np.isfinite(dt)], [50.0, 95.0, 100.0]),
"dt_too_short_count": int(np.count_nonzero(dt < 0.03)),
"dt_too_long_count": int(np.count_nonzero(dt > 0.25)),
"baseline_jump_rate_over_45deg_s_count": int(np.count_nonzero(
finite_vector[:-1] & finite_vector[1:] & (rate > 45.0)
)),
}
def _hpr_axis_mapping(session, hpr) -> dict[str, object]:
valid = hpr.valid & np.isfinite(hpr.t_s)
if np.count_nonzero(valid) < 8:
return {"available": False, "reason": "fewer_than_8_q4_hpr_samples"}
t = hpr.t_s[valid]
dt = np.diff(t)
keep = np.r_[True, (dt > 0.03) & (dt <= 0.25)]
t = t[keep]
# hpr arrays preserve GNHPR order after time sorting; heading must unwrap.
hpr_rows = sorted(session.rtk_by_type.get("GNHPR", []), key=lambda row: _f(row, "t_device_s"))
heading = np.unwrap(np.deg2rad(np.asarray([_f(row, "heading_deg") for row in hpr_rows])))[valid][keep]
pitch = np.asarray([_f(row, "pitch_deg") for row in hpr_rows])[valid][keep]
if t.size < 8:
return {"available": False, "reason": "insufficient_contiguous_q4_hpr"}
heading_rate = np.gradient(heading, t)
gyro = np.column_stack([np.interp(t, session.imu.t_s, session.imu.gyro_rad_s[:, axis]) for axis in range(3)])
correlation = []
for axis in range(3):
value = np.corrcoef(heading_rate, gyro[:, axis])[0, 1]
correlation.append(float(value) if np.isfinite(value) else np.nan)
best_axis = int(np.nanargmax(np.abs(correlation))) if np.any(np.isfinite(correlation)) else None
return {
"available": best_axis is not None,
"hpr_heading_unwrapped_range_deg": float(np.degrees(np.ptp(heading))),
"hpr_heading_net_rotation_deg": float(np.degrees(heading[-1] - heading[0])),
"hpr_heading_cumulative_absolute_rotation_deg": float(np.degrees(np.sum(np.abs(np.diff(heading))))),
"hpr_pitch_range_deg": float(np.ptp(pitch)),
"hpr_pitch_cumulative_absolute_change_deg": float(np.sum(np.abs(np.diff(pitch)))),
"heading_rate_to_imu_gyro_correlation_xyz": np.asarray(correlation),
"best_correlated_imu_axis": best_axis,
"expected_z_axis_correlation": correlation[2],
"note": "heading is unwrapped; sign depends on GNHPR clockwise-from-north convention",
}
def _bias_absorption_check(session, intervals: list[dict[str, object]]) -> dict[str, object]:
report = audit_imu(session.imu)
checks = []
for item in intervals:
if item["duration_s"] < 1.0:
continue
start_s, end_s = item["start_s"], item["end_s"]
raw = _gyro_metrics(session, start_s, end_s)
static_corrected = _gyro_metrics(session, start_s, end_s, report.gyro_bias_rad_s)
t, gyro = _interval_imu(session, start_s, end_s)
mean = np.mean(gyro, axis=0) if gyro.size else np.zeros(3)
mean_removed = _gyro_metrics(session, start_s, end_s, mean)
raw_abs = np.asarray(raw["cumulative_absolute_rotation_xyz_deg"])
removed_abs = np.asarray(mean_removed["cumulative_absolute_rotation_xyz_deg"])
ratio = removed_abs / np.maximum(raw_abs, 1e-9)
checks.append({
"start_s": start_s, "end_s": end_s,
"static_bias_rad_s": report.gyro_bias_rad_s,
"segment_mean_gyro_rad_s": mean,
"segment_mean_removed_to_raw_abs_rotation_ratio_xyz": ratio,
"static_bias_corrected": static_corrected,
"mean_removal_would_absorb_motion": bool(np.any(ratio < 0.5)),
})
return {"imu_static_audit": report, "interval_checks": checks,
"note": "Engineering audit integrates raw gyro; it does not subtract a segment mean."}
def _unit_check(session) -> dict[str, object]:
gyro = session.imu.gyro_rad_s
norm = np.linalg.norm(gyro, axis=1)
p99 = float(np.percentile(norm, 99.0)) if norm.size else np.nan
return {
"gyro_p99_norm_rad_s": p99,
"gyro_p99_norm_deg_s": float(np.degrees(p99)),
"gyro_peak_norm_rad_s": float(np.max(norm)) if norm.size else np.nan,
"suspect_deg_per_second_stored_as_rad_per_second": bool(np.isfinite(p99) and p99 > 20.0),
"suspect_near_zero_gyro_scale": bool(np.isfinite(p99) and p99 < 1e-4),
"unit_contract": "unified imu.npz gyro_rad_s is radians per second",
}
def _candidate_scores(session_audit: dict[str, object]) -> dict[str, float]:
raw = session_audit["raw_valid_intervals"]
if not raw:
return {"circle": 0.0, "left_right": 0.0, "slope": 0.0}
cumulative = np.zeros(3)
net = np.zeros(3)
for interval in raw:
gyro = interval["gyro"]
value = np.asarray(gyro["cumulative_absolute_rotation_xyz_deg"], dtype=float)
signed = np.asarray(gyro["net_rotation_xyz_deg"], dtype=float)
if np.all(np.isfinite(value)):
cumulative += value
if np.all(np.isfinite(signed)):
net += signed
axis = session_audit["axis_mapping"]
heading_range = abs(float(axis.get("hpr_heading_unwrapped_range_deg", 0.0) or 0.0))
heading_abs = abs(float(axis.get("hpr_heading_cumulative_absolute_rotation_deg", 0.0) or 0.0))
heading_net = abs(float(axis.get("hpr_heading_net_rotation_deg", 0.0) or 0.0))
pitch_range = abs(float(axis.get("hpr_pitch_range_deg", 0.0) or 0.0))
pitch_abs = abs(float(axis.get("hpr_pitch_cumulative_absolute_change_deg", 0.0) or 0.0))
return {
"circle": max(heading_range, cumulative[2]),
"left_right": max(0.0, heading_abs - heading_net, cumulative[2] - abs(net[2])),
"slope": max(cumulative[0], cumulative[1]) ** 2 / max(1.0, cumulative[2]),
}
def _select_candidates(audits: list[dict[str, object]]) -> dict[str, dict[str, object] | None]:
remaining = list(audits)
chosen: dict[str, dict[str, object] | None] = {}
for kind in ("circle", "left_right", "slope"):
ranked = sorted(remaining, key=lambda item: item["candidate_scores"][kind], reverse=True)
choice = ranked[0] if ranked and ranked[0]["candidate_scores"][kind] > 0.0 else None
chosen[kind] = None if choice is None else {
"session_id": choice["session_id"], "score_deg": choice["candidate_scores"][kind],
"selection_metric": {
"circle": "max(unwrapped HPR heading range, raw gyro-z net/absolute rotation)",
"left_right": "unwrapped HPR heading cumulative change minus net change, cross-checked with gyro-z",
"slope": "tilt-dominance: max(raw gyro-x/y cumulative rotation)^2 / raw gyro-z cumulative rotation",
}[kind],
}
if choice is not None:
remaining.remove(choice)
return chosen
def _audit_session(session, period_s: float) -> dict[str, object]:
reference = _height_reference([session])
hpr = _all_hpr(session)
best_rows = session.rtk_by_type.get("BESTNAVA", [])
records = _raw_records(session)
raw_valid = _coalesce(records, include=True, label="raw_valid", session=session,
best_rows=best_rows, hpr=hpr)
raw_rejected = _coalesce(records, include=False, label="dropped_before_raw_valid", session=session,
best_rows=best_rows, hpr=hpr)
r0_nodes = [] if reference is None else _nodes(session, reference, period_s)
r0_intervals, r0_cuts, r0_node_intervals = _r0_runs_and_cuts(
session, r0_nodes, hpr, best_rows, period_s
)
all_qualified = _segments([session], period_s)
qualified = [segment for segment in all_qualified if segment.session_id == session.session_id]
qualified_intervals = _qualified_summary(session, qualified, hpr, best_rows)
dropped_r0 = _dropped_r0_runs(session, r0_nodes, qualified, hpr, best_rows)
r0_selected_times = np.asarray([node.t_s for node in r0_nodes])
decimated = []
for record in records:
if not record["accepted"]:
continue
t_s = float(record["t_s"])
selected = r0_selected_times.size and np.min(np.abs(r0_selected_times - t_s)) < 1e-8
if not selected:
decimated.append({**record, "accepted": False, "reasons": ["sample_period_decimation"]})
decimation_cuts = _coalesce(decimated, include=False, label="dropped_raw_to_R0", session=session,
best_rows=best_rows, hpr=hpr)
raw_total, r0_total, qualified_total = map(_stage_total, (raw_valid, r0_intervals, qualified_intervals))
retention = {
"raw_to_R0": {
"bestnava_count_ratio": r0_total["bestnava_count"] / max(raw_total["bestnava_count"], 1),
"doppler_count_ratio": r0_total["doppler_count"] / max(raw_total["doppler_count"], 1),
"duration_ratio": r0_total["duration_s"] / max(raw_total["duration_s"], 1e-9),
},
"R0_to_qualified": {
"bestnava_count_ratio": qualified_total["bestnava_count"] / max(r0_total["bestnava_count"], 1),
"doppler_count_ratio": qualified_total["doppler_count"] / max(r0_total["doppler_count"], 1),
"duration_ratio": qualified_total["duration_s"] / max(r0_total["duration_s"], 1e-9),
},
}
audit = {
"session_id": session.session_id, "batch_id": session.batch_id,
"raw_valid_intervals": raw_valid, "R0_after_cuts_intervals": r0_intervals,
"qualified_segments": qualified_intervals,
"stage_statistics": {
"raw_valid": _stage_statistics(session, raw_valid),
"R0_after_cuts": _stage_statistics(session, r0_intervals),
"qualified": _stage_statistics(session, qualified_intervals),
},
"retention": retention,
"cut_intervals": [*raw_rejected, *decimation_cuts, *r0_cuts, *dropped_r0],
"r0_consecutive_node_intervals": r0_node_intervals,
"interval_penetration": _interval_penetration(
raw_valid, r0_intervals, qualified_intervals,
[*raw_rejected, *decimation_cuts, *r0_cuts, *dropped_r0],
),
"hpr_chain_diagnostics": _hpr_chain_diagnostics(hpr),
"axis_mapping": _hpr_axis_mapping(session, hpr),
"unit_check": _unit_check(session),
"bias_absorption_check": _bias_absorption_check(session, raw_valid),
}
audit["candidate_scores"] = _candidate_scores(audit)
return audit
def main(argv: list[str] | None = None) -> int:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--manifest", type=Path, required=True)
parser.add_argument("--output", type=Path, required=True)
parser.add_argument("--sample-period-s", type=float, default=1.0)
parser.add_argument("--session", action="append", help="Optional session id; may repeat.")
parser.add_argument(
"--inventory-only", action="store_true",
help="Only scan raw-valid gyro/HPR excitation; skip R0 and qualified-segment work.",
)
args = parser.parse_args(argv)
sessions = load_unified_sessions(
args.manifest,
selected_session_ids=None if args.session is None else set(args.session),
)
if args.inventory_only:
audits = []
for session in sessions:
hpr = _all_hpr(session)
best_rows = session.rtk_by_type.get("BESTNAVA", [])
records = _raw_records(session)
raw_valid = _coalesce(records, include=True, label="raw_valid", session=session,
best_rows=best_rows, hpr=hpr)
audit = {
"session_id": session.session_id,
"batch_id": session.batch_id,
"raw_valid_intervals": raw_valid,
"hpr_chain_diagnostics": _hpr_chain_diagnostics(hpr),
"axis_mapping": _hpr_axis_mapping(session, hpr),
"unit_check": _unit_check(session),
}
audit["candidate_scores"] = _candidate_scores(audit)
audits.append(audit)
scope = "raw-valid motion inventory only; no R0/qualified work or optimisation"
else:
audits = [_audit_session(session, args.sample_period_s) for session in sessions]
scope = "motion-excitation penetration audit only; no lever fit/prior/bootstrap/sensitivity"
payload = {
"scope": scope,
"sample_period_s": args.sample_period_s,
"session_count": len(audits),
"selected_dynamic_candidates": _select_candidates(audits),
"sessions": audits,
}
args.output.parent.mkdir(parents=True, exist_ok=True)
args.output.write_text(json.dumps(_jsonable(payload), ensure_ascii=False, indent=2, allow_nan=False) + "\n", encoding="utf-8")
print(json.dumps({
"session_count": len(audits),
"selected_dynamic_candidates": payload["selected_dynamic_candidates"],
}, ensure_ascii=False, indent=2))
return 0
if __name__ == "__main__":
raise SystemExit(main())
tools/audit_rtk_imu_multihorizon.py
+149
View File
@@ -0,0 +1,149 @@
#!/usr/bin/env python3
'''Multi-horizon open-loop RTK/IMU propagation audit without optimization.'''
from __future__ import annotations
import argparse, json, sys
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
from imu_lidar.imu_preintegration import preintegrate_imu
from rtk_imu.rtk_imu_engineering import (
G_ENU, _all_hpr, _height_reference, _hpr_factor_observation, _world_rtk)
from rtk_imu.rtk_imu_multisource import load_unified_sessions
from tools.audit_rtk_imu_factor_consistency import (
MECHANICAL_L_I_M, _best_arrays, _jsonable, _nearest_imu, _summary)
HORIZONS_S = (1.,2.,5.,10.,20.)
def _angle_deg(left,right):
return float(np.degrees(np.arccos(np.clip(np.dot(left,right),-1.,1.))))
def _scalar_summary(values):
a = np.asarray(values,dtype=float)
if not a.size:
return {'count':0,'rms':np.nan,'p95_abs':np.nan}
return {'count':len(a),'rms':float(np.sqrt(np.mean(a*a))),
'p95_abs':float(np.percentile(np.abs(a),95.))}
def _evaluate_session(session,reference,rotation,lever,bg,ba):
_,times,p_ant,v_ant = _best_arrays(session,reference)
hpr = _all_hpr(session)
baseline_I = rotation.T[:,0]
values = {h:{'position':[],'velocity':[],'attitude':[],'baseline':[]}
for h in HORIZONS_S}
pair_pre = []
for left,right in zip(times[:-1],times[1:]):
if not .5 <= right-left <= 1.75:
pair_pre.append(None)
else:
pair_pre.append(preintegrate_imu(
session.imu.t_s,session.imu.gyro_rad_s,session.imu.acc_m_s2,
float(left),float(right),bg,ba))
for start,t0 in enumerate(times):
baseline0,_,valid0,_,_ = _hpr_factor_observation(hpr,float(t0))
imu0 = _nearest_imu(session,float(t0))
if not valid0 or imu0 is None: continue
R0 = _world_rtk(baseline0) @ rotation
p_i0 = p_ant[start]-R0@lever
v_i0 = v_ant[start]-R0@np.cross(session.imu.gyro_rad_s[imu0]-bg,lever)
delta_R, delta_v, delta_p = np.eye(3), np.zeros(3), np.zeros(3)
elapsed, matched = 0., set()
for end in range(start+1,len(times)):
pre = pair_pre[end-1]
if pre is None: break
delta_p = delta_p + delta_v*pre.duration_s + delta_R@pre.delta_p
delta_v = delta_v + delta_R@pre.delta_v
delta_R = delta_R@pre.delta_R
elapsed = float(times[end]-t0)
if elapsed > max(HORIZONS_S)+.25: break
candidates = [h for h in HORIZONS_S if h not in matched and abs(elapsed-h) <= .25]
if not candidates: continue
horizon = min(candidates,key=lambda h:abs(elapsed-h))
matched.add(horizon)
baseline1,_,valid1,_,_ = _hpr_factor_observation(hpr,float(times[end]))
imu1 = _nearest_imu(session,float(times[end]))
if not valid1 or imu1 is None: continue
p_i1 = p_i0+v_i0*elapsed+.5*G_ENU*elapsed**2+R0@delta_p
v_i1 = v_i0+G_ENU*elapsed+R0@delta_v
R1 = R0@delta_R
predicted_p = p_i1+R1@lever
predicted_v = v_i1+R1@np.cross(session.imu.gyro_rad_s[imu1]-bg,lever)
observed_R1 = _world_rtk(baseline1)@rotation
values[horizon]['position'].append(predicted_p-p_ant[end])
values[horizon]['velocity'].append(predicted_v-v_ant[end])
values[horizon]['attitude'].append(
np.degrees(Rotation.from_matrix(observed_R1.T@R1).magnitude()))
values[horizon]['baseline'].append(_angle_deg(R1@baseline_I,baseline1))
summary = {str(int(h)):{
'position_error_m':_summary(values[h]['position']),
'velocity_error_m_s':_summary(values[h]['velocity']),
'attitude_level_completed_error_deg':_scalar_summary(values[h]['attitude']),
'observable_baseline_angular_error_deg':_scalar_summary(values[h]['baseline'])}
for h in HORIZONS_S}
return summary, values
def _summarize_values(values):
return {str(int(h)):{
'position_error_m':_summary(values[h]['position']),
'velocity_error_m_s':_summary(values[h]['velocity']),
'attitude_level_completed_error_deg':_scalar_summary(values[h]['attitude']),
'observable_baseline_angular_error_deg':_scalar_summary(values[h]['baseline'])}
for h in HORIZONS_S}
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('--manifest',type=Path,required=True)
parser.add_argument('--output',type=Path,required=True)
parser.add_argument('--session',action='append',required=True)
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
default=[.4543066225,-.0026392019,.0122384129])
parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
default=MECHANICAL_L_I_M.tolist())
parser.add_argument('--gyro-bias-rad-s',nargs=3,type=float,default=[0.,0.,0.])
parser.add_argument('--accel-bias-m-s2',nargs=3,type=float,default=[0.,0.,0.])
args = parser.parse_args()
sessions = load_unified_sessions(args.manifest,selected_session_ids=set(args.session))
reference = _height_reference(sessions)
if reference is None: raise RuntimeError('no BEST height reference')
rotation = Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
lever = np.asarray(args.mechanical_l_I_m)
bg, ba = np.asarray(args.gyro_bias_rad_s), np.asarray(args.accel_bias_m_s2)
combined = {h:{'position':[],'velocity':[],'attitude':[],'baseline':[]}
for h in HORIZONS_S}
per_session = {}
for session in sessions:
per_session[session.session_id], raw = _evaluate_session(
session,reference,rotation,lever,bg,ba)
for h in HORIZONS_S:
for key in combined[h]: combined[h][key].extend(raw[h][key])
aggregate = _summarize_values(combined)
one, twenty = aggregate['1'], aggregate['20']
growth = {
'position_p95_ratio_20s_over_1s':twenty['position_error_m']['vector_p95']/one['position_error_m']['vector_p95'],
'velocity_p95_ratio_20s_over_1s':twenty['velocity_error_m_s']['vector_p95']/one['velocity_error_m_s']['vector_p95'],
'attitude_p95_ratio_20s_over_1s':twenty['observable_baseline_angular_error_deg']['p95_abs']/one['observable_baseline_angular_error_deg']['p95_abs']}
significant = bool(
growth['position_p95_ratio_20s_over_1s'] >= 3.
and twenty['position_error_m']['vector_p95']-one['position_error_m']['vector_p95'] >= .5)
payload = {
'scope':'open-loop only; no least_squares/free/prior/LOO/bootstrap/sensitivity',
'least_squares_called':False,'rotation_source':'R2G_gravity_level_prior',
'mechanical_l_I_m':lever,'gyro_bias_rad_s':bg,'accel_bias_m_s2':ba,
'bias_source':'current nominal engineering bias; configurable CLI; no fit artifact bias was persisted',
'aggregate_by_horizon_s':aggregate,'per_session_by_horizon_s':per_session,
'one_second_reanchored_control':{
'definition':'each interval restarts from observed BEST p/v and HPR-completed R',
'result':one},
'growth_20s_over_1s':growth,
'significant_accumulated_drift':significant,
'legacy_long_segment_deterministic_model_deprecated':significant}
args.output.parent.mkdir(parents=True,exist_ok=True)
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
print(json.dumps(_jsonable({'aggregate':aggregate,'growth':growth,
'legacy_deprecated':significant}),ensure_ascii=False,indent=2))
return 0
if __name__ == '__main__':
raise SystemExit(main())
tools/audit_rtk_imu_propagation_bias_root_cause.py
+304
View File
@@ -0,0 +1,304 @@
#!/usr/bin/env python3
'''Root-cause audit for paired BEST/Doppler propagation innovations.'''
from __future__ import annotations
import argparse,json,sys
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT=Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
from rtk_imu.rtk_imu_engineering import G0,G_ENU,_height_reference,_nodes,_world_rtk
from rtk_imu.rtk_imu_multisource import load_unified_sessions
from rtk_imu.rtk_imu_node_graph import build_problem
from tools.audit_rtk_imu_factor_consistency import _jsonable
from tools.audit_rtk_imu_heldout_innovation import _calibration_biases
from tools.audit_rtk_imu_innovation_noise import _factor_report,_interval_innovations
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
def _vector_summary(values):
a=np.asarray(values,dtype=float).reshape(-1,3)
if not len(a): return {'count':0}
norm=np.linalg.norm(a,axis=1)
return {'count':len(a),'bias':np.mean(a,axis=0),
'axis_rms':np.sqrt(np.mean(a*a,axis=0)),
'axis_p95_abs':np.percentile(np.abs(a),95,axis=0),
'vector_rms':float(np.sqrt(np.mean(norm*norm))),
'vector_p95':float(np.percentile(norm,95)),
'empirical_covariance':np.cov(a,rowvar=False)}
def _correlation(left,right):
a,b=np.asarray(left),np.asarray(right); result=np.full(3,np.nan)
for axis in range(3):
if len(a)>2 and np.std(a[:,axis])>1e-12 and np.std(b[:,axis])>1e-12:
result[axis]=np.corrcoef(a[:,axis],b[:,axis])[0,1]
return result
def _root_report(position,velocity):
p={x['interval_id']:x for x in position}; v={x['interval_id']:x for x in velocity}
ids=sorted(set(p)&set(v)); records=[]
for key in ids:
left,right=p[key],v[key]; dt=float(left['dt_s'])
a_p=2.*np.asarray(left['residual'])/(dt*dt)
a_v=np.asarray(right['residual'])/dt
R=np.asarray(left['R0_WI'])
records.append({**{k:left[k] for k in (
'interval_id','session','motion','t_s','speed_bin','gyro_bin')},
'dt_s':dt,'a_position':a_p,'a_velocity':a_v,
'difference':a_v-a_p,'a_common':.5*(a_p+a_v),
'a_common_body':R.T@(.5*(a_p+a_v))})
def summarize(items):
if not items:
return {'interval_count':0,
'a_err_from_position_m_s2':{'count':0},
'a_err_from_velocity_m_s2':{'count':0},
'per_axis_correlation':[np.nan]*3,
'mean_direction_cosine':np.nan,
'mean_magnitude_ratio_velocity_over_position':np.nan,
'difference_velocity_minus_position_m_s2':{'count':0},
'common_acceleration_world_m_s2':{'count':0},
'common_acceleration_body_m_s2':{'count':0},
'equivalent_horizontal_tilt_rad':np.nan,
'equivalent_horizontal_tilt_deg':np.nan}
ap=[x['a_position'] for x in items]; av=[x['a_velocity'] for x in items]
diff=[x['difference'] for x in items]; common=[x['a_common'] for x in items]
body=[x['a_common_body'] for x in items]
mean_p=np.mean(ap,axis=0); mean_v=np.mean(av,axis=0)
denom=np.linalg.norm(mean_p)*np.linalg.norm(mean_v)
cosine=float(mean_p@mean_v/denom) if denom>1e-12 else np.nan
ratio=float(np.linalg.norm(mean_v)/max(np.linalg.norm(mean_p),1e-12))
body_bias=np.mean(body,axis=0)
body_std=np.std(body,axis=0)
horizontal=float(np.linalg.norm(body_bias[:2]))
return {'interval_count':len(items),
'a_err_from_position_m_s2':_vector_summary(ap),
'a_err_from_velocity_m_s2':_vector_summary(av),
'per_axis_correlation':_correlation(ap,av),
'mean_direction_cosine':cosine,'mean_magnitude_ratio_velocity_over_position':ratio,
'difference_velocity_minus_position_m_s2':_vector_summary(diff),
'common_acceleration_world_m_s2':_vector_summary(common),
'common_acceleration_body_m_s2':_vector_summary(body),
'body_bias_stability_std_over_bias_norm':float(
np.linalg.norm(body_std)/max(np.linalg.norm(body_bias),1e-12)),
'constant_body_accelerometer_bias_direction_stable':bool(
np.linalg.norm(body_std)<=np.linalg.norm(body_bias)),
'equivalent_horizontal_tilt_rad':horizontal/G0,
'equivalent_horizontal_tilt_deg':float(np.degrees(horizontal/G0)),
'gravity_tilt_leakage_magnitude_le_0p5deg':bool(
np.degrees(horizontal/G0)<=.5)}
def grouped(key):
groups={}
for record in records: groups.setdefault(record[key],[]).append(record)
return {name:summarize(items) for name,items in groups.items()}
overall=summarize(records)
detection_checks={}
if records:
mp=np.asarray(overall['a_err_from_position_m_s2']['bias'])
mv=np.asarray(overall['a_err_from_velocity_m_s2']['bias'])
detection_checks={'mean_direction_cosine_ge_0p95':
overall['mean_direction_cosine']>=.95,
'mean_magnitude_ratio_in_0p75_1p25':
.75<=overall['mean_magnitude_ratio_velocity_over_position']<=1.25,
'mean_acceleration_difference_norm_le_0p05_m_s2':
np.linalg.norm(mv-mp)<=.05}
detected=bool(all(detection_checks.values()))
else:
detected=False
return {'overall':overall,'per_session':grouped('session'),
'by_motion_class':grouped('motion'),'by_speed':grouped('speed_bin'),
'by_gyro_norm':grouped('gyro_bin'),
'common_acceleration_detection_gate':{
'thresholds':{'direction_cosine_min':.95,
'magnitude_ratio_range':[.75,1.25],
'mean_difference_norm_max_m_s2':.05},
'checks':detection_checks,'passed':detected},
'common_constant_acceleration_error_detected':detected},records
def _outside_targets(t,intervals):
return all(not (start-1.<=t<=end+1.) for start,end in intervals)
def _physical_static_biases(sessions,reference,R,heldout):
ranges={}
for item in heldout:
ranges.setdefault(item['session_id'],[]).append((item['start_s'],item['end_s']))
result={}
for session in sessions:
estimates=[]; times=[]
for node in _nodes(session,reference,1.):
if (node.zupt_static and node.hpr_factor_valid and
_outside_targets(node.t_s,ranges.get(session.session_id,[]))):
R_WI=_world_rtk(node.baseline_enu)@R
estimates.append(node.accel_m_s2-R_WI.T@(-G_ENU)); times.append(node.t_s)
if len(estimates)>=3:
a=np.asarray(estimates)
result[session.session_id]={'available':True,'sample_count':len(a),
'time_min_s':min(times),'time_max_s':max(times),
'physical_accel_bias_m_s2':np.median(a,axis=0),
'sample_axis_std_m_s2':np.std(a,axis=0),
'method':('target-excluded zupt_static + fixed R2G/HPR gravity; '
'horizontal components remain gravity-tilt confounded')}
else:
result[session.session_id]={'available':False,'sample_count':len(estimates),
'reason':'fewer than 3 target-excluded independent static nodes'}
return result
def _bias_distribution(values,key):
a=np.asarray([x[key] for x in values.values()],dtype=float)
return {'session_count':len(a),'mean':np.mean(a,axis=0),'std':np.std(a,axis=0),
'min':np.min(a,axis=0),'max':np.max(a,axis=0),
'peak_to_peak':np.ptp(a,axis=0)}
def main():
p=argparse.ArgumentParser(description=__doc__)
p.add_argument('--manifest',type=Path,required=True)
p.add_argument('--calibration-selection',type=Path,required=True)
p.add_argument('--all-selection',type=Path,required=True)
p.add_argument('--engineering-result',type=Path,required=True)
p.add_argument('--output',type=Path,required=True)
p.add_argument('--rotation-rpy-deg',nargs=3,type=float,
default=[.4543066225,-.0026392019,.0122384129])
args=p.parse_args()
engineering=json.loads(args.engineering_result.read_text(encoding='utf-8'))
calibration=json.loads(args.calibration_selection.read_text(encoding='utf-8'))
selected=json.loads(args.all_selection.read_text(encoding='utf-8'))
calibration_ids={x['candidate_id'] for x in calibration['selected_windows']}
heldout=[x for x in selected['selected_windows']
if x['candidate_id'] not in calibration_ids]
if len(calibration_ids)!=47 or len(heldout)!=267:
raise RuntimeError(f'expected 47+267 windows, got {len(calibration_ids)}+{len(heldout)}')
lever=np.asarray(engineering['prior_constrained_solution']['result']['final_l_I_m'])
frozen=np.array([-.4518015159,-.2644749820,.7314656115])
if not np.allclose(lever,frozen,atol=1e-10):
raise RuntimeError('candidate lever differs from frozen root-cause value')
sessions=load_unified_sessions(
args.manifest,selected_session_ids={x['session_id'] for x in heldout})
reference=_height_reference(sessions)
segments=_restore_segments(sessions,reference,heldout,1.)
R=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
problems=[build_problem(segment,R,lever,.006) for segment in segments]
calibration_bias=_calibration_biases(engineering,problems)
physical_bias=_physical_static_biases(sessions,reference,R,heldout)
motion_map={'0808_20260808_092827':'circle',
'0808_20260808_082148':'left_right',
'0815_20260812_123424':'slope'}
def evaluate(mode):
output={'best_position':[],'doppler':[],'imu_preintegration':[]}
for problem in problems:
session_id=problem.segment.session_id
if mode=='calibration_frozen':
bg=calibration_bias[session_id]['gyro_bias_rad_s']
ba=calibration_bias[session_id]['accel_bias_m_s2']
elif mode=='zero':
bg=np.zeros(3); ba=np.zeros(3)
else:
source=physical_bias[session_id]
if not source['available']: continue
bg=np.zeros(3); ba=source['physical_accel_bias_m_s2']
motion=motion_map.get(session_id,'other_recovered_dynamic')
records=_interval_innovations(problem,motion,bg,ba)
for key,value in records.items(): output[key].extend(value)
root,pairs=_root_report(output['best_position'],output['doppler'])
return {'bias_source':mode,
'BEST_position_innovation':_factor_report(output['best_position']),
'Doppler_innovation':_factor_report(output['doppler']),
'IMU_preintegration_innovation':_factor_report(output['imu_preintegration']),
'propagation_acceleration_consistency':root},pairs,output
modes={}; pairs={}; raw={}
for mode in ('calibration_frozen','zero','session_static_physical'):
modes[mode],pairs[mode],raw[mode]=evaluate(mode)
static_ids={x['interval_id'] for x in pairs['session_static_physical']}
common_subset={}
for mode in ('calibration_frozen','zero','session_static_physical'):
pos=[x for x in raw[mode]['best_position'] if x['interval_id'] in static_ids]
vel=[x for x in raw[mode]['doppler'] if x['interval_id'] in static_ids]
root,_=_root_report(pos,vel)
common_subset[mode]={'BEST_position_innovation':_factor_report(pos),
'Doppler_innovation':_factor_report(vel),
'propagation_acceleration_consistency':root}
def bias_norm(report,factor):
return float(np.linalg.norm(report[factor]['overall']['innovation_bias']))
A,C=common_subset['calibration_frozen'],common_subset['session_static_physical']
static_available=bool(static_ids)
best_reduction=(bias_norm(C,'BEST_position_innovation')/
max(bias_norm(A,'BEST_position_innovation'),1e-12)
if static_available else np.inf)
doppler_reduction=(bias_norm(C,'Doppler_innovation')/
max(bias_norm(A,'Doppler_innovation'),1e-12)
if static_available else np.inf)
nuisance_transfer=bool(static_available and best_reduction<=.5 and doppler_reduction<=.5)
high_gyro={}
for factor in ('BEST_position_innovation','Doppler_innovation'):
summary=modes['calibration_frozen'][factor]['by_gyro_norm'].get('gyro_ge_0p10',{})
limit=.5
high_gyro[factor]={'sample_count':summary.get('sample_count',0),
'bias_norm':float(np.linalg.norm(summary.get('innovation_bias',[np.inf]*3))),
'vector_p95':summary.get('vector_p95',np.inf),
'passed':bool(summary.get('sample_count',0)>=20 and
np.linalg.norm(summary['innovation_bias'])<=.10 and
summary['vector_p95']<=limit)}
common_detected=modes['calibration_frozen'][
'propagation_acceleration_consistency'][
'common_constant_acceleration_error_detected']
extrinsic_sensitive=bool(common_detected and
all(x['passed'] for x in high_gyro.values()))
propagation_passed=bool(nuisance_transfer)
graph_bias=engineering['prior_constrained_solution'][
'calibration_only_frozen_bias_by_session']
graph_distribution=_bias_distribution(graph_bias,'accel_bias_m_s2')
physical_available={k:v for k,v in physical_bias.items() if v['available']}
physical_values={k:{'accel_bias_m_s2':v['physical_accel_bias_m_s2']}
for k,v in physical_available.items()}
payload={'scope':'propagation bias root-cause only; frozen extrinsic and covariance',
'lever_reoptimized':False,'R2G_refit':False,'covariance_retuned':False,
'R0_parser_modified':False,'new_window_selection':False,
'data_only_free_bootstrap_LOO_called':False,
'fixed_l_I_m':lever,'fixed_rotation_rpy_deg':args.rotation_rpy_deg,
'calibration_window_count':47,'heldout_window_count':267,
'target_GNSS_observation_used_for_bias_estimation':False,
'bias_semantics':{
'graph_nuisance_accel_bias':(
'node-graph nuisance absorbing IMU/model/attitude effects; '
'not assumed transferable physical sensor zero bias'),
'physical_IMU_accel_bias':(
'target-excluded session static estimate; horizontal components '
'remain gravity-tilt confounded')},
'graph_nuisance_accel_bias_distribution_m_s2':graph_distribution,
'per_session_graph_nuisance_bias':graph_bias,
'per_session_static_physical_bias':physical_bias,
'static_physical_bias_distribution_m_s2':(
_bias_distribution(physical_values,'accel_bias_m_s2')
if physical_values else {'session_count':0}),
'bias_source_ablation':modes,
'common_static_interval_subset_ablation':common_subset,
'static_common_subset_interval_count':len(static_ids),
'physical_over_calibration_bias_norm_ratio':{
'BEST_position':best_reduction,'Doppler':doppler_reduction},
'common_constant_acceleration_error_detected':common_detected,
'nuisance_bias_transfer_failure_detected':nuisance_transfer,
'physical_ba_ablation_available':static_available,
'nuisance_bias_transfer_assessment':(
'confirmed' if nuisance_transfer else
'not_testable_no_independent_static_segments' if not static_available
else 'not_confirmed_by_static_ablation'),
'lever_sensitive_high_gyro_validation':high_gyro,
'independent_propagation_validation_passed':propagation_passed,
'independent_extrinsic_sensitive_validation_passed':extrinsic_sensitive,
'engineering_translation_accepted':False,
'acceptance_modified_by_this_audit':False}
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
compact={'common_constant_acceleration_error_detected':common_detected,
'static_session_count':len(physical_available),
'static_common_subset_interval_count':len(static_ids),
'bias_norm_ratio':payload['physical_over_calibration_bias_norm_ratio'],
'nuisance_bias_transfer_failure_detected':nuisance_transfer,
'high_gyro':high_gyro,
'independent_propagation_validation_passed':propagation_passed,
'independent_extrinsic_sensitive_validation_passed':extrinsic_sensitive,
'engineering_translation_accepted':False,
'calibration_frozen_acceleration':
modes['calibration_frozen']['propagation_acceleration_consistency']['overall']}
print(json.dumps(_jsonable(compact),ensure_ascii=False,indent=2))
return 0
if __name__=='__main__': raise SystemExit(main())
tools/export_g90_rtk_to_sessions.py
+314
View File
@@ -0,0 +1,314 @@
#!/usr/bin/env python3
"""Cut G90 captures into per-window RTK CSV files.
NMEA GGA/GNHPR UTC is the measurement time. Host receive UTC is retained only
for diagnostics. The measurement UTC is mapped onto the IMU device clock with
one robust affine clock model per window.
"""
from __future__ import annotations
import argparse
import csv
import json
import math
import sys
from datetime import datetime, timedelta, timezone
from pathlib import Path
import numpy as np
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
from tools.h32_dlog.timeutil import utc_dotnet_ticks_to_unix_s
from tools.rscap_v2.capture_format_v2 import file_summary, read_capture
from tools.rscap_v2.g90_rtk import RtkSentence, iter_g90_sentences
from tools.time_alignment import AffineClockModel, fit_affine_clock
LOCAL_TZ = timezone(timedelta(hours=8))
HPR_MATCH_S = 0.08
CSV_FIELDS = [
"t",
"t_measurement_utc_s",
"t_host_utc_s",
"receive_delay_s",
"t_local",
"receive_utc_ticks",
"lat_deg",
"lon_deg",
"altitude_m",
"fix_quality",
"satellites",
"hdop",
"heading_deg",
"pitch_deg",
"roll_deg",
"heading_quality",
"heading_satellites",
"heading_age_s",
"heading_station_id",
"heading_valid",
"gga_utc",
"hpr_utc",
"hpr_measurement_utc_s",
"checksum_valid",
]
def _fmt(value) -> str:
if value is None:
return ""
if isinstance(value, bool):
return "1" if value else "0"
if isinstance(value, float):
if math.isnan(value):
return ""
return f"{value:.12g}"
return str(value)
def load_imu_clock_model(
imu_csv: Path,
) -> tuple[float, float, float, float, AffineClockModel]:
"""Return device/host spans and a robust ``IMU device -> host UTC`` model."""
t_device: list[float] = []
t_host: list[float] = []
with imu_csv.open("r", encoding="utf-8", newline="") as handle:
reader = csv.DictReader(handle)
for row in reader:
t_device.append(float(row["t"]))
t_host.append(float(row["t_host_utc_s"]))
if not t_host:
raise ValueError(f"empty IMU csv: {imu_csv}")
model = fit_affine_clock(np.asarray(t_device), np.asarray(t_host))
return min(t_device), max(t_device), min(t_host), max(t_host), model
def sentence_host_s(row: RtkSentence) -> float:
return utc_dotnet_ticks_to_unix_s(row.receive_utc_ticks)
def nmea_utc_to_unix_s(value: str | None, receive_host_s: float) -> float:
"""Resolve NMEA ``hhmmss.s`` to the UTC day nearest host receive time."""
if value is None or not str(value).strip():
raise ValueError("missing NMEA UTC time")
packed = float(value)
hour = int(packed // 10000)
minute = int((packed - hour * 10000) // 100)
second = packed - hour * 10000 - minute * 100
if not (0 <= hour < 24 and 0 <= minute < 60 and 0.0 <= second < 60.0):
raise ValueError(f"invalid NMEA UTC time: {value!r}")
receive = datetime.fromtimestamp(receive_host_s, tz=timezone.utc)
midnight = datetime(
receive.year,
receive.month,
receive.day,
tzinfo=timezone.utc,
).timestamp()
same_day = midnight + hour * 3600 + minute * 60 + second
return min(
(same_day - 86400.0, same_day, same_day + 86400.0),
key=lambda candidate: abs(candidate - receive_host_s),
)
def sentence_measurement_utc_s(row: RtkSentence) -> float:
return nmea_utc_to_unix_s(
row.fields.get("position_time_utc"),
sentence_host_s(row),
)
def nearest_hpr(gga: RtkSentence, hpr_rows: list[RtkSentence]) -> RtkSentence | None:
if not hpr_rows:
return None
lo, hi = 0, len(hpr_rows) - 1
target = sentence_measurement_utc_s(gga)
while lo < hi:
mid = (lo + hi) // 2
if sentence_measurement_utc_s(hpr_rows[mid]) < target:
lo = mid + 1
else:
hi = mid
best = hpr_rows[lo]
if lo > 0 and abs(sentence_measurement_utc_s(hpr_rows[lo - 1]) - target) < abs(
sentence_measurement_utc_s(best) - target
):
best = hpr_rows[lo - 1]
if abs(sentence_measurement_utc_s(best) - target) > HPR_MATCH_S:
return None
return best
def merged_row(
gga: RtkSentence,
hpr: RtkSentence | None,
imu_to_host: AffineClockModel,
) -> dict:
t_host = sentence_host_s(gga)
t_measurement = sentence_measurement_utc_s(gga)
t_hpr = None if hpr is None else sentence_measurement_utc_s(hpr)
local = datetime.fromtimestamp(t_measurement, tz=timezone.utc).astimezone(LOCAL_TZ)
fields = gga.fields
hpr_fields = hpr.fields if hpr is not None else {}
checksum = gga.checksum_valid and (hpr is None or hpr.checksum_valid)
return {
"t": imu_to_host.inverse(t_measurement),
"t_measurement_utc_s": t_measurement,
"t_host_utc_s": t_host,
"receive_delay_s": t_host - t_measurement,
"t_local": local.strftime("%Y-%m-%dT%H:%M:%S.%f")[:-3],
"receive_utc_ticks": gga.receive_utc_ticks,
"lat_deg": fields.get("lat_deg"),
"lon_deg": fields.get("lon_deg"),
"altitude_m": fields.get("altitude_m"),
"fix_quality": fields.get("fix_quality"),
"satellites": fields.get("satellites"),
"hdop": fields.get("hdop"),
"heading_deg": hpr_fields.get("heading_deg"),
"pitch_deg": hpr_fields.get("pitch_deg"),
"roll_deg": hpr_fields.get("roll_deg"),
"heading_quality": hpr_fields.get("heading_quality"),
"heading_satellites": hpr_fields.get("heading_satellites"),
"heading_age_s": hpr_fields.get("heading_age_s"),
"heading_station_id": hpr_fields.get("heading_station_id"),
"heading_valid": hpr_fields.get("heading_valid"),
"gga_utc": fields.get("position_time_utc"),
"hpr_utc": hpr_fields.get("position_time_utc"),
"hpr_measurement_utc_s": t_hpr,
"checksum_valid": checksum,
}
def write_rtk_csv(path: Path, rows: list[dict]) -> None:
path.parent.mkdir(parents=True, exist_ok=True)
with path.open("w", newline="", encoding="utf-8") as handle:
writer = csv.DictWriter(handle, fieldnames=CSV_FIELDS)
writer.writeheader()
for row in rows:
writer.writerow({key: _fmt(row[key]) for key in CSV_FIELDS})
def discover_windows(sessions_root: Path) -> list[Path]:
windows = sorted(
path
for path in sessions_root.iterdir()
if path.is_dir() and (path / "imu.csv").is_file()
)
if not windows:
raise SystemExit(f"no session dirs with imu.csv under {sessions_root}")
return windows
def find_default_rscap(sessions_root: Path) -> Path:
parents = [sessions_root, sessions_root.parent]
matches: list[Path] = []
for folder in parents:
matches.extend(sorted(folder.glob("wheeltec-g90*.rscap")))
matches.extend(sorted(folder.glob("*g90*.rscap")))
if not matches:
raise SystemExit(f"no G90 .rscap next to {sessions_root}")
return matches[0]
def _delay_summary(rows: list[dict]) -> dict[str, float] | None:
if not rows:
return None
values = np.asarray([row["receive_delay_s"] for row in rows], dtype=np.float64)
return {
"median_s": float(np.median(values)),
"p05_s": float(np.percentile(values, 5.0)),
"p95_s": float(np.percentile(values, 95.0)),
"std_s": float(np.std(values)),
}
def main(argv: list[str] | None = None) -> int:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--sessions-root", type=Path, required=True)
parser.add_argument("--rtk-rscap", type=Path, action="append")
parser.add_argument("--overwrite", action="store_true")
args = parser.parse_args(argv)
sessions_root = args.sessions_root.resolve()
rscaps = [path.resolve() for path in (args.rtk_rscap or [find_default_rscap(sessions_root)])]
for rscap in rscaps:
if not rscap.is_file():
raise SystemExit(f"missing RTK capture: {rscap}")
sentences: list[RtkSentence] = []
captures_meta = []
for rscap in rscaps:
print(f"reading {rscap}", flush=True)
capture = read_capture(rscap)
print(f"chunks={len(capture.chunks)}", flush=True)
sentences.extend(iter_g90_sentences(capture))
captures_meta.append(file_summary(capture))
sentences.sort(key=lambda row: row.receive_utc_ticks)
gga_all = [row for row in sentences if row.sentence_type == "GGA"]
hpr_all = [row for row in sentences if row.sentence_type == "GNHPR"]
hpr_all.sort(key=sentence_measurement_utc_s)
print(
f"parsed sentences={len(sentences)} GGA={len(gga_all)} GNHPR={len(hpr_all)}",
flush=True,
)
summaries = {
"rtk_rscap": [str(path) for path in rscaps],
"captures": captures_meta,
"parsed_sentences": len(sentences),
"gga": len(gga_all),
"gnhpr": len(hpr_all),
"time_source": "NMEA measurement UTC mapped through IMU device->host affine clock",
"windows": [],
}
for window in discover_windows(sessions_root):
out_csv = window / "rtk.csv"
if out_csv.exists() and not args.overwrite:
raise SystemExit(f"{out_csv} exists; pass --overwrite")
dev_min, dev_max, host_min, host_max, imu_to_host = load_imu_clock_model(
window / "imu.csv"
)
gga = [
row
for row in gga_all
if dev_min <= imu_to_host.inverse(sentence_measurement_utc_s(row)) <= dev_max
]
hpr = [
row
for row in hpr_all
if dev_min - HPR_MATCH_S
<= imu_to_host.inverse(sentence_measurement_utc_s(row))
<= dev_max + HPR_MATCH_S
]
merged = [merged_row(row, nearest_hpr(row, hpr), imu_to_host) for row in gga]
write_rtk_csv(out_csv, merged)
brief = {
"window": window.name,
"imu_host_span_s": [host_min, host_max],
"imu_device_span_s": [dev_min, dev_max],
"imu_device_to_host_clock": imu_to_host.to_dict(),
"rtk_receive_delay": _delay_summary(merged),
"gga": len(gga),
"gnhpr_in_window": len(hpr),
"rows_written": len(merged),
"heading_matched": sum(1 for row in merged if row["heading_deg"] is not None),
"fix_quality_4_or_5": sum(
1 for row in merged if row["fix_quality"] in {4, 5}
),
"out": str(out_csv),
}
summaries["windows"].append(brief)
print(json.dumps(brief, ensure_ascii=False), flush=True)
manifest = sessions_root / "rtk_export_summary.json"
manifest.write_text(json.dumps(summaries, ensure_ascii=False, indent=2) + "\n", encoding="utf-8")
print(f"summary: {manifest}")
return 0
if __name__ == "__main__":
raise SystemExit(main())
tools/export_rtk_imu_unified.py
+290
View File
@@ -0,0 +1,290 @@
#!/usr/bin/env python3
"""Export paired G90/HI13 captures without replacing sensor time by host time."""
from __future__ import annotations
import argparse
import csv
import json
import re
import sys
from collections import Counter
from datetime import datetime, timedelta, timezone
from pathlib import Path
import numpy as np
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
from tools.h32_dlog.timeutil import utc_dotnet_ticks_to_unix_s
from tools.rscap_v2.capture_format_v2 import file_summary, read_capture
from tools.rscap_v2.g90_rtk import RtkSentence, iter_g90_sentences
from tools.rscap_v2.hi13_imu import Hi13Sample, iter_hi13_imu_samples
from tools.time_alignment import AffineClockModel, fit_affine_clock
GPS_EPOCH_UNIX_S = datetime(1980, 1, 6, tzinfo=timezone.utc).timestamp()
DEFAULT_SOURCES = (
("0808", Path("D:/data/raw_serial_capture_v2"), "20260808"),
("0815", Path("D:/data/0815/raw_serial_capture_v2"), None),
("0819", Path("D:/data/0819/raw_serial_capture_v2"), None),
)
RTK_FIELDS = [
"message_type", "t_device_s", "measurement_utc_s", "gnss_time_s",
"gnss_week", "gnss_tow_ms", "leap_seconds", "host_receive_utc_s",
"host_minus_measurement_s", "receive_utc_ticks", "checksum_valid",
"position_time_utc", "lat_deg", "lon_deg", "altitude_m",
"position_status", "position_type", "position_fixed", "fix_quality",
"satellites", "solution_satellites", "hdop", "undulation_m",
"lat_std_m", "lon_std_m", "altitude_std_m", "differential_age_s",
"solution_age_s", "station_id", "heading_deg", "pitch_deg", "roll_deg",
"heading_quality", "heading_satellites", "heading_age_s",
"heading_station_id", "heading_valid", "baseline_length_m",
"heading_type", "heading_solution_satellites", "velocity_status",
"velocity_type", "doppler_velocity_valid", "velocity_latency_s",
"velocity_age_s", "horizontal_speed_m_s", "track_ground_deg",
"velocity_east_m_s", "velocity_north_m_s", "vertical_speed_m_s",
"horizontal_speed_std_m_s", "vertical_speed_std_m_s", "raw_line",
]
def _capture_time(path: Path) -> datetime:
match = re.search(r"_(\d{8}-\d{6}\.\d+)_", path.name)
if not match:
raise ValueError(f"capture filename has no timestamp: {path}")
return datetime.strptime(match.group(1), "%Y%m%d-%H%M%S.%f")
def discover_pairs(
sources: tuple[tuple[str, Path, str | None], ...],
*,
max_start_delta_s: float = 1.5,
) -> tuple[list[dict], list[dict]]:
pairs: list[dict] = []
unmatched: list[dict] = []
for batch, folder, date_prefix in sources:
rtk = sorted(folder.glob("wheeltec-g90*.rscap"))
imu = sorted(folder.glob("hi13*.rscap"))
if date_prefix:
rtk = [path for path in rtk if date_prefix in path.name]
imu = [path for path in imu if date_prefix in path.name]
available = set(imu)
for rtk_path in rtk:
candidates = sorted(
(
(abs((_capture_time(path) - _capture_time(rtk_path)).total_seconds()), path)
for path in available
),
key=lambda item: item[0],
)
if not candidates or candidates[0][0] > max_start_delta_s:
unmatched.append({"batch": batch, "rtk_rscap": str(rtk_path)})
continue
delta, imu_path = candidates[0]
available.remove(imu_path)
stamp = _capture_time(rtk_path).strftime("%Y%m%d_%H%M%S")
pairs.append(
{
"session_id": f"{batch}_{stamp}",
"batch_id": batch,
"rtk_rscap": rtk_path,
"imu_rscap": imu_path,
"capture_start_delta_s": delta,
}
)
return pairs, unmatched
def _fit_clock(samples: list[Hi13Sample]) -> AffineClockModel:
stride = max(1, len(samples) // 20000)
selected = samples[::stride]
device = np.asarray([sample.t_s for sample in selected], dtype=float)
host = np.asarray(
[utc_dotnet_ticks_to_unix_s(sample.host_receive_utc_ticks) for sample in selected],
dtype=float,
)
return fit_affine_clock(device, host)
def _nmea_utc_to_unix_s(value: str, host_s: float) -> float:
packed = float(value)
hour = int(packed // 10000)
minute = int((packed - hour * 10000) // 100)
second = packed - hour * 10000 - minute * 100
receive = datetime.fromtimestamp(host_s, tz=timezone.utc)
midnight = datetime(receive.year, receive.month, receive.day, tzinfo=timezone.utc).timestamp()
same_day = midnight + hour * 3600 + minute * 60 + second
return min((same_day - 86400.0, same_day, same_day + 86400.0),
key=lambda candidate: abs(candidate - host_s))
def _measurement_time(row: RtkSentence) -> tuple[float, float | None]:
fields = row.fields
week = fields.get("gnss_week")
tow_ms = fields.get("gnss_tow_ms")
leap = fields.get("leap_seconds")
if week is not None and tow_ms is not None:
gnss_s = float(week) * 604800.0 + float(tow_ms) * 1e-3
utc_s = GPS_EPOCH_UNIX_S + gnss_s - float(leap or 0)
return utc_s, gnss_s
value = fields.get("position_time_utc")
if value is None:
raise ValueError(f"{row.sentence_type} has no sensor measurement time")
host_s = utc_dotnet_ticks_to_unix_s(row.receive_utc_ticks)
return _nmea_utc_to_unix_s(str(value), host_s), None
def _format(value: object) -> object:
if value is None:
return ""
if isinstance(value, bool):
return int(value)
return value
def _write_imu_npz(path: Path, samples: list[Hi13Sample]) -> None:
np.savez_compressed(
path,
system_time_s=np.asarray([row.t_s for row in samples], dtype=np.float64),
system_time_ms=np.asarray([row.system_time_ms for row in samples], dtype=np.uint32),
host_receive_utc_s=np.asarray(
[utc_dotnet_ticks_to_unix_s(row.host_receive_utc_ticks) for row in samples],
dtype=np.float64,
),
gyro_rad_s=np.asarray([row.gyro_rad_s for row in samples], dtype=np.float64),
accel_m_s2=np.asarray([row.accel_m_s2 for row in samples], dtype=np.float64),
rpy_deg=np.asarray([row.rpy_deg for row in samples], dtype=np.float64),
quaternion_wxyz=np.asarray([row.quaternion_wxyz for row in samples], dtype=np.float64),
mag_ut=np.asarray([row.mag_ut for row in samples], dtype=np.float64),
pps_sync_stamp_ms=np.asarray([row.pps_sync_stamp_ms for row in samples], dtype=np.uint16),
temperature_c=np.asarray([row.temperature_c for row in samples], dtype=np.int16),
air_pressure_pa=np.asarray([row.air_pressure_pa for row in samples], dtype=np.float64),
frame_tag=np.asarray([row.frame_tag for row in samples], dtype=np.uint8),
)
def _write_rtk_csv(
path: Path,
rows: list[RtkSentence],
clock: AffineClockModel,
) -> Counter:
counts: Counter = Counter()
with path.open("w", encoding="utf-8", newline="") as stream:
writer = csv.DictWriter(stream, fieldnames=RTK_FIELDS)
writer.writeheader()
for row in rows:
host_s = utc_dotnet_ticks_to_unix_s(row.receive_utc_ticks)
try:
measurement_s, gnss_s = _measurement_time(row)
t_device_s = clock.inverse(measurement_s)
host_minus_measurement_s = host_s - measurement_s
except (TypeError, ValueError):
measurement_s = None
gnss_s = None
t_device_s = None
host_minus_measurement_s = None
counts[f"{row.sentence_type}_invalid_measurement_time"] += 1
fields = dict(row.fields)
fields.update(
{
"message_type": row.sentence_type,
"t_device_s": t_device_s,
"measurement_utc_s": measurement_s,
"gnss_time_s": gnss_s,
"host_receive_utc_s": host_s,
"host_minus_measurement_s": host_minus_measurement_s,
"receive_utc_ticks": row.receive_utc_ticks,
"checksum_valid": row.checksum_valid,
"raw_line": row.raw_line,
}
)
writer.writerow({name: _format(fields.get(name)) for name in RTK_FIELDS})
counts[row.sentence_type] += 1
return counts
def export_pair(pair: dict, output_root: Path, *, overwrite: bool) -> dict:
destination = output_root / str(pair["session_id"])
if destination.exists() and not overwrite:
raise FileExistsError(f"{destination} exists; pass --overwrite")
destination.mkdir(parents=True, exist_ok=True)
imu_capture = read_capture(pair["imu_rscap"])
rtk_capture = read_capture(pair["rtk_rscap"])
imu_rows = iter_hi13_imu_samples(imu_capture)
if len(imu_rows) < 2:
raise ValueError(f"not enough CRC-valid HI91 rows: {pair['imu_rscap']}")
if np.any(np.diff(np.asarray([row.t_s for row in imu_rows])) <= 0):
raise ValueError(f"HI13 system_time is not strictly increasing: {pair['imu_rscap']}")
clock = _fit_clock(imu_rows)
rtk_rows = iter_g90_sentences(rtk_capture)
_write_imu_npz(destination / "imu.npz", imu_rows)
counts = _write_rtk_csv(destination / "rtk.csv", rtk_rows, clock)
quaternion = np.asarray([row.quaternion_wxyz for row in imu_rows], dtype=float)
quaternion_norm = np.linalg.norm(quaternion, axis=1)
summary = {
**{key: str(value) if isinstance(value, Path) else value for key, value in pair.items()},
"time_policy": {
"master": "HI13 system_time; RTK GNSS measurement time mapped into that clock",
"host_receive_time": "diagnostic and affine cross-clock bridge only",
},
"imu_capture": file_summary(imu_capture),
"rtk_capture": file_summary(rtk_capture),
"imu_valid_rows": len(imu_rows),
"imu_time_span_s": float(imu_rows[-1].t_s - imu_rows[0].t_s),
"quaternion_norm_p01_p50_p99": [
float(np.percentile(quaternion_norm, percentile)) for percentile in (1, 50, 99)
],
"rtk_records": dict(counts),
"clock_model_device_to_host": clock.to_dict(),
}
(destination / "export_summary.json").write_text(
json.dumps(summary, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
)
return summary
def main(argv: list[str] | None = None) -> int:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--output-root", type=Path, required=True)
parser.add_argument("--overwrite", action="store_true")
parser.add_argument("--resume", action="store_true")
parser.add_argument("--session", action="append")
args = parser.parse_args(argv)
pairs, unmatched = discover_pairs(DEFAULT_SOURCES)
if args.session:
selected = set(args.session)
pairs = [pair for pair in pairs if pair["session_id"] in selected]
args.output_root.mkdir(parents=True, exist_ok=True)
summaries = []
for index, pair in enumerate(pairs, 1):
print(f"[{index}/{len(pairs)}] {pair['session_id']}", flush=True)
summary_path = args.output_root / pair["session_id"] / "export_summary.json"
if args.resume and summary_path.is_file():
summaries.append(json.loads(summary_path.read_text(encoding="utf-8")))
continue
summaries.append(export_pair(pair, args.output_root, overwrite=args.overwrite))
manifest = {
"schema_version": 3,
"session_count": len(summaries),
"unmatched_rtk": unmatched,
"sessions": [
{
"session_id": item["session_id"],
"batch_id": item["batch_id"],
"directory": str((args.output_root / item["session_id"]).resolve()),
"rtk_records": item["rtk_records"],
"imu_valid_rows": item["imu_valid_rows"],
}
for item in summaries
],
}
(args.output_root / "manifest.json").write_text(
json.dumps(manifest, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
)
print(json.dumps({"output_root": str(args.output_root), **manifest}, ensure_ascii=False, indent=2))
return 0
if __name__ == "__main__":
raise SystemExit(main())
tools/finalize_rtk_imu_engineering_release.py
+139
View File
@@ -0,0 +1,139 @@
#!/usr/bin/env python3
'''Combine final mechanical-prior engineering release gates without refitting.'''
from __future__ import annotations
import argparse,json,sys
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT=Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
from imu_lidar.geometry import make_transform
from tools.audit_rtk_imu_factor_consistency import _jsonable
SUMMARY=('Translation is mechanically anchored and dynamically validated. '
'The current dataset does not independently observe translation accurately '
'enough for data-only calibration, and does not provide meaningful refinement '
'beyond the mechanical prior.')
def main():
p=argparse.ArgumentParser(description=__doc__)
p.add_argument('--calibration',type=Path,required=True)
p.add_argument('--heldout-postfit',type=Path,required=True)
p.add_argument('--innovation',type=Path,required=True)
p.add_argument('--sensitivity',type=Path,required=True)
p.add_argument('--convergence-retry',type=Path,required=True)
p.add_argument('--propagation-root-cause',type=Path)
p.add_argument('--output',type=Path,required=True)
args=p.parse_args()
calibration=json.loads(args.calibration.read_text(encoding='utf-8'))
heldout=json.loads(args.heldout_postfit.read_text(encoding='utf-8'))
innovation=json.loads(args.innovation.read_text(encoding='utf-8'))
sensitivity=json.loads(args.sensitivity.read_text(encoding='utf-8'))
retry=json.loads(args.convergence_retry.read_text(encoding='utf-8'))
root_cause=(None if args.propagation_root_cause is None else
json.loads(args.propagation_root_cause.read_text(encoding='utf-8')))
lever=np.asarray(calibration['prior_constrained_solution']['result']['final_l_I_m'])
R=Rotation.from_euler('xyz',[.4543066225,-.0026392019,.0122384129],
degrees=True).as_matrix()
candidate_T=make_transform(-R@lever,R)
candidate_inverse=np.linalg.inv(candidate_T)
inverse_error=float(np.linalg.norm(candidate_T@candidate_inverse-np.eye(4)))
if inverse_error>=1e-10: raise RuntimeError('candidate transforms are not inverse')
comparison=calibration['comparisons']
def nonconflicting(name):
value=comparison[name]
return (value['relative_cost_delta']<=.05 and
all(x['p95_abs_delta']<=.25
for x in value['factor_residual_delta'].values()))
mechanical_consistent=bool(
nonconflicting('fixed_vs_free') and nonconflicting('prior_data_vs_free'))
overall=heldout['heldout_validation']['overall']
physical_checks={
'all_267_converged_after_retry':
retry['heldout_convergence_after_retry']==1.,
'BEST_position_vector_p95_le_0p20_m':
overall['best_position_physical_m']['vector_p95']<=.20,
'Doppler_vector_p95_le_0p50_m_s':
overall['doppler_physical_m_s']['vector_p95']<=.50,
'HPR_normalized_p95_le_4':
overall['residual_by_factor']['hpr']['p95_abs']<=4.,
'preintegration_normalized_p95_le_3':
overall['residual_by_factor']['imu_preintegration']['p95_abs']<=3.}
physical_passed=bool(all(physical_checks.values()))
statistical_passed=bool(.25<=overall['global_chi_square_per_dof']<=4.)
underdispersion=bool(physical_passed and not statistical_passed and
overall['global_chi_square_per_dof']<.25)
independent=bool(innovation['independent_heldout_innovation_passed'])
rotation=bool(sensitivity['rotation_sensitivity_passed'])
accepted=bool(mechanical_consistent and physical_passed and independent and rotation)
payload={'scope':'final mechanical-prior RTK-IMU engineering release decision',
'no_refit_performed':True,'data_only_full_free_called':False,
'bootstrap_called':False,'loo_called':False,
'covariance_retuned':False,'new_window_selection_called':False,
'parser_R0_modified':False,
'data_only_translation_accepted':False,
'translation_refined_by_data':False,
'mechanical_prior_consistent_with_calibration':mechanical_consistent,
'heldout_physical_validation_passed':physical_passed,
'heldout_physical_gate_checks':physical_checks,
'heldout_statistical_scale_passed':statistical_passed,
'heldout_postfit_chi_square_per_dof':
overall['global_chi_square_per_dof'],
'heldout_covariance_underdispersion_warning':underdispersion,
'independent_heldout_innovation_passed':independent,
'common_constant_acceleration_error_detected':(
None if root_cause is None else
root_cause['common_constant_acceleration_error_detected']),
'independent_propagation_validation_passed':(
None if root_cause is None else
root_cause['independent_propagation_validation_passed']),
'independent_extrinsic_sensitive_validation_passed':(
None if root_cause is None else
root_cause['independent_extrinsic_sensitive_validation_passed']),
'rotation_sensitivity_passed':rotation,
'engineering_translation_acceptance_formula':(
'mechanical_prior_consistent_with_calibration AND '
'heldout_physical_validation_passed AND '
'independent_heldout_innovation_passed AND rotation_sensitivity_passed'),
'engineering_translation_accepted':accepted,
'result_nature':'mechanically anchored + dynamically validated',
'summary':SUMMARY,'forbidden_descriptions':[
'data-only calibrated translation','dynamically refined mechanical lever'],
'candidate_l_I_engineering_m':lever,
'candidate_T_RTK_IMU':candidate_T,
'candidate_T_IMU_RTK':candidate_inverse,
'candidate_transform_inverse_error_norm':inverse_error,
'l_I_engineering_m':lever if accepted else None,
'T_RTK_IMU':candidate_T if accepted else None,
'T_IMU_RTK':candidate_inverse if accepted else None,
'transform_convention':{
'equation':'p_RTK = R_RTK_IMU * p_IMU + t_RTK_IMU',
'translation':'t_RTK_IMU = -R_RTK_IMU * l_I',
'RTK_origin':'ANT1 phase center'},
'rotation_source':'R2G_gravity_level_prior',
'translation_conditional_on_rotation':True,
'evidence':{
'calibration_path':str(args.calibration),
'heldout_postfit_path':str(args.heldout_postfit),
'innovation_path':str(args.innovation),
'sensitivity_path':str(args.sensitivity),
'convergence_retry_path':str(args.convergence_retry),
'propagation_root_cause_path':(
None if args.propagation_root_cause is None
else str(args.propagation_root_cause)),
'posterior_prior_variance_ratio':
calibration['prior_constrained_solution']['posterior_prior_variance_ratio'],
'heldout_convergence_after_retry':
retry['heldout_convergence_after_retry'],
'rotation_sensitivity_summary':sensitivity['summary']}}
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
print(json.dumps(_jsonable({key:payload[key] for key in (
'data_only_translation_accepted','translation_refined_by_data',
'mechanical_prior_consistent_with_calibration',
'heldout_physical_validation_passed','heldout_statistical_scale_passed',
'heldout_covariance_underdispersion_warning',
'independent_heldout_innovation_passed','rotation_sensitivity_passed',
'engineering_translation_accepted')}),indent=2))
return 0
if __name__=='__main__': raise SystemExit(main())
tools/refine_rtk_imu_windows_by_actual_information.py
+137
View File
@@ -0,0 +1,137 @@
#!/usr/bin/env python3
'''Refine non-overlapping windows after comparing predicted and actual lever information.'''
from __future__ import annotations
import argparse,json,sys
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT=Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
from rtk_imu.rtk_imu_engineering import _height_reference
from rtk_imu.rtk_imu_multisource import load_unified_sessions
from rtk_imu.rtk_imu_node_graph import build_problem,linearized_lever_information
from tools.audit_rtk_imu_factor_consistency import _jsonable
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
from tools.select_rtk_imu_windows_by_lever_information import (
STD_GATE,_overlap,_sample_keys,_summary,_window_candidates)
def _sqrt_psd(matrix,inverse=False):
values,vectors=np.linalg.eigh(.5*(matrix+matrix.T))
values=np.maximum(values,1e-12)
scale=1./np.sqrt(values) if inverse else np.sqrt(values)
return (vectors*scale)@vectors.T
def main():
parser=argparse.ArgumentParser(description=__doc__)
parser.add_argument('--manifest',type=Path,required=True)
parser.add_argument('--base-selection',type=Path,required=True)
parser.add_argument('--actual-result',type=Path,required=True)
parser.add_argument('--output',type=Path,required=True)
parser.add_argument('--sample-period-s',type=float,default=1.)
parser.add_argument('--window-duration-s',type=float,default=15.)
parser.add_argument('--max-additional-windows',type=int,default=400)
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
default=[.4543066225,-.0026392019,.0122384129])
args=parser.parse_args()
base=json.loads(args.base_selection.read_text(encoding='utf-8'))
actual=json.loads(args.actual_result.read_text(encoding='utf-8'))
actual_solution=next(iter(actual['solutions'].values()))
lever=np.asarray(actual_solution['final_l_I_m'],dtype=float)
actual_cov=np.asarray(actual_solution['lever_covariance_m2'],dtype=float)
actual_information=np.linalg.pinv(actual_cov,rcond=1e-9)
predicted_information=sum((np.asarray(item['single_window_information'])
for item in base['selected_windows']),np.zeros((3,3)))
correction=_sqrt_psd(actual_information)@_sqrt_psd(predicted_information,True)
sessions=load_unified_sessions(args.manifest)
reference=_height_reference(sessions)
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
restored=_restore_segments(sessions,reference,base['selected_windows'],
args.sample_period_s)
session_by_id={session.session_id:session for session in sessions}
selected=[]
for item,segment in zip(base['selected_windows'],restored):
selected.append({**item,'segment':segment,
'session':session_by_id[item['session_id']]})
candidates=[entry for entry in _window_candidates(
sessions,reference,args.sample_period_s,args.window_duration_s)
if not any(_overlap(entry,item) for item in selected)]
for entry in candidates:
problem=build_problem(entry['segment'],rotation,lever,args.hpr_direct_sigma_rad)
raw,_,singular,weak=linearized_lever_information(problem,lever)
entry['raw_information']=raw
entry['information']=correction@raw@correction.T
entry['single_window_singular_values']=singular
entry['single_window_weakest_direction_I']=weak
total=actual_information.copy()
curve=[{'window_count':len(selected),'added_candidate_id':'actual_base',
'information_source':'actual_joint_free_schur',**_summary(total)}]
remaining=list(candidates); saturation_count=0
stop_reason='candidate_exhausted'
while remaining and len(selected)<len(base['selected_windows'])+args.max_additional_windows:
feasible=[entry for entry in remaining
if not any(_overlap(entry,item) for item in selected)]
if not feasible: break
ranked=[]
for entry in feasible:
summary=_summary(total+entry['information'])
ranked.append((summary['lambda_min'],summary['logdet'],entry,summary))
_,_,choice,summary=max(ranked,key=lambda item:(item[0],item[1]))
gain=summary['lambda_min']-curve[-1]['lambda_min']
selected.append(choice); remaining.remove(choice); total+=choice['information']
curve.append({'window_count':len(selected),'added_candidate_id':choice['candidate_id'],
'delta_lambda_min':gain,'information_source':'actual-calibrated prediction',
**summary})
saturation_count=saturation_count+1 if gain<.01 else 0
if np.all(np.asarray(summary['std_m'])<=STD_GATE):
stop_reason='actual_calibrated_observability_gate_reached'; break
if saturation_count>=3:
stop_reason='incremental_lambda_min_gain_saturated'; break
else:
if len(selected)>=len(base['selected_windows'])+args.max_additional_windows:
stop_reason='max_additional_windows_reached'
seen={'imu':set(),'gnss':set(),'hpr':set()}; duplicate={key:0 for key in seen}
output=[]
for order,entry in enumerate(selected):
keys=_sample_keys(entry); shared={key:len(value&seen[key]) for key,value in keys.items()}
for key,value in keys.items(): duplicate[key]+=shared[key]; seen[key].update(value)
information=np.asarray(entry['information'] if 'information' in entry
else entry['single_window_information'])
output.append({'selection_order':order,'candidate_id':entry['candidate_id'],
'session_id':entry['session_id'],'start_s':entry['start_s'],
'end_s':entry['end_s'],'duration_s':entry['duration_s'],
'node_count':entry['node_count'],'seed_window':order<len(base['selected_windows']),
'single_window_information':information,
'raw_single_window_information':entry.get('raw_information'),
'sample_count':{key:len(value) for key,value in keys.items()},
'shared_sample_count_with_previous':shared})
overlap={'duplicate_sample_count':duplicate,
'all_selected_windows_time_nonoverlapping_within_session':not any(
_overlap(a,b) for i,a in enumerate(selected) for b in selected[i+1:]),
'unique_sample_count':{key:len(value) for key,value in seen.items()}}
payload={'scope':'actual-H calibrated incremental lever-information selection',
'manual_prior_used':False,'base_window_count':len(base['selected_windows']),
'candidate_count':len(candidates),'selected_window_count':len(selected),
'actual_base_l_I_m':lever,'actual_base_information':actual_information,
'predicted_base_information':predicted_information,
'information_congruence_correction':correction,
'selection_objective':'maximize lambda_min(H_l); break ties by logdet(H_l)',
'std_gate_m':STD_GATE,'stop_reason':stop_reason,
'selected_windows':output,'lever_std_vs_information_curve':curve,
'sample_overlap_audit':overlap,
'final_calibrated_linearized_observable':bool(
np.all(np.asarray(curve[-1]['std_m'])<=STD_GATE))}
args.output.parent.mkdir(parents=True,exist_ok=True)
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
print(json.dumps(_jsonable({'selected_window_count':len(selected),
'additional_window_count':len(selected)-len(base['selected_windows']),
'stop_reason':stop_reason,'final_curve':curve[-1],
'sample_overlap_audit':overlap}),ensure_ascii=False,indent=2))
return 0
if __name__=='__main__':
raise SystemExit(main())
tools/retry_rtk_imu_heldout_nonconverged.py
+104
View File
@@ -0,0 +1,104 @@
#!/usr/bin/env python3
'''Retry only held-out fixed-lever windows that ended at max_nfev.'''
from __future__ import annotations
import argparse,json,sys
from concurrent.futures import ProcessPoolExecutor
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT=Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
from rtk_imu.rtk_imu_engineering import _height_reference
from rtk_imu.rtk_imu_multisource import load_unified_sessions
from rtk_imu.rtk_imu_node_graph import (
build_problem,fit_states_at_fixed_lever,summarize_fixed_state_values)
from tools.audit_rtk_imu_factor_consistency import _jsonable
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
def _retry(task):
index,problem,lever,state,max_nfev=task
value,meta=fit_states_at_fixed_lever(
problem,lever,max_nfev,initial_state_values=state)
return index,value,meta
def main():
p=argparse.ArgumentParser(description=__doc__)
p.add_argument('--manifest',type=Path,required=True)
p.add_argument('--calibration-selection',type=Path,required=True)
p.add_argument('--all-selection',type=Path,required=True)
p.add_argument('--engineering-result',type=Path,required=True)
p.add_argument('--checkpoint-dir',type=Path,required=True)
p.add_argument('--output',type=Path,required=True)
p.add_argument('--retry-checkpoint-dir',type=Path,required=True)
p.add_argument('--max-nfev',type=int,default=120)
p.add_argument('--workers',type=int,default=4)
p.add_argument('--rotation-rpy-deg',nargs=3,type=float,
default=[.4543066225,-.0026392019,.0122384129])
args=p.parse_args()
engineering=json.loads(args.engineering_result.read_text(encoding='utf-8'))
calibration=json.loads(args.calibration_selection.read_text(encoding='utf-8'))
selected=json.loads(args.all_selection.read_text(encoding='utf-8'))
ids={x['candidate_id'] for x in calibration['selected_windows']}
heldout=[x for x in selected['selected_windows'] if x['candidate_id'] not in ids]
lever=np.asarray(engineering['engineering_l_I_m'],dtype=float)
bad=[]
for index,item in enumerate(heldout):
data=np.load(args.checkpoint_dir/f'{index:04d}.npz',allow_pickle=False)
meta=json.loads(str(data['optimizer']))
if not meta['success']:
bad.append((index,item,data['state'].copy(),meta))
if len(bad)!=10: raise RuntimeError(f'expected 10 non-converged windows, got {len(bad)}')
sessions=load_unified_sessions(
args.manifest,selected_session_ids={item['session_id'] for _,item,_,_ in bad})
reference=_height_reference(sessions)
selections=[item for _,item,_,_ in bad]
segments=_restore_segments(sessions,reference,selections,1.)
R=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
problems=[build_problem(segment,R,lever,.006) for segment in segments]
tasks=[(local,problem,lever,bad[local][2],args.max_nfev)
for local,problem in enumerate(problems)]
with ProcessPoolExecutor(max_workers=args.workers) as executor:
retried=list(executor.map(_retry,tasks))
retried.sort(key=lambda x:x[0])
args.retry_checkpoint_dir.mkdir(parents=True,exist_ok=True)
mapping={'0808_20260808_092827':'circle',
'0808_20260808_082148':'left_right',
'0815_20260812_123424':'slope'}
results=[]
for local,state,meta in retried:
original_index,item,_,previous=bad[local]
summary=summarize_fixed_state_values([problems[local]],[state],lever)
np.savez_compressed(args.retry_checkpoint_dir/f'{original_index:04d}.npz',
state=state,optimizer=json.dumps(meta))
results.append({'heldout_index':original_index,
'candidate_id':item['candidate_id'],'session_id':item['session_id'],
'time_range_s':[item['start_s'],item['end_s']],
'motion_class':mapping.get(item['session_id'],'other_recovered_dynamic'),
'original_termination_reason':previous['message'],
'original_nfev':previous['nfev'],'original_final_cost':previous['cost'],
'retry_started_from_original_final_state':True,
'retry_termination_reason':meta['message'],'retry_nfev':meta['nfev'],
'retry_initial_cost':meta['initial_cost'],'retry_final_cost':meta['cost'],
'retry_hit_max_nfev':bool(not meta['success'] and meta['nfev']>=args.max_nfev),
'retry_success':meta['success'],'residual':summary})
converged=sum(x['retry_success'] for x in results)
payload={'scope':'10 held-out max_nfev windows; exact-model continuation retry',
'lever_reoptimized':False,'covariance_parameters_modified':False,
'parser_R0_modified':False,'windows_removed':False,
'fixed_l_I_m':lever,'retry_max_nfev':args.max_nfev,
'initial_nonconverged_count':len(results),
'converged_after_retry_count':converged,
'heldout_convergence_after_retry':(257+converged)/267.,
'remaining_nonconverged_count':len(results)-converged,
'windows':results}
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
print(json.dumps(_jsonable({'converged_after_retry':converged,
'remaining':len(results)-converged,
'heldout_convergence_after_retry':payload['heldout_convergence_after_retry'],
'windows':[{'index':x['heldout_index'],'session':x['session_id'],
'motion':x['motion_class'],'success':x['retry_success'],
'nfev':x['retry_nfev'],'cost':x['retry_final_cost']}
for x in results]}),ensure_ascii=False,indent=2))
return 0
if __name__=='__main__': raise SystemExit(main())
tools/rscap_v2/g90_rtk.py
+298
View File
@@ -0,0 +1,298 @@
"""Decode calibration-relevant Wheeltec G90 logs from a V2 capture.
GNSS-owned measurement time is preserved for every record. Host receive time
only identifies the chunk that completed the line and must not be substituted
for the measurement timestamp.
"""
from __future__ import annotations
import bisect
import math
from dataclasses import dataclass
from .capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments
def nmea_checksum_valid(line: str) -> bool:
star = line.rfind("*")
if star < 0:
return False
try:
expected = int(line[star + 1 : star + 3], 16)
except ValueError:
return False
value = 0
for char in line[1:star]:
value ^= ord(char)
return value == expected
def unicore_checksum_valid(line: str) -> bool:
"""Validate the CRC32 suffix used by Unicore hash-prefixed logs."""
star = line.rfind("*")
if star < 0:
return False
try:
expected = int(line[star + 1 : star + 9], 16)
except ValueError:
return False
crc = 0
for value in line[1:star].encode("ascii", "replace"):
crc ^= value
for _ in range(8):
crc = (crc >> 1) ^ (0xEDB88320 if crc & 1 else 0)
return (crc & 0xFFFFFFFF) == expected
def g90_checksum_valid(line: str) -> bool:
if line.startswith("$"):
return nmea_checksum_valid(line)
if line.startswith("#"):
return unicore_checksum_valid(line)
return False
def _safe_float(value: str):
try:
return float(value)
except (TypeError, ValueError):
return None
def _safe_int(value: str):
try:
return int(value)
except (TypeError, ValueError):
return None
def parse_nmea_latlon(value: str, hemisphere: str):
raw = _safe_float(value)
if raw is None:
return None
degrees = math.floor(raw / 100.0)
result = degrees + (raw - degrees * 100.0) / 60.0
if hemisphere.upper() in ("S", "W"):
result = -result
return result
def parse_gga(line: str) -> dict:
fields = line[: line.rfind("*")].split(",")
if len(fields) < 10:
raise ValueError("GGA has too few fields")
return {
"type": "GGA",
"position_time_utc": fields[1],
"lat_deg": parse_nmea_latlon(fields[2], fields[3]),
"lon_deg": parse_nmea_latlon(fields[4], fields[5]),
"fix_quality": _safe_int(fields[6]),
"satellites": _safe_int(fields[7]),
"hdop": _safe_float(fields[8]),
"altitude_m": _safe_float(fields[9]),
}
def parse_gnhpr(line: str) -> dict:
fields = line[: line.rfind("*")].split(",")
if len(fields) < 7:
raise ValueError("GNHPR has too few fields")
quality = _safe_int(fields[5])
return {
"type": "GNHPR",
"position_time_utc": fields[1],
"heading_deg": _safe_float(fields[2]),
"pitch_deg": _safe_float(fields[3]),
"roll_deg": _safe_float(fields[4]),
"heading_quality": quality,
"heading_satellites": _safe_int(fields[6]),
"heading_age_s": _safe_float(fields[7]) if len(fields) > 7 else None,
"heading_station_id": fields[8] if len(fields) > 8 else None,
"heading_valid": quality == 4,
}
def _split_unicore(line: str) -> tuple[list[str], list[str]]:
before_checksum = line[: line.rfind("*")]
header, payload = before_checksum.split(";", 1)
return header[1:].split(","), payload.split(",")
def _parse_unicore_header(fields: list[str]) -> dict:
if len(fields) < 9:
raise ValueError("Unicore ASCII header is incomplete")
return {
"gnss_week": _safe_int(fields[4]),
"gnss_tow_ms": _safe_int(fields[5]),
"leap_seconds": _safe_int(fields[8]),
}
def parse_bestnava(line: str) -> dict:
"""Parse BESTNAVA position and Doppler-velocity fields."""
header, fields = _split_unicore(line)
if len(fields) < 30:
raise ValueError("BESTNAVA has too few fields")
result = {
"type": "BESTNAVA",
**_parse_unicore_header(header),
"position_status": fields[0],
"position_type": fields[1],
"lat_deg": _safe_float(fields[2]),
"lon_deg": _safe_float(fields[3]),
"altitude_m": _safe_float(fields[4]),
"undulation_m": _safe_float(fields[5]),
"lat_std_m": _safe_float(fields[7]),
"lon_std_m": _safe_float(fields[8]),
"altitude_std_m": _safe_float(fields[9]),
"station_id": fields[10].strip('"'),
"differential_age_s": _safe_float(fields[11]),
"solution_age_s": _safe_float(fields[12]),
"satellites": _safe_int(fields[13]),
"solution_satellites": _safe_int(fields[14]),
"velocity_status": fields[21],
"velocity_type": fields[22],
"velocity_latency_s": _safe_float(fields[23]),
"velocity_age_s": _safe_float(fields[24]),
"horizontal_speed_m_s": _safe_float(fields[25]),
"track_ground_deg": _safe_float(fields[26]),
"vertical_speed_m_s": _safe_float(fields[27]),
"vertical_speed_std_m_s": _safe_float(fields[28]),
"horizontal_speed_std_m_s": _safe_float(fields[29]),
}
speed = result["horizontal_speed_m_s"]
track = result["track_ground_deg"]
if speed is not None and track is not None:
angle = math.radians(track)
result["velocity_east_m_s"] = speed * math.sin(angle)
result["velocity_north_m_s"] = speed * math.cos(angle)
else:
result["velocity_east_m_s"] = None
result["velocity_north_m_s"] = None
result["position_fixed"] = (
result["position_status"] == "SOL_COMPUTED"
and result["position_type"] == "NARROW_INT"
)
result["doppler_velocity_valid"] = (
result["velocity_status"] == "SOL_COMPUTED"
and result["velocity_type"] == "DOPPLER_VELOCITY"
)
return result
def parse_pvtslna(line: str) -> dict:
"""Parse PVTSLNA as a quality-rich fallback/diagnostic record."""
header, fields = _split_unicore(line)
if len(fields) < 34:
raise ValueError("PVTSLNA has too few fields")
speed_north = _safe_float(fields[17])
speed_east = _safe_float(fields[18])
return {
"type": "PVTSLNA",
**_parse_unicore_header(header),
"position_type": fields[0],
"altitude_m": _safe_float(fields[1]),
"lat_deg": _safe_float(fields[2]),
"lon_deg": _safe_float(fields[3]),
"altitude_std_m": _safe_float(fields[4]),
"lat_std_m": _safe_float(fields[5]),
"lon_std_m": _safe_float(fields[6]),
"differential_age_s": _safe_float(fields[7]),
"psr_position_type": fields[8],
"undulation_m": _safe_float(fields[12]),
"satellites": _safe_int(fields[13]),
"solution_satellites": _safe_int(fields[14]),
"velocity_north_m_s": speed_north,
"velocity_east_m_s": speed_east,
"horizontal_speed_m_s": (
None if speed_north is None or speed_east is None
else math.hypot(speed_north, speed_east)
),
"vertical_speed_m_s": _safe_float(fields[19]),
"heading_type": fields[20],
"baseline_length_m": _safe_float(fields[21]),
"heading_deg": _safe_float(fields[22]),
"pitch_deg": _safe_float(fields[23]),
"heading_satellites": _safe_int(fields[24]),
"heading_solution_satellites": _safe_int(fields[25]),
"gdop": _safe_float(fields[28]),
"pdop": _safe_float(fields[29]),
"hdop": _safe_float(fields[30]),
"htdop": _safe_float(fields[31]),
"tdop": _safe_float(fields[32]),
"position_fixed": fields[0] == "NARROW_INT",
}
def _chunk_starts(chunks: list[RawChunk]) -> list[int]:
starts = []
cursor = 0
for chunk in chunks:
starts.append(cursor)
cursor += len(chunk.raw)
return starts
def _host_ticks_for_span(chunks: list[RawChunk], starts: list[int], end: int) -> int:
end_index = max(0, min(len(chunks) - 1, bisect.bisect_left(starts, end) - 1))
return chunks[end_index].receive_utc_ticks
@dataclass(frozen=True)
class RtkSentence:
sentence_type: str
receive_utc_ticks: int
checksum_valid: bool
fields: dict
raw_line: str
def iter_g90_sentences(capture: CaptureFile) -> list[RtkSentence]:
"""Parse native asynchronous GGA/GNHPR/BESTNAVA/PVTSLNA records."""
rows: list[RtkSentence] = []
for _segment_id, chunks in iter_contiguous_segments(capture.chunks):
stream = b"".join(chunk.raw for chunk in chunks)
starts = _chunk_starts(chunks)
cursor = 0
while cursor < len(stream):
newline = stream.find(b"\n", cursor)
if newline < 0:
break
end = newline + 1
raw_line = stream[cursor:end].rstrip(b"\r\n")
cursor = end
if not raw_line:
continue
line = raw_line.decode("ascii", "replace")
parser = None
if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
parser = parse_gga
elif line.startswith("$GNHPR"):
parser = parse_gnhpr
elif line.startswith("#BESTNAVA"):
parser = parse_bestnava
elif line.startswith("#PVTSLNA"):
parser = parse_pvtslna
if parser is None:
continue
ticks = _host_ticks_for_span(chunks, starts, end)
try:
fields = parser(line)
except ValueError:
continue
rows.append(
RtkSentence(
sentence_type=str(fields["type"]),
receive_utc_ticks=int(ticks),
checksum_valid=g90_checksum_valid(line),
fields=fields,
raw_line=line,
)
)
rows.sort(key=lambda row: row.receive_utc_ticks)
return rows
tools/rscap_v2/hi13_imu.py
+57 -18
View File
@@ -12,6 +12,7 @@ HI91 (preferred for calibration):
from __future__ import annotations
import struct
from dataclasses import dataclass
import numpy as np
@@ -22,6 +23,29 @@ G0 = 9.80665
DEG2RAD = np.pi / 180.0
@dataclass(frozen=True)
class Hi13Sample:
"""One CRC-valid native HI91 record.
t_s/system_time_ms are sensor-owned. Host receive ticks are retained only
for clock diagnostics and cross-clock fitting.
"""
t_s: float
system_time_ms: int
device_timestamp_us: int
host_receive_utc_ticks: int
gyro_rad_s: tuple[float, float, float]
accel_m_s2: tuple[float, float, float]
rpy_deg: tuple[float, float, float]
quaternion_wxyz: tuple[float, float, float, float]
mag_ut: tuple[float, float, float]
pps_sync_stamp_ms: int
temperature_c: int
air_pressure_pa: float
frame_tag: int = 0x91
def crc16_hi13(frame: bytes, payload_length: int) -> int:
crc = 0
for value in frame[:4]:
@@ -41,8 +65,8 @@ def _update_crc16(crc: int, value: int) -> int:
return crc
def parse_hi91_frame(raw: bytes) -> tuple[tuple[float, float, float], tuple[float, float, float], int] | None:
"""Return (gyro_rad_s, accel_m_s2, device_timestamp_ms) for a CRC-valid HI91 frame."""
def parse_hi91_sample(raw: bytes, host_receive_utc_ticks: int = 0) -> Hi13Sample | None:
"""Decode every calibration-relevant field from a CRC-valid HI91 frame."""
if len(raw) < 6 + 76:
return None
@@ -54,12 +78,34 @@ def parse_hi91_frame(raw: bytes) -> tuple[tuple[float, float, float], tuple[floa
expected = raw[4] | (raw[5] << 8)
if crc16_hi13(raw, payload_length) != expected:
return None
device_ms = struct.unpack_from("<I", raw, 14)[0]
device_ms = int(struct.unpack_from("<I", raw, 14)[0])
ax, ay, az = struct.unpack_from("<fff", raw, 18)
gx, gy, gz = struct.unpack_from("<fff", raw, 30)
gyro = (gx * DEG2RAD, gy * DEG2RAD, gz * DEG2RAD)
accel = (ax * G0, ay * G0, az * G0)
return gyro, accel, int(device_ms)
return Hi13Sample(
t_s=float(device_ms) * 1e-3,
system_time_ms=device_ms,
device_timestamp_us=device_ms * 1000,
host_receive_utc_ticks=int(host_receive_utc_ticks),
gyro_rad_s=gyro,
accel_m_s2=accel,
rpy_deg=struct.unpack_from("<fff", raw, 54),
quaternion_wxyz=struct.unpack_from("<ffff", raw, 66),
mag_ut=struct.unpack_from("<fff", raw, 42),
pps_sync_stamp_ms=int(struct.unpack_from("<H", raw, 7)[0]),
temperature_c=int(struct.unpack_from("<b", raw, 9)[0]),
air_pressure_pa=float(struct.unpack_from("<f", raw, 10)[0]),
)
def parse_hi91_frame(raw: bytes) -> tuple[tuple[float, float, float], tuple[float, float, float], int] | None:
"""Backward-compatible compact HI91 decoder."""
sample = parse_hi91_sample(raw)
if sample is None:
return None
return sample.gyro_rad_s, sample.accel_m_s2, sample.system_time_ms
def iter_hi13_imu_samples(
@@ -67,14 +113,14 @@ def iter_hi13_imu_samples(
*,
host_utc_ticks_min: int | None = None,
host_utc_ticks_max: int | None = None,
) -> list[ImuSample]:
) -> list[Hi13Sample]:
"""Return CRC-valid HI91 samples sorted by device timestamp.
Streams chunk-by-chunk (no giant join) and can skip whole chunks outside the
host UTC receive window before parsing.
"""
samples: list[ImuSample] = []
samples: list[Hi13Sample] = []
carry = b""
for chunk in capture.chunks:
if host_utc_ticks_min is not None and chunk.receive_utc_ticks < host_utc_ticks_min:
@@ -102,25 +148,16 @@ def iter_hi13_imu_samples(
if end > len(stream):
carry = stream[sync:]
break
parsed = parse_hi91_frame(stream[sync:end])
host_ticks = chunk.receive_utc_ticks
parsed = parse_hi91_sample(stream[sync:end], host_ticks)
cursor = end
if parsed is None:
continue
gyro, accel, device_ms = parsed
host_ticks = chunk.receive_utc_ticks
if host_utc_ticks_min is not None and host_ticks < host_utc_ticks_min:
continue
if host_utc_ticks_max is not None and host_ticks > host_utc_ticks_max:
continue
samples.append(
ImuSample(
t_s=float(device_ms) * 1e-3,
gyro_rad_s=gyro,
accel_m_s2=accel,
host_receive_utc_ticks=host_ticks,
device_timestamp_us=int(device_ms) * 1000,
)
)
samples.append(parsed)
else:
carry = b""
samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us))
@@ -129,8 +166,10 @@ def iter_hi13_imu_samples(
__all__ = [
"ImuSample",
"Hi13Sample",
"crc16_hi13",
"iter_hi13_imu_samples",
"parse_hi91_frame",
"parse_hi91_sample",
"samples_to_arrays",
]
tools/run_rtk_imu_calibration.py
+99
View File
@@ -0,0 +1,99 @@
#!/usr/bin/env python3
"""Run the independent RTK--IMU calibration against the project inventory."""
from __future__ import annotations
import argparse
import json
import sys
from pathlib import Path
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
from rtk_imu.rtk_imu_replay import (
DEFAULT_RTK_FRAME_DEFINITION,
DEFAULT_RTK_REFERENCE_POINT,
load_inventory,
load_sessions,
run_calibration,
)
def main(argv: list[str] | None = None) -> int:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--inventory",
type=Path,
default=ROOT / "artifacts" / "rtk_imu_inventory_v1" / "rtk_session_inventory.csv",
)
parser.add_argument(
"--output-dir",
type=Path,
default=ROOT / "artifacts" / "rtk_imu_calibration_v1",
)
parser.add_argument("--session", action="append", help="session id to include; repeatable")
parser.add_argument("--batch", action="append", help="batch id to include; repeatable")
parser.add_argument("--rotation-only", action="store_true")
parser.add_argument("--no-loo", action="store_true")
parser.add_argument("--knot-step-s", type=float, default=2.0)
parser.add_argument("--per-batch", action="store_true")
parser.add_argument("--rtk-frame-definition", default=DEFAULT_RTK_FRAME_DEFINITION)
parser.add_argument("--rtk-reference-point", default=DEFAULT_RTK_REFERENCE_POINT)
args = parser.parse_args(argv)
entries = load_inventory(args.inventory)
if args.session:
selected = set(args.session)
entries = [entry for entry in entries if entry.session_id in selected]
if args.batch:
selected_batches = set(args.batch)
entries = [entry for entry in entries if entry.batch_id in selected_batches]
if not entries:
raise SystemExit("no inventory rows match the requested selection")
sessions = load_sessions(entries)
rotation, translation = run_calibration(
sessions,
args.output_dir,
rotation_only=args.rotation_only,
compute_loo=not args.no_loo,
knot_step_s=args.knot_step_s,
rtk_frame_definition=args.rtk_frame_definition,
rtk_reference_point=args.rtk_reference_point,
)
print(
json.dumps(
{
"output": str(args.output_dir.resolve()),
"sessions": [session.session_id for session in sessions],
"rotation_ok": rotation.ok,
"rotation_rpy_deg": rotation.rpy_deg.tolist(),
"rotation_rms_deg": rotation.residual_rms_deg,
"translation_ok": None if translation is None else translation.ok,
"translation_m": None if translation is None else translation.t_RTK_IMU_m.tolist(),
},
ensure_ascii=False,
indent=2,
)
)
if args.per_batch:
for batch in sorted({entry.batch_id for entry in entries}):
batch_entries = [entry for entry in entries if entry.batch_id == batch]
if len(batch_entries) < 2:
continue
run_calibration(
load_sessions(batch_entries),
args.output_dir / "per_batch" / batch,
rotation_only=args.rotation_only,
compute_loo=False,
knot_step_s=args.knot_step_s,
rtk_frame_definition=args.rtk_frame_definition,
rtk_reference_point=args.rtk_reference_point,
)
return 0
if __name__ == "__main__":
raise SystemExit(main())
tools/run_rtk_imu_engineering_6dof.py
+125
View File
@@ -0,0 +1,125 @@
#!/usr/bin/env python3
"""Run the conditional engineering RTK--IMU 6DoF lever-arm branch."""
from __future__ import annotations
import argparse
import json
import math
import sys
from dataclasses import asdict
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
from rtk_imu.rtk_imu_engineering import (
DEFAULT_MANUAL_L_I_M,
DEFAULT_MANUAL_L_I_COVARIANCE_M2,
solve_engineering_6dof,
)
from rtk_imu.rtk_imu_multisource import load_unified_sessions
def _jsonable(value):
if isinstance(value, np.ndarray):
return _jsonable(value.tolist())
if isinstance(value, np.generic):
return _jsonable(value.item())
if isinstance(value, float):
return value if math.isfinite(value) else None
if hasattr(value, "__dataclass_fields__"):
return {key: _jsonable(item) for key, item in asdict(value).items()}
if isinstance(value, dict):
return {str(key): _jsonable(item) for key, item in value.items()}
if isinstance(value, (list, tuple)):
return [_jsonable(item) for item in value]
return value
def main(argv: list[str] | None = None) -> int:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--manifest", type=Path, required=True)
parser.add_argument("--output", type=Path, required=True)
parser.add_argument(
"--session", action="append", required=True,
help="Dynamic/static sessions intentionally admitted to this engineering run.",
)
parser.add_argument(
"--rotation-rpy-deg", nargs=3, type=float,
default=[0.4543066225, -0.0026392019, 0.0122384129],
)
parser.add_argument(
"--manual-l-i-m", nargs=3, type=float, default=DEFAULT_MANUAL_L_I_M.tolist(),
help="Mechanical ANT1 phase-centre lever arm p_ANT1^I in metres.",
)
prior_group = parser.add_mutually_exclusive_group()
prior_group.add_argument(
"--manual-l-i-std-m", nargs=3, type=float,
default=np.sqrt(np.diag(DEFAULT_MANUAL_L_I_COVARIANCE_M2)).tolist(),
help="Mechanical 1-sigma prior standard deviation for lx, ly, lz in metres.",
)
prior_group.add_argument(
"--manual-l-i-covariance-m2", nargs=9, type=float,
help="Row-major 3x3 mechanical prior covariance in m^2.",
)
parser.add_argument(
"--no-manual-prior", action="store_true",
help="Run a free solve only; retain the mechanical reference in no optimisation factor.",
)
parser.add_argument(
"--segment-id", action="append",
help="Strict-continuity segment id from the excitation audit; may be repeated.",
)
parser.add_argument("--sample-period-s", type=float, default=0.5)
parser.add_argument("--skip-loo", action="store_true")
parser.add_argument("--run-bootstrap", action="store_true")
parser.add_argument("--bootstrap-repetitions", type=int, default=40)
parser.add_argument("--bootstrap-seed", type=int, default=0)
parser.add_argument("--run-rotation-sensitivity", action="store_true")
args = parser.parse_args(argv)
manual_l_i_m = None if args.no_manual_prior else args.manual_l_i_m
prior_covariance = None
if not args.no_manual_prior:
if args.manual_l_i_std_m is not None:
prior_covariance = np.diag(np.square(args.manual_l_i_std_m))
elif args.manual_l_i_covariance_m2 is not None:
prior_covariance = np.asarray(args.manual_l_i_covariance_m2, dtype=float).reshape(3, 3)
else:
parser.error("mechanical prior requires --manual-l-i-std-m or --manual-l-i-covariance-m2")
sessions = load_unified_sessions(args.manifest, selected_session_ids=set(args.session))
rotation = Rotation.from_euler("xyz", args.rotation_rpy_deg, degrees=True).as_matrix()
result = solve_engineering_6dof(
sessions,
R_RTK_IMU=rotation,
manual_l_I_m=manual_l_i_m,
manual_l_I_covariance_m2=prior_covariance,
sample_period_s=args.sample_period_s,
run_loo=not args.skip_loo,
run_bootstrap=args.run_bootstrap,
bootstrap_repetitions=args.bootstrap_repetitions,
bootstrap_seed=args.bootstrap_seed,
run_rotation_sensitivity=args.run_rotation_sensitivity,
selected_segment_ids=None if args.segment_id is None else set(args.segment_id),
)
payload = {
"data_only_6dof_accepted": False,
"data_only_translation_accepted": result.data_only_translation_accepted,
"engineering_6dof": _jsonable(result),
}
args.output.parent.mkdir(parents=True, exist_ok=True)
args.output.write_text(
json.dumps(payload, ensure_ascii=False, indent=2, allow_nan=False) + "\n", encoding="utf-8"
)
print(json.dumps(payload, ensure_ascii=False, indent=2, allow_nan=False))
return 0
if __name__ == "__main__":
raise SystemExit(main())
tools/run_rtk_imu_engineering_free_base.py
+254
View File
@@ -0,0 +1,254 @@
#!/usr/bin/env python3
"""Run one prior-free engineering base fit on high-excitation qualified segments.
This diagnostic never adds a mechanical lever factor, never profiles the
mechanical reference, and never runs LOO/bootstrap/rotation sensitivity.
"""
from __future__ import annotations
import argparse
import json
import math
import sys
from dataclasses import asdict
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
from rtk_imu.rtk_imu_engineering import (
MIN_SEGMENT_DURATION_S,
MIN_SEGMENT_NODE_COUNT,
NUISANCE_DOF_PER_SEGMENT,
_Segment,
_fit_segments,
_fit_summary,
_height_reference,
_initial_parameters,
_marginal_lever_information,
_nodes,
_residual,
_segment_residual_size,
_world_rtk,
)
from imu_lidar.imu_preintegration import preintegrate_imu
from rtk_imu.rtk_imu_multisource import load_unified_sessions
MANUAL_REFERENCE_L_I_M = np.array([-0.45072, -0.25682, 0.73208], dtype=float)
def _jsonable(value):
if isinstance(value, np.ndarray):
return _jsonable(value.tolist())
if isinstance(value, np.generic):
return _jsonable(value.item())
if isinstance(value, float):
return value if math.isfinite(value) else None
if hasattr(value, "__dataclass_fields__"):
return {key: _jsonable(item) for key, item in asdict(value).items()}
if isinstance(value, dict):
return {str(key): _jsonable(item) for key, item in value.items()}
if isinstance(value, (tuple, list)):
return [_jsonable(item) for item in value]
return value
def _r0_segment_candidates(sessions, period_s: float) -> list[tuple[object, tuple, str]]:
"""Split R0 first; defer expensive preintegration until a candidate is selected."""
reference = _height_reference(sessions)
if reference is None:
return []
candidates: list[tuple[object, tuple, str]] = []
for session in sessions:
nodes = _nodes(session, reference, period_s)
start = 0
qualifying_index = 0
for end in range(1, len(nodes) + 1):
if end != len(nodes) and nodes[end].continuity_id == nodes[end - 1].continuity_id:
continue
run = tuple(nodes[start:end])
start = end
if len(run) < MIN_SEGMENT_NODE_COUNT or run[-1].t_s - run[0].t_s < MIN_SEGMENT_DURATION_S:
continue
if not any(node.hpr_factor_valid for node in run):
continue
candidates.append((session, run, f"{session.session_id}:{qualifying_index:02d}"))
qualifying_index += 1
return candidates
def _build_segment(session, run: tuple, segment_id: str) -> _Segment | None:
pre = tuple(preintegrate_imu(
session.imu.t_s, session.imu.gyro_rad_s, session.imu.acc_m_s2, left.t_s, right.t_s,
) for left, right in zip(run[:-1], run[1:]))
if any(item.duration_s <= 0.0 for item in pre):
return None
initial_hpr = next((node for node in run if node.hpr_factor_valid), None)
if initial_hpr is None:
return None
return _Segment(segment_id, session.session_id, run, pre, _world_rtk(initial_hpr.baseline_enu))
def _gyro_abs_rotation_deg(session, segment) -> np.ndarray:
start_s, end_s = segment.nodes[0].t_s, segment.nodes[-1].t_s
mask = (session.imu.t_s >= start_s) & (session.imu.t_s <= end_s)
t_s = session.imu.t_s[mask]
gyro = session.imu.gyro_rad_s[mask]
if t_s.size < 2:
return np.zeros(3)
return np.degrees(np.trapezoid(np.abs(gyro), t_s, axis=0))
def _category_score(category: str, gyro_abs_deg: np.ndarray) -> float:
if category in {"circle", "left_right"}:
return float(gyro_abs_deg[2])
return float(np.hypot(gyro_abs_deg[0], gyro_abs_deg[1]))
def _marginal_for_segment_indices(jacobian: np.ndarray, residual: np.ndarray,
segments, indices: list[int]) -> tuple[np.ndarray, np.ndarray, float, int, np.ndarray]:
row = 0
rows: list[np.ndarray] = []
for index, segment in enumerate(segments):
count = _segment_residual_size(segment)
if index in indices:
rows.append(np.arange(row, row + count))
row += count
selected_rows = np.concatenate(rows) if rows else np.empty(0, dtype=int)
columns = [0, 1, 2]
for index in indices:
offset = 3 + NUISANCE_DOF_PER_SEGMENT * index
columns.extend(range(offset, offset + NUISANCE_DOF_PER_SEGMENT))
marginal, singular, condition, rank, weakest, _ = _marginal_lever_information(
jacobian[np.ix_(selected_rows, columns)], residual[selected_rows]
)
return marginal, singular, condition, rank, weakest
def main() -> int:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--manifest", type=Path, required=True)
parser.add_argument("--output", type=Path, required=True)
parser.add_argument("--circle-session", required=True)
parser.add_argument("--left-right-session", required=True)
parser.add_argument("--slope-session", required=True)
parser.add_argument("--top-per-category", type=int, default=10)
parser.add_argument("--sample-period-s", type=float, default=1.0)
parser.add_argument("--rotation-rpy-deg", nargs=3, type=float,
default=[0.4543066225, -0.0026392019, 0.0122384129])
args = parser.parse_args()
category_by_session = {
args.circle_session: "circle",
args.left_right_session: "left_right",
args.slope_session: "slope",
}
sessions = load_unified_sessions(args.manifest, selected_session_ids=set(category_by_session))
candidates: dict[str, list[tuple[float, object, tuple, str, np.ndarray]]] = {
key: [] for key in category_by_session.values()
}
all_candidates = _r0_segment_candidates(sessions, args.sample_period_s)
for session, run, segment_id in all_candidates:
category = category_by_session[session.session_id]
gyro_abs = _gyro_abs_rotation_deg(session, type("Run", (), {"nodes": run})())
candidates[category].append((_category_score(category, gyro_abs), session, run, segment_id, gyro_abs))
selected: list[object] = []
selected_entries: list[dict[str, object]] = []
category_indices: dict[str, list[int]] = {}
for category, entries in candidates.items():
selected_before = len(selected)
for score, session, run, segment_id, gyro_abs in sorted(entries, key=lambda item: item[0], reverse=True):
segment = _build_segment(session, run, segment_id)
if segment is None:
continue
selected.append(segment)
selected_entries.append({
"category": category, "segment_id": segment.segment_id,
"session_id": segment.session_id, "duration_s": segment.nodes[-1].t_s - segment.nodes[0].t_s,
"node_count": len(segment.nodes), "excitation_score_deg": score,
"cumulative_absolute_gyro_rotation_xyz_deg": gyro_abs,
})
if len(selected) - selected_before >= args.top_per_category:
break
category_indices[category] = list(range(selected_before, len(selected)))
rotation = Rotation.from_euler("xyz", args.rotation_rpy_deg, degrees=True).as_matrix()
initial_parameters = _initial_parameters(selected)
initial_residual = _residual(initial_parameters, selected, rotation)
fit, residual, detail = _fit_segments(selected, rotation)
summary = _fit_summary(fit, residual, detail)
if fit is None or summary is None:
raise RuntimeError("no selected qualified segment could be fit")
contributions: dict[str, dict[str, object]] = {}
contribution_sum = np.zeros((3, 3))
for category, indices in category_indices.items():
marginal, singular, condition, rank, weakest = _marginal_for_segment_indices(
fit.jac, residual, selected, indices
)
contribution_sum += marginal
contributions[category] = {
"selected_segment_count": len(indices),
"lever_marginal_information": marginal,
"lever_information_singular_values": singular,
"condition_number": condition,
"precision_rank": rank,
"weakest_direction_I": weakest,
"axis_information_diagonal_I": np.diag(marginal),
}
total_diag = np.diag(summary.lever_marginal_information)
for category, item in contributions.items():
item["axis_information_fraction_of_total_I"] = np.divide(
item["axis_information_diagonal_I"], total_diag,
out=np.full(3, np.nan), where=np.abs(total_diag) > 1e-12,
)
payload = {
'solver_diagnostics': {
'initial_cost': 0.5 * float(np.dot(initial_residual, initial_residual)),
'final_cost': 0.5 * float(np.dot(residual, residual)),
'cost_reduction': 0.5 * float(
np.dot(initial_residual, initial_residual) - np.dot(residual, residual)
),
'cost_definition': '0.5 * unmodified residual squared norm, comparable initial/final',
'scipy_final_huber_cost': float(fit.cost),
'nfev': int(fit.nfev), 'optimality': float(fit.optimality),
'gradient_norm': float(np.linalg.norm(fit.grad)),
'initial_l_I_m': initial_parameters[:3], 'final_l_I_m': fit.x[:3],
'l_step_norm_m': float(np.linalg.norm(fit.x[:3] - initial_parameters[:3])),
},
"scope": "prior-free free base fit only; no mechanical factor/LOO/bootstrap/rotation sensitivity",
"rotation_source": "R2G_gravity_level_prior",
"translation_conditional_on_rotation": True,
"manual_reference_comparison_only": {
"manual_l_I_m": MANUAL_REFERENCE_L_I_M,
"free_minus_manual_l_I_m": summary.l_I_m - MANUAL_REFERENCE_L_I_M,
"euclidean_delta_m": float(np.linalg.norm(summary.l_I_m - MANUAL_REFERENCE_L_I_M)),
},
"sample_period_s": args.sample_period_s,
"all_qualified_segment_count": len(all_candidates),
"selected_segment_count": len(selected),
"selected_segments": selected_entries,
"free_solution": _jsonable(summary),
"motion_category_information_contributions": _jsonable(contributions),
"axis_information_total_I": np.diag(summary.lever_marginal_information),
"marginal_additivity_error_fro": float(np.linalg.norm(
contribution_sum - summary.lever_marginal_information, ord="fro"
)),
}
args.output.parent.mkdir(parents=True, exist_ok=True)
args.output.write_text(json.dumps(_jsonable(payload), ensure_ascii=False, indent=2, allow_nan=False) + "\n", encoding="utf-8")
print(json.dumps({
"selected_segment_count": len(selected),
"free_l_I_m": _jsonable(summary.l_I_m),
"free_l_I_std_m": _jsonable(summary.l_I_std_m),
"lever_information_singular_values": _jsonable(summary.lever_information_singular_values),
"condition_number": summary.lever_information_condition_number,
"precision_rank": summary.lever_precision_rank,
"bestnava_xyz_vector_rms_p95_m": [summary.bestnava_xyz_residual.vector_rms, summary.bestnava_xyz_residual.vector_p95],
"doppler_vector_rms_p95_m_s": [summary.doppler_velocity_residual.vector_rms, summary.doppler_velocity_residual.vector_p95],
}, ensure_ascii=False, indent=2))
return 0
if __name__ == "__main__":
raise SystemExit(main())
tools/run_rtk_imu_mechanical_prior_branch.py
+144
View File
@@ -0,0 +1,144 @@
#!/usr/bin/env python3
'''Run fixed-mechanical and soft-prior solutions on immutable 47-window baseline.'''
from __future__ import annotations
import argparse,hashlib,json,sys
from concurrent.futures import ThreadPoolExecutor
from dataclasses import asdict
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT=Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
from rtk_imu.rtk_imu_engineering import _height_reference
from rtk_imu.rtk_imu_multisource import load_unified_sessions
from rtk_imu.rtk_imu_node_graph import (
NODE_DOF,build_problem,fit_states_at_fixed_lever,solve_free_lever_many,
summarize_fixed_state_values)
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_jsonable
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
MANUAL_STD_M=np.array([.02,.02,.03])
MANUAL_COVARIANCE_M2=np.diag(MANUAL_STD_M**2)
def _factor_delta(candidate,baseline):
output={}
for key in ('best_position','doppler','hpr','imu_preintegration'):
left,right=candidate['residual_by_factor'][key],baseline['residual_by_factor'][key]
output[key]={'rms_delta':left['rms']-right['rms'],
'p95_abs_delta':left['p95_abs']-right['p95_abs'],
'nis_per_dof_delta':left['chi_square_per_dof']-right['chi_square_per_dof']}
return output
def main():
parser=argparse.ArgumentParser(description=__doc__)
parser.add_argument('--manifest',type=Path,required=True)
parser.add_argument('--selection',type=Path,required=True)
parser.add_argument('--free-baseline',type=Path,required=True)
parser.add_argument('--output',type=Path,required=True)
parser.add_argument('--state-output',type=Path)
parser.add_argument('--sample-period-s',type=float,default=1.)
parser.add_argument('--fixed-max-nfev',type=int,default=120)
parser.add_argument('--prior-max-nfev',type=int,default=120)
parser.add_argument('--workers',type=int,default=4)
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
default=[.4543066225,-.0026392019,.0122384129])
args=parser.parse_args()
selection=json.loads(args.selection.read_text(encoding='utf-8'))
baseline=json.loads(args.free_baseline.read_text(encoding='utf-8'))
if len(selection['selected_windows'])!=47:
raise RuntimeError('engineering branch requires immutable 47-window selection')
ids={item['session_id'] for item in selection['selected_windows']}
sessions=load_unified_sessions(args.manifest,selected_session_ids=ids)
reference=_height_reference(sessions)
segments=_restore_segments(sessions,reference,selection['selected_windows'],
args.sample_period_s)
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
mechanical=MECHANICAL_L_I_M.copy()
problems=[build_problem(segment,rotation,mechanical,args.hpr_direct_sigma_rad)
for segment in segments]
with ThreadPoolExecutor(max_workers=args.workers) as executor:
fitted=list(executor.map(lambda problem:fit_states_at_fixed_lever(
problem,mechanical,args.fixed_max_nfev),problems))
fixed_states=[item[0] for item in fitted]
fixed_optimizer=[item[1] for item in fitted]
fixed_summary=summarize_fixed_state_values(problems,fixed_states,mechanical)
prior_result,prior_states=solve_free_lever_many(
problems,mechanical,args.prior_max_nfev,fixed_states,
lever_prior_mean_m=mechanical,
lever_prior_covariance_m2=MANUAL_COVARIANCE_M2,
return_state_values=True)
prior_result=asdict(prior_result)
prior_data=summarize_fixed_state_values(
problems,prior_states,np.asarray(prior_result['final_l_I_m']))
bias_values={}
for segment,state in zip(segments,prior_states):
states=np.asarray(state).reshape(-1,NODE_DOF)
values=bias_values.setdefault(segment.session_id,{'bg':[],'ba':[]})
values['bg'].extend(states[:,9:12])
values['ba'].extend(states[:,12:15])
calibration_bias={session_id:{
'gyro_bias_rad_s':np.median(values['bg'],axis=0),
'accel_bias_m_s2':np.median(values['ba'],axis=0),
'node_count':len(values['bg'])}
for session_id,values in bias_values.items()}
free=baseline['solutions']['mechanical']
posterior_cov=np.asarray(prior_result['lever_covariance_m2'])
variance_ratio=np.diag(posterior_cov)/np.diag(MANUAL_COVARIANCE_M2)
prior_pull=(np.asarray(prior_result['final_l_I_m'])-mechanical)/MANUAL_STD_M
translation_refined=bool(np.all(variance_ratio<=.90))
comparisons={'fixed_vs_free':{
'cost_delta':fixed_summary['cost']-free['final_cost'],
'relative_cost_delta':fixed_summary['cost']/free['final_cost']-1.,
'factor_residual_delta':_factor_delta(fixed_summary,free)},
'prior_data_vs_free':{
'cost_delta':prior_data['cost']-free['final_cost'],
'relative_cost_delta':prior_data['cost']/free['final_cost']-1.,
'factor_residual_delta':_factor_delta(prior_data,free)}}
baseline_hash=hashlib.sha256(args.free_baseline.read_bytes()).hexdigest()
payload={'scope':'47-window mechanical-prior engineering branch',
'data_only_translation_accepted':False,
'immutable_free_baseline':{'path':str(args.free_baseline),
'sha256':baseline_hash,'solution':free},
'selected_window_count':len(segments),'selection_path':str(args.selection),
'manual_l_I_m':mechanical,'manual_l_I_std_m':MANUAL_STD_M,
'manual_l_I_covariance_m2':MANUAL_COVARIANCE_M2,
'fixed_mechanical_solution':{'l_I_m':mechanical,
'window_optimizer':fixed_optimizer,'data_summary':fixed_summary},
'prior_constrained_solution':{'result':prior_result,
'data_only_summary_excluding_prior_factor':prior_data,
'posterior_covariance_m2':posterior_cov,
'posterior_prior_variance_ratio':variance_ratio,
'prior_pull_sigma':prior_pull,
'calibration_only_frozen_bias_by_session':calibration_bias},
'comparisons':comparisons,
'translation_refinement_gate':{
'required_max_axis_variance_ratio':.90,
'passed':translation_refined},
'translation_refined_by_data':translation_refined,
'heldout_validation_called':False,
'engineering_translation_accepted':False}
args.output.parent.mkdir(parents=True,exist_ok=True)
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
if args.state_output is not None:
sizes=np.asarray([len(value) for value in prior_states],dtype=int)
args.state_output.parent.mkdir(parents=True,exist_ok=True)
np.savez_compressed(args.state_output,
states=np.concatenate(prior_states),offsets=np.cumsum(np.r_[0,sizes]),
candidate_ids=np.asarray(
[item['candidate_id'] for item in selection['selected_windows']]))
print(json.dumps(_jsonable({'fixed':fixed_summary,
'prior_l_I_m':prior_result['final_l_I_m'],
'prior_data_cost':prior_data['cost'],'prior_map_cost':prior_result['final_cost'],
'variance_ratio':variance_ratio,'prior_pull_sigma':prior_pull,
'translation_refined_by_data':translation_refined,
'fixed_optimizer_failed_count':sum(not item['success'] for item in fixed_optimizer)}),
ensure_ascii=False,indent=2))
return 0
if __name__=='__main__':
raise SystemExit(main())
tools/run_rtk_imu_mechanical_prior_heldout.py
+212
View File
@@ -0,0 +1,212 @@
#!/usr/bin/env python3
'''Validate an engineering lever on disjoint held-out windows.'''
from __future__ import annotations
import argparse,json,sys
from concurrent.futures import ProcessPoolExecutor,as_completed
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT=Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
from imu_lidar.geometry import make_transform
from rtk_imu.rtk_imu_engineering import _height_reference
from rtk_imu.rtk_imu_multisource import load_unified_sessions
from rtk_imu.rtk_imu_node_graph import build_problem,fit_states_at_fixed_lever,residual
from tools.audit_rtk_imu_factor_consistency import _jsonable
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
FACTORS=('best_position','doppler','hpr','imu_preintegration')
PHYSICAL=('best_position_physical','doppler_physical','hpr_physical')
def _fit_checkpoint(task):
index,problem,lever,max_nfev,path_text=task
path=Path(path_text)
if path.exists():
data=np.load(path,allow_pickle=False)
return index,data['state'],json.loads(str(data['optimizer']))
state,meta=fit_states_at_fixed_lever(problem,lever,max_nfev)
np.savez_compressed(path,state=state,optimizer=json.dumps(meta))
return index,state,meta
def _stats(values,dof=None):
a=np.asarray(values,dtype=float).reshape(-1)
if not a.size:
return {'count':0,'dof':0,'rms':np.nan,'p50_abs':np.nan,
'p95_abs':np.nan,'p99_abs':np.nan,'nis':np.nan,
'chi_square_per_dof':np.nan}
dof=len(a) if dof is None else max(int(dof),1); nis=float(a@a)
return {'count':len(a),'dof':dof,'rms':float(np.sqrt(np.mean(a*a))),
'p50_abs':float(np.percentile(np.abs(a),50)),
'p95_abs':float(np.percentile(np.abs(a),95)),
'p99_abs':float(np.percentile(np.abs(a),99)),
'nis':nis,'chi_square_per_dof':nis/dof}
def _vectors(values):
a=np.asarray(values,dtype=float).reshape(-1,3)
if not a.size:
return {'count':0,'axis_rms':[np.nan]*3,'axis_p95_abs':[np.nan]*3,
'vector_rms':np.nan,'vector_p95':np.nan}
norm=np.linalg.norm(a,axis=1)
return {'count':len(a),'axis_rms':np.sqrt(np.mean(a*a,axis=0)),
'axis_p95_abs':np.percentile(np.abs(a),95,axis=0),
'vector_rms':float(np.sqrt(np.mean(norm*norm))),
'vector_p95':float(np.percentile(norm,95))}
def _summarize(records):
factor={key:[] for key in FACTORS}; physical={key:[] for key in PHYSICAL}
joined=[]; state_dimension=0; other_effective=0
for record in records:
joined.extend(record['all']); state_dimension+=record['state_dimension']
other_effective+=record['other_effective']
for key in FACTORS: factor[key].extend(record['factor'].get(key,[]))
for key in PHYSICAL: physical[key].extend(record['physical'].get(key,[]))
effective=sum(2*len(v)//3 if k=='hpr' else len(v)
for k,v in factor.items())+other_effective
dof=max(effective-state_dimension,1); a=np.asarray(joined,dtype=float)
converged=sum(bool(x['optimizer']['success']) for x in records)
return {'window_count':len(records),'optimizer_converged_count':converged,
'optimizer_converged_fraction':converged/max(len(records),1),
'total_residual_dimension':len(a),'state_dimension':state_dimension,
'statistical_dof':dof,'total_nis':float(a@a),
'global_chi_square_per_dof':float((a@a)/dof),
'residual_by_factor':{key:_stats(value,2*len(value)//3 if key=='hpr' else None)
for key,value in factor.items()},
'best_position_physical_m':_vectors(physical['best_position_physical']),
'doppler_physical_m_s':_vectors(physical['doppler_physical']),
'hpr_physical_rad':_vectors(physical['hpr_physical'])}
def _gate(summary):
factor=summary['residual_by_factor']
checks={'optimizer_converged_fraction_ge_0p90':
summary['optimizer_converged_fraction']>=.90,
'global_chi_square_per_dof_in_0p25_4':
.25<=summary['global_chi_square_per_dof']<=4.,
'best_vector_p95_le_0p20_m':
summary['best_position_physical_m']['vector_p95']<=.20,
'doppler_vector_p95_le_0p50_m_s':
summary['doppler_physical_m_s']['vector_p95']<=.50,
'hpr_normalized_p95_le_4':factor['hpr']['p95_abs']<=4.,
'preintegration_normalized_p95_le_3':
factor['imu_preintegration']['p95_abs']<=3.}
return {'uses_existing_P0p5_physical_and_statistical_health_gates':True,
'checks':checks,'passed':bool(all(checks.values()))}
def main():
p=argparse.ArgumentParser(description=__doc__)
p.add_argument('--manifest',type=Path,required=True)
p.add_argument('--calibration-selection',type=Path,required=True)
p.add_argument('--all-selection',type=Path,required=True)
p.add_argument('--engineering-result',type=Path,required=True)
p.add_argument('--output',type=Path,required=True)
p.add_argument('--checkpoint-dir',type=Path,required=True)
p.add_argument('--sample-period-s',type=float,default=1.)
p.add_argument('--max-nfev',type=int,default=120)
p.add_argument('--workers',type=int,default=4)
p.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
p.add_argument('--rotation-rpy-deg',nargs=3,type=float,
default=[.4543066225,-.0026392019,.0122384129])
p.add_argument('--circle-session',default='0808_20260808_092827')
p.add_argument('--left-right-session',default='0808_20260808_082148')
p.add_argument('--slope-session',default='0815_20260812_123424')
args=p.parse_args()
engineering=json.loads(args.engineering_result.read_text(encoding='utf-8'))
calibration=json.loads(args.calibration_selection.read_text(encoding='utf-8'))
all_windows=json.loads(args.all_selection.read_text(encoding='utf-8'))
calibration_ids={x['candidate_id'] for x in calibration['selected_windows']}
heldout=[x for x in all_windows['selected_windows']
if x['candidate_id'] not in calibration_ids]
if len(calibration_ids)!=47 or len(heldout)!=267:
raise RuntimeError(f'expected 47+267 windows, got {len(calibration_ids)}+{len(heldout)}')
shared=sum(x.get('shared_sample_count_with_previous',{}).get(k,0)
for x in all_windows['selected_windows'] for k in ('imu','gnss','hpr'))
if shared: raise RuntimeError(f'selection contains {shared} shared samples')
lever=np.asarray(engineering['prior_constrained_solution']['result']['final_l_I_m'])
sessions=load_unified_sessions(
args.manifest,selected_session_ids={x['session_id'] for x in heldout})
reference=_height_reference(sessions)
segments=_restore_segments(sessions,reference,heldout,args.sample_period_s)
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
problems=[build_problem(x,rotation,lever,args.hpr_direct_sigma_rad) for x in segments]
args.checkpoint_dir.mkdir(parents=True,exist_ok=True)
fitted=[None]*len(problems)
tasks=[(i,problem,lever,args.max_nfev,
str(args.checkpoint_dir/f'{i:04d}.npz'))
for i,problem in enumerate(problems)]
with ProcessPoolExecutor(max_workers=args.workers) as executor:
futures=[executor.submit(_fit_checkpoint,task) for task in tasks]
completed=0
for future in as_completed(futures):
index,state,meta=future.result(); fitted[index]=(state,meta); completed+=1
if completed%10==0: print(f'held-out checkpoint {completed}/{len(problems)}',flush=True)
mapping={args.circle_session:'circle',args.left_right_session:'left_right',
args.slope_session:'slope'}
records=[]
for item,problem,fit in zip(heldout,problems,fitted):
state,optimizer=fit; detail={}
all_residual=residual(problem,state,details=detail,lever_override=lever)
known=sum(len(detail.get(k,[])) for k in FACTORS)
records.append({'candidate_id':item['candidate_id'],
'session_id':item['session_id'],
'motion_class':mapping.get(item['session_id'],'other_recovered_dynamic'),
'start_s':item['start_s'],'end_s':item['end_s'],
'state_dimension':len(state),'optimizer':optimizer,
'all':all_residual.tolist(),'other_effective':len(all_residual)-known,
'factor':{k:detail.get(k,[]) for k in FACTORS},
'physical':{k:detail.get(k,[]) for k in PHYSICAL}})
overall=_summarize(records)
by_session={key:_summarize([x for x in records if x['session_id']==key])
for key in sorted({x['session_id'] for x in records})}
by_motion={key:_summarize([x for x in records if x['motion_class']==key])
for key in sorted({x['motion_class'] for x in records})}
overall_gate=_gate(overall)
session_gates={k:_gate(v) for k,v in by_session.items()}
motion_gates={k:_gate(v) for k,v in by_motion.items()}
heldout_passed=bool(overall_gate['passed'] and
all(x['passed'] for x in session_gates.values()) and
all(x['passed'] for x in motion_gates.values()))
comparisons=engineering['comparisons']
def nonconflicting(name):
c=comparisons[name]
return (c['relative_cost_delta']<=.05 and
all(x['p95_abs_delta']<=.25 for x in c['factor_residual_delta'].values()))
fit_nonconflict=nonconflicting('fixed_vs_free') and nonconflicting('prior_data_vs_free')
T_rtk_imu=make_transform(-rotation@lever,rotation)
T_imu_rtk=np.linalg.inv(T_rtk_imu)
accepted=bool(fit_nonconflict and heldout_passed)
payload={**engineering,
'scope':'47-window mechanical-prior engineering branch + disjoint held-out validation',
'heldout_validation_called':True,
'engineering_lever_source':'prior_constrained_solution',
'engineering_l_I_m':lever,'calibration_heldout_overlap_count':0,
'heldout_window_count':len(heldout),
'heldout_validation':{'lever_reoptimized':False,
'nuisance_states_optimized_per_window':True,
'motion_class_mapping':mapping,
'other_sessions_class':'other_recovered_dynamic',
'overall':overall,'by_session':by_session,'by_motion_class':by_motion,
'overall_gate':overall_gate,'session_gates':session_gates,
'motion_class_gates':motion_gates,'passed':heldout_passed},
'calibration_fit_nonconflict_gate':{'relative_cost_increase_max':.05,
'per_factor_normalized_p95_increase_max':.25,
'fixed_passed':nonconflicting('fixed_vs_free'),
'prior_passed':nonconflicting('prior_data_vs_free'),
'passed':fit_nonconflict},
'engineering_translation_accepted':accepted,
'data_only_translation_accepted':False,
'result_nature':'mechanical lever + dynamic-data consistency validation; not data-only translation calibration',
'rotation_source':'R2G_gravity_level_prior',
'translation_conditional_on_rotation':True,
'candidate_T_RTK_IMU':T_rtk_imu,'candidate_T_IMU_RTK':T_imu_rtk,
'T_RTK_IMU':T_rtk_imu if accepted else None,
'T_IMU_RTK':T_imu_rtk if accepted else None,
'transform_convention':('T_RTK_IMU maps IMU coordinates into ANT1 RTK frame; '
'l_I=p_ANT1^I; T_IMU_RTK translation equals l_I')}
args.output.parent.mkdir(parents=True,exist_ok=True)
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
print(json.dumps(_jsonable({'engineering_l_I_m':lever,
'fit_nonconflict':fit_nonconflict,'heldout_overall':overall,
'heldout_passed':heldout_passed,
'engineering_translation_accepted':accepted}),ensure_ascii=False,indent=2))
return 0
if __name__=='__main__': raise SystemExit(main())
tools/run_rtk_imu_mechanical_prior_rotation_sensitivity.py
+153
View File
@@ -0,0 +1,153 @@
#!/usr/bin/env python3
'''Run 18 fixed-R2G perturbation prior-constrained node-graph solves.'''
from __future__ import annotations
import argparse,json,sys
from concurrent.futures import ProcessPoolExecutor,as_completed
from dataclasses import asdict
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT=Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
from imu_lidar.geometry import make_transform
from rtk_imu.rtk_imu_engineering import _height_reference
from rtk_imu.rtk_imu_multisource import load_unified_sessions
from rtk_imu.rtk_imu_node_graph import (
build_problem,solve_free_lever_many,summarize_fixed_state_values)
from tools.audit_rtk_imu_factor_consistency import _jsonable
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
_CONTEXT={}
MANUAL=np.array([-.45072,-.25682,.73208])
STD=np.array([.02,.02,.03])
COV=np.diag(STD**2)
def _initialize_worker(segments,states,rpy,max_nfev,sigma,checkpoint_dir):
_CONTEXT.update(segments=segments,states=states,rpy=np.asarray(rpy),
max_nfev=max_nfev,sigma=sigma,checkpoint_dir=Path(checkpoint_dir))
def _solve(task):
axis,sign,angle=task
name=f'{axis}_{sign*angle:+.1f}deg'
path=_CONTEXT['checkpoint_dir']/f'{name}.json'
if path.exists(): return json.loads(path.read_text(encoding='utf-8'))
vector=np.zeros(3); vector['xyz'.index(axis)]=np.deg2rad(sign*angle)
nominal_R=Rotation.from_euler('xyz',_CONTEXT['rpy'],degrees=True).as_matrix()
perturbed_R=Rotation.from_rotvec(vector).as_matrix()@nominal_R
problems=[build_problem(segment,perturbed_R,MANUAL,_CONTEXT['sigma'])
for segment in _CONTEXT['segments']]
result,states=solve_free_lever_many(problems,MANUAL,_CONTEXT['max_nfev'],
_CONTEXT['states'],lever_prior_mean_m=MANUAL,
lever_prior_covariance_m2=COV,return_state_values=True)
result=asdict(result); lever=np.asarray(result['final_l_I_m'])
data=summarize_fixed_state_values(problems,states,lever)
transform=make_transform(-perturbed_R@lever,perturbed_R)
payload={'name':name,'axis':axis,'signed_angle_deg':sign*angle,
'success':result['success'],'message':result['message'],'nfev':result['nfev'],
'final_l_I_m':lever,'delta_l_from_nominal_m':None,
'map_cost':result['final_cost'],'data_cost':data['cost'],
'global_chi_square_per_dof':data['chi_square_per_dof'],
'residual_by_factor':data['residual_by_factor'],
'physical_residual':{
'BEST_position_m':data['best_position_physical_m'],
'Doppler_m_s':data['doppler_physical_m_s'],
'HPR_rad':data['hpr_physical_rad']},
'posterior_covariance_m2':result['lever_covariance_m2'],
'posterior_std_m':np.sqrt(np.diag(result['lever_covariance_m2'])),
'prior_pull_sigma':(lever-MANUAL)/STD,
'T_RTK_IMU':transform}
path.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
return _jsonable(payload)
def main():
p=argparse.ArgumentParser(description=__doc__)
p.add_argument('--manifest',type=Path,required=True)
p.add_argument('--selection',type=Path,required=True)
p.add_argument('--engineering-result',type=Path,required=True)
p.add_argument('--nominal-states',type=Path,required=True)
p.add_argument('--output',type=Path,required=True)
p.add_argument('--checkpoint-dir',type=Path,required=True)
p.add_argument('--workers',type=int,default=3)
p.add_argument('--max-nfev',type=int,default=120)
p.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
p.add_argument('--rotation-rpy-deg',nargs=3,type=float,
default=[.4543066225,-.0026392019,.0122384129])
args=p.parse_args()
engineering=json.loads(args.engineering_result.read_text(encoding='utf-8'))
selection=json.loads(args.selection.read_text(encoding='utf-8'))
if len(selection['selected_windows'])!=47:
raise RuntimeError('sensitivity requires immutable 47-window selection')
sessions=load_unified_sessions(args.manifest,
selected_session_ids={x['session_id'] for x in selection['selected_windows']})
segments=_restore_segments(sessions,_height_reference(sessions),
selection['selected_windows'],1.)
saved=np.load(args.nominal_states,allow_pickle=False)
offsets=saved['offsets']; flat=saved['states']
states=[flat[offsets[i]:offsets[i+1]] for i in range(len(offsets)-1)]
if list(saved['candidate_ids'])!=[x['candidate_id'] for x in selection['selected_windows']]:
raise RuntimeError('nominal state checkpoint does not match selection order')
args.checkpoint_dir.mkdir(parents=True,exist_ok=True)
tasks=[(axis,sign,angle) for axis in 'xyz'
for angle in (.1,.3,.5) for sign in (-1.,1.)]
results=[]
with ProcessPoolExecutor(max_workers=args.workers,initializer=_initialize_worker,
initargs=(segments,states,args.rotation_rpy_deg,args.max_nfev,
args.hpr_direct_sigma_rad,str(args.checkpoint_dir))) as executor:
futures=[executor.submit(_solve,task) for task in tasks]
for future in as_completed(futures):
result=future.result(); results.append(result)
print('completed',result['name'],flush=True)
results.sort(key=lambda x:('xyz'.index(x['axis']),x['signed_angle_deg']))
nominal=engineering['prior_constrained_solution']
nominal_l=np.asarray(nominal['result']['final_l_I_m'])
nominal_data=nominal['data_only_summary_excluding_prior_factor']
nominal_R=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
nominal_T=make_transform(-nominal_R@nominal_l,nominal_R)
for result in results:
result['delta_l_from_nominal_m']=np.asarray(result['final_l_I_m'])-nominal_l
result['delta_l_norm_m']=float(np.linalg.norm(result['delta_l_from_nominal_m']))
result['transform_translation_delta_m']=(
np.asarray(result['T_RTK_IMU'])[:3,3]-nominal_T[:3,3])
result['transform_translation_delta_norm_m']=float(np.linalg.norm(
result['transform_translation_delta_m']))
result['transform_rotation_delta_deg']=abs(result['signed_angle_deg'])
result['relative_data_cost_delta']=result['data_cost']/nominal_data['cost']-1.
result['factor_p95_delta_sigma']={key:
result['residual_by_factor'][key]['p95_abs']-
nominal_data['residual_by_factor'][key]['p95_abs']
for key in ('best_position','doppler','hpr','imu_preintegration')}
delta=np.asarray([x['delta_l_from_nominal_m'] for x in results])
at_point3=[x for x in results if abs(x['signed_angle_deg'])==.3]
checks={'all_18_complete_and_converged':
len(results)==18 and all(x['success'] for x in results),
'max_delta_norm_at_0p3deg_le_0p10m':
max(x['delta_l_norm_m'] for x in at_point3)<=.10,
'max_delta_norm_all_le_0p15m':
max(x['delta_l_norm_m'] for x in results)<=.15,
'relative_data_cost_increase_all_le_0p05':
max(x['relative_data_cost_delta'] for x in results)<=.05,
'factor_normalized_p95_increase_all_le_0p5sigma':
max(v for x in results for v in x['factor_p95_delta_sigma'].values())<=.5}
passed=bool(all(checks.values()))
payload={'scope':'18 prior-constrained fixed-R2G perturbation solves',
'data_only_free_called':False,'bootstrap_called':False,'loo_called':False,
'parser_R0_covariance_modified':False,'calibration_window_count':47,
'nominal_rotation_rpy_deg':args.rotation_rpy_deg,
'nominal_l_I_m':nominal_l,'nominal_T_RTK_IMU':nominal_T,
'perturbations':results,'summary':{
'max_abs_delta_l_xyz_m':np.max(np.abs(delta),axis=0),
'max_delta_l_norm_m':float(np.max(np.linalg.norm(delta,axis=1))),
'max_transform_translation_delta_norm_m':max(
x['transform_translation_delta_norm_m'] for x in results)},
'diagnostic_gate_thresholds_frozen_before_run':{
'max_delta_norm_at_0p3deg_m':.10,'max_delta_norm_all_m':.15,
'relative_data_cost_increase':.05,
'factor_normalized_p95_increase_sigma':.5},
'gate_checks':checks,'rotation_sensitivity_passed':passed}
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
print(json.dumps(_jsonable({'summary':payload['summary'],
'checks':checks,'rotation_sensitivity_passed':passed}),indent=2))
return 0
if __name__=='__main__': raise SystemExit(main())
tools/run_rtk_imu_multisource.py
+42
View File
@@ -0,0 +1,42 @@
#!/usr/bin/env python3
"""Run R1b/R2V/R2G/R3 on unified native-time G90/HI13 exports."""
from __future__ import annotations
import argparse
import json
import sys
from pathlib import Path
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
from rtk_imu.rtk_imu_multisource import (
load_unified_sessions,
result_to_jsonable,
solve_r3,
)
def main(argv: list[str] | None = None) -> int:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--manifest", type=Path, required=True)
parser.add_argument("--output", type=Path, required=True)
parser.add_argument("--session", action="append")
parser.add_argument("--level-static", action="append", required=True)
args = parser.parse_args(argv)
selected = None if not args.session else set(args.session) | set(args.level_static)
sessions = load_unified_sessions(args.manifest, selected_session_ids=selected)
result = solve_r3(sessions, level_static_session_ids=set(args.level_static))
payload = result_to_jsonable(result)
args.output.parent.mkdir(parents=True, exist_ok=True)
args.output.write_text(
json.dumps(payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
)
print(json.dumps(payload, ensure_ascii=False, indent=2))
return 0
if __name__ == "__main__":
raise SystemExit(main())
tools/run_rtk_imu_node_graph_fixed_lever.py
+75
View File
@@ -0,0 +1,75 @@
#!/usr/bin/env python3
'''Run one 10-20 s per-node state graph with a fixed mechanical lever.'''
from __future__ import annotations
import argparse, json, sys
from dataclasses import asdict
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
from rtk_imu.rtk_imu_engineering import _height_reference
from rtk_imu.rtk_imu_multisource import load_unified_sessions
from rtk_imu.rtk_imu_node_graph import build_problem, solve_fixed_lever
from tools.audit_rtk_imu_factor_consistency import (
MECHANICAL_L_I_M, _build_segment, _jsonable, _qualified_runs)
def _select_window(sessions,reference,period,target_duration):
best = None
for session,run,segment_id in _qualified_runs(sessions,reference,period):
for start in range(len(run)):
target_t = run[start].t_s+target_duration
end = int(np.searchsorted([node.t_s for node in run],target_t))
if end >= len(run): continue
window = tuple(run[start:end+1])
duration = window[-1].t_s-window[0].t_s
if not 10. <= duration <= 20.: continue
best_count = sum(node.source == 'BESTNAVA' for node in window)
doppler_count = sum(node.velocity_enu_m_s is not None for node in window)
hpr_count = sum(node.hpr_factor_valid for node in window)
gyro_score = sum(np.linalg.norm(node.gyro_rad_s) for node in window)
score = 10.*best_count+10.*doppler_count+2.*hpr_count+gyro_score
candidate = (score,session,window,f'{segment_id}:window_{start:03d}',
best_count,doppler_count,hpr_count)
if best is None or candidate[0] > best[0]: best = candidate
if best is None: raise RuntimeError('no 10-20 s qualified window')
score,session,window,segment_id,best_count,doppler_count,hpr_count = best
segment = _build_segment(session,window,segment_id)
if segment is None: raise RuntimeError('selected window preintegration failed')
return segment,{'score':score,'session_id':session.session_id,
'segment_id':segment_id,'start_s':window[0].t_s,'end_s':window[-1].t_s,
'duration_s':window[-1].t_s-window[0].t_s,'node_count':len(window),
'best_count':best_count,'doppler_count':doppler_count,'hpr_factor_count':hpr_count}
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('--manifest',type=Path,required=True)
parser.add_argument('--output',type=Path,required=True)
parser.add_argument('--session',required=True)
parser.add_argument('--sample-period-s',type=float,default=1.)
parser.add_argument('--target-duration-s',type=float,default=15.)
parser.add_argument('--max-nfev',type=int,default=30)
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
default=[.4543066225,-.0026392019,.0122384129])
parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
default=MECHANICAL_L_I_M.tolist())
args = parser.parse_args()
sessions = load_unified_sessions(args.manifest,selected_session_ids={args.session})
reference = _height_reference(sessions)
if reference is None: raise RuntimeError('no BEST reference')
segment,selection = _select_window(
sessions,reference,args.sample_period_s,args.target_duration_s)
rotation = Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
problem = build_problem(segment,rotation,np.asarray(args.mechanical_l_I_m))
result = solve_fixed_lever(problem,args.max_nfev)
payload = {'scope':'single 10-20 s node-state graph; fixed mechanical lever',
'translation_variable_enabled':False,'free_prior_loo_bootstrap_sensitivity_called':False,
'selection':selection,'result':asdict(result)}
args.output.parent.mkdir(parents=True,exist_ok=True)
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
print(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2))
return 0
if __name__ == '__main__':
raise SystemExit(main())
tools/run_rtk_imu_node_graph_free_circle.py
+79
View File
@@ -0,0 +1,79 @@
#!/usr/bin/env python3
'''Three-start, no-prior free-lever solve on the frozen P0.5 circle window.'''
from __future__ import annotations
import argparse,json,sys
from dataclasses import asdict
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT=Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
from rtk_imu.rtk_imu_engineering import _height_reference
from rtk_imu.rtk_imu_multisource import load_unified_sessions
from rtk_imu.rtk_imu_node_graph import build_problem,solve_free_lever
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_jsonable
from tools.run_rtk_imu_node_graph_fixed_lever import _select_window
def main():
parser=argparse.ArgumentParser(description=__doc__)
parser.add_argument('--manifest',type=Path,required=True)
parser.add_argument('--output',type=Path,required=True)
parser.add_argument('--circle-session',required=True)
parser.add_argument('--sample-period-s',type=float,default=1.)
parser.add_argument('--target-duration-s',type=float,default=15.)
parser.add_argument('--max-nfev',type=int,default=120)
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
default=[.4543066225,-.0026392019,.0122384129])
parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
default=MECHANICAL_L_I_M.tolist())
parser.add_argument('--large-perturbation-m',nargs=3,type=float,
default=[.5,-.5,.5])
args=parser.parse_args()
sessions=load_unified_sessions(
args.manifest,selected_session_ids={args.circle_session})
reference=_height_reference(sessions)
segment,selection=_select_window(
sessions,reference,args.sample_period_s,args.target_duration_s)
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
mechanical=np.asarray(args.mechanical_l_I_m,dtype=float)
problem=build_problem(segment,rotation,mechanical,args.hpr_direct_sigma_rad)
starts={'zero':np.zeros(3),'mechanical':mechanical,
'mechanical_large_perturbation':mechanical+args.large_perturbation_m}
results={name:asdict(solve_free_lever(problem,value,args.max_nfev))
for name,value in starts.items()}
for result in results.values():
covariance=np.asarray(result['lever_covariance_m2'])
result['lever_std_m']=np.sqrt(np.maximum(np.diag(covariance),0.))
result['delta_to_mechanical_m']=np.asarray(result['final_l_I_m'])-mechanical
result['xy_marginal_covariance_m2']=covariance[:2,:2]
result['xy_information_singular_values']=np.linalg.svd(
np.linalg.pinv(covariance[:2,:2],rcond=1e-9),compute_uv=False)
solutions=np.asarray([value['final_l_I_m'] for value in results.values()])
spread=float(max(np.linalg.norm(a-b) for a in solutions for b in solutions))
xy_spread=float(max(np.linalg.norm(a[:2]-b[:2]) for a in solutions for b in solutions))
circle_xy_observable=bool(all(value['success'] and
np.all(np.asarray(value['lever_std_m'])[:2]<=.15) for value in results.values()))
payload={'scope':'circle single-segment no-prior three-start free lever',
'translation_variable_enabled':True,'manual_prior_used':False,
'loo_bootstrap_sensitivity_called':False,'selection':selection,
'fixed_rotation_rpy_deg':args.rotation_rpy_deg,
'hpr_direct_sigma_rad':args.hpr_direct_sigma_rad,
'mechanical_reference_m':mechanical,'large_perturbation_m':args.large_perturbation_m,
'solutions':results,'maximum_solution_spread_m':spread,
'maximum_xy_solution_spread_m':xy_spread,
'circle_only_observability_gate':{
'xy_marginal_std_max_m':.15,'passed':circle_xy_observable},
'circle_only_data_only_accepted':circle_xy_observable,
'joint_free_blocked_by_circle_gate':False,
'covariance_postfit_scaled':False}
args.output.parent.mkdir(parents=True,exist_ok=True)
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
print(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2))
return 0
if __name__=='__main__':
raise SystemExit(main())
tools/run_rtk_imu_node_graph_free_joint.py
+85
View File
@@ -0,0 +1,85 @@
#!/usr/bin/env python3
'''Three-start, no-prior joint free-lever solve on fixed P0.5 motion windows.'''
from __future__ import annotations
import argparse,json,sys
from dataclasses import asdict
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT=Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
from rtk_imu.rtk_imu_engineering import _height_reference
from rtk_imu.rtk_imu_multisource import load_unified_sessions
from rtk_imu.rtk_imu_node_graph import build_problem,solve_free_lever_many
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_jsonable
from tools.run_rtk_imu_node_graph_fixed_lever import _select_window
def main():
parser=argparse.ArgumentParser(description=__doc__)
parser.add_argument('--manifest',type=Path,required=True)
parser.add_argument('--output',type=Path,required=True)
parser.add_argument('--circle-session',required=True)
parser.add_argument('--left-right-session',required=True)
parser.add_argument('--slope-session',required=True)
parser.add_argument('--sample-period-s',type=float,default=1.)
parser.add_argument('--target-duration-s',type=float,default=15.)
parser.add_argument('--max-nfev',type=int,default=120)
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
default=[.4543066225,-.0026392019,.0122384129])
parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
default=MECHANICAL_L_I_M.tolist())
parser.add_argument('--large-perturbation-m',nargs=3,type=float,
default=[.5,-.5,.5])
args=parser.parse_args()
categories={'circle':args.circle_session,'left_right':args.left_right_session,
'slope':args.slope_session}
sessions=load_unified_sessions(
args.manifest,selected_session_ids=set(categories.values()))
reference=_height_reference(sessions)
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
mechanical=np.asarray(args.mechanical_l_I_m,dtype=float)
problems,selections=[],{}
for motion,session_id in categories.items():
segment,selection=_select_window(
[s for s in sessions if s.session_id==session_id],reference,
args.sample_period_s,args.target_duration_s)
problems.append(build_problem(
segment,rotation,mechanical,args.hpr_direct_sigma_rad))
selections[motion]=selection
starts={'zero':np.zeros(3),'mechanical':mechanical,
'mechanical_large_perturbation':mechanical+args.large_perturbation_m}
results={name:asdict(solve_free_lever_many(problems,value,args.max_nfev))
for name,value in starts.items()}
for result in results.values():
covariance=np.asarray(result['lever_covariance_m2'])
result['lever_std_m']=np.sqrt(np.maximum(np.diag(covariance),0.))
result['delta_to_mechanical_m']=np.asarray(result['final_l_I_m'])-mechanical
solutions=np.asarray([value['final_l_I_m'] for value in results.values()])
spread=float(max(np.linalg.norm(a-b) for a in solutions for b in solutions))
gates={name:{'optimizer_converged':value['success'],
'lever_marginal_std':bool(np.all(np.asarray(value['lever_std_m'])<=[.15,.15,.20])),
'lever_information_rank':value['lever_precision_rank']==3,
'lever_information_condition':value['lever_information_condition_number']<=1e6,
'lever_min_information':min(value['lever_information_singular_values'])>=1e-3}
for name,value in results.items()}
observable=bool(all(all(gate.values()) for gate in gates.values()))
payload={'scope':'three-motion joint no-prior three-start free lever',
'translation_variable_enabled':True,'manual_prior_used':False,
'loo_bootstrap_sensitivity_called':False,'selections':selections,
'fixed_rotation_rpy_deg':args.rotation_rpy_deg,
'hpr_direct_sigma_rad':args.hpr_direct_sigma_rad,
'mechanical_reference_m':mechanical,'solutions':results,
'maximum_solution_spread_m':spread,'observability_gates':gates,
'joint_data_only_translation_observable':observable,
'manual_prior_started':False,'covariance_postfit_scaled':False}
args.output.parent.mkdir(parents=True,exist_ok=True)
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
print(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2))
return 0
if __name__=='__main__':
raise SystemExit(main())
tools/run_rtk_imu_node_graph_free_selected.py
+120
View File
@@ -0,0 +1,120 @@
#!/usr/bin/env python3
'''Three-start joint free solve on information-selected windows.'''
from __future__ import annotations
import argparse,json,sys
from concurrent.futures import ThreadPoolExecutor
from dataclasses import asdict
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT=Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
from rtk_imu.rtk_imu_engineering import _height_reference
from rtk_imu.rtk_imu_multisource import load_unified_sessions
from rtk_imu.rtk_imu_node_graph import (
build_problem,fit_states_at_fixed_lever,solve_free_lever_many)
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_build_segment,_jsonable,_qualified_runs
def _restore_segments(sessions,reference,selections,period_s):
runs=list(_qualified_runs(sessions,reference,period_s)); restored=[]
for item in selections:
match=None
for session,run,_ in runs:
if session.session_id!=item['session_id']: continue
contained=(run[0].t_s<=item['start_s']+1e-6 and
run[-1].t_s>=item['end_s']-1e-6)
if not contained: continue
nodes=tuple(node for node in run
if item['start_s']-1e-6<=node.t_s<=item['end_s']+1e-6)
if len(nodes)==item['node_count']:
match=_build_segment(session,nodes,item['candidate_id']); break
if match is None: raise RuntimeError('cannot restore '+item['candidate_id'])
restored.append(match)
return restored
def main():
parser=argparse.ArgumentParser(description=__doc__)
parser.add_argument('--manifest',type=Path,required=True)
parser.add_argument('--selection',type=Path,required=True)
parser.add_argument('--output',type=Path,required=True)
parser.add_argument('--sample-period-s',type=float,default=1.)
parser.add_argument('--max-nfev',type=int,default=120)
parser.add_argument('--prefit-max-nfev',type=int,default=50)
parser.add_argument('--prefit-workers',type=int,default=4)
parser.add_argument('--start-name',choices=['all','zero','mechanical',
'mechanical_large_perturbation'],default='all')
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
default=[.4543066225,-.0026392019,.0122384129])
parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
default=MECHANICAL_L_I_M.tolist())
parser.add_argument('--large-perturbation-m',nargs=3,type=float,
default=[.5,-.5,.5])
args=parser.parse_args()
selection=json.loads(args.selection.read_text(encoding='utf-8'))
ids={item['session_id'] for item in selection['selected_windows']}
sessions=load_unified_sessions(args.manifest,selected_session_ids=ids)
reference=_height_reference(sessions)
segments=_restore_segments(sessions,reference,selection['selected_windows'],
args.sample_period_s)
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
mechanical=np.asarray(args.mechanical_l_I_m,dtype=float)
problems=[build_problem(segment,rotation,mechanical,args.hpr_direct_sigma_rad)
for segment in segments]
starts={'zero':np.zeros(3),'mechanical':mechanical,
'mechanical_large_perturbation':mechanical+args.large_perturbation_m}
if args.start_name!='all': starts={args.start_name:starts[args.start_name]}
results={}; prefit={}
for name,value in starts.items():
with ThreadPoolExecutor(max_workers=args.prefit_workers) as executor:
fitted=list(executor.map(
lambda problem:fit_states_at_fixed_lever(
problem,value,args.prefit_max_nfev),problems))
state_values=[item[0] for item in fitted]
prefit[name]=[item[1] for item in fitted]
results[name]=asdict(solve_free_lever_many(
problems,value,args.max_nfev,state_values))
for result in results.values():
covariance=np.asarray(result['lever_covariance_m2'])
result['lever_std_m']=np.sqrt(np.maximum(np.diag(covariance),0.))
result['delta_to_mechanical_m']=np.asarray(result['final_l_I_m'])-mechanical
solutions=np.asarray([value['final_l_I_m'] for value in results.values()])
spread=float(max(np.linalg.norm(a-b) for a in solutions for b in solutions))
gates={name:{'optimizer_converged':value['success'],
'lever_marginal_std':bool(np.all(np.asarray(value['lever_std_m'])<=[.15,.15,.20])),
'lever_information_rank':value['lever_precision_rank']==3,
'lever_information_condition':value['lever_information_condition_number']<=1e6,
'lever_min_information':min(value['lever_information_singular_values'])>=1e-3}
for name,value in results.items()}
observable=bool(all(all(gate.values()) for gate in gates.values()))
payload={'scope':'information-selected multi-window no-prior three-start free lever',
'translation_variable_enabled':True,'manual_prior_used':False,
'loo_bootstrap_sensitivity_called':False,
'selection_artifact':str(args.selection),'selected_window_count':len(segments),
'start_name':args.start_name,'nuisance_prefit':prefit,
'nuisance_prefit_is_not_lever_prior':True,
'sample_overlap_audit':selection['sample_overlap_audit'],
'information_curve':selection['lever_std_vs_information_curve'],
'fixed_rotation_rpy_deg':args.rotation_rpy_deg,
'hpr_direct_sigma_rad':args.hpr_direct_sigma_rad,
'mechanical_reference_m':mechanical,'solutions':results,
'maximum_solution_spread_m':spread,'observability_gates':gates,
'data_only_translation_observable':observable,
'manual_prior_started':False,'covariance_postfit_scaled':False}
args.output.parent.mkdir(parents=True,exist_ok=True)
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
print(json.dumps(_jsonable({'selected_window_count':len(segments),
'solutions':{name:{'success':value['success'],'l_I_m':value['final_l_I_m'],
'std_m':value['lever_std_m'],'cost':value['final_cost'],
'chi_square_per_dof':value['chi_square_per_dof'],
'singular_values':value['lever_information_singular_values']}
for name,value in results.items()},'maximum_solution_spread_m':spread,
'data_only_translation_observable':observable}),ensure_ascii=False,indent=2))
return 0
if __name__=='__main__':
raise SystemExit(main())
tools/run_rtk_imu_node_graph_p05.py
+106
View File
@@ -0,0 +1,106 @@
#!/usr/bin/env python3
'''P0.5 fixed-lever node-graph covariance and motion-window audit.'''
from __future__ import annotations
import argparse, json, sys
from dataclasses import asdict
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
from rtk_imu.rtk_imu_engineering import _height_reference
from rtk_imu.rtk_imu_multisource import load_unified_sessions
from rtk_imu.rtk_imu_node_graph import (
build_problem,solve_fixed_lever,solve_fixed_lever_many)
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_jsonable
from tools.run_rtk_imu_node_graph_fixed_lever import _select_window
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('--manifest',type=Path,required=True)
parser.add_argument('--output',type=Path,required=True)
parser.add_argument('--circle-session',required=True)
parser.add_argument('--left-right-session',required=True)
parser.add_argument('--slope-session',required=True)
parser.add_argument('--sample-period-s',type=float,default=1.)
parser.add_argument('--target-duration-s',type=float,default=15.)
parser.add_argument('--max-nfev',type=int,default=50)
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
default=[.4543066225,-.0026392019,.0122384129])
parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
default=MECHANICAL_L_I_M.tolist())
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
args = parser.parse_args()
categories = {'circle':args.circle_session,'left_right':args.left_right_session,
'slope':args.slope_session}
sessions = load_unified_sessions(args.manifest,selected_session_ids=set(categories.values()))
reference = _height_reference(sessions)
rotation = Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
lever = np.asarray(args.mechanical_l_I_m)
problems, selections, separate = [], {}, {}
for category,session_id in categories.items():
local = [session for session in sessions if session.session_id == session_id]
segment,selection = _select_window(
local,reference,args.sample_period_s,args.target_duration_s)
problem = build_problem(segment,rotation,lever,args.hpr_direct_sigma_rad)
problems.append(problem); selections[category] = selection
separate[category] = asdict(solve_fixed_lever(problem,args.max_nfev))
joint = asdict(solve_fixed_lever_many(problems,args.max_nfev))
fixed_pass = bool(
all(value['success'] for value in separate.values()) and joint['success']
and joint['chi_square_per_dof'] >= .25
and joint['chi_square_per_dof'] <= 4.
and joint['final_position_residual_m']['vector_p95'] <= .20
and joint['final_velocity_residual_m_s']['vector_p95'] <= .50)
payload = {'scope':'P0.5 three-motion fixed-lever only',
'translation_variable_enabled':False,
'manual_prior_loo_bootstrap_sensitivity_called':False,
'covariance_model':{
'source':'independent_innovation_audit_three_motion',
'best_position_xyz_m':[.06,.06,.12],
'doppler_xyz_m_s':[.15,.15,.30],
'hpr_direct_angular_rad':args.hpr_direct_sigma_rad,
'hpr_bridge_rule':'sqrt(direct_sigma^2 + dropout_extra_variance)',
'imu_preintegration':'unchanged physical covariance',
'bias_random_walk':'unchanged static/Allan/device model',
'postfit_global_scale_applied':False},
'fixed_l_I_m':lever,'selections':selections,
'separate_fixed_lever':separate,'joint_fixed_lever':joint,
'fixed_lever_covariance_gate':{
'chi_square_per_dof_range':[.25,4.],
'position_vector_p95_max_m':.20,
'velocity_vector_p95_max_m_s':.50,
'passed':fixed_pass},
'whitening_diagnosis':{
'normalized_scale_consistent':joint['chi_square_per_dof'] >= .25,
'joint_preintegration_nis_per_dof':
joint['final_residual_by_factor']['imu_preintegration']['chi_square_per_dof'],
'joint_preintegration_normalized_p95':
joint['final_residual_by_factor']['imu_preintegration']['p95_abs'],
'preintegration_sigma_distribution':joint['preintegration_covariance_sigma'],
'interpretation':(
'absolute preintegration sigma is small, but per-node states satisfy process '
'factors almost exactly while BEST/Doppler/HPR normalized residuals are also '
'well below one; current factor covariance set is collectively overconservative'
)},
'free_lever_unlocked':fixed_pass}
args.output.parent.mkdir(parents=True,exist_ok=True)
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
compact = {'selections':selections,
'separate':{key:{'success':value['success'],
'chi_square_per_dof':value['chi_square_per_dof'],
'position_p95_m':value['final_position_residual_m']['vector_p95'],
'velocity_p95_m_s':value['final_velocity_residual_m_s']['vector_p95'],
'factor_stats':value['final_residual_by_factor']}
for key,value in separate.items()},
'joint':{'success':joint['success'],'chi_square_per_dof':joint['chi_square_per_dof'],
'position_p95_m':joint['final_position_residual_m']['vector_p95'],
'velocity_p95_m_s':joint['final_velocity_residual_m_s']['vector_p95'],
'factor_stats':joint['final_residual_by_factor']},
'free_lever_unlocked':fixed_pass}
print(json.dumps(_jsonable(compact),ensure_ascii=False,indent=2))
return 0
if __name__ == '__main__':
raise SystemExit(main())
tools/select_rtk_imu_windows_by_lever_information.py
+174
View File
@@ -0,0 +1,174 @@
#!/usr/bin/env python3
'''Select non-overlapping qualified windows by incremental lever information.'''
from __future__ import annotations
import argparse,json,sys
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT=Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
from rtk_imu.rtk_imu_engineering import _all_hpr,_height_reference
from rtk_imu.rtk_imu_multisource import load_unified_sessions
from rtk_imu.rtk_imu_node_graph import build_problem,linearized_lever_information
from tools.audit_rtk_imu_factor_consistency import _build_segment,_jsonable,_qualified_runs
from tools.run_rtk_imu_node_graph_fixed_lever import _select_window
STD_GATE=np.array([.15,.15,.20])
def _overlap(left,right):
return left['session_id']==right['session_id'] and not (
left['end_s']<right['start_s'] or right['end_s']<left['start_s'])
def _summary(information):
covariance=np.linalg.pinv(information,rcond=1e-9)
singular=np.linalg.svd(information,compute_uv=False)
sign,logdet=np.linalg.slogdet(information)
return {'std_m':np.sqrt(np.maximum(np.diag(covariance),0.)),
'singular_values':singular,'lambda_min':singular[-1],
'logdet':float(logdet) if sign>0 else -np.inf}
def _window_candidates(sessions,reference,period_s,duration_s):
candidates=[]
for session,run,segment_id in _qualified_runs(sessions,reference,period_s):
times=np.asarray([node.t_s for node in run])
start=0
while start<len(run):
end=int(np.searchsorted(times,times[start]+duration_s))
if end>=len(run): break
window=tuple(run[start:end+1])
if 10.<=window[-1].t_s-window[0].t_s<=20.:
candidate_id=f'{segment_id}:info_window_{start:04d}'
segment=_build_segment(session,window,candidate_id)
if segment is not None:
candidates.append({'candidate_id':candidate_id,
'session_id':session.session_id,'start_s':window[0].t_s,
'end_s':window[-1].t_s,'duration_s':window[-1].t_s-window[0].t_s,
'node_count':len(window),'segment':segment,'session':session})
start=end+1
return candidates
def _sample_keys(entry):
session=entry['session']; start,end=entry['start_s'],entry['end_s']
imu=set((session.session_id,int(round(t*1e6))) for t in session.imu.t_s
if start<=t<=end)
gnss=set((session.session_id,int(round(node.t_s*1e6)),node.source)
for node in entry['segment'].nodes)
hpr=_all_hpr(session)
hpr_keys=set((session.session_id,int(round(hpr.t_s[i]*1e6)))
for i in hpr.valid_indices if start<=hpr.t_s[i]<=end)
return {'imu':imu,'gnss':gnss,'hpr':hpr_keys}
def main():
parser=argparse.ArgumentParser(description=__doc__)
parser.add_argument('--manifest',type=Path,required=True)
parser.add_argument('--output',type=Path,required=True)
parser.add_argument('--circle-session',required=True)
parser.add_argument('--left-right-session',required=True)
parser.add_argument('--slope-session',required=True)
parser.add_argument('--sample-period-s',type=float,default=1.)
parser.add_argument('--window-duration-s',type=float,default=15.)
parser.add_argument('--max-additional-windows',type=int,default=100)
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
parser.add_argument('--linearization-l-I-m',nargs=3,type=float,
default=[-.53243,-.42662,.74660])
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
default=[.4543066225,-.0026392019,.0122384129])
args=parser.parse_args()
sessions=load_unified_sessions(args.manifest)
reference=_height_reference(sessions)
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
lever=np.asarray(args.linearization_l_I_m,dtype=float)
categories={'circle':args.circle_session,'left_right':args.left_right_session,
'slope':args.slope_session}
selected=[]
for motion,session_id in categories.items():
session=[s for s in sessions if s.session_id==session_id]
segment,meta=_select_window(
session,reference,args.sample_period_s,args.window_duration_s)
selected.append({**meta,'candidate_id':meta['segment_id'],'motion':motion,
'segment':segment,'session':session[0],'seed_window':True})
candidates=[entry for entry in _window_candidates(
sessions,reference,args.sample_period_s,args.window_duration_s)
if not any(_overlap(entry,seed) for seed in selected)]
for entry in [*selected,*candidates]:
problem=build_problem(entry['segment'],rotation,lever,args.hpr_direct_sigma_rad)
information,_,singular,weak=linearized_lever_information(problem,lever)
entry['problem']=problem; entry['information']=information
entry['single_window_singular_values']=singular
entry['single_window_weakest_direction_I']=weak
total=sum((entry['information'] for entry in selected),np.zeros((3,3)))
curve=[{'window_count':len(selected),'added_candidate_id':'seed_three_motion',
**_summary(total)}]
remaining=list(candidates)
saturation_count=0
stop_reason='candidate_exhausted'
while remaining and len(selected)<3+args.max_additional_windows:
feasible=[entry for entry in remaining
if not any(_overlap(entry,item) for item in selected)]
if not feasible: break
ranked=[]
for entry in feasible:
summary=_summary(total+entry['information'])
ranked.append((summary['lambda_min'],summary['logdet'],entry,summary))
_,_,choice,summary=max(ranked,key=lambda item:(item[0],item[1]))
gain=summary['lambda_min']-curve[-1]['lambda_min']
selected.append(choice); remaining.remove(choice); total+=choice['information']
curve.append({'window_count':len(selected),'added_candidate_id':choice['candidate_id'],
'delta_lambda_min':gain,**summary})
saturation_count=saturation_count+1 if gain<.01 else 0
if np.all(np.asarray(summary['std_m'])<=STD_GATE):
stop_reason='linearized_observability_gate_reached'; break
if saturation_count>=3:
stop_reason='incremental_lambda_min_gain_saturated'; break
else:
if len(selected)>=3+args.max_additional_windows:
stop_reason='max_additional_windows_reached'
seen={'imu':set(),'gnss':set(),'hpr':set()}
duplicate={'imu':0,'gnss':0,'hpr':0}
selected_output=[]
for order,entry in enumerate(selected):
keys=_sample_keys(entry)
shared={name:len(value&seen[name]) for name,value in keys.items()}
for name,value in keys.items():
duplicate[name]+=shared[name]; seen[name].update(value)
selected_output.append({'selection_order':order,
'candidate_id':entry['candidate_id'],'session_id':entry['session_id'],
'start_s':entry['start_s'],'end_s':entry['end_s'],
'duration_s':entry['duration_s'],'node_count':entry['node_count'],
'seed_window':entry.get('seed_window',False),
'single_window_information':entry['information'],
'single_window_singular_values':entry['single_window_singular_values'],
'single_window_weakest_direction_I':entry['single_window_weakest_direction_I'],
'sample_count':{name:len(value) for name,value in keys.items()},
'shared_sample_count_with_previous':shared})
payload={'scope':'all recovered qualified dynamics; information-based window selection',
'manual_prior_used':False,'linearization_l_I_m':lever,
'linearization_point_is_not_prior_factor':True,
'candidate_count':len(candidates),'selected_window_count':len(selected),
'selection_objective':'maximize lambda_min(H_l); break ties by logdet(H_l)',
'std_gate_m':STD_GATE,'stop_reason':stop_reason,
'selected_windows':selected_output,'lever_std_vs_information_curve':curve,
'sample_overlap_audit':{'duplicate_sample_count':duplicate,
'all_selected_windows_time_nonoverlapping_within_session':not any(
_overlap(a,b) for i,a in enumerate(selected) for b in selected[i+1:]),
'unique_sample_count':{name:len(value) for name,value in seen.items()}},
'final_linearized_observable':bool(np.all(
np.asarray(curve[-1]['std_m'])<=STD_GATE))}
args.output.parent.mkdir(parents=True,exist_ok=True)
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
print(json.dumps(_jsonable({'candidate_count':len(candidates),
'selected_window_count':len(selected),'stop_reason':stop_reason,
'final_curve':curve[-1],'sample_overlap_audit':payload['sample_overlap_audit']}),
ensure_ascii=False,indent=2))
return 0
if __name__=='__main__':
raise SystemExit(main())
tools/time_alignment.py
+97
View File
@@ -0,0 +1,97 @@
"""Small, dependency-light helpers for mapping independent sensor clocks."""
from __future__ import annotations
from dataclasses import asdict, dataclass
import numpy as np
@dataclass(frozen=True)
class AffineClockModel:
"""Numerically stable affine map ``y = y_ref + scale * (x - x_ref)``."""
x_ref: float
y_ref: float
scale: float
sample_count: int
inlier_count: int
residual_std_s: float
residual_p95_s: float
def map(self, value: float | np.ndarray) -> float | np.ndarray:
array = np.asarray(value, dtype=np.float64)
mapped = self.y_ref + self.scale * (array - self.x_ref)
return float(mapped) if array.ndim == 0 else mapped
def inverse(self, value: float | np.ndarray) -> float | np.ndarray:
if abs(self.scale) < 1e-12:
raise ValueError("clock model scale is zero")
array = np.asarray(value, dtype=np.float64)
mapped = self.x_ref + (array - self.y_ref) / self.scale
return float(mapped) if array.ndim == 0 else mapped
def to_dict(self) -> dict[str, float | int]:
return asdict(self)
def fit_affine_clock(
x: np.ndarray,
y: np.ndarray,
*,
max_iterations: int = 4,
min_residual_gate_s: float = 5e-4,
) -> AffineClockModel:
"""Robustly fit an affine clock map while rejecting receive-time spikes.
``x`` and ``y`` may have large, unrelated epochs. Centering around their
medians avoids losing precision when host UTC is around 1e9 seconds.
"""
x_values = np.asarray(x, dtype=np.float64).reshape(-1)
y_values = np.asarray(y, dtype=np.float64).reshape(-1)
finite = np.isfinite(x_values) & np.isfinite(y_values)
x_values = x_values[finite]
y_values = y_values[finite]
if x_values.size < 2:
raise ValueError("need at least two finite clock samples")
x_ref = float(np.median(x_values))
y_ref = float(np.median(y_values))
dx = x_values - x_ref
dy = y_values - y_ref
inliers = np.ones(x_values.size, dtype=bool)
scale = 1.0
offset = 0.0
for _ in range(max_iterations):
local_x = dx[inliers]
local_y = dy[inliers]
denom = float(local_x @ local_x)
if denom < 1e-18:
raise ValueError("clock samples do not span enough time")
scale = float(local_x @ local_y / denom)
offset = float(np.median(local_y - scale * local_x))
residual = dy - (offset + scale * dx)
center = float(np.median(residual[inliers]))
mad = float(np.median(np.abs(residual[inliers] - center)))
sigma = 1.4826 * mad
gate = max(float(min_residual_gate_s), 6.0 * sigma)
updated = np.abs(residual - center) <= gate
if np.count_nonzero(updated) < 2 or np.array_equal(updated, inliers):
break
inliers = updated
# Fold the small centered intercept into y_ref so map/inverse stay simple.
y_ref += offset
residual = y_values - (y_ref + scale * (x_values - x_ref))
residual_inliers = residual[inliers]
return AffineClockModel(
x_ref=x_ref,
y_ref=y_ref,
scale=scale,
sample_count=int(x_values.size),
inlier_count=int(np.count_nonzero(inliers)),
residual_std_s=float(np.std(residual_inliers)),
residual_p95_s=float(np.percentile(np.abs(residual_inliers), 95.0)),
)
tools/validate_rtk_hpr_dropout_bridge.py
+182
View File
@@ -0,0 +1,182 @@
#!/usr/bin/env python3
"""Read-only artificial GNHPR dropout validation for the engineering R0 bridge.
For each requested gap, Q4 HPR samples inside an otherwise 0.1 s contiguous
run are withheld. Their true baseline directions are compared with normalized
linear interpolation and short-horizon HI13 gyro propagation. No lever-arm
solve, prior, bootstrap, sensitivity run, or threshold update is performed.
"""
from __future__ import annotations
import argparse
import json
import math
import sys
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
from rtk_imu.rtk_imu_engineering import _all_hpr, _baseline_angle_deg, _interpolate_baseline, _world_rtk
from rtk_imu.rtk_imu_multisource import load_unified_sessions
GAPS_S = (0.2, 0.3, 0.4, 0.5, 0.6, 0.8)
R2G_RPY_DEG = (0.4543066225, -0.0026392019, 0.0122384129)
BASELINE_FACTOR_SIGMA_DEG = float(np.degrees(0.035))
def _jsonable(value):
if isinstance(value, np.ndarray):
return _jsonable(value.tolist())
if isinstance(value, np.generic):
return _jsonable(value.item())
if isinstance(value, float):
return value if math.isfinite(value) else None
if isinstance(value, dict):
return {str(k): _jsonable(v) for k, v in value.items()}
if isinstance(value, (tuple, list)):
return [_jsonable(v) for v in value]
return value
def _contiguous_runs(times: np.ndarray, valid: np.ndarray) -> list[np.ndarray]:
indices = np.flatnonzero(valid)
if indices.size == 0:
return []
runs: list[list[int]] = [[int(indices[0])]]
for previous, index in zip(indices[:-1], indices[1:]):
dt = float(times[index] - times[previous])
if 0.075 <= dt <= 0.125:
runs[-1].append(int(index))
else:
runs.append([int(index)])
return [np.asarray(run, dtype=int) for run in runs if len(run) >= 3]
def _propagate_baseline(session, R_WI_start: np.ndarray, baseline_I: np.ndarray,
start_s: float, targets_s: np.ndarray) -> list[np.ndarray]:
"""Propagate with vectorized gyro interpolation over the short withheld gap."""
targets = np.asarray(targets_s, dtype=float)
if targets.size == 0:
return []
grid = np.unique(np.concatenate(([start_s], session.imu.t_s[
(session.imu.t_s > start_s) & (session.imu.t_s < targets[-1])], targets)))
gyro = np.column_stack([
np.interp(grid, session.imu.t_s, session.imu.gyro_rad_s[:, axis]) for axis in range(3)
])
R_WI = np.asarray(R_WI_start, dtype=float).copy()
predicted: list[np.ndarray] = []
target_index = 0
for index, (left, right) in enumerate(zip(grid[:-1], grid[1:])):
omega = 0.5 * (gyro[index] + gyro[index + 1])
R_WI = R_WI @ Rotation.from_rotvec(omega * float(right - left)).as_matrix()
while target_index < targets.size and abs(targets[target_index] - right) < 1e-9:
predicted.append(R_WI @ baseline_I)
target_index += 1
return predicted
def _session_validation(session, rotation: np.ndarray, max_windows: int) -> dict[str, object]:
hpr = _all_hpr(session)
runs = _contiguous_runs(hpr.t_s, hpr.valid)
baseline_I = rotation.T[:, 0]
result: dict[str, object] = {"session_id": session.session_id, "q4_runs": len(runs), "gaps": {}}
for requested_gap in GAPS_S:
interpolation_errors: list[float] = []
propagation_errors: list[float] = []
actual_gaps: list[float] = []
windows = 0
for run in runs:
nominal_dt = float(np.median(np.diff(hpr.t_s[run])))
step_count = max(2, int(round(requested_gap / nominal_dt)))
# The endpoints remain observed, every internal Q4 point is withheld.
for start in range(0, run.size - step_count, max(1, step_count)):
subset = run[start:start + step_count + 1]
if subset.size != step_count + 1:
continue
left, right = int(subset[0]), int(subset[-1])
actual_gap = float(hpr.t_s[right] - hpr.t_s[left])
if abs(actual_gap - requested_gap) > 0.08:
continue
withheld = subset[1:-1]
if withheld.size == 0:
continue
left_b, right_b = hpr.baseline_enu[left], hpr.baseline_enu[right]
targets = hpr.t_s[withheld]
predicted_prop = _propagate_baseline(
session, _world_rtk(left_b) @ rotation, baseline_I, float(hpr.t_s[left]), targets
)
for index, predicted in zip(withheld, predicted_prop):
fraction = float((hpr.t_s[index] - hpr.t_s[left]) / actual_gap)
interpolation_errors.append(_baseline_angle_deg(
_interpolate_baseline(left_b, right_b, fraction), hpr.baseline_enu[index]
))
propagation_errors.append(_baseline_angle_deg(predicted, hpr.baseline_enu[index]))
actual_gaps.append(actual_gap)
windows += 1
if windows >= max_windows:
break
if windows >= max_windows:
break
def summary(errors: list[float]) -> dict[str, float | int | None]:
finite = np.asarray([e for e in errors if np.isfinite(e)], dtype=float)
if finite.size == 0:
return {"count": 0, "p50_deg": None, "p95_deg": None, "max_deg": None}
return {"count": int(finite.size), "p50_deg": float(np.percentile(finite, 50)),
"p95_deg": float(np.percentile(finite, 95)), "max_deg": float(np.max(finite))}
interp, prop = summary(interpolation_errors), summary(propagation_errors)
choice = "linear_baseline_interpolation" if (interp["p95_deg"] or np.inf) <= (prop["p95_deg"] or np.inf) else "imu_gyro_propagation"
selected_p95 = interp["p95_deg"] if choice.startswith("linear") else prop["p95_deg"]
result["gaps"][f"{requested_gap:.1f}"] = {
"requested_gap_s": requested_gap,
"actual_gap_p50_s": float(np.median(actual_gaps)) if actual_gaps else None,
"window_count": windows,
"linear_baseline_interpolation": interp,
"imu_gyro_propagation": prop,
"preferred_method_by_p95": choice,
"selected_p95_deg": selected_p95,
"within_baseline_factor_sigma": bool(selected_p95 is not None and selected_p95 <= BASELINE_FACTOR_SIGMA_DEG),
}
return result
def main() -> int:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--manifest", type=Path, required=True)
parser.add_argument("--output", type=Path, required=True)
parser.add_argument("--session", action="append")
parser.add_argument("--max-windows", type=int, default=500)
args = parser.parse_args()
sessions = load_unified_sessions(args.manifest, selected_session_ids=None if args.session is None else set(args.session))
rotation = Rotation.from_euler("xyz", R2G_RPY_DEG, degrees=True).as_matrix()
session_results = [_session_validation(session, rotation, args.max_windows) for session in sessions]
recommendation: dict[str, object] = {}
for gap in GAPS_S:
entries = [item["gaps"][f"{gap:.1f}"] for item in session_results if item["gaps"].get(f"{gap:.1f}")]
p95 = [entry["selected_p95_deg"] for entry in entries if entry["selected_p95_deg"] is not None]
recommendation[f"{gap:.1f}"] = {
"sessions_with_samples": len(p95),
"worst_session_selected_p95_deg": float(max(p95)) if p95 else None,
"passes_all_sessions_factor_sigma": bool(p95 and max(p95) <= BASELINE_FACTOR_SIGMA_DEG),
}
passing = [float(key) for key, value in recommendation.items() if value["passes_all_sessions_factor_sigma"]]
payload = {
"scope": "artificial HPR dropout validation only; no solve/prior/bootstrap/sensitivity/threshold change",
"r2g_rotation_rpy_deg": R2G_RPY_DEG,
"baseline_factor_sigma_deg": BASELINE_FACTOR_SIGMA_DEG,
"sessions": session_results,
"bridge_recommendation": recommendation,
"largest_gap_passing_all_sessions_s": max(passing) if passing else None,
}
args.output.parent.mkdir(parents=True, exist_ok=True)
args.output.write_text(json.dumps(_jsonable(payload), ensure_ascii=False, indent=2) + "\n", encoding="utf-8")
print(json.dumps({"largest_gap_passing_all_sessions_s": payload["largest_gap_passing_all_sessions_s"],
"bridge_recommendation": recommendation}, ensure_ascii=False, indent=2))
return 0
if __name__ == "__main__":
raise SystemExit(main())