94 changed files with 122180 additions and 1171 deletions
+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
+37 -167
View File
@@ -1,184 +1,54 @@
# LiDARIMU 外参标定
# 车辆多传感器外参标定
用连续行驶中的 LiDAR 与 IMU 相对运动,估计安装外参与时间偏置:
本仓库维护两条彼此独立的外参标定流程:**雷达–IMU** 和 **RTKIMU**。两条流程复用少量通用的几何、IMU 读取和预积分代码,但不共用观测模型、求解器或验收结论。
## 从这里开始
| 目标 | 阅读文档 | 当前可交付范围 |
| --- | --- | --- |
| 标定雷达与 IMU | [雷达–IMU 标定](docs/雷达-IMU标定.md) | 旋转与时间关系已有实车候选;平移尚不可交付。 |
| 标定双天线 RTK 与 IMU | [RTKIMU 标定](docs/RTK-IMU标定.md) | 旋转已固定;杆臂以机械测量为基准,并完成动态一致性验证,尚未获得正式工程放行。 |
不要将任何一条流程中被拒绝的平移结果当作完整六自由度外参。是否接受结果,以相应结果 JSON 的 `accepted` 字段和门禁结论为准,而不是仅看优化是否收敛。
## 两条流程的关系
```text
p_IMU = T_IMU_lidar · p_lidar
雷达–IMU
点云相对运动 + IMU 预积分
→ 旋转与时间关系;平移受 IMU 位移误差和激励不足限制
RTKIMU
双天线基线 + GNSS 位置/速度 + IMU
→ 天线相位中心相对 IMU 的旋转和杆臂一致性验证
```
**当前阶段:** 算法与合成自检已闭环;已提供 `tools/export_rscap_to_v1.py`N300 `.rscap` + H32 dlog/MSOP → 中间格式);**合格实车验收尚未完成**,故正式外参尚未对实车落盘交付
选择 RTK–IMU 不是用 RTK 替换雷达。它用于解决“雷达–IMU 的平移在当前实车数据上不可可靠交付”这一独立问题;RTK–IMU 的外参也不能自动转换为雷达–IMU 外参
---
## 仓库结构
## 先看什么(对外三份就够)
| 顺序 | 文档 | 用途 |
| --- | -------------------------------------- | -------------------- |
| 1 | **本 README** | 做什么、怎么跑、结果怎么判 |
| 2 | [docs/V1_数据格式.md](docs/V1_数据格式.md) | 中间格式 + 原始数据导出命令 |
| 3 | [docs/标定流程与采集清单.md](docs/标定流程与采集清单.md) | 现场怎么采(合格数据要求) |
其余(方法细述、测试说明、源码职责、CHANGELOG)给深入阅读 / 改代码时用,见文末。
---
## 1. 输入 / 输出
| 输入 | 说明 |
| --------- | ----------------------------------------------------------- |
| `imu.csv` | `t,gx,gy,gz,ax,ay,az`(秒;rad/sm/s²);`t` 用设备时间 |
| 雷达会话目录 | `frames_index.csv` + `frames/*.npz`(米制 XYZ |
| 车辆 YAML | 轴向与时间语义;外参真值可空(`config/vehicle_installation.template.yaml` |
| 输出 | 说明 |
| ------------------ | ---------------------- |
| `T_IMU_lidar.json` | 外参 |
| `time_offset.json` | `t_imu = t_lidar + δt` |
| `summary.json` | 状态、残差、可观性 |
新车原始数据导出(H32 dlog/zip + HI13 rscap):
```powershell
python tools\export_rscap_to_v1.py `
--imu-rscap path\to\hi13r4-imu.rscap `
--imu-kind hi13 `
--lidar-dlog path\to\session_or_recovered.zip `
--host-start 2026-08-08T17:40:05 `
--host-end 2026-08-08T17:45:15 `
--out path\to\session_v1 `
--require-difop
```text
calibration/
├─ imu_lidar/ 雷达–IMU 专用算法和命令行入口
├─ rtk_imu/ RTKIMU 专用算法
├─ tools/ 原始数据导出、求解、审计和结果汇总入口
├─ docs/ 两份规范说明:雷达–IMU、RTK–IMU
├─ artifacts/ 已提交的结果摘要与审核证据
├─ config/ 车辆安装与流程配置
└─ tests/ 自动化测试
```
---
`rtk_imu/` 只允许复用 `imu_lidar/` 中的通用模块:`contracts.py``geometry.py``geodesy.py``imu_io.py``imu_preintegration.py``rotation_handeye.py`。它不依赖雷达专用的点云读取、去畸变、配准、雷达流程编排或雷达联合优化器。
## 数据与过程文件
原始 `.rscap`、统一导出数据、节点状态、检查点和调试中间结果只保留本地;仓库只提交可复核的代码、工具、配置、说明和结果摘要。RTK–IMU 的正式结果索引在 [artifacts/rtk_imu_calibration_v3/README.md](artifacts/rtk_imu_calibration_v3/README.md)。
## 2. 一键复现(合成,不需实车)
## 开发与验证
```powershell
cd <本仓库根目录>
python -m pip install -e ".[dev]"
python -m pip install -e ".[open3d]" # 推荐
powershell -File tools\reproduce_synthetic.ps1
python -m pytest -q
```
证明:链路可跑通,能收回已知 yaw / δt。
不证明:实车安装精度、平移可交付。
产物在 `examples/synthetic_session/out/`(含 `summary.json``motion_pairs.json`)。叠点查看:
```powershell
# 优先读取 summary 同目录的 motion_pairs.json,按需加载点云(无需重算配准)
python tools\visualize_pair_3d.py `
--lidar examples\synthetic_session\lidar `
--summary examples\synthetic_session\out\summary.json `
--pair-index 0
```
旧标定目录若缺少缓存,可只补导出运动对(不重求解外参):
```powershell
python tools\export_motion_pairs_for_viz.py `
--lidar path\to\lidar `
--imu path\to\imu.csv `
--summary path\to\out\summary.json
```
`1``4` 切换叠点模式;`N`/`P` 切换运动对。
---
## 3. 真实数据怎么跑
1. 按采集清单录制(设备时间;静止 + 低速转弯;有结构场景)
2. 导出中间格式(第1节命令)
3. 填写车辆 YAML 的轴向与时间语义
4. 标定:
```powershell
python -m imu_lidar.cli run `
--vehicle-config config\vehicle_installation.template.yaml `
--imu path\to\session_v1\imu.csv `
--lidar path\to\session_v1\lidar `
--output path\to\out `
--mode rotation_only `
--time-offset-search-s 2.0
```
1.`summary.json`,再叠点 / 用验证会话复核后才交付
| 模式 | 交付 | 成功标志 |
| ------------------- | ------- | --------------------------- |
| `rotation_only`(先做) | 旋转 + δt | `rotation_only_accepted` |
| `full_se3`(激励够再试) | + 可观平移 | `full_se3_accepted`(否则平移拒绝) |
| `summary.json` 状态 | 含义 |
| ---------------------------------------------- | ----------- |
| `rotation_only_accepted` / `full_se3_accepted` | 可进入验证 |
| `full_se3_rejected_due_to_observability` | 旋转可用,平移不交 |
| `blocked` | **不可作安装参数** |
预期量级:旋转约 0.5°–2°;水平平移数厘米~十几厘米;无坡时竖直常不可观。
---
## 4. 方法(一句话)
关键帧雷达配准得 **B**,同区间 IMU 预积分得 **A**,解 `R_A R_X ≈ R_X R_B`;再估 δt。可观时才在 `full_se3` 下交平移。
---
## 5. 试验边界(勿误读)
| | 合成 pytest | 旧车 S2 线下 |
| ------ | ------------------------ | ----------------- |
| 目的 | 回归算法 | 验证旧主机时间数据上链路能跑完 |
| 期望 | `rotation_only_accepted` | `blocked`**(预期)** |
| 当安装参数? | 否 | **否** |
细节:[tests/README.md](tests/README.md)。
---
## 6. 仓库结构与其余文档
```text
imu_lidar/ 算法与 CLI
config/ 车辆配置模板
docs/ 采集清单、数据格式、方法细述
tools/ 导出、合成复现、可视化
tests/ 自动化测试
```
| 文档 | 何时看 |
| ------------------------------------------------ | ---------------- |
| [docs/IMU-LiDAR标定.md](docs/IMU-LiDAR标定.md) | 要看方法约定与实现状态表 |
| [tests/README.md](tests/README.md) | 要看合成用例 / S2 烟测记录 |
| [imu_lidar/文件职责说明.md](imu_lidar/文件职责说明.md) | 要改源码 |
| [imu_lidar/CHANGELOG.md](imu_lidar/CHANGELOG.md) | 要查改动史 |
改算法请同步职责说明与 CHANGELOG;改对外用法请更新本 README。
雷达点云处理需要时再安装可选依赖:`python -m pip install -e ".[open3d]"`
@@ -0,0 +1,17 @@
# RTKIMU 标定产物说明
- `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` 明显不具机械真实性,禁止用于车辆配置。完整解释见 [RTKIMU 标定说明](../../docs/RTK-IMU标定.md)。
@@ -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"
}
]
}
@@ -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"
]
}
@@ -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"
}
@@ -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"
]
}
@@ -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"
}
]
}
@@ -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"
]
}
@@ -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"
}
@@ -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"
]
}
@@ -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"
}
]
}
@@ -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"
]
}
@@ -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"
}
@@ -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"
}
]
}
@@ -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"
]
}
@@ -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"
}
@@ -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"
}
]
}
@@ -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"
]
}
@@ -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"
}
@@ -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"
}
]
}
@@ -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"
]
}
@@ -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"
}
@@ -0,0 +1,24 @@
# RTKIMU V3 结果与审核证据
本目录保存**已提交的审核快照**。它不是原始数据目录,也不是日常运行的输出目录;原始 `.rscap`、统一导出、节点状态、checkpoint 和调试文件均应保留在本地工作目录。
## 先读哪个文件
1. `engineering_release_decision.json`:唯一的最终结论,包含候选杆臂、两个互逆变换、验收状态、禁止的表述和全部证据路径。
2. `mechanical_prior_engineering_47_window.json`:47 个非重叠标定窗口中,无先验、固定机械杆臂和机械软约束结果的对比。
3. `mechanical_prior_engineering_heldout.json`:267 个独立留出窗口的物理残差与收敛情况。
4. `heldout_independent_innovation.json``propagation_bias_root_cause_audit.json`:解释最终未放行的独立传播误差。
## 文件分组
| 分组 | 文件 | 说明 |
| --- | --- | --- |
| 发布结论 | `engineering_release_decision.json` | 当前唯一的放行判断。 |
| 旋转与数据质量 | `multisource_result.json``bestnava_doppler_factor_yield_*.json``hpr_dropout_bridge_*.json` | 原始数据、旋转和双天线观测可用性的审核。 |
| 杆臂与选窗 | `lever_information_window_selection*.json``node_graph_free_information_selected_mechanical.json``mechanical_prior_engineering_47_window.json` | 无先验可观性、固定窗口及机械杆臂一致性。 |
| 独立验证 | `mechanical_prior_engineering_heldout.json``heldout_independent_innovation.json``heldout_nonconverged_retry.json``mechanical_prior_rotation_sensitivity.json` | 留出数据、创新、重试和旋转扰动检查。 |
| 根因和历史审计 | `propagation_bias_root_cause_audit.json``motion_excitation_*.json``factor_consistency_audit.json` 等 | 帮助理解过程和未解决问题;不替代发布结论。 |
`lever_information_window_selection.json` 记录 47 个标定窗口;`lever_information_window_selection_refined.json` 记录 314 个全部非重叠窗口,可从中扣除前者得到 267 个留出窗口。`mechanical_prior_engineering_47_window_states.npz` 是历史缓存,不是必需输入,也不应重新提交。
当前结论、术语和可执行复现步骤见 [RTKIMU 标定](../../docs/RTK-IMU标定.md)。
@@ -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
}
}
}
File diff suppressed because it is too large Load Diff
File diff suppressed because it is too large Load Diff
File diff suppressed because it is too large Load Diff
File diff suppressed because it is too large Load Diff
File diff suppressed because it is too large Load Diff
File diff suppressed because it is too large Load Diff
File diff suppressed because it is too large Load Diff
File diff suppressed because it is too large Load Diff
+52
View File
@@ -0,0 +1,52 @@
# RTKIMU 候选数据清点(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 syaw变化约485° | 多圈 yaw 与 XY 杠杆臂 |
| `priority_174905_175450` | 100% GGA质量4Pitch跨度约11.4°;XY约58×22 m | 强候选:yaw、pitch、XY/Z耦合解除 |
| `priority_175910_180530` | 100% GGA质量4Pitch跨度约13.6°;XY约144×59 m | 最强候选:长基线、pitch、平移 |
| `slope_190548_190730` | 100% GGA质量4Pitch跨度约7.4°;约102 s | 坡度/Pitch补充 |
| `circle_193412_193642` | 100% GGA质量4yaw变化约445° | 平面旋转与XY杠杆臂 |
| `loop_194223_195003` | 仅1个异常GGAyaw累计变化约1203°;约460 s | 最强 yaw/多圈转弯候选 |
| `accel_195608_195958` | 100% GGA质量4XY约46×23 m;约230 s | 加减速、速度和水平杠杆臂 |
| `motion_sms_154023_154359` | 全窗仅约49%为质量4;可用连续子段约100 s | 仅用15:41:28.215: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`
## 已发现的 LiDARRTK 资料边界
`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 标定,且比当前纯 LiDARIMU Phase-B 更有希望约束 XYRTK 提供绝对位置,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,外参共享,并检查安装期间是否发生机械变动。
@@ -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
@@ -1,209 +0,0 @@
# 20260808 HI13 + H32LiDARIMU 标定现状与问题
> 数据:`D:\data\calibration_usable_20260808`
> 可用会话:`sessions_v1_host_aligned`(三优先窗)
> 当前结果目录:各窗 `out_fixed_dt0/`
> 清单:`sessions_v1_host_aligned/calibration_manifest_fixed_dt0.json`
> 车辆配置:`config/vehicle_hi13_h32_20260808.yaml`
> 约定外参:`p_IMU = T_IMU_lidar · p_lidar`
---
## 1. 一句话结论
**旋转 + 主机桥接时间对齐可以冻结;平移(full_se3)尚不可正式交付。**
三窗 `rotation_only`(δt=0)结果跨窗一致,**不必因平移先验 Z 修正而重跑旋转**。
---
## 2. 当前可用结果(`out_fixed_dt0`
约定:`p_IMU = T_IMU_lidar · p_lidar`;本轮交付 **仅旋转**`t = [0,0,0]``time_offset_s = 0`
| 窗 | 状态 | δt | roll/pitch/yaw (°) | 手眼 RMS (°) | 手眼对数 | vs CAD prior |
|----|------|----|---------------------|--------------|----------|--------------|
| `priority_174005_174515` | `rotation_only_accepted` | 0 | 0.398 / +0.108 / **89.998** | 0.625 | 1374 | 0.413° |
| `priority_174905_175450` | 同上 | 0 | 0.316 / 0.352 / **90.002** | 0.293 | 1182 | 0.473° |
| `priority_175910_180530` | 同上 | 0 | 0.373 / 0.036 / **90.005** | 0.786 | 789 | 0.375° |
- 跨窗旋转互差约 **0.15°–0.47°**(相对三窗均值 ≤0.26°)。
- CAD/安装平移先验只用于后续 SE3 / 校验,不写入本轮交付 `T`
- 原始摘要:各窗 `out_fixed_dt0/summary.json`;总表 `calibration_manifest_fixed_dt0.json`
### 2.1 窗1 `priority_174005_174515` — `R_IMU_lidar`
- 路径:`...\priority_174005_174515\out_fixed_dt0\summary.json`
- rpy_deg_xyz`[-0.39806616272552936, 0.10842366761721789, 89.99788311264182]`
- quaternion_xyzw`[-0.0031254048203223084, -0.0017872288323561246, 0.7070914597978358, 0.707112936622415]`
```text
R =
[[ 3.6946588133e-05, -0.9999758655693408, -0.0069474393698490 ],
[ 0.9999982088237712, 2.3798651350e-05, 0.0018925598731371 ],
[-0.0018923488575947, -0.0069474968493909, 0.9999740753156198 ]]
t = [0, 0, 0]
```
### 2.2 窗2 `priority_174905_175450` — `R_IMU_lidar`
- 路径:`...\priority_174905_175450\out_fixed_dt0\summary.json`
- rpy_deg_xyz`[-0.31554362742542746, -0.35231680831805484, 90.00193338170823]`
- quaternion_xyzw`[0.00022698471903919405, -0.004121119694188399, 0.7071067019847445, 0.7070948146172911]`
```text
R =
[[-3.3743238552e-05, -0.9999848355714863, -0.0055070399001942 ],
[ 0.9999810938467022, 1.2097239027e-07, -0.0061491421465438 ],
[ 0.0061490495645171, -0.0055071432752239, 0.9999659297008071 ]]
t = [0, 0, 0]
```
### 2.3 窗3 `priority_175910_180530` — `R_IMU_lidar`
- 路径:`...\priority_175910_180530\out_fixed_dt0\summary.json`
- rpy_deg_xyz`[-0.37335575043737196, -0.03585856981709423, 90.00538974486676]`
- quaternion_xyzw`[-0.002082462199099642, -0.002525219418619018, 0.7071355299053291, 0.7070704554452735]`
```text
R =
[[-9.4068775206e-05, -0.9999787650354249, -0.0065161821101807 ],
[ 0.9999997997313592, -8.9988606603e-05, -0.0006264497522950 ],
[ 0.0006258500675082, -0.0065162397345547, 0.9999785732361544 ]]
t = [0, 0, 0]
```
### 相对历史失败轮次
| 轮次 | 问题 | 结果 |
|------|------|------|
| `sessions_v1_aligned` | 首帧强行对齐设备钟 | 三窗手眼失败,RMS ~9°–12° |
| 自由估 δt + signed refine | 窗3 δt 漂到 0.48 s;窗2 yaw≈19° | 跨窗 yaw 矛盾(81°/19°/93°) |
| **本轮 fixed δt=0** | 主机桥接后冻结时间 | 三窗 yaw≈90°,可互证 |
---
## 3. 已澄清并写入配置的坐标系 / 先验
### 3.1 车体与传感器
- 车体:X 前 / Y 左 / Z 上;雷达与 IMU 安装在 **X 正方向**(后轮轴前方)。
- CAD 图纸可能画成 +X 朝后,那只是读图坐标系,**不是**车体真实轴。
- IMUHI13 RFUX 右 / Y 前 / Z 上),原始数据不做轴向重映射。
- 雷达 NPZ:假定与车体一致(X 前 / Y 左 / Z 上)。
### 3.2 安装量(`translation_m`
| 传感器 | X / Y(后轮轴中心) | Z(离地) |
|--------|---------------------|-----------|
| IMU | 2.574 / 0.0365 m | 0.8925 + 0.294 = **1.1865 m** |
| 雷达 | 2.522 / 0.00002 m | 相位中心离地 **1.994999879 m** |
- 后轮轴中心离地:**294 mm**(Z 用离地高时加在 CAD 轴心高上)。
- 雷达 CAD `dZ=1.637499879 m`;相位中心离地还需加后轮轴离地 `0.294 m` 和相位中心偏移 `0.0635 m`,最终为 `1.994999879 m`
### 3.3 导出外参先验
- `R_IMU_lidar` ≈ yaw 90°:`[[0,-1,0],[1,0,0],[0,0,1]]`(软约束 σ=15°)。
- `t_IMU_lidar`**`[0.0365, -0.0518, 0.8085]` m**(相对 Z = 1.994999879 1.1865 = 0.808499879 m)。
- **旋转先验不因 Z 修正改变**;平移先验 Z 更新为 0.808499879 m。
---
## 4. 现存问题清单
### P1. IMU 预积分平移 `Δp` 不可用(阻塞正式平移)
- 现象:可视化模式 4 若用完整 `X⁻¹ A X`,橙/蓝点云常呈**上下错层**(Z 差米级~几十米)。
- 根因:加速度预积分缺少可靠重力/零偏处理,`t_A` 尤其 Z 发散;**不是旋转外参错了**。
- 旁证:相对 GICP 的旋转残差中位约 0.16°;`|t_A|` 中位却常 >1 m。
- 影响:`full_se3` / 依赖 IMU 位移的平移估计不可信。
- 缓解(已做):`visualize_pair_3d.py``rotation_only` 默认模式 4 = **R 共轭 + GICP 的 t_B**`--mode4-translation gicp|imu|auto`)。
### P2. 平面运动导致竖直平移弱可观
- 三优先窗以水平转弯为主,缺少缓坡/俯仰激励。
- 流水线门控已给出 `translation_accepted=false`
- 即使打开平移先验(σ≈5 cm),弱激励下结果易变成**先验回显**,不宜当标定成功。
### P3. 时间偏移若再自由估计会被带偏(已规避,需保持)
- 主机 UTC 桥接(MSOP/IMU `HostReceiveUtc`)后,两路已在同一时间轴,残差通常几十毫秒量级。
- 若再做有符号 δt 精修,会与错误/未收敛的 R 耦合,窗3 曾从约 −0.12 s 走到 **0.48 s**。
- **现行做法**:桥接会话使用 `--fixed-time-offset-s 0 --no-signed-time-refine`
### P4. 单窗低残差 ≠ 外参正确(历史教训)
- 自由 δt 轮次中,窗2 手眼 RMS 最低(~0.3°)但 yaw≈19°,与 CAD/其他窗差 60°+。
- 平面运动下 yaw 外参可出现多个能拟合 `R_A R_X ≈ R_X R_B` 的解。
- **必须**做跨窗一致性 + 可视化叠点,不能只看单窗 RMS。
### P5. 旋转软先验尚未做无先验对照
- 当前 σ=15°;笔记显示 Tsai 初值本身已接近(约 0.3°–1.1° RMS),不像纯先验硬拽。
- 仍缺一次:关闭先验或放大 `sigma_deg` 的对照,以排除「只是被拉到 90°」的疑虑。
### P6. 文档与操作约定未完全同步(工程)
- README 需明确写清:host-bridge 后固定 δt=0、禁用 signed refine、rotation_only 可视化用法。
- 交付物目前缺一版「冻结的联合/中位 R + 使用说明」JSON/报告(旋转可交,平移明确不交)。
---
## 5. 不该做 / 可以做
| 动作 | 建议 |
|------|------|
| 因 Z 先验修正重跑三窗 rotation_only | **不必**R 未依赖新 t |
| 正式交付 6-DOF / 信赖当前 `Δp` 估 t | **不要** |
| 试验性 `full_se3`(固定 R、δt=0、新 t 先验) | 可做,结果标「实验」 |
| 可视化验收模式 3 vs 4(gicp 平移) | **建议做** |
| 无先验 / 大 σ 旋转对照 | **建议做** |
| 冻结交付 `R` + `δt=0` 说明 | **建议做** |
| 补采缓坡或加强垂直尺寸约束后再估 t | 正式平移前需要 |
---
## 6. 建议下一步顺序
1. **验收旋转**:三窗抽转弯运动对,模式 3/4 叠点;可选无先验对照。
2. **定稿旋转**:三窗中位或联合手眼 → 交付 `R_IMU_lidar` +「δt=0(主机桥接)」说明;**明确不交 t**。
3. **工程收尾**:README 主机桥接配方;需要时再整理联合标定脚本入口。
4. **平移(靠后)**:改善 IMU 位移模型或改用更可靠的位移观测 + 竖直激励后,再用新 `t` 先验跑 SE3。
---
## 7. 常用路径与命令
```text
数据根:
D:\data\calibration_usable_20260808\sessions_v1_host_aligned\
结果:
...\priority_XXXX\out_fixed_dt0\summary.json
...\priority_XXXX\out_fixed_dt0\motion_pairs.json
...\calibration_manifest_fixed_dt0.json
```
```powershell
# 可视化(rotation_only 默认模式4用 GICP 平移)
python tools\visualize_pair_3d.py `
--lidar D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\lidar `
--summary D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\out_fixed_dt0\summary.json `
--pair-index 0
# 若要看「坏 Δp」导致的错层效果:
# --mode4-translation imu
```
---
## 8. 问题优先级(跟踪用)
| ID | 严重度 | 状态 | 标题 |
|----|--------|------|------|
| P1 | 高 | 未解决 | IMU `Δp` 不可用,阻塞正式平移 |
| P2 | 高 | 未解决 | 平面运动,竖直 t 弱可观 |
| P3 | 高 | 已规避 | 自由 δt / signed refine 带偏(需保持冻结) |
| P4 | 中 | 已吸收教训 | 单窗低残差不可单独验收 |
| P5 | 中 | 待做 | 无旋转先验对照 |
| P6 | 低 | 待做 | README/交付物同步 |
-195
View File
@@ -1,195 +0,0 @@
# LiDARIMU 外参标定说明
**用途:** 方法约定与实现状态(深入阅读)。日常使用请先看根目录 [`README.md`](../README.md)。
本文说明标定目标、约定、流水线与实现状态。代码在 `imu_lidar/`。场地采集见 [`标定流程与采集清单.md`](标定流程与采集清单.md)。
---
## 1. 目标与约定
估计安装外参:
```text
p_IMU = T_IMU_lidar · p_lidar
```
约定:`T_A_B` 表示把 **B 系点**变换到 **A 系**
相对运动手眼模型:
```text
A_ij ≈ IMU 在 [t_i, t_j] 的相对运动(预积分)
B_ij ≈ LiDAR 在同时间段的相对运动(关键帧配准)
A X ≈ X B
X = T_IMU_lidar
```
旋转子问题(常规主交付):
```text
R_A R_X = R_X R_B
→ R_IMU_lidar
```
若已有完整六自由度外参,且另有 `T_RTK_lidar`,可链式得到:
```text
T_lidar_IMU = inverse(T_IMU_lidar)
T_RTK_IMU = T_RTK_lidar @ T_lidar_IMU
```
**不要**把 IMU 加速度二次积分成轨迹,再当作绝对位姿去做完整六自由度手眼。
---
## 2. 交付分层
| 层级 | 交付 | 数据最低要求 |
|---|---|---|
| 第一步 | 旋转 + 时间偏置 δt | **设备时间戳**;静止 + 低速转弯 /「8」字;结构化场景 |
| 第二步 | 上一步 + 可观的水平平移 | 更多转弯半径与加减速 |
| 第三步 | 完整六自由度(含可靠竖直分量) | 缓坡俯仰激励,或外测垂直杆臂先验 |
可观性不过关 → 只交旋转,不强交“假精确”六自由度。
仅主机接收时间、或残差未过门控的结果 → **不要当作正式安装参数**
---
## 3. 总体原则
1. **时间同步优先于外参**:δt 未对齐时,旋转与平移都不可信。使用设备时间戳。
2. **先求旋转,再求平移**:旋转通常更稳;平面运动下竖直平移常常不可观。
3. **用连续运动标定**:停车多站、再靠站间长积分,不适合作为纯 IMU 外参主流程。
4. **可观性门控**:过不了就降级交付。
5. **残差小 ≠ 标定对**:需叠点云 / 跨会话等独立验证。
6. **首轮建议低速**:先保证配准与时间对齐;点云去畸变可选。
7. **安装参数不写死在源码**:轴向与时间语义进 YAML;机械尺寸可作检查,不能伪装成已标定平移。
8. **注意耦合**:时间相关峰很弱时,δt、航向角与陀螺零偏可能互相补偿,结果不可当真。
---
## 4. 流水线(现行实现)
```text
vehicle_config
→ timestamp_audit
→ imu_audit(静止零偏等)
→ time_offset:粗估 δt
→ keyframes / [可选] deskew
→ motion_pairs:完整 IMU 预积分 + 配准 B
→ rotation_handeye:加权求解 R
→ (有候选 R 时)精修 δt,必要时交替重建运动对
→ joint_optimizer:精修旋转与陀螺零偏;可观且 full_se3 时再估平移等
→ finalize
```
| 模块 | 文件 | 职责 |
|---|---|---|
| 配置 | `vehicle_config.py` | 读安装 YAML |
| IO | `imu_io.py` / `lidar_io.py` | 标准 CSV / 帧目录 |
| 质检 | `timestamp_audit.py` / `imu_audit.py` | 时间域、静止零偏 |
| δt | `time_offset.py` | 粗估 + 有符号精修 |
| 运动 | `keyframes.py` / `registration.py` / `lidar_deskew.py` | 关键帧、配准、可选去畸变 |
| IMU 侧 | `imu_preintegration.py` / `motion_pairs.py` | 预积分与运动对 |
| 求解 | `rotation_handeye.py` / `joint_optimizer.py` / `observability.py` | 手眼、联合精修、门控 |
| 编排 | `pipeline.py` / `cli.py` / `finalize.py` | 入口与落盘 |
输入中间格式见 [V1_数据格式.md](V1_数据格式.md)。改动史见 [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md)。
### 运行示例
```powershell
python -m pip install -e ".[dev]"
python -m pip install -e ".[open3d]" # 可选
python -m imu_lidar.cli plan --mode rotation_only
python -m imu_lidar.cli run `
--vehicle-config config\vehicle_installation.template.yaml `
--imu path\to\imu.csv `
--lidar path\to\lidar_session `
--output path\to\output `
--mode rotation_only `
--time-offset-search-s 2.0
```
合成自检:`python tools\generate_synthetic_session.py` 后跑 CLI,再 `python -m pytest -q`
### 结果状态
| status | 含义 |
|---|---|
| `rotation_only_accepted` | 旋转过门,可交旋转与报告中的 δt |
| `full_se3_accepted` | 可观且联合优化通过,可交完整 `T` |
| `full_se3_rejected_due_to_observability` | 旋转可用,平移未接受 |
| `blocked` | 质检 / δt / 手眼残差等硬门失败,**不交付** |
`rotation_only` 模式下:不要把未标定的机械平移拼进 4×4 伪装成完整标定。
---
## 5. 采集要点
单趟动态会话:
```text
[静止 2030 s] → [低速激励 38 min] → [再静止 1020 s]
```
优先激励:
- 低速「8」字 / 左右圆(旋转主激励)
- 直线加减速(有助于时间对齐与水平平移)
- 缓坡(仅完整六自由度需要):约 3°~8°,连续长度优先 ≥ 20~30 m
硬条件:IMU / LiDAR **设备时间戳**;结构化场景;标定全程安装不得改动。
更完整的现场清单见 [标定流程与采集清单.md](标定流程与采集清单.md)。
---
## 6. 数学上允许与禁止
禁止:加速度二次积分当真值轨迹;把只有旋转的相对运动硬补成完整六自由度;用最终外参反向筛边掩盖失败。
允许:预积分旋转手眼求旋转;在可观时用预积分残差联合估计平移,并精修零偏等辅助量。
---
## 7. 车辆配置
使用 `--vehicle-config vehicle_installation.yaml`
模板中安装平移/旋转保持未标定状态,直至实测确认;禁止写死某车杆臂冒充结果。
---
## 8. 实现状态(当前阶段)
本仓库**仅此一条**标定路径:连续运动关键帧 + IMU 预积分。对外总览与「合成 / 旧车 / 合格数据预期」见根目录 [`README.md`](../README.md) §0。
| 项 | 状态 |
|---|---|
| 连续运动关键帧标定流水线 | 已实现(现行唯一路径) |
| 加权预积分 / 加权手眼 | 已实现 |
| 旋转预积分因子 + 有符号 δt 精修 | 已实现 |
| 完整预积分(含速度/位移增量)与可观时的平移优化 | 已实现 |
| 可观性门控 / `rotation_only` | 已实现 |
| 合成数据 pytest / 一键复现 | 已实现(证明链路与已知 yaw/δt,不证明实车精度) |
| 旧车主机时间烟测(S2) | 线下可跑;预期 `blocked`,不当交付 |
| 设备时间新车数据正式验收 | **待做** |
| 原生存储一键导出为中间格式 | 已提供 `tools/export_rscap_to_v1.py`N300 rscap + H32 dlog/MSOP → V1dlog 用 DIFOP 通道角) |
细项见 [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md)。
---
## 9. 相关文档
| 内容 | 路径 | 备注 |
|---|---|---|
| 对外总览(优先) | 根目录 [`README.md`](../README.md) | 日常入口 |
| 采集清单 | [`标定流程与采集清单.md`](标定流程与采集清单.md) | 现场 |
| 数据格式 / 导出 | [`V1_数据格式.md`](V1_数据格式.md) | 中间格式 |
| 文件职责说明 | [`imu_lidar/文件职责说明.md`](../imu_lidar/文件职责说明.md) | 改代码 |
| 改动史 | [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md) | 改代码 |
+241
View File
@@ -0,0 +1,241 @@
# RTKIMU 标定
本文是本仓库 RTK–IMU 标定的唯一规范说明,覆盖数据、方法、当前结果、限制和复现。它不使用项目内部阶段代号作为前提。
## 结论与适用范围
本次标定的正确表述是:**双天线方向和水平静止时的重力约束给出了固定旋转;天线相位中心到 IMU 的平移以机械测量为绝对基准,RTK 与 IMU 动态数据对该安装关系完成了独立一致性与稳定性验证。** 动态数据未能独立、精确地估计完整三维杆臂,但没有发现机械测量值与真实运动矛盾。
| 项目 | 当前值或状态 |
| --- | --- |
| 天线相位中心相对 IMU 的杆臂 `l_I` | `[-0.4518015159, -0.2644749820, 0.7314656115] m` |
| 固定旋转来源 | 水平静止时的双天线基线与重力约束 |
| 固定旋转近似 RPYX/Y/Z | `[0.454°, -0.003°, 0.012°]` |
| 仅由数据估计三维平移 | 未通过:`data_only_translation_accepted=false` |
| 正式工程平移验收 | 未通过:`engineering_translation_accepted=false` |
| 杆臂敏感的高动态独立验证 | 通过:未发现机械杆臂冲突 |
该候选可用于算法联调和工程验证,但不能称为“数据独立完成的完整六自由度标定”或“正式工程放行的外参”。低速、低角速度留出数据仍显示约 `0.20 m/s²` 的共同加速度传播误差;其根因未关闭前,工程放行保持为否。机器可读结论见 [工程发布决定](../artifacts/rtk_imu_calibration_v3/engineering_release_decision.json)。
## 安装与坐标定义
- 主天线为 ANT1,位于车辆前进方向左侧;从天线为 ANT2,位于右侧;两者平行安装。
- 双天线报文的 heading 是**主天线到从天线**的方向。
- IMU 的 `+Y` 指向车辆前进方向;天线基线因此指向车辆右侧。
- GGA 的参考点为 ANT1 相位中心,离地 `1.916499878 m`
- `l_I = p_ANT1^I` 表示从 IMU 原点指向 ANT1 相位中心、并在 IMU 坐标系表达的向量。
输出变换的定义为:
```text
p_RTK = R_RTK_IMU · p_IMU + t_RTK_IMU
p_IMU = R_RTK_IMUᵀ · p_RTK + l_I
t_RTK_IMU = -R_RTK_IMU · l_I
```
RTK 原点就是 ANT1,所以 `T_IMU_RTK` 的平移等于 `l_I`,两个变换必须互逆。下游实现必须按这些公式检查方向和符号。
## 原始数据与统一导出
使用三个日期的 G90 双天线 RTK 与 HI13 IMU 原始 `.rscap`。按捕获开始时间在 1.5 秒内配对,共导出 55 组会话;0815 的一组无匹配 HI13,已记录为 unmatched,不参与联合求解。
| 批次 | 配对会话 | GNHPR | BESTNAVA | PVTSLNA | Q4 双天线方向 | 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 |
旧导出以 GGA 为中心,将最近的 GNHPR 合并到同一行,且没有保留 BESTNAVA/PVTSLNA;这会把异步报文伪装成同时观测,也不能独立使用 GNSS 位置和 Doppler 速度。
新导出保留每条异步报文:
| 文件 | 内容 |
| --- | --- |
| `imu.npz` | HI13 `system_time`、三轴陀螺/加速度、RPY、四元数、磁场、PPS、温度、气压和主机接收时间 |
| `rtk.csv` | 各 GGA/GNHPR/BESTNAVA/PVTSLNA 的测量时间、质量、校验和和原始字段 |
| `export_summary.json` | 报文统计、四元数检查与跨时钟诊断 |
| `manifest.json` | 会话、批次、导出目录与未配对记录 |
HI13 `system_time` 是主时间轴。G90 的 GNSS UTC 或周/TOW 时间原样保留,并映射到此主时间轴;主机接收时间只用于时钟桥接、延迟和抖动诊断,绝不能作为采样时间。
## 求解与验证流程
```text
统一导出原始报文
→ 检查校验和、固定解、时间连续性和 IMU 预积分覆盖
→ 由双天线基线和重力固定旋转
→ 以节点状态图检查无机械先验时的杆臂可观性
→ 以机械杆臂为基准进行动态一致性验证
→ 使用未参与标定的数据和高动态转弯/坡道复核
```
### 旋转
双天线基线只提供两个方向自由度,不能单独推出完整三维姿态。因此旋转经过三类检查:
1. 连续高质量基线与 IMU 相对转动,检查基线在 IMU 中的方向。
2. 基线加高速近似直线 Doppler 速度,独立检查前向;因可用高速样本不足且与 IMU 航向存在不一致,它只作诊断。
3. 明确真实水平、车辆静止的会话中,基线确定横向,HI13 重力确定竖直,叉乘得到前向。这是当前固定旋转的正式来源。
旧式“直接把 GNHPR 三轴姿态与 IMU 做手眼”仅作诊断,不再作为正式外参。
### 平移与杆臂
每个 GNSS 节点包含姿态、位置、速度、陀螺偏置和加速度偏置。相邻节点由带协方差的 IMU 预积分连接;BESTNAVA 约束三维位置,Doppler 约束速度,GGA 只在 BEST 缺失时补充水平位置,双天线方向是可选姿态观测。GGA 的 MSL 高程绝不用于 ENU 的 Z 轴。
```text
p_ANT1^W = p_IMU^W + R_WI · l_I
v_ANT1^W = v_IMU^W + R_WI · ((ω - b_g) × l_I)
```
无机械先验求解只用来检查可观性:边缘化其他状态后,检查三维杆臂的信息矩阵、协方差、最弱方向和不同初值稳定性。本批数据不能稳定约束完整 XYZ,所以不会用其点估计替代机械值。
在同一批非重叠高动态窗口中,已比较无先验诊断解、固定机械杆臂和机械软约束解。机械值没有显著恶化位置、速度或双天线残差;但数据相对机械先验的信息增益不足,不能证明数据细化了杆臂。因此绝对值仍以机械测量为准。
## 如何使用结果
可以:固定本文旋转和机械杆臂,用于定位、融合或控制算法验证;并把“机械测量 + 动态一致性验证”写入配置或报告。
不能:把它描述为数据独立的三维平移标定、完整六自由度正式放行,或仅凭优化收敛就宣称杆臂真实。
## 结果文件与本地过程文件
`artifacts/rtk_imu_calibration_v3/` 是**本次提交的审核快照**,不是原始数据目录,也不是每次运行的工作目录。阅读顺序如下:
| 类别 | 主要文件 | 用途 |
| --- | --- | --- |
| 最终结论 | `engineering_release_decision.json` | 唯一的放行状态、候选杆臂、两个互逆变换和限制说明;下游首先读取它。 |
| 旋转证据 | `multisource_result.json` | 双天线方向、重力和速度诊断的旋转结果。 |
| 杆臂一致性 | `mechanical_prior_engineering_47_window.json` | 同一 47 个标定窗口上无先验、固定机械值和机械软约束三种结果的比较。 |
| 独立验证 | `mechanical_prior_engineering_heldout.json``heldout_independent_innovation.json` | 未参与标定的 267 个窗口的物理残差和独立传播创新。 |
| 稳定性与根因 | `mechanical_prior_rotation_sensitivity.json``propagation_bias_root_cause_audit.json` | 固定旋转扰动的影响,以及低速传播公共加速度误差的诊断。 |
| 固定选窗输入 | `lever_information_window_selection.json``lever_information_window_selection_refined.json` | 分别记录 47 个标定窗口和 314 个全部非重叠窗口;后者用于从中扣除 47 个标定窗口,得到 267 个留出窗口。 |
| 辅助审计 | `bestnava_doppler_factor_yield_*.json``hpr_dropout_bridge_*.json``motion_excitation_*.json` 等 | 记录数据保留率、双天线短缺口处理和运动激励检查;它们解释流程选择,不单独决定放行。 |
目录中还保留少量历史诊断和调试快照,文件名含 `debug``fast_diagnostic``p0` 或旧 node-graph 阶段。这些不是当前发布结论,阅读时应以 `engineering_release_decision.json` 及其 `evidence` 字段指向的文件为准。`mechanical_prior_engineering_47_window_states.npz` 是一次历史状态缓存;它不是复现的必要输入,也不应在后续运行中再次提交。
原始 `.rscap`、统一导出目录、运行中的 `.npz` 状态、checkpoint 和临时 JSON 应放在仓库外或被忽略的本地工作目录。不要把它们覆盖到 `artifacts/rtk_imu_calibration_v3/`,以免把正式审核快照和个人运行过程混在一起。
## 复现
### 复现范围
| 目标 | 是否需要原始数据 | 推荐操作 |
| --- | --- | --- |
| 核验当前发布结论 | 否 | 读取 `engineering_release_decision.json`,并按其 `evidence` 字段查看证据 JSON。 |
| 重做数据导出和固定旋转 | 是 | 按下文步骤 1–2 运行;结果应与本文的会话数、时间规则和旋转量级一致。 |
| 重做机械杆臂一致性与留出验证 | 是 | 按步骤 3–5 使用冻结选窗文件;计算量较大,所有输出放到本地工作目录。 |
| 生成新的工程结论 | 是,且需新的审核决策 | 不要复用或覆盖当前发布快照;应新建工作目录和结果目录,并重新执行完整门禁。 |
以下命令用于重建本次发布所依据的流程。原始数据路径不提交仓库;导出脚本目前的三批默认来源定义在 `tools/export_rtk_imu_unified.py``DEFAULT_SOURCES`。若本机原始数据不在这些位置,应先在该常量中仅替换本地路径,保持批次和文件配对规则不变。
### 1. 导出 55 组统一数据
```powershell
python -m pip install -e ".[dev]"
$WORK = "D:\data\rtk_imu_reproduce" # 本地工作目录,不提交
$UNIFIED = "$WORK\unified"
python tools\export_rtk_imu_unified.py --output-root $UNIFIED --overwrite
$MANIFEST = "$UNIFIED\manifest.json"
```
检查 `manifest.json`:应有 55 个配对会话,0815 的一个单独 G90 记录在 `unmatched`;每个会话应包含 `imu.npz``rtk.csv``export_summary.json`。确认 HI13 `system_time` 是主时间轴,主机接收时间没有成为观测采样时间。
### 2. 重建固定旋转
本次发布使用两个明确水平、车辆静止的会话。两个 `--level-static` 参数都必须提供:
```powershell
$ROTATION = "$WORK\rotation.json"
python tools\run_rtk_imu_multisource.py `
--manifest $MANIFEST `
--level-static 0819_20260819_072130 `
--level-static 0819_20260819_073045 `
--output $ROTATION
```
检查输出中“基线 + 重力 + 水平场地约束”的旋转是否接近 `[0.454°, -0.003°, 0.012°]`。如果差异明显,应停止后续步骤,先核对天线方向、会话是否真实水平和时间/坐标定义;不要直接求杆臂。
### 3. 选择并诊断 47 个标定窗口
本次种子运动分别是持续绕圈、左右转向和坡道。脚本会在不共享 IMU/GNSS/HPR 样本的前提下,按杆臂信息增益补充窗口;当前发布的固定结果为 47 个窗口。
```powershell
$SEL47 = "$WORK\selection_47.json"
python tools\select_rtk_imu_windows_by_lever_information.py `
--manifest $MANIFEST `
--circle-session 0808_20260808_092827 `
--left-right-session 0808_20260808_082148 `
--slope-session 0815_20260812_123424 `
--rotation-rpy-deg 0.4543066225 -0.0026392019 0.0122384129 `
--output $SEL47
$FREE47 = "$WORK\free_47.json"
python tools\run_rtk_imu_node_graph_free_selected.py `
--manifest $MANIFEST `
--selection $SEL47 `
--rotation-rpy-deg 0.4543066225 -0.0026392019 0.0122384129 `
--start-name all `
--output $FREE47
```
`$FREE47` 只用于确认无机械先验时的可观性和多初值稳定性;它不是可交付杆臂,也不应因此修改机械值。
### 4. 重做机械杆臂一致性比较
```powershell
$ENGINEERING = "$WORK\mechanical_47.json"
python tools\run_rtk_imu_mechanical_prior_branch.py `
--manifest $MANIFEST `
--selection $SEL47 `
--free-baseline $FREE47 `
--rotation-rpy-deg 0.4543066225 -0.0026392019 0.0122384129 `
--state-output "$WORK\mechanical_47_states.npz" `
--output $ENGINEERING
```
结果必须同时比较无先验、固定机械杆臂和机械软约束杆臂。`mechanical_47_states.npz` 仅为本地缓存,不提交。
### 5. 重做留出验证和发布汇总
本次留出验证使用 314 个冻结的全部非重叠窗口,其中 47 个是标定窗口、267 个是留出窗口。为了精确复现本次窗口划分,直接使用仓库中的 `lever_information_window_selection_refined.json`,不要重新选择或改变窗口。
```powershell
$ALL314 = "artifacts\rtk_imu_calibration_v3\lever_information_window_selection_refined.json"
$HELDOUT = "$WORK\heldout.json"
python tools\run_rtk_imu_mechanical_prior_heldout.py `
--manifest $MANIFEST `
--calibration-selection $SEL47 `
--all-selection $ALL314 `
--engineering-result $ENGINEERING `
--checkpoint-dir "$WORK\heldout_checkpoints" `
--rotation-rpy-deg 0.4543066225 -0.0026392019 0.0122384129 `
--output $HELDOUT
python tools\audit_rtk_imu_heldout_innovation.py `
--manifest $MANIFEST `
--calibration-selection $SEL47 `
--all-selection $ALL314 `
--engineering-result $ENGINEERING `
--rotation-rpy-deg 0.4543066225 -0.0026392019 0.0122384129 `
--output "$WORK\heldout_innovation.json"
python tools\audit_rtk_imu_propagation_bias_root_cause.py `
--manifest $MANIFEST `
--calibration-selection $SEL47 `
--all-selection $ALL314 `
--engineering-result $ENGINEERING `
--rotation-rpy-deg 0.4543066225 -0.0026392019 0.0122384129 `
--output "$WORK\propagation_bias_root_cause.json"
```
旋转敏感性、未收敛窗口重试和最终汇总属于发布级验证;它们不重新拟合杆臂。若要重新生成最终发布 JSON,必须连同上述审计结果、旋转敏感性结果和重试结果一起传给 `tools/finalize_rtk_imu_engineering_release.py`。在没有完成这些验证时,不得将本地结果标为成功。
## 代码边界与审核证据
RTK 专用代码位于 `rtk_imu/`,命令行和审计工具位于 `tools/`。它只复用 `imu_lidar/` 的通用几何、地理坐标、IMU 读取、预积分和旋转初始化模块;不依赖雷达点云、配准或雷达联合优化代码。
- [多源旋转结果](../artifacts/rtk_imu_calibration_v3/multisource_result.json)
- [47 窗口机械杆臂比较](../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/heldout_independent_innovation.json)
- [传播误差根因审计](../artifacts/rtk_imu_calibration_v3/propagation_bias_root_cause_audit.json)
-109
View File
@@ -1,109 +0,0 @@
# V1 标准中间数据格式
**用途:** 标定程序读入的 CSV/NPZ 约定,以及新车原始数据如何导出。总览见根目录 [README](../README.md)。
## 从原始数据导出
**推荐(新 H32 + HI13):** HI13 `.rscap` + 雷达 Medulla dlog / recovered zipraw MSOP + DIFOP)。
```powershell
python tools\export_rscap_to_v1.py `
--imu-rscap path\to\hi13r4-imu.rscap `
--imu-kind hi13 `
--lidar-dlog path\to\session_or_dlog_or_recovered.zip `
--host-start 2026-08-08T17:40:05 `
--host-end 2026-08-08T17:45:15 `
--out path\to\session_v1 `
--frame-stride 5 `
--require-difop
```
`--lidar-dlog` 可为:标准 `dobject/`+`dobject_recording/` 目录,或 recovered zip`indices.log` + `data.bin`)。
`--imu-kind``hi13` / `n300` / `auto`(默认按文件名推断)。
`--host-start/end`:按本地墙钟切窗(仅裁剪;标定主轴仍是设备时间)。
默认 DObject`frontlidar-msop-raw``frontlidar-difop-raw`
**兼容旧 MSOP-only `.rscap`**
```powershell
python tools\export_rscap_to_v1.py `
--imu-rscap path\to\n300.rscap `
--lidar-rscap path\to\h32_msop.rscap `
--out path\to\session_v1 `
--frame-stride 1
```
产出:`imu.csv``lidar/`(含 `frames_index.csv`)、`export_summary.json`
标定主轴仍是**设备时间**;同时写出**主机 UTC 接收时间**,用于把雷达帧桥接到 IMU 设备钟(禁止把两边设备时间第一帧强行重合)。
## IMU
文件:`imu.csv``imu.npz`
### CSV
```text
t,gx,gy,gz,ax,ay,az,t_host_utc_s,receive_utc_ticks
0.000000000,0.01,-0.02,0.00,0.05,-0.03,9.81,1754646005.123,6389...
...
```
| 列 | 含义 | 单位 |
|---|---|---|
| t | IMU 设备时钟时间 | s |
| gx,gy,gz | 角速度 | rad/s |
| ax,ay,az | 比力/加速度 | m/s² |
| t_host_utc_s | 主机 UTC 接收时间(Unix | s |
| receive_utc_ticks | 同上,.NET UTC ticks | — |
### NPZ
数组:`t (N,)`, `gyro (N,3)`, `acc (N,3)`,含义同上。
> IMU 与 LiDAR 的时间原点可以不同。流水线会估计常值偏置:`t_imu = t_lidar + delta_t`。
## LiDAR
目录结构:
```text
lidar_session/
├── frames_index.csv
└── frames/
├── frame_00000.npz
├── frame_00001.npz
└── ...
```
### frames_index.csv
```text
frame_id,filename,t_start,t_end,host_receive_utc_ticks,t_host_utc_s,host_receive_utc_end_ticks,t_host_utc_end_s
0,frames/frame_00000.npz,10.000,10.100,6389...,1754646005.12,6389...,1754646005.22
```
| 列 | 含义 |
|---|---|
| t_start / t_end | H32 MSOP **设备时间**(秒) |
| t_host_utc_s / t_host_utc_end_s | 帧首/末包 **HostReceiveUtcTicks** → Unix 秒 |
也兼容旧列名 `file`。对齐脚本用主机 UTC 把 `t_*` 重写到 IMU 设备钟后再跑标定。
### 每帧 NPZ
- `points`: `float64/float32`,形状 `(N, 3)`LiDAR 直角坐标系,单位米
## 时间不同步能不能用?
可以,前提是:
1. 两边都覆盖同一段**有角速度激励**的物理运动(尤其是转弯);
2. 偏置近似为**常数**(短会话);
3. `--time-offset-search-s` 足够覆盖可能的偏移(默认 ±1 s,可加大)。
若两段数据完全不是同一趟行驶,或只有静止 IMU、没有重叠运动,则无法估 δt,标定会被 `blocked`
## 最小可用会话
- IMU:建议含静止段 + 运动段,采样率稳定
- LiDAR:建议 ≥ 20 帧,场景有墙/柱等结构,含转弯
-157
View File
@@ -1,157 +0,0 @@
# 纯 LiDAR–IMU:标定流程与采集清单
**用途:** 现场怎么采合格数据(看完根目录 [README](../README.md) 后再看本文即可)。
算法命令与结果判读以 README 为准;改动史见 [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md)。
仅有激光雷达与 IMU、无 RTK/绝对位姿时的推荐采集与流程要点。
目标外参:`p_IMU = T_IMU_lidar · p_lidar``T_A_B` 表示把 B 系点变到 A 系)。
---
## 1. 原则(先读)
1. **时间对齐优先**:时钟偏置未对准时,旋转与平移都不可信;正式数据用**设备时间戳**。
2. **先旋转,再平移**:旋转通常更稳;平面低速时竖直方向平移常常不可观。
3. **用连续运动**:停车多站适合 RTK 手眼,不适合作为纯 IMU 外参主流程。
4. **可观才交平移**:激励不够就只交旋转,不强交“假精确”六自由度。
5. **残差小 ≠ 标定对**:需叠点云、跨会话等独立验证。
相对运动模型:
```text
A ≈ IMU 预积分相对运动(关键帧区间)
B ≈ 雷达关键帧配准相对运动
R_A · R_X ≈ R_X · R_B → 先求旋转
完整模式且可观时再求平移 t
```
---
## 2. 端到端流程(与现行代码一致)
```text
确认轴向 / 单位 / 时间语义
→ 现场采集(首尾静止 + 低速多转弯;建议 ≥2 段独立会话)
→ 导出标准中间格式(imu.csv + lidar 会话目录)
→ 质检(时间 / IMU)不通过则停
→ 粗估时间偏置 δt
→ 关键帧 → 配准得 B;完整 IMU 预积分得 A(旋转/速度/位移增量)
→ 加权旋转手眼得 R
→ 用 R 精修 δt,必要时重新组对再解 R(可交替数轮)
→ 联合精修 R 与常值陀螺零偏
→ full_se3 且可观:再估重力、关键帧速度、时变零偏与平移 t
→ 写出 T / δt / summary → 叠点云 / 跨会话验证后交付
```
| 步骤 | 现行模块 | 说明 |
| --- | -------------------------------------------------------------------- | --------------------------------------- |
| 质检 | `timestamp_audit` / `imu_audit` | 含静止段陀螺零偏初值 |
| 时间 | `time_offset` | 模长相关粗估 + 有符号三轴精修 |
| 运动对 | `keyframes` / `registration` / `imu_preintegration` / `motion_pairs` | 预积分始终算满;手眼先用旋转 |
| 旋转 | `rotation_handeye` | 加权手眼 |
| 精修 | `joint_optimizer` / `observability` | `rotation_only` 到旋转为止;`full_se3` 可观才碰平移 |
点云去畸变(`lidar_deskew`)可选;低速首轮可不依赖。
---
## 3. 采集设计
### 3.1 单趟会话结构
```text
静止 2030 s → 连续运动 38 min → 再静止 1020 s
```
运动优先:低速「8」字 / 左右圆;再补加减速直线。
要可靠竖直方向外参时,另加缓坡(约 3°~8°,有效长度优先 ≥20~30 m),或改用外测垂直尺寸先验。
### 3.2 会话安排
| 会话 | 作用 |
| ----- | ---------------------------------- |
| A | 主标定 |
| B | 独立验证(**同一场地**换一条不完全相同的路线即可,不参与求外参) |
| C(可选) | 不同速度/路线,测稳定性 |
安装全程不得改动。多会话不要跨会话拼运动对。
### 3.3 录制字段(原始,勿先做姿态融合)
- IMU:设备时间、陀螺、加速度(建议同时留主机接收时间便于排查)
- LiDAR:每帧起止时间(最好有包级/逐点时间)、原始点云
中间格式见 `[V1_数据格式.md](V1_数据格式.md)`
---
## 4. 现场 Checklist
**出发前**
- [ ] 安装固定;草图/卷尺粗测仅作参考,不当真值
- [ ] 单位与轴向确认;设备时间可写盘
- [ ] 结构化路线(墙/杆/路缘),避开空旷无特征区
- [ ] 存储与供电充足
**录制中**
- [ ] 首尾静止;中间有明显左右转与加减速
- [ ] 不改安装、不切换时间源
- [ ] 记录会话 ID、天气、异常(急刹、掉包等)
**当场快查**
- [ ] IMU 静止段平稳,转弯时角速度明显
- [ ] 点云帧数/点数正常,无明显大面积丢帧
- [ ] 雷达与 IMU 时间覆盖同一时段
**回实验室**
- [ ] 已导出中间格式并通过质检
- [ ] 本次目标:`rotation_only` 还是尝试 `full_se3`
- [ ] 若要竖直方向:确认真有俯仰/高度激励,否则降级交付
---
## 5. 精度预期
| 量 | 较现实范围 | 说明 |
| ---------- | --------- | ---------------- |
| 旋转 | 约 0.5°–2° | 最精确部分 |
| 水平平移 | 数厘米~十几厘米 | 强依赖配准、激励与同步 |
| 竖直 / 部分杠杆臂 | 往往更差甚至不可观 | 无高度激励时无法得出“精确 z” |
如果条件有限,优先交付:**可靠旋转 + δt + 可观的平移分量(若有)+ 明确限制说明**。
满足条件后的模式与成功标志见根目录 [`README.md`](../README.md) §0。
---
## 6. 交付物建议
程序默认写出:`T_IMU_lidar.json``time_offset.json``summary.json`
完整报告目录还可补充:可观性结论、跨会话对比、运动对质量表、限制说明(尤其竖直方向与时间同步方式)。
```text
静止 + 激励录制(多会话)
→ 质检 → 估 δt → 关键帧 A/B → 先解 R
→ 精修 δt 与 R → 可观则求 t → 验证后交付
```
-209
View File
@@ -1,209 +0,0 @@
# 20260808 HI13 + H32LiDARIMU 标定现状与问题
> 数据:`D:\data\calibration_usable_20260808`
> 可用会话:`sessions_v1_host_aligned`(三优先窗)
> 当前结果目录:各窗 `out_fixed_dt0/`
> 清单:`sessions_v1_host_aligned/calibration_manifest_fixed_dt0.json`
> 车辆配置:`config/vehicle_hi13_h32_20260808.yaml`
> 约定外参:`p_IMU = T_IMU_lidar · p_lidar`
---
## 1. 一句话结论
**旋转 + 主机桥接时间对齐可以冻结;平移(full_se3)尚不可正式交付。**
三窗 `rotation_only`(δt=0)结果跨窗一致,**不必因平移先验 Z 修正而重跑旋转**。
---
## 2. 当前可用结果(`out_fixed_dt0`
约定:`p_IMU = T_IMU_lidar · p_lidar`;本轮交付 **仅旋转**`t = [0,0,0]``time_offset_s = 0`
| 窗 | 状态 | δt | roll/pitch/yaw (°) | 手眼 RMS (°) | 手眼对数 | vs CAD prior |
|----|------|----|---------------------|--------------|----------|--------------|
| `priority_174005_174515` | `rotation_only_accepted` | 0 | 0.398 / +0.108 / **89.998** | 0.625 | 1374 | 0.413° |
| `priority_174905_175450` | 同上 | 0 | 0.316 / 0.352 / **90.002** | 0.293 | 1182 | 0.473° |
| `priority_175910_180530` | 同上 | 0 | 0.373 / 0.036 / **90.005** | 0.786 | 789 | 0.375° |
- 跨窗旋转互差约 **0.15°–0.47°**(相对三窗均值 ≤0.26°)。
- CAD/安装平移先验只用于后续 SE3 / 校验,不写入本轮交付 `T`
- 原始摘要:各窗 `out_fixed_dt0/summary.json`;总表 `calibration_manifest_fixed_dt0.json`
### 2.1 窗1 `priority_174005_174515` — `R_IMU_lidar`
- 路径:`...\priority_174005_174515\out_fixed_dt0\summary.json`
- rpy_deg_xyz`[-0.39806616272552936, 0.10842366761721789, 89.99788311264182]`
- quaternion_xyzw`[-0.0031254048203223084, -0.0017872288323561246, 0.7070914597978358, 0.707112936622415]`
```text
R =
[[ 3.6946588133e-05, -0.9999758655693408, -0.0069474393698490 ],
[ 0.9999982088237712, 2.3798651350e-05, 0.0018925598731371 ],
[-0.0018923488575947, -0.0069474968493909, 0.9999740753156198 ]]
t = [0, 0, 0]
```
### 2.2 窗2 `priority_174905_175450` — `R_IMU_lidar`
- 路径:`...\priority_174905_175450\out_fixed_dt0\summary.json`
- rpy_deg_xyz`[-0.31554362742542746, -0.35231680831805484, 90.00193338170823]`
- quaternion_xyzw`[0.00022698471903919405, -0.004121119694188399, 0.7071067019847445, 0.7070948146172911]`
```text
R =
[[-3.3743238552e-05, -0.9999848355714863, -0.0055070399001942 ],
[ 0.9999810938467022, 1.2097239027e-07, -0.0061491421465438 ],
[ 0.0061490495645171, -0.0055071432752239, 0.9999659297008071 ]]
t = [0, 0, 0]
```
### 2.3 窗3 `priority_175910_180530` — `R_IMU_lidar`
- 路径:`...\priority_175910_180530\out_fixed_dt0\summary.json`
- rpy_deg_xyz`[-0.37335575043737196, -0.03585856981709423, 90.00538974486676]`
- quaternion_xyzw`[-0.002082462199099642, -0.002525219418619018, 0.7071355299053291, 0.7070704554452735]`
```text
R =
[[-9.4068775206e-05, -0.9999787650354249, -0.0065161821101807 ],
[ 0.9999997997313592, -8.9988606603e-05, -0.0006264497522950 ],
[ 0.0006258500675082, -0.0065162397345547, 0.9999785732361544 ]]
t = [0, 0, 0]
```
### 相对历史失败轮次
| 轮次 | 问题 | 结果 |
|------|------|------|
| `sessions_v1_aligned` | 首帧强行对齐设备钟 | 三窗手眼失败,RMS ~9°–12° |
| 自由估 δt + signed refine | 窗3 δt 漂到 0.48 s;窗2 yaw≈19° | 跨窗 yaw 矛盾(81°/19°/93°) |
| **本轮 fixed δt=0** | 主机桥接后冻结时间 | 三窗 yaw≈90°,可互证 |
---
## 3. 已澄清并写入配置的坐标系 / 先验
### 3.1 车体与传感器
- 车体:X 前 / Y 左 / Z 上;雷达与 IMU 安装在 **X 正方向**(后轮轴前方)。
- CAD 图纸可能画成 +X 朝后,那只是读图坐标系,**不是**车体真实轴。
- IMUHI13 RFUX 右 / Y 前 / Z 上),原始数据不做轴向重映射。
- 雷达 NPZ:假定与车体一致(X 前 / Y 左 / Z 上)。
### 3.2 安装量(`translation_m`
| 传感器 | X / Y(后轮轴中心) | Z(离地) |
|--------|---------------------|-----------|
| IMU | 2.574 / 0.0365 m | 0.8925 + 0.294 = **1.1865 m** |
| 雷达 | 2.522 / 0.00002 m | 相位中心离地 **1.994999879 m** |
- 后轮轴中心离地:**294 mm**(Z 用离地高时加在 CAD 轴心高上)。
- 雷达 CAD `dZ=1.637499879 m`;相位中心离地还需加后轮轴离地 `0.294 m` 和相位中心偏移 `0.0635 m`,最终为 `1.994999879 m`
### 3.3 导出外参先验
- `R_IMU_lidar` ≈ yaw 90°:`[[0,-1,0],[1,0,0],[0,0,1]]`(软约束 σ=15°)。
- `t_IMU_lidar`**`[0.0365, -0.0518, 0.8085]` m**(相对 Z = 1.994999879 1.1865 = 0.808499879 m)。
- **旋转先验不因 Z 修正改变**;平移先验 Z 更新为 0.808499879 m。
---
## 4. 现存问题清单
### P1. IMU 预积分平移 `Δp` 不可用(阻塞正式平移)
- 现象:可视化模式 4 若用完整 `X⁻¹ A X`,橙/蓝点云常呈**上下错层**(Z 差米级~几十米)。
- 根因:加速度预积分缺少可靠重力/零偏处理,`t_A` 尤其 Z 发散;**不是旋转外参错了**。
- 旁证:相对 GICP 的旋转残差中位约 0.16°;`|t_A|` 中位却常 >1 m。
- 影响:`full_se3` / 依赖 IMU 位移的平移估计不可信。
- 缓解(已做):`visualize_pair_3d.py``rotation_only` 默认模式 4 = **R 共轭 + GICP 的 t_B**`--mode4-translation gicp|imu|auto`)。
### P2. 平面运动导致竖直平移弱可观
- 三优先窗以水平转弯为主,缺少缓坡/俯仰激励。
- 流水线门控已给出 `translation_accepted=false`
- 即使打开平移先验(σ≈5 cm),弱激励下结果易变成**先验回显**,不宜当标定成功。
### P3. 时间偏移若再自由估计会被带偏(已规避,需保持)
- 主机 UTC 桥接(MSOP/IMU `HostReceiveUtc`)后,两路已在同一时间轴,残差通常几十毫秒量级。
- 若再做有符号 δt 精修,会与错误/未收敛的 R 耦合,窗3 曾从约 −0.12 s 走到 **0.48 s**。
- **现行做法**:桥接会话使用 `--fixed-time-offset-s 0 --no-signed-time-refine`
### P4. 单窗低残差 ≠ 外参正确(历史教训)
- 自由 δt 轮次中,窗2 手眼 RMS 最低(~0.3°)但 yaw≈19°,与 CAD/其他窗差 60°+。
- 平面运动下 yaw 外参可出现多个能拟合 `R_A R_X ≈ R_X R_B` 的解。
- **必须**做跨窗一致性 + 可视化叠点,不能只看单窗 RMS。
### P5. 旋转软先验尚未做无先验对照
- 当前 σ=15°;笔记显示 Tsai 初值本身已接近(约 0.3°–1.1° RMS),不像纯先验硬拽。
- 仍缺一次:关闭先验或放大 `sigma_deg` 的对照,以排除「只是被拉到 90°」的疑虑。
### P6. 文档与操作约定未完全同步(工程)
- README 需明确写清:host-bridge 后固定 δt=0、禁用 signed refine、rotation_only 可视化用法。
- 交付物目前缺一版「冻结的联合/中位 R + 使用说明」JSON/报告(旋转可交,平移明确不交)。
---
## 5. 不该做 / 可以做
| 动作 | 建议 |
|------|------|
| 因 Z 先验修正重跑三窗 rotation_only | **不必**R 未依赖新 t |
| 正式交付 6-DOF / 信赖当前 `Δp` 估 t | **不要** |
| 试验性 `full_se3`(固定 R、δt=0、新 t 先验) | 可做,结果标「实验」 |
| 可视化验收模式 3 vs 4(gicp 平移) | **建议做** |
| 无先验 / 大 σ 旋转对照 | **建议做** |
| 冻结交付 `R` + `δt=0` 说明 | **建议做** |
| 补采缓坡或加强垂直尺寸约束后再估 t | 正式平移前需要 |
---
## 6. 建议下一步顺序
1. **验收旋转**:三窗抽转弯运动对,模式 3/4 叠点;可选无先验对照。
2. **定稿旋转**:三窗中位或联合手眼 → 交付 `R_IMU_lidar` +「δt=0(主机桥接)」说明;**明确不交 t**。
3. **工程收尾**:README 主机桥接配方;需要时再整理联合标定脚本入口。
4. **平移(靠后)**:改善 IMU 位移模型或改用更可靠的位移观测 + 竖直激励后,再用新 `t` 先验跑 SE3。
---
## 7. 常用路径与命令
```text
数据根:
D:\data\calibration_usable_20260808\sessions_v1_host_aligned\
结果:
...\priority_XXXX\out_fixed_dt0\summary.json
...\priority_XXXX\out_fixed_dt0\motion_pairs.json
...\calibration_manifest_fixed_dt0.json
```
```powershell
# 可视化(rotation_only 默认模式4用 GICP 平移)
python tools\visualize_pair_3d.py `
--lidar D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\lidar `
--summary D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\out_fixed_dt0\summary.json `
--pair-index 0
# 若要看「坏 Δp」导致的错层效果:
# --mode4-translation imu
```
---
## 8. 问题优先级(跟踪用)
| ID | 严重度 | 状态 | 标题 |
|----|--------|------|------|
| P1 | 高 | 未解决 | IMU `Δp` 不可用,阻塞正式平移 |
| P2 | 高 | 未解决 | 平面运动,竖直 t 弱可观 |
| P3 | 高 | 已规避 | 自由 δt / signed refine 带偏(需保持冻结) |
| P4 | 中 | 已吸收教训 | 单窗低残差不可单独验收 |
| P5 | 中 | 待做 | 无旋转先验对照 |
| P6 | 低 | 待做 | README/交付物同步 |
+166
View File
@@ -0,0 +1,166 @@
# 雷达–IMU 标定
本文是本仓库雷达–IMU 标定的唯一规范说明,覆盖目标、数据格式、运行方式、当前实车结果和采集要求。
## 目标与当前结论
目标是求雷达坐标系到 IMU 坐标系的安装关系:
```text
p_IMU = T_IMU_lidar · p_lidar
```
`T_A_B` 表示把 B 坐标系中的点变换到 A 坐标系。流程利用连续行驶中的相对运动,而不是把 IMU 加速度二次积分的长时间轨迹当作绝对真值。
当前 2026-08-08 实车数据的结论是:**旋转和主机桥接后的时间关系可作为候选冻结;完整平移不可交付。** 不应把旋转结果和零平移拼成看似完整的 4×4 外参,更不能把当前 IMU 位移预积分用于正式平移标定。
| 项目 | 当前状态 |
| --- | --- |
| 连续运动雷达–IMU 流程 | 已实现并有合成测试 |
| 实车旋转候选 | 三个窗口一致,可用于进一步验证 |
| 实车时间关系 | 主机桥接后冻结为 `0 s`,不再自由精修 |
| 实车完整平移 | 不可观且受预积分位移误差影响,拒绝交付 |
## 方法概览
```text
设备时间与惯性质量检查
→ 估计或确认两传感器时间关系
→ 雷达关键帧配准得到相对运动
→ 同一时间区间的 IMU 预积分得到相对运动
→ 先求旋转手眼关系
→ 只有平移可观时才进入完整六自由度优化
```
对关键帧区间 `[i, j]`
```text
A_ij ≈ IMU 预积分相对运动
B_ij ≈ 雷达点云配准相对运动
A_ij · X ≈ X · B_ij
X = T_IMU_lidar
```
旋转子问题为 `R_A · R_X = R_X · R_B`。旋转通常比平移稳定:加速度偏置和重力方向的微小误差会在位移积分中快速累积,平面转弯又难以提供竖直杆臂信息。因此,完整六自由度只有在残差、跨会话一致性和可观性同时通过时才允许输出。
## 当前实车结果与限制
当前可用的三段优先窗口均在主机时间桥接后固定 `time_offset_s=0`,并禁用自由的有符号时间精修:
| 会话窗口 | roll / pitch / yaw(°) | 手眼旋转 RMS(°) |
| --- | --- | ---: |
| `priority_174005_174515` | `-0.398 / 0.108 / 89.998` | 0.625 |
| `priority_174905_175450` | `-0.316 / -0.352 / 90.002` | 0.293 |
| `priority_175910_180530` | `-0.373 / -0.036 / 90.005` | 0.786 |
三段旋转相互差约为 `0.15°–0.47°`。但以下问题阻止平移放行:
1. IMU 预积分的位移,尤其竖直分量,会出现米级甚至更大的漂移;它不能作为可靠平移观测。
2. 当前重点会话以平面转弯为主,缺少持续坡道或俯仰激励,竖直平移弱可观。
3. 允许时间偏移自由精修时,时间与旋转曾发生相互补偿,产生跨窗口矛盾的航向。因此桥接后的偏移固定为零,而不是再次为降低残差调整它。
4. 单一窗口残差小不等于外参正确;必须检查跨窗口一致性与点云叠加。
这也是仓库增加 RTK–IMU 流程的原因:不是替代雷达旋转标定,而是为天线相位中心到 IMU 的平移提供更直接的 GNSS 位置和速度观测。两条流程边界见仓库 [README](../README.md)。
## 输入、输出与配置
### 输入
```text
imu.csv
t,gx,gy,gz,ax,ay,az
单位:秒、rad/s、m/s²;t 应为设备时间
lidar_session/
frames_index.csv 帧编号、文件名、开始/结束时间
frames/frame_XXXXX.npz 米制 XYZ 点云
vehicle_installation.yaml
坐标轴、时间语义、可选机械安装信息
```
原始 HI13 IMU `.rscap` 和 H32 雷达 dlog/MSOP 可转换为上述格式:
```powershell
python tools\export_rscap_to_v1.py `
--imu-rscap path\to\hi13-imu.rscap `
--imu-kind hi13 `
--lidar-dlog path\to\session_or_recovered.zip `
--host-start 2026-08-08T17:40:05 `
--host-end 2026-08-08T17:45:15 `
--out path\to\session_v1 `
--require-difop
```
### 输出
| 文件 | 含义 |
| --- | --- |
| `T_IMU_lidar.json` | 通过验收时的外参;仅旋转模式下不能把其中平移当交付值 |
| `time_offset.json` | 定义为 `t_imu = t_lidar + δt` 的时间关系 |
| `summary.json` | 状态、残差、可观性和拒绝原因 |
| `motion_pairs.json` | 用于复核和点云可视化的相对运动对 |
## 运行与判读
安装依赖:
```powershell
python -m pip install -e ".[dev]"
python -m pip install -e ".[open3d]" # 点云处理需要时安装
```
先运行仅旋转模式:
```powershell
python -m imu_lidar.cli run `
--vehicle-config config\vehicle_installation.template.yaml `
--imu path\to\session_v1\imu.csv `
--lidar path\to\session_v1\lidar `
--output path\to\out `
--mode rotation_only `
--time-offset-search-s 2.0
```
| `summary.json` 状态 | 含义 |
| --- | --- |
| `rotation_only_accepted` | 旋转与时间关系可进入独立验证;不代表平移可用 |
| `full_se3_accepted` | 仅当完整平移可观且门禁通过时,才可交付完整外参 |
| `full_se3_rejected_due_to_observability` | 旋转可用,平移拒绝 |
| `blocked` | 时间、质量或模型门禁失败,不能作安装参数 |
合成复现可验证软件链路,不证明实车精度:
```powershell
powershell -File tools\reproduce_synthetic.ps1
python -m pytest -q
```
## 采集要求
单次有效会话建议:
```text
静止 20–30 s → 连续低速多方向转弯 3–8 min → 静止 1020 s
```
- 必须保留雷达和 IMU 的设备时间戳;不能仅依赖主机接收时间。
- 在结构化环境采集,保证点云配准具有稳定几何约束。
- 旋转标定优先包含左右圆或“8”字;直线加减速有助于时间和水平平移。
- 若要尝试完整平移,增加连续缓坡、俯仰或其他竖直激励;仅平面运动通常不足。
- 标定全过程不得改变两传感器安装关系。
## 代码结构
`imu_lidar/` 是雷达–IMU 专用实现:
| 组件 | 作用 |
| --- | --- |
| `cli.py``pipeline.py` | 命令行入口与流程编排 |
| `lidar_io.py``registration.py``keyframes.py` | 点云读取、配准和关键帧 |
| `timestamp_audit.py``imu_audit.py``time_offset.py` | 时间和 IMU 质量检查、时间关系估计 |
| `imu_preintegration.py``motion_pairs.py` | IMU 预积分与相对运动对 |
| `rotation_handeye.py``joint_optimizer.py``observability.py` | 旋转求解、联合优化和可观性门禁 |
| `finalize.py` | 结果写出 |
历史改动见 [imu_lidar/CHANGELOG.md](../imu_lidar/CHANGELOG.md),自动化测试说明见 [tests/README.md](../tests/README.md)。
+4 -4
View File
@@ -103,7 +103,7 @@
- **原本**:对外说明仍偶发「方案二」等旧称呼;根 README 缺少一眼可读的阶段 / 合成 vs 旧车 / 合格数据预期;烟测配置与对比脚本文件名带 `scheme2`
- **改成**
-`[README.md](../README.md)` 增加 §0「现状一览」;明确仓库只有一条连续运动标定路径。
- `[tests/README.md](../tests/README.md)``[docs/IMU-LiDAR标定.md](../docs/IMU-LiDAR标定.md)`、本目录说明同步边界与阶段。
- `[tests/README.md](../tests/README.md)``[docs/雷达-IMU标定.md](../docs/雷达-IMU标定.md)`、本目录说明同步边界与阶段。
- `config/s2_old_smoke.yaml``tools/compare_s2_runs.py` 替换旧 `*scheme2*` 命名。
---
@@ -131,7 +131,7 @@
### 文档同步 + 合成数据一键复现
- **原本**`docs/标定流程与采集清单.md` 仍偏旧版「待写代码 / 因子图设想」;根 README 缺少清晰的一键复现入口与输入输出总表。
- **原本**采集与流程说明仍偏旧版「待写代码 / 因子图设想」;根 README 缺少清晰的一键复现入口与输入输出总表。
- **改成**
- 采集清单与现行流水线对齐(完整预积分、δt↔R 交替、可观时再估平移)。
- 新增 `tools/reproduce_synthetic.py` / `.ps1``tools/show_calibration_report.py`;合成生成写入 `meta.json`;根 README 增加「系统输入输出 + 一键复现」。
@@ -148,8 +148,8 @@
- **原本**:根 README / `docs` / 包说明仍对照已删除的静站路径与内部阶段黑话;`pyproject` 仍声明已删除的 `static_station` 包。
- **改成**
- 删除旧静站文档;采集清单定为 `[docs/标定流程与采集清单.md](../docs/标定流程与采集清单.md)`
-`[README.md](../README.md)``[docs/IMU-LiDAR标定.md](../docs/IMU-LiDAR标定.md)`、本目录说明改为对外可读,只保留连续运动标定路径。
- 删除旧静站文档;采集清单并入规范说明
-`[README.md](../README.md)``[docs/雷达-IMU标定.md](../docs/雷达-IMU标定.md)`、本目录说明改为对外可读,只保留连续运动标定路径。
- `pyproject.toml` 仅保留 `imu_lidar` / `tools`
---
+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)
+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])
+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)))
-82
View File
@@ -1,82 +0,0 @@
# `imu_lidar` 模块说明
**用途:**`imu_lidar/` 源码时查阅。对外用法见根目录 [`README.md`](../README.md)。
本包实现 LiDAR–IMU 外参标定:在连续行驶数据上选取关键帧,用 IMU 预积分与雷达配准构造相对运动对,求解安装外参。
```text
A ≈ 关键帧间 IMU 相对运动(预积分:旋转 / 速度增量 / 位移增量)
B ≈ 关键帧间雷达配准
解 R_A R_X = R_X R_B → 旋转外参(手眼阶段只用旋转)
再精修旋转与陀螺零偏;在完整六自由度模式下,可观时再估计平移等
```
入口:
```powershell
python -m imu_lidar.cli plan
python -m imu_lidar.cli run --vehicle-config ... --imu ... --lidar ... --output ...
```
整体流程由 `pipeline.py` 串联。
修改本目录代码时,请同步更新本说明,并在 [`CHANGELOG.md`](CHANGELOG.md) 追加「时间戳 + 原本 → 改成」。
---
## 流水线顺序与文件
| 顺序 | 文件 | 作用 |
| --- | ----------------------- | ----------------------------- |
| 0 | `contracts.py` | 公共数据类型与状态枚举 |
| 0 | `geometry.py` | 刚体变换与旋转工具 |
| 0 | `vehicle_config.py` | 读取并校验车辆 YAML |
| 1 | `imu_io.py` | 读标准 IMU 中间格式 |
| 1 | `lidar_io.py` | 读标准雷达会话目录 |
| 2 | `timestamp_audit.py` | 时间单调 / 频率 / 空洞检查 |
| 3 | `imu_audit.py` | 静止零偏、加速度模长检查、建议竖直轴 |
| 4 | `time_offset.py` | 粗估时间偏置 δt,并用旋转外参精修 |
| 5 | `registration.py` | 帧间点云配准 |
| 5 | `keyframes.py` | 按运动量抽取关键帧 |
| 5 | `lidar_deskew.py` | 可选点云去畸变(低速可关) |
| 6 | `imu_preintegration.py` | IMU 预积分(旋转及速度/位移增量、协方差、零偏雅可比) |
| 6 | `motion_pairs.py` | 构造运动对;手眼使用其中的旋转 |
| 6 | `motion_pairs_io.py` | 运动对 JSON 缓存读写(供可视化直读) |
| 7 | `rotation_handeye.py` | 加权旋转手眼 |
| 8 | `observability.py` | 旋转 / 平移可观性检查 |
| 8 | `joint_optimizer.py` | 联合精修;完整模式下可估计平移、重力、速度与时变零偏 |
| 9 | `finalize.py` | 写出结果 JSON(含 `motion_pairs.json` |
| — | `pipeline.py` | 编排全流程 |
| — | `cli.py` | 命令行入口 |
| — | `CHANGELOG.md` | 改动记录 |
---
## 运行模式要点
- **运动对**始终计算完整预积分量(旋转、速度增量、位移增量及不确定度)。
- `--mode rotation_only`:只精修旋转与常值陀螺零偏,交付旋转与时间偏置。
- `--mode full_se3`:当前完成 Phase-A 后明确拒绝平移;待 Phase-B/C 会话状态重构完成后再恢复完整 SE(3) 交付。
---
## 输入格式
```text
imu.csv # t,gx,gy,gz,ax,ay,az(建议设备时间)
lidar_session/
frames_index.csv # frame_id,filename,t_start,t_end
frames/frame_XXXXX.npz # points: (N,3) 米
```
原始 N300 `.rscap` + H32 dlog(或旧 MSOP `.rscap`)用仓库工具导出:`python tools/export_rscap_to_v1.py ...`(见 [`docs/V1_数据格式.md`](../docs/V1_数据格式.md))。
---
## 当前能力
- 本包是仓库**唯一**标定路径:质检 → 时间偏置 → 关键帧配对 → 旋转手眼 → 联合精修 →(可选)完整六自由度 → 报告
- 点云去畸变:可选
- 阶段与用法见根目录 [`README.md`](../README.md)
- 改动史:[`CHANGELOG.md`](CHANGELOG.md)
+2 -2
View File
@@ -1,7 +1,7 @@
[project]
name = "lidar-imu-calibration"
version = "0.3.0"
description = "LiDARIMU 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"]
+1
View File
@@ -0,0 +1 @@
"""RTK-IMU extrinsic calibration algorithms and data interfaces."""
+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()
File diff suppressed because it is too large Load Diff
+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)
+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
+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
+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),
)
+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),
)
+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])
+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
+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))
+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():
+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)
+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.)
+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
+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())
+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())
+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())
+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())
+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())
+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())
+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())
@@ -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())
+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())
+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())
@@ -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())
@@ -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())
+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())
+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
+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",
]
+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())
+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())
+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())
@@ -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())
@@ -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())
@@ -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())
+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())
@@ -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())
@@ -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())
@@ -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())
@@ -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())
+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())
@@ -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())
+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)),
)
+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())