Compare commits
4
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 lichun.qu
|
0ec0f40406 | ||
|
lichun.qu
|
32d8af8ed9 | ||
|
lichun.qu
|
d14ae74117 | ||
|
lichun.qu
|
c2da6dd192 |
+19
@@ -8,3 +8,22 @@ build/
|
|||||||
examples/synthetic_session/
|
examples/synthetic_session/
|
||||||
.venv/
|
.venv/
|
||||||
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
|
||||||
|
|||||||
@@ -0,0 +1,177 @@
|
|||||||
|
# RTK-IMU 标定：流程、结果与复现
|
||||||
|
|
||||||
|
本文是本次 G90 双天线 RTK 与 HI13 IMU 标定的工程入口。算法代码在 `rtk_imu/`，命令入口在 `tools/`，正式审核结果在 `artifacts/rtk_imu_calibration_v3/`。原始 `.rscap`、统一导出数据、状态快照与 checkpoint 均只保留本地，不提交仓库。
|
||||||
|
|
||||||
|
## 一句话结论
|
||||||
|
|
||||||
|
RTK-IMU 旋转外参由双天线基线方向与水平静止重力约束获得；平移外参以机械测量为绝对基准，再通过 RTK+IMU 联合状态图、高动态转弯/坡道数据、独立 held-out 数据和旋转扰动测试进行一致性验证。
|
||||||
|
|
||||||
|
当前动态数据不足以独立高精度求出完整 XYZ 杆臂，因此它不是 data-only translation calibration。机械测量给出了杆臂的绝对值；在未参与标定的数据、转弯等杆臂敏感运动以及旋转外参扰动测试中，均未发现该机械外参存在明显矛盾或不稳定性。当前结果的正确表述是：**机械杆臂锚定，并经动态数据一致性验证的工程候选**。
|
||||||
|
|
||||||
|
## 当前工程候选与状态
|
||||||
|
|
||||||
|
坐标约定：`l_I = p_ANT1^I`，即 ANT1（主天线、左侧）相位中心在 IMU 坐标系中的位置。车体/RTK 安装坐标为：+X 为 ANT1（左）到 ANT2（右），+Y 为车辆前进方向且与 IMU +Y 同向，+Z 向上；GGA 参考点为 ANT1 相位中心，离地 `1.916499878 m`。
|
||||||
|
|
||||||
|
- 固定旋转来源：`R2G_gravity_level_prior`。
|
||||||
|
- 固定旋转近似 RPY（X/Y/Z）：`[0.454°, -0.003°, 0.012°]`。
|
||||||
|
- 当前工程候选杆臂：`l_I = [-0.4518015159, -0.2644749820, 0.7314656115] m`。
|
||||||
|
- 变换约定：`p_RTK = R_RTK_IMU * p_IMU + t_RTK_IMU`。
|
||||||
|
- 对应候选 `T_RTK_IMU`（四舍五入到 6 位小数）：
|
||||||
|
|
||||||
|
```text
|
||||||
|
[[ 1.000000, -0.000214, -0.000044, 0.451777],
|
||||||
|
[ 0.000214, 0.999969, -0.007929, 0.270363],
|
||||||
|
[ 0.000046, 0.007929, 0.999969, -0.729325],
|
||||||
|
[ 0.000000, 0.000000, 0.000000, 1.000000]]
|
||||||
|
```
|
||||||
|
|
||||||
|
当前门禁状态必须同时保留：
|
||||||
|
|
||||||
|
- `data_only_translation_accepted=false`：数据本身没有提供足够稳定的完整平移可观性。
|
||||||
|
- `engineering_translation_accepted=false`：独立传播验证仍存在公共加速度偏差，不能声称正式工程放行。
|
||||||
|
- `independent_extrinsic_sensitive_validation_passed=true`：杆臂敏感的高动态验证未发现机械杆臂冲突。
|
||||||
|
- `heldout_physical_validation_passed=true`、`rotation_sensitivity_passed=true`：固定候选在 held-out 物理残差和旋转扰动中保持一致。
|
||||||
|
|
||||||
|
因此不得描述为“data-only 标定平移”或“动态数据已精确细化机械杆臂”。完整、机器可读的结论见 [engineering_release_decision.json](artifacts/rtk_imu_calibration_v3/engineering_release_decision.json)。
|
||||||
|
|
||||||
|
## 求解流程
|
||||||
|
|
||||||
|
1. **统一原始数据导出。** 使用 G90/HI13 设备时间作为主时间轴；host receive time 仅用于诊断。G90 保留 GGA 质量、GNHPR 基线及质量、BESTNAVA Doppler velocity，必要时 PVTSLNA；HI13 保留 system time、gyro、accel、姿态/四元数和 host receive time。
|
||||||
|
2. **R0 连续性与质量控制。** 校验 checksum、RTK Fixed、设备时间单调性、IMU 覆盖、baseline jump 与测量间隔。轨迹连续性由 IMU 设备时间和预积分覆盖决定；孤立 HPR 缺失/Q5 只禁用或降权 HPR factor，不切断 IMU 轨迹。
|
||||||
|
3. **旋转外参。** R1b 从基线与 IMU 动态估计 ANT1→ANT2 在 IMU 中的 2DoF 方向；R2V 用基线与高质量 Doppler 速度作独立诊断；R2G 使用水平静止场地中的基线＋重力＋地面水平先验补齐完整旋转。R2G 是本次正式固定旋转来源，旧 GNHPR 三轴手眼只作诊断。
|
||||||
|
4. **节点状态图。** 每个 GNSS node 包含 `R,p,v,bg,ba` 的 15DoF 状态；相邻 node 由 covariance-whitened IMU preintegration 和 bias random walk 连接。BEST 约束 XYZ，GGA 仅在 BEST 缺失时约束 XY，Doppler 约束速度，HPR 是可选姿态/基线因子。
|
||||||
|
5. **机械锚定的平移验证。** 杆臂以机械值为基准，分别比较无先验 free、固定机械杆臂和软先验解。free 解只用于观测性诊断，不能因为数值收敛就替代机械值。
|
||||||
|
6. **独立验证。** 使用 circle、left-right、slope 的高动态非重叠窗口；再使用剩余 held-out 窗口、旋转 ±扰动敏感性、独立 innovation 和 propagation-bias root-cause audit 复核。
|
||||||
|
|
||||||
|
## 证据与限制
|
||||||
|
|
||||||
|
47 个非重叠标定窗口上的 free/fixed/prior 对比表明机械先验与数据拟合相容，但 posterior/prior 方差比没有显示足够的数据驱动细化，因此 `translation_refined_by_data=false`。剩余 267 个 frozen held-out 窗口的物理验证通过；然而独立传播创新在低速、低角速度区间同时出现位置和 Doppler 的同向偏差，等效为约 `0.20 m/s²` 的公共传播加速度误差。
|
||||||
|
|
||||||
|
该误差在 `|omega|` 很小时不能优先归因于杆臂速度项 `R*(omega × l)`，因此它不单独否决高动态杆臂敏感验证；但在传播模型根因关闭前，也不能把候选杆臂标为正式工程已放行。
|
||||||
|
|
||||||
|
主要审核证据：
|
||||||
|
|
||||||
|
- [47 窗口机械分支](artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_47_window.json)
|
||||||
|
- [held-out 验证](artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_heldout.json)
|
||||||
|
- [独立创新审计](artifacts/rtk_imu_calibration_v3/heldout_independent_innovation.json)
|
||||||
|
- [传播偏差根因审计](artifacts/rtk_imu_calibration_v3/propagation_bias_root_cause_audit.json)
|
||||||
|
- [旋转敏感性](artifacts/rtk_imu_calibration_v3/mechanical_prior_rotation_sensitivity.json)
|
||||||
|
|
||||||
|
## 如何复现
|
||||||
|
|
||||||
|
### 1. 准备环境和原始数据
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
cd <repository-root>
|
||||||
|
python -m pip install -e ".[dev]"
|
||||||
|
```
|
||||||
|
|
||||||
|
将三批 G90/HI13 原始 `.rscap` 会话放到本机的数据位置。数据位置不写入仓库；`tools/export_rtk_imu_unified.py` 中的会话配对清单必须与实际采集文件一致。
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
$OUT = "artifacts\rtk_imu_calibration_v3\reproduce"
|
||||||
|
python tools\export_rtk_imu_unified.py --output-root "$OUT\unified" --overwrite
|
||||||
|
$MANIFEST = "$OUT\unified\manifest.json"
|
||||||
|
```
|
||||||
|
|
||||||
|
导出后应检查每个会话目录中的 `export_summary.json`，并确认 `manifest.json` 中记录的传感器时间轴没有被 host receive time 替换。
|
||||||
|
|
||||||
|
### 2. 复现固定旋转
|
||||||
|
|
||||||
|
`<flat-static-session-id>` 必须是已确认地面水平、车辆静止的会话；可多次传入 `--level-static`。R2V 只作独立诊断，不替代 R2G。
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\run_rtk_imu_multisource.py `
|
||||||
|
--manifest $MANIFEST `
|
||||||
|
--level-static <flat-static-session-id> `
|
||||||
|
--output "$OUT\r2g_multisource.json"
|
||||||
|
```
|
||||||
|
|
||||||
|
检查输出的 R2G 旋转与本 README 的近似 RPY 一致后，将该旋转固定为后续 node graph 的 `--rotation-rpy-deg 0.454 -0.003 0.012`。若 R2G 不一致，应停止，先复核天线方向、场地水平和时间/轴定义，不应继续求杆臂。
|
||||||
|
|
||||||
|
### 3. 复现窗口选择与无先验诊断
|
||||||
|
|
||||||
|
从 circle、left-right、slope 三类会话中选择高质量窗口；窗口不可共享 IMU/GNSS/HPR 样本。仓库提交的 `lever_information_window_selection*.json` 是本次审核所用选择结果，可用于对照。
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\select_rtk_imu_windows_by_lever_information.py `
|
||||||
|
--manifest $MANIFEST `
|
||||||
|
--circle-session <circle-session-id> `
|
||||||
|
--left-right-session <left-right-session-id> `
|
||||||
|
--slope-session <slope-session-id> `
|
||||||
|
--output "$OUT\selection.json" `
|
||||||
|
--rotation-rpy-deg 0.454 -0.003 0.012
|
||||||
|
|
||||||
|
python tools\run_rtk_imu_node_graph_free_selected.py `
|
||||||
|
--manifest $MANIFEST `
|
||||||
|
--selection "$OUT\selection.json" `
|
||||||
|
--output "$OUT\free_baseline.json" `
|
||||||
|
--start-name all `
|
||||||
|
--rotation-rpy-deg 0.454 -0.003 0.012
|
||||||
|
```
|
||||||
|
|
||||||
|
free solve 的作用是输出边缘化杆臂信息、协方差、最弱方向和多初值稳定性；本次数据若仍未达到完整 XYZ 可观性，不得扩大无先验求解规模来强行放行。
|
||||||
|
|
||||||
|
### 4. 复现机械杆臂分支和 held-out 验证
|
||||||
|
|
||||||
|
固定工程候选杆臂并使用同一批非重叠标定窗口。`states.npz` 和 checkpoint-dir 是本地过程产物，应保持被 `.gitignore` 排除。
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\run_rtk_imu_mechanical_prior_branch.py `
|
||||||
|
--manifest $MANIFEST `
|
||||||
|
--selection "$OUT\selection.json" `
|
||||||
|
--free-baseline "$OUT\free_baseline.json" `
|
||||||
|
--output "$OUT\mechanical_47_window.json" `
|
||||||
|
--state-output "$OUT\states.npz" `
|
||||||
|
--rotation-rpy-deg 0.454 -0.003 0.012
|
||||||
|
|
||||||
|
python tools\run_rtk_imu_mechanical_prior_heldout.py `
|
||||||
|
--manifest $MANIFEST `
|
||||||
|
--calibration-selection "$OUT\selection.json" `
|
||||||
|
--all-selection <all-nonoverlapping-selection.json> `
|
||||||
|
--engineering-result "$OUT\mechanical_47_window.json" `
|
||||||
|
--output "$OUT\heldout.json" `
|
||||||
|
--checkpoint-dir "$OUT\heldout_checkpoints" `
|
||||||
|
--rotation-rpy-deg 0.454 -0.003 0.012
|
||||||
|
```
|
||||||
|
|
||||||
|
随后运行独立 innovation、传播根因审计和旋转敏感性。它们不重新优化杆臂，不应被用于调 covariance、R2G 或机械先验。
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\audit_rtk_imu_heldout_innovation.py `
|
||||||
|
--manifest $MANIFEST `
|
||||||
|
--calibration-selection "$OUT\selection.json" `
|
||||||
|
--all-selection <all-nonoverlapping-selection.json> `
|
||||||
|
--engineering-result "$OUT\mechanical_47_window.json" `
|
||||||
|
--output "$OUT\heldout_innovation.json" `
|
||||||
|
--rotation-rpy-deg 0.454 -0.003 0.012
|
||||||
|
|
||||||
|
python tools\audit_rtk_imu_propagation_bias_root_cause.py `
|
||||||
|
--manifest $MANIFEST `
|
||||||
|
--calibration-selection "$OUT\selection.json" `
|
||||||
|
--all-selection <all-nonoverlapping-selection.json> `
|
||||||
|
--engineering-result "$OUT\mechanical_47_window.json" `
|
||||||
|
--output "$OUT\propagation_bias_root_cause.json" `
|
||||||
|
--rotation-rpy-deg 0.454 -0.003 0.012
|
||||||
|
```
|
||||||
|
|
||||||
|
最终仅汇总已生成的结果，不在 release 阶段重新拟合：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\finalize_rtk_imu_engineering_release.py `
|
||||||
|
--calibration "$OUT\mechanical_47_window.json" `
|
||||||
|
--heldout-postfit "$OUT\heldout.json" `
|
||||||
|
--innovation "$OUT\heldout_innovation.json" `
|
||||||
|
--sensitivity <rotation_sensitivity.json> `
|
||||||
|
--convergence-retry <heldout_retry.json> `
|
||||||
|
--propagation-root-cause "$OUT\propagation_bias_root_cause.json" `
|
||||||
|
--output "$OUT\engineering_release_decision.json"
|
||||||
|
```
|
||||||
|
|
||||||
|
## 相关文件
|
||||||
|
|
||||||
|
- [RTK-IMU 代码包](rtk_imu/README.md)
|
||||||
|
- [多源导出与旋转 V3 说明](docs/rtk_imu_multisource_v3.md)
|
||||||
|
- [engineering 6DoF 说明](docs/rtk_imu_engineering_6dof.md)
|
||||||
|
- [历史链路审计](docs/rtk_imu_calibration.md)
|
||||||
|
- [正式结果索引](artifacts/rtk_imu_calibration_v3/README.md)
|
||||||
@@ -0,0 +1,17 @@
|
|||||||
|
# RTK–IMU 标定产物说明
|
||||||
|
|
||||||
|
- `all_sessions/`：8 会话、5 s 平移节点、带 conditional rotation LOO 和 translation LOO 的当前完整基线。
|
||||||
|
- `rotation_hpr_time/`：改用 GNHPR 自带测量时刻后的全量 rotation-only 对照。
|
||||||
|
- `rotation_smoke/`：较早的 GGA 最近邻姿态时刻对照，不作为当前结果。
|
||||||
|
- `batch_0808_full_smoke/`：0808 三会话完整诊断。
|
||||||
|
- `batch_0815_rotation/`：0815 四会话 rotation-only 诊断。
|
||||||
|
- `single_smoke/`：早期单会话性能/数值冒烟，不作为当前结果。
|
||||||
|
|
||||||
|
每个正式运行目录包含：
|
||||||
|
|
||||||
|
- `dataset_audit.json`：样本数、固定解比例、共同时间范围和 ENU 原点。
|
||||||
|
- `rotation_result.json`：旋转、RPY、时间审计、GNHPR 候选、偏置、残差、协方差、逐会话指标和 LOO。
|
||||||
|
- `translation_result.json`：杆臂、平移、齐次矩阵、协方差/秩、位置/速度残差、偏置和 LOO。
|
||||||
|
- `summary.json`：供程序读取的最终状态和候选矩阵。
|
||||||
|
|
||||||
|
当前 `all_sessions/summary.json` 是 `diagnostic_not_accepted`。其中平移约 `[0.771, 0.569, -21.073] m` 明显不具机械真实性，禁止用于车辆配置。完整解释见 `docs/rtk_imu_calibration.md`。
|
||||||
@@ -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 @@
|
|||||||
|
# RTK-IMU V3 正式结果集
|
||||||
|
|
||||||
|
本目录只提交复现工程结论所需的审核结果。运行过程中的调试输出、状态快照、逐窗口 checkpoint 和旧版结果保留在本机，由仓库根目录 `.gitignore` 排除。
|
||||||
|
|
||||||
|
## 当前结论
|
||||||
|
|
||||||
|
- 固定旋转来源：`R2G_gravity_level_prior`。
|
||||||
|
- 工程候选杆臂：`l_I = p_ANT1^I = [-0.4518015159, -0.2644749820, 0.7314656115] m`。
|
||||||
|
- `data_only_translation_accepted=false`。
|
||||||
|
- `engineering_translation_accepted=false`，原因是独立传播验证仍受公共加速度偏差影响。
|
||||||
|
- `independent_extrinsic_sensitive_validation_passed=true`；当前结果可描述为机械杆臂经动态数据一致性验证的工程候选，不得描述为 data-only 平移标定结果。
|
||||||
|
|
||||||
|
## 文件说明
|
||||||
|
|
||||||
|
- `engineering_release_decision.json`：最终状态、变换和放行判定。
|
||||||
|
- `mechanical_prior_engineering_47_window.json`：相同 47 个非重叠窗口上的 free/fixed/prior 对比。
|
||||||
|
- `node_graph_free_information_selected_mechanical.json`：冻结的无先验 free-solve 基线。
|
||||||
|
- `lever_information_window_selection*.json`：窗口选择及实际边缘信息复核。
|
||||||
|
- `mechanical_prior_engineering_heldout.json`：未参与标定窗口的 held-out 验证。
|
||||||
|
- `heldout_independent_innovation.json`、`heldout_nonconverged_retry.json`：独立创新与失败窗口重试结果。
|
||||||
|
- `mechanical_prior_rotation_sensitivity.json`：固定候选杆臂的旋转扰动敏感性结果。
|
||||||
|
- `propagation_bias_root_cause_audit.json`：独立传播公共加速度误差根因审计。
|
||||||
|
|
||||||
|
结果 JSON 是审核快照；需要重新生成时应通过 `tools/` 中相应入口运行，且不得提交运行产生的 checkpoint 或 `.npz` 状态文件。
|
||||||
@@ -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
+1302
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,52 @@
|
|||||||
|
# RTK–IMU 候选数据清点（2026-08-20）
|
||||||
|
|
||||||
|
本目录记录 0808、0815、0819 三批 LiDAR/IMU 会话所对应的 G90 RTK 原始记录与当前导出状态。此处只做数据血缘和可用性评估，尚未求解 `T_RTK_IMU`。
|
||||||
|
|
||||||
|
## 时间与质量约定
|
||||||
|
|
||||||
|
- 原始 RTK 为 Wheeltec G90 V2 `.rscap`，包含 `$GNGGA/$GPGGA` 位置和 `$GNHPR` heading/pitch/roll。
|
||||||
|
- 切窗使用 NMEA 报文自带的测量 UTC，再通过每个会话的 IMU device→host affine clock 映射到 IMU 设备时间。
|
||||||
|
- 主机接收时间比 NMEA 测量时间晚约 3–4 s，且有波动；不能按接收时间直接切窗。
|
||||||
|
- 当前项目门控按 GGA `fix_quality=4` 和 HPR `heading_quality∈{4,5}` 判断固定位置/有效航向。
|
||||||
|
- `heading_valid` 未覆盖全部 GGA 行主要因为 GGA 与 HPR 频率不同、最近邻匹配阈值为 80 ms；不代表该会话航向整体失效。
|
||||||
|
|
||||||
|
## 数据映射与质量
|
||||||
|
|
||||||
|
详细机器可读清单见 `rtk_session_inventory.csv`。
|
||||||
|
|
||||||
|
| 会话 | RTK结论 | 适合的标定作用 |
|
||||||
|
| --- | --- | --- |
|
||||||
|
| `priority_174005_174515` | 100% GGA质量4；有效航向约309 s；yaw变化约485° | 多圈 yaw 与 XY 杠杆臂 |
|
||||||
|
| `priority_174905_175450` | 100% GGA质量4；Pitch跨度约11.4°；XY约58×22 m | 强候选：yaw、pitch、XY/Z耦合解除 |
|
||||||
|
| `priority_175910_180530` | 100% GGA质量4；Pitch跨度约13.6°；XY约144×59 m | 最强候选：长基线、pitch、平移 |
|
||||||
|
| `slope_190548_190730` | 100% GGA质量4；Pitch跨度约7.4°；约102 s | 坡度/Pitch补充 |
|
||||||
|
| `circle_193412_193642` | 100% GGA质量4；yaw变化约445° | 平面旋转与XY杠杆臂 |
|
||||||
|
| `loop_194223_195003` | 仅1个异常GGA；yaw累计变化约1203°；约460 s | 最强 yaw/多圈转弯候选 |
|
||||||
|
| `accel_195608_195958` | 100% GGA质量4；XY约46×23 m；约230 s | 加减速、速度和水平杠杆臂 |
|
||||||
|
| `motion_sms_154023_154359` | 全窗仅约49%为质量4；可用连续子段约100 s | 仅用15:41:28.2–15:43:08.15固定解/有效航向段 |
|
||||||
|
|
||||||
|
## 导出状态
|
||||||
|
|
||||||
|
- 0808 当前 `sessions_v2_device_affine` 原先没有 RTK CSV，本次已从原始 G90 `.rscap` 按 NMEA 测量 UTC补导三个会话，未覆盖旧文件。
|
||||||
|
- 0815 `sessions_v2_device_affine` 与 dense5 slope 已经采用同一测量时间导出规则，无需重导。
|
||||||
|
- 0819 `sessions_v2_device_affine` 与 dense5 motion 已经采用同一规则；需要在求解器中按质量与时间连续段过滤，而不是重新解释为全窗固定解。
|
||||||
|
- 0808 旧 `sessions_v1_host_aligned_00` RTK CSV 不含 `t_measurement_utc_s` 字段，只保留作历史对照；新求解应使用 `sessions_v2_device_affine`。
|
||||||
|
|
||||||
|
## 已发现的 LiDAR–RTK 资料边界
|
||||||
|
|
||||||
|
`D:\data\calibration_usable_20260808\rtk_lidar_station_report*` 保存的是静止站点候选：27个站点、29个候选段，并非包含 `T_RTK_lidar`、协方差和留一验证的正式手眼结果。本轮在 0808 数据目录的 JSON/YAML/Markdown/CSV/日志中没有找到 `T_RTK_lidar` 矩阵。若要通过链式关系得到 LiDAR–IMU，需要继续定位原手眼结果及其坐标约定：
|
||||||
|
|
||||||
|
```text
|
||||||
|
T_IMU_lidar = inverse(T_RTK_IMU) @ T_RTK_lidar
|
||||||
|
```
|
||||||
|
|
||||||
|
## 初步可行性判断
|
||||||
|
|
||||||
|
这些数据足以启动直接 RTK–IMU 标定，且比当前纯 LiDAR–IMU Phase-B 更有希望约束 XY：RTK 提供绝对位置，GNHPR 提供航向和 Pitch，多会话包含长基线、转弯、加减速与坡度。仍需注意：
|
||||||
|
|
||||||
|
1. GNHPR roll 的变化仅约0.006°–0.065°，不能指望它提供有效 roll 激励。
|
||||||
|
2. 应先用 RTK heading/pitch 角速度与 IMU gyro 做残余时间偏置和坐标轴验证，再求旋转。
|
||||||
|
3. 平移应使用 RTK绝对位置 + IMU预积分的联合状态模型，估计共享 `T_RTK_IMU`、每会话速度/bias；不应把RTK轨迹简单二次差分后直接最小二乘。
|
||||||
|
4. `motion_sms` 必须仅使用其连续固定解子段。
|
||||||
|
5. 跨0808/0815/0819时应使用每会话IMU bias，外参共享，并检查安装期间是否发生机械变动。
|
||||||
|
|
||||||
@@ -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
|
||||||
|
@@ -0,0 +1,168 @@
|
|||||||
|
# RTK–IMU 标定链路审计与当前方案
|
||||||
|
|
||||||
|
> 2026-08-21 多源原始报文重构、R1b/R2V/R2G/R3 实现与三批数据实测结果，
|
||||||
|
> 见 [rtk_imu_multisource_v3.md](rtk_imu_multisource_v3.md)。本页保留旧式
|
||||||
|
> GGA/GNHPR 链路的审计背景；完整 HPR 手眼仍只允许作为诊断。
|
||||||
|
|
||||||
|
## 结论
|
||||||
|
|
||||||
|
当前数据可以验证“双天线基线方向与 IMU 的一致性”，但不能单独标定一个无歧义的完整三自由度 RTK 姿态外参。
|
||||||
|
|
||||||
|
G90 的 heading 表示主天线 ANT1 到从天线 ANT2 的基线方位。实车中主天线在左、从天线在右，因此该基线指向车体右侧，不是前进方向。IMU 的 +Y 指向车前；结合静止重力数据支持 IMU +Z 向上，可得到安装先验：IMU +X 指向车右，和 ANT1→ANT2 同向。
|
||||||
|
|
||||||
|
双天线只能观测这根基线的方位和仰角，绕基线自身的旋转不可观。GNHPR pitch 是横向基线的仰角，更接近车体横滚响应，不能当作坡道上的纵向车体 pitch；roll 字段也不是独立的第三姿态观测。因此，旧链路把 HPR 拼成完整 SO(3) 再做三轴手眼，是坡道 RMS、会话牵引和不稳定 yaw/pitch 的主要来源。
|
||||||
|
|
||||||
|
当前实现保留旧式完整 HPR 结果作为诊断量，但不允许它通过完整旋转门禁；平移在完整旋转通过前被硬冻结。
|
||||||
|
|
||||||
|
## 坐标系和参考点
|
||||||
|
|
||||||
|
统一输出约定：
|
||||||
|
|
||||||
|
```text
|
||||||
|
p_RTK = R_RTK_IMU p_IMU + t_RTK_IMU
|
||||||
|
```
|
||||||
|
|
||||||
|
RTK 车固坐标定义：
|
||||||
|
|
||||||
|
- +X：ANT1（主天线、左侧）→ ANT2（从天线、右侧），指向车右。
|
||||||
|
- +Y：车辆前进方向，与 IMU +Y 同向。
|
||||||
|
- +Z：车辆上方；静止重力数据支持 IMU +Z 向上。
|
||||||
|
- 该完整三轴定义包含机械安装先验；GNHPR 实际直接观测的只有 +X 基线。
|
||||||
|
|
||||||
|
位置参考点：
|
||||||
|
|
||||||
|
- GGA 为 ANT1/主天线相位中心。
|
||||||
|
- 相位中心离地高度为 1.916499878 m。
|
||||||
|
|
||||||
|
## 求解链路
|
||||||
|
|
||||||
|
### R0：协议、数据质量和时间
|
||||||
|
|
||||||
|
1. 校验 NMEA checksum。
|
||||||
|
2. GGA 位置只接收 Q=4 固定解。
|
||||||
|
3. GNHPR 姿态只接收 Q=4 固定解；Q=5 浮点解仅进入诊断统计，不参与标定。
|
||||||
|
4. 保留 GNHPR 自己的卫星数、差分龄期和基站号，不能再用 GGA 卫星数替代 HPR 质量。
|
||||||
|
5. 姿态使用 `hpr_measurement_utc_s` 映射后的 IMU 设备时间。
|
||||||
|
6. 遇到 HPR 无效、浮点、时间间隔异常或基线跳变时切断连续段，运动对不得跨断点。
|
||||||
|
|
||||||
|
### T0：残余时间偏移
|
||||||
|
|
||||||
|
只使用可观测的有符号 heading 角速度与 IMU `gyro_z` 做相关扫描。角速度模长会混入不可观的绕基线旋转，不再作为时间审计依据。
|
||||||
|
|
||||||
|
时间偏移只用于初始化/诊断。只有相关峰足够高、与次峰分离且峰宽足够窄时才应用，否则保持 0 s。
|
||||||
|
|
||||||
|
### R1：双天线基线一致性
|
||||||
|
|
||||||
|
对每个连续固定解片段构造 0.75 s、1.5 s、3.0 s 的相对运动。以安装先验 `u_IMU=[1,0,0]` 检验：
|
||||||
|
|
||||||
|
```text
|
||||||
|
angle(u_RTK(t0), u_RTK(t1))
|
||||||
|
≈ angle(u_IMU, ΔR_IMU(t0,t1) u_IMU)
|
||||||
|
```
|
||||||
|
|
||||||
|
该标量约束不虚构 RTK 的前向轴和上向轴。输出整体、逐会话 RMS/P95、分轴诊断及最坏时间区间。所有会话的总权重归一，避免高激励或样本更多的会话支配结果。
|
||||||
|
|
||||||
|
### R2：旧式完整 HPR 诊断
|
||||||
|
|
||||||
|
为兼容历史输出，将基线补成零 roll 的数学坐标架，再运行完整手眼。这个结果仅用于暴露符号错误、异常会话和旧结果变化，不能作为可交付外参。
|
||||||
|
|
||||||
|
LOO 删除一个会话后，会重新优化剩余会话的陀螺零偏，不再固定全量数据的 nuisance 参数。
|
||||||
|
|
||||||
|
### T1：平移
|
||||||
|
|
||||||
|
只有完整三自由度旋转“可观且通过”时，才允许进入杆臂和平移求解。当前条件不满足，因此：
|
||||||
|
|
||||||
|
- 不运行平移优化；
|
||||||
|
- 不输出 `translation_result.json`；
|
||||||
|
- `t_RTK_IMU_m` 和 `T_RTK_IMU` 为 null；
|
||||||
|
- 历史巨大 Z、米级不确定度和约 1 m/s 速度残差不再消耗优化时间。
|
||||||
|
|
||||||
|
## 当前 8 会话结果
|
||||||
|
|
||||||
|
结果目录：`artifacts/rtk_imu_calibration_v2/all_sessions`。
|
||||||
|
|
||||||
|
```text
|
||||||
|
可观测基线一致性：
|
||||||
|
RMS / median / P95 = 0.593665 / 0.146040 / 1.212972 deg
|
||||||
|
priority_175910 RMS / P95 = 1.316292 / 2.472570 deg
|
||||||
|
slope RMS / P95 = 1.264514 / 2.475899 deg
|
||||||
|
最坏区间 = priority_175910，约 5.084 deg
|
||||||
|
baseline gate = passed
|
||||||
|
|
||||||
|
时间偏移：
|
||||||
|
全局候选 = +0.005 s
|
||||||
|
峰值相关 = 0.909019
|
||||||
|
近峰宽度 = [-0.110, +0.170] s
|
||||||
|
实际应用 = 0 s
|
||||||
|
|
||||||
|
旧式完整 HPR 诊断：
|
||||||
|
RPY = [-0.132765, -0.297433, -1.558358] deg
|
||||||
|
RMS / P95 = 1.172397 / 2.404977 deg
|
||||||
|
std = [0.615416, 0.437419, 3.687145] deg
|
||||||
|
priority_175910 re-optimized LOO = 1.698555 deg
|
||||||
|
max re-optimized LOO = 1.699427 deg（删除 slope）
|
||||||
|
legacy numeric gate = failed
|
||||||
|
full attitude observable = false
|
||||||
|
|
||||||
|
平移：
|
||||||
|
frozen / not run
|
||||||
|
```
|
||||||
|
|
||||||
|
旧结果中的 `priority_175910` 条件 LOO 为 5.14°。严格剔除 Q5、会话等权、断段保护以及 LOO 重估零偏后，该会话完整诊断 LOO 降为 1.699°。它和坡道会话仍是主要异常源，但现在异常集中在基线仰角通道，而不是 heading：这更符合原始 HPR 中 Q5、低卫星数和 pitch 大幅波动的事实。
|
||||||
|
|
||||||
|
## 已排除或仍存在的漏洞
|
||||||
|
|
||||||
|
- 已修复：heading 误当车前方向。
|
||||||
|
- 已修复：GNHPR pitch 误当纵向车体 pitch。
|
||||||
|
- 已修复：不可观 roll 注入完整姿态。
|
||||||
|
- 已修复：Q5 浮点 HPR 进入标定。
|
||||||
|
- 已修复：HPR 质量字段在导出时丢失。
|
||||||
|
- 已修复：相对运动跨越无效段或跳变。
|
||||||
|
- 已修复：样本多的会话权重过大。
|
||||||
|
- 已修复：LOO 固定全量会话零偏。
|
||||||
|
- 已修复：时间相关使用三轴角速度模长。
|
||||||
|
- 已修复：旋转未通过仍继续优化平移。
|
||||||
|
- 仍存在：当前运动不能稳定地从 GGA 速度补全车前/车上方向。
|
||||||
|
- 仍存在：基线绕轴自由度没有独立传感器观测。
|
||||||
|
- 仍存在：`priority_175910` 和坡道的基线仰角存在局部异常。
|
||||||
|
|
||||||
|
## 如何得到可交付的完整旋转
|
||||||
|
|
||||||
|
按优先级建议：
|
||||||
|
|
||||||
|
1. 采一组专用数据：空旷区域、全程 RTK fixed、较长直线加减速、左右转、坡道上下行，保留原始 GGA/GNHPR/IMU 时间和全部质量字段。
|
||||||
|
2. 用高质量前向速度补第二根轴。只在速度足够高、航向变化平缓的区间用 GGA course，并在同一优化中建模 ANT1 杆臂、非完整车辆侧向速度约束和时间偏移。
|
||||||
|
3. 用静止重力补上向轴时，必须把“地面水平/车辆静止”写成显式先验，并将结果标记为安装先验约束解，而不是双天线数据独立解。
|
||||||
|
4. 若能取得 G90 内部融合后的完整 INS 姿态、第三天线、轮速/转角或可靠车体姿态源，优先作为第二独立方向。
|
||||||
|
5. 完整旋转通过留出验证后，才恢复平移；平移应同时估计杆臂、速度、加计偏置，并检查垂向高程基准。
|
||||||
|
|
||||||
|
当前数据上尝试用平滑 GGA 速度直接补全姿态，结果随平滑窗口明显变化，受低速、转弯杆臂和高程差分噪声影响，不能进入正式结果。
|
||||||
|
|
||||||
|
## 与主流开源方法的对应
|
||||||
|
|
||||||
|
- Kalibr：角速度相关适合作为时间偏移初始化，不应在宽峰时强行采用候选值。
|
||||||
|
- iKalibr：采用连续时间轨迹联合估计时空参数，并强调充分激励；适合后续专用数据。
|
||||||
|
- MINS：异步测量插值并把传感器外参、时间和 nuisance 状态一起估计。
|
||||||
|
- GICI-LIB：因子图中显式进行 GNSS/INS 初始化、质量控制和异常值处理。
|
||||||
|
|
||||||
|
本项目暂不直接引入这些大型框架，而是吸收其原则：先保证物理可观测性和数据质量，再进行联合优化；不能用自由状态吸收错误模型。
|
||||||
|
|
||||||
|
参考：
|
||||||
|
|
||||||
|
- Unicore N4 Reference Commands Manual
|
||||||
|
- Unicore UM982 User Manual
|
||||||
|
- https://github.com/Unsigned-Long/iKalibr
|
||||||
|
- https://github.com/ethz-asl/kalibr
|
||||||
|
- https://github.com/rpng/MINS
|
||||||
|
- https://github.com/chichengcn/gici-open
|
||||||
|
|
||||||
|
## 代码入口
|
||||||
|
|
||||||
|
- `rtk_imu/rtk_attitude.py`：GNHPR 基线语义及零 roll 数学补全。
|
||||||
|
- `rtk_imu/rtk_io.py`：RTK CSV、Q4/Q5 和 checksum 质量门禁。
|
||||||
|
- `rtk_imu/rtk_imu_rotation.py`：时间审计、连续段、基线审计、旧式诊断和重优化 LOO。
|
||||||
|
- `rtk_imu/rtk_imu_replay.py`：坐标定义、参考点、平移冻结和 JSON 输出。
|
||||||
|
- `tools/rscap_v2/g90_rtk.py`：原始 GNHPR 解析及质量字段。
|
||||||
|
- `tools/export_g90_rtk_to_sessions.py`：GNHPR 质量字段导出。
|
||||||
|
- `tools/run_rtk_imu_calibration.py`：端到端命令行入口。
|
||||||
|
- `tests/test_rtk_imu_calibration.py`：轴定义、Q4/Q5、时间和预积分回归测试。
|
||||||
@@ -0,0 +1,108 @@
|
|||||||
|
# RTK–IMU Engineering 6DoF 分支
|
||||||
|
|
||||||
|
该分支与 V3 `data_only` 严格链路并列，不改变其门禁结论：
|
||||||
|
|
||||||
|
```text
|
||||||
|
data_only_6dof_accepted = false
|
||||||
|
```
|
||||||
|
|
||||||
|
工程分支固定使用真实水平静止场地得到的 R2G 旋转，默认参考值为
|
||||||
|
`RPY=[0.4543066, -0.0026392, 0.0122384] deg`，输出始终记录：
|
||||||
|
|
||||||
|
```text
|
||||||
|
rotation_source = R2G_gravity_level_prior
|
||||||
|
translation_conditional_on_rotation = true
|
||||||
|
```
|
||||||
|
|
||||||
|
## 条件模型
|
||||||
|
|
||||||
|
估计量是 `l_I = p_ANT1^I`。每个连续质量段使用 HI13 设备时间、raw gyro/acc 动态预积分，融合 RTK Fixed 的 GGA/BESTNAVA 位置、BESTNAVA Doppler velocity 和 Q4 GNHPR 的 ANT1→ANT2 基线。逐段 nuisance state 包含初始姿态、IMU 原点初始位置、速度、gyro bias 和 acc bias。HI13 absolute quaternion/RPY（尤其 absolute yaw）不进入机械外参因子；R2V 仅作诊断。
|
||||||
|
|
||||||
|
静止相关因子分为两类：
|
||||||
|
|
||||||
|
- `gravity_candidate`：连续约 1.5 s 的低角速度、gyro/acc 方差稳定且加速度模长接近重力；它不能等价为静止。
|
||||||
|
- `zupt_static`：在 gravity candidate 基础上，必须同时有至少约 1 s 的连续 BESTNAVA Doppler 速度接近 0。高速或匀速直线不会加入 ZUPT。
|
||||||
|
|
||||||
|
连续段复用 R0 质量断点：checksum 无效、定位非 Fixed、GNHPR 非 Q4、设备时间回跳、HPR 测量间隙、baseline jump、位置测量间隙或 IMU gap 都会切段，禁止跨断点预积分。 断后片段还必须至少包含 6 个求解节点且持续不少于 5 s；更短的局部欠约束微段不会跨断点拼接，而是直接不进入杆臂优化。
|
||||||
|
|
||||||
|
## 高程与残差口径
|
||||||
|
|
||||||
|
- GGA 只约束 XY；GGA MSL altitude 不定义也不参与 ENU-Z。
|
||||||
|
- ENU-Z reference 只来自有效 Fixed BESTNAVA altitude；BESTNAVA 才约束 XYZ。
|
||||||
|
- 输出分别为 `gga_xy_residual`、`bestnava_xyz_residual` 和 `doppler_velocity_residual`。未进入 Z factor 的 GGA 高度不进入垂向残差统计。
|
||||||
|
|
||||||
|
## 可观性与验收
|
||||||
|
|
||||||
|
杆臂可观性不再使用全状态最小奇异向量。状态分为杆臂 `l` 与 nuisance state，对优化 Hessian 计算 Schur complement：
|
||||||
|
|
||||||
|
```text
|
||||||
|
H_l_marg = H_ll - H_ln pinv(H_nn) H_nl
|
||||||
|
```
|
||||||
|
|
||||||
|
只对该 3×3 marginal lever information 做 SVD，并输出：
|
||||||
|
|
||||||
|
- `l_I_marginal_covariance_m2`、`l_I_std_m`
|
||||||
|
- `lever_information_singular_values`
|
||||||
|
- `lever_information_condition_number`
|
||||||
|
- `lever_precision_rank`
|
||||||
|
- `weakest_lever_direction_I`
|
||||||
|
|
||||||
|
门禁分为两层：
|
||||||
|
|
||||||
|
- `solver_health_gates` 只判断优化是否收敛、数值是否有限且残差未发散。
|
||||||
|
- `engineering_acceptance_gates` 使用更严格的杆臂 marginal std/information/rank/condition、BESTNAVA XYZ、GGA XY、Doppler velocity、LOO、bootstrap、旋转敏感性和可选手量一致性。
|
||||||
|
|
||||||
|
任一核心门禁失败时，`engineering_6dof_accepted=false`。没有完整执行 bootstrap 和 18 组旋转敏感性时，两项门禁明确为 false，不会把阶段性 base/LOO 结果误标为正式放行。
|
||||||
|
|
||||||
|
Bootstrap 按 session 有放回抽样，并保留重复 session 的 multiplicity；重复抽中的 session 会重复贡献其全部连续段。
|
||||||
|
|
||||||
|
## 机械杆臂软先验与双解输出
|
||||||
|
|
||||||
|
`--manual-l-i-m` 仅在同时提供 `--manual-l-i-std-m` 或完整
|
||||||
|
`--manual-l-i-covariance-m2` 时才成为白化高斯软因子。求解器始终先运行无先验
|
||||||
|
`free_solution`，再运行 `prior_constrained_solution`；输出还包含机械参考及两者到
|
||||||
|
参考的差值。当前活动 engineering 解为 prior-constrained 解（若启用），但 acceptance
|
||||||
|
额外要求 free-solve 也与机械参考一致，软先验不能掩盖不可观或数据矛盾。
|
||||||
|
|
||||||
|
示例（数值需使用实际机械测量的 1σ，不可把示例值当作默认）：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
--manual-l-i-m -0.45072 -0.25682 0.73208 `
|
||||||
|
--manual-l-i-std-m <sigma_x_m> <sigma_y_m> <sigma_z_m>
|
||||||
|
```
|
||||||
|
## 坐标转换
|
||||||
|
|
||||||
|
```text
|
||||||
|
T_RTK_IMU: p_RTK = R_RTK_IMU p_IMU - R_RTK_IMU l_I
|
||||||
|
T_IMU_RTK: p_IMU = R_RTK_IMU^T p_RTK + l_I
|
||||||
|
```
|
||||||
|
|
||||||
|
RTK 原点是 ANT1，因此 `T_IMU_RTK.translation == l_I`，两矩阵必须互逆。
|
||||||
|
|
||||||
|
## 分阶段运行
|
||||||
|
|
||||||
|
第一阶段默认只运行 base fit、marginal observability、residual audit 和 LOO：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools/run_rtk_imu_engineering_6dof.py `
|
||||||
|
--manifest D:\data\rtk_imu_unified_v3\manifest.json `
|
||||||
|
--output artifacts/rtk_imu_calibration_v3/engineering_6dof_base_loo.json `
|
||||||
|
--session <session-a> `
|
||||||
|
--session <session-b> `
|
||||||
|
--session <session-c>
|
||||||
|
```
|
||||||
|
|
||||||
|
只有 base/LOO 合理后，才显式启动全量验证：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools/run_rtk_imu_engineering_6dof.py `
|
||||||
|
--manifest D:\data\rtk_imu_unified_v3\manifest.json `
|
||||||
|
--output artifacts/rtk_imu_calibration_v3/engineering_6dof_full.json `
|
||||||
|
--session <session-a> `
|
||||||
|
--session <session-b> `
|
||||||
|
--session <session-c> `
|
||||||
|
--run-bootstrap --bootstrap-repetitions 40 --bootstrap-seed 0 `
|
||||||
|
--run-rotation-sensitivity
|
||||||
|
```
|
||||||
|
|
||||||
|
正式会话应覆盖直行加减速、左右转、坡道和明确水平静止段。不得为了运行时间降低验收门禁或把未完成验证标为成功。
|
||||||
@@ -0,0 +1,175 @@
|
|||||||
|
# RTK–IMU 多源原始数据与旋转标定 V3
|
||||||
|
|
||||||
|
## 可行性结论
|
||||||
|
|
||||||
|
重构方向正确且必要。原始捕获中确实存在旧导出链路忽略的 BESTNAVA、
|
||||||
|
PVTSLNA 和 HI13 姿态/四元数。它们可以补充速度、质量、绝对姿态和静止
|
||||||
|
重力约束，但本批数据尚不能通过完整旋转门禁：
|
||||||
|
|
||||||
|
- BESTNAVA 约 0.8–1 Hz，严格高速样本很少。
|
||||||
|
- 当前动态主要是平面 yaw，R1b 的两个倾斜自由度仍弱可观。
|
||||||
|
- HI13 四元数与 GNSS 真北存在稳定但非机械的 yaw 偏差，可能来自磁偏角或
|
||||||
|
内部导航融合。
|
||||||
|
- 两个明确水平静止会话的 R2G 很稳定，但它是水平/重力先验约束解。
|
||||||
|
|
||||||
|
因此，本次重构显著提高了诊断能力，也防止错误外参进入平移；它没有把信息
|
||||||
|
不足包装成“标定成功”。
|
||||||
|
|
||||||
|
## 三批原始数据
|
||||||
|
|
||||||
|
扫描根目录：
|
||||||
|
|
||||||
|
- 0808：D:\data\raw_serial_capture_v2，仅选 20260808。
|
||||||
|
- 0815：D:\data\0815\raw_serial_capture_v2；目录实际包含 20260812–14。
|
||||||
|
- 0819：D:\data\0819\raw_serial_capture_v2。
|
||||||
|
|
||||||
|
按文件名捕获开始时间在 1.5 s 内一一配对：
|
||||||
|
|
||||||
|
| 批次 | G90/HI13 配对 | GNHPR | BESTNAVA | PVTSLNA | Q4 HPR | Fixed Doppler |
|
||||||
|
|---|---:|---:|---:|---:|---:|---:|
|
||||||
|
| 0808 | 18 | 46,180 | 8,291 | 5,993 | 45,606 | 8,128 |
|
||||||
|
| 0815 | 28 | 96,241 | 12,660 | 9,638 | 27,045 | 3,385 |
|
||||||
|
| 0819 | 9 | 15,717 | 2,029 | 1,427 | 14,976 | 1,930 |
|
||||||
|
|
||||||
|
共完成 55 组统一导出。0815 有一组 G90 没有匹配 HI13，manifest 将其列为
|
||||||
|
unmatched。发现一条 NMEA 时间为 0913A：保留原始行，设备时间留空并隔离，
|
||||||
|
没有回退到 host receive time。
|
||||||
|
|
||||||
|
速度不等于高速激励。增加速度 ≥1.5 m/s、水平速度标准差 ≤0.25 m/s 后，
|
||||||
|
三个批次分别只剩 44、8、0 条候选。
|
||||||
|
|
||||||
|
## 统一导出
|
||||||
|
|
||||||
|
输出根目录：D:\data\rtk_imu_unified_v3。
|
||||||
|
|
||||||
|
每组捕获：
|
||||||
|
|
||||||
|
- imu.npz：HI13 system_time、gyro、accel、RPY、WXYZ quaternion、磁场、
|
||||||
|
PPS stamp、温度、气压和 host receive UTC。
|
||||||
|
- rtk.csv：每条原生异步 GGA/GNHPR/BESTNAVA/PVTSLNA 单独成行，不再把
|
||||||
|
HPR/BEST/PVT 最近邻挂到 GGA。
|
||||||
|
- export_summary.json：原始捕获摘要、报文数量、四元数范数和设备到 host
|
||||||
|
的仿射时钟诊断。
|
||||||
|
- manifest.json：全部会话、批次、目录和 unmatched 记录。
|
||||||
|
|
||||||
|
时间规则：
|
||||||
|
|
||||||
|
1. HI13 system_time 是 IMU 主时间轴。
|
||||||
|
2. BESTNAVA/PVTSLNA 使用 GNSS week/TOW 和 leap seconds。
|
||||||
|
3. GGA/GNHPR 使用报文自己的 UTC time-of-day。
|
||||||
|
4. RTK GNSS 测量时刻通过 HI13 device→host 仿射模型映射进 HI13 设备时钟。
|
||||||
|
5. host receive time 只用于跨时钟桥接、延迟和抖动诊断，绝不替代采样时刻。
|
||||||
|
6. checksum 无效、时间畸形或非固定解记录保留，但不进入求解。
|
||||||
|
|
||||||
|
## 新旋转链路
|
||||||
|
|
||||||
|
### R1b：基线在 IMU 中的 2DoF 方向
|
||||||
|
|
||||||
|
对连续 Q4 GNHPR 基线和 HI13 陀螺相对旋转使用不变量：
|
||||||
|
|
||||||
|
angle(b_ENU(t0), b_ENU(t1))
|
||||||
|
= angle(b_IMU, DeltaR_IMU b_IMU)
|
||||||
|
|
||||||
|
估计 b_IMU 的两个倾斜自由度及逐会话陀螺零偏。安装信息只用于选择 +X
|
||||||
|
半球，并施加明确记录的弱 20° 先验。按会话等权，输出残差、协方差和信息
|
||||||
|
奇异值。
|
||||||
|
|
||||||
|
### R2V：基线 + Doppler velocity
|
||||||
|
|
||||||
|
筛选条件：
|
||||||
|
|
||||||
|
- BESTNAVA position 为 SOL_COMPUTED/NARROW_INT；
|
||||||
|
- velocity 为 SOL_COMPUTED/DOPPLER_VELOCITY；
|
||||||
|
- checksum 有效；
|
||||||
|
- 水平速度 ≥1.5 m/s、速度标准差 ≤0.25 m/s；
|
||||||
|
- |IMU gyro_z| ≤3°/s；
|
||||||
|
- 速度方向与横向基线接近正交；
|
||||||
|
- GNHPR Q4，并有时间邻近的合法 HI13 quaternion。
|
||||||
|
|
||||||
|
基线给车右，Doppler velocity 给车前，叉积给车上。HI13 quaternion 的
|
||||||
|
body/world 和 ENU/NED 候选全部评分，只保留残差最小者。该方法会把 HI13
|
||||||
|
导航 yaw 偏差带入候选，因此必须和 R2G 交叉验证。
|
||||||
|
|
||||||
|
### R2G：基线 + 水平静止重力
|
||||||
|
|
||||||
|
只允许调用方明确标记的水平静止会话。本次使用：
|
||||||
|
|
||||||
|
- 0819_20260819_072130（flat_static_hdg207）
|
||||||
|
- 0819_20260819_073045（flat_static_hdg082）
|
||||||
|
|
||||||
|
每 10 s 分块，要求 gyro norm ≤0.35°/s，且加速度模长距标准重力不超过
|
||||||
|
0.15 m/s²。R1b 提供车右，加速度中值提供车上，叉积得到车前。
|
||||||
|
|
||||||
|
### R3：稳定性与正式门禁
|
||||||
|
|
||||||
|
R2V/R2G 都输出：
|
||||||
|
|
||||||
|
- 样本和会话数量；
|
||||||
|
- SO(3) RMS/P95；
|
||||||
|
- 旋转向量样本标准差和均值协方差；
|
||||||
|
- leave-one-session；
|
||||||
|
- 10 样本或 10 s block-out。
|
||||||
|
|
||||||
|
只有 R1b 可观、R2V 和 R2G 各自稳定、二者差异 ≤2° 时，才将
|
||||||
|
translation_unlocked 设为 true。旧 GNHPR 三轴手眼不参与正式门禁。
|
||||||
|
|
||||||
|
## 首轮实测
|
||||||
|
|
||||||
|
R1b：
|
||||||
|
|
||||||
|
b_IMU = [0.999999976, -0.000213959, -0.000044368]
|
||||||
|
tilt_yz = [-0.0123, -0.0025] deg
|
||||||
|
pairs = 2091
|
||||||
|
RMS / P95 = 0.8086 / 1.7105 deg
|
||||||
|
information singular values = [1.176e-2, 8.476e-4]
|
||||||
|
std = [27.77, 7.47] deg
|
||||||
|
gate = failed
|
||||||
|
|
||||||
|
点估计接近 +X 是安装先验与名义轴一致的结果；巨大协方差说明不能宣称
|
||||||
|
数据独立估出了这两个小角。
|
||||||
|
|
||||||
|
R2V：
|
||||||
|
|
||||||
|
qualifying samples / sessions = 41 / 2
|
||||||
|
selected quaternion convention = HI13_q_body_to_ENU
|
||||||
|
RPY = [0.7532, 0.0708, -9.2944] deg
|
||||||
|
RMS / P95 = 1.8436 / 3.4975 deg
|
||||||
|
max leave-one-session = 3.4519 deg
|
||||||
|
gate = failed
|
||||||
|
|
||||||
|
R2G：
|
||||||
|
|
||||||
|
level-static blocks / sessions = 53 / 2
|
||||||
|
RPY = [0.4543, -0.0026, 0.0122] deg
|
||||||
|
RMS / P95 = 0.2542 / 0.2732 deg
|
||||||
|
max leave-one-session = 0.2688 deg
|
||||||
|
gate = passed (level/gravity-prior constrained)
|
||||||
|
|
||||||
|
R2V 与 R2G 的 SO(3) 差异为 9.3119°。R3 最终：
|
||||||
|
|
||||||
|
full_rotation_accepted = false
|
||||||
|
translation_unlocked = false
|
||||||
|
|
||||||
|
## 下一轮数据要求
|
||||||
|
|
||||||
|
1. BESTNAVA 改为至少 10 Hz，并确认 Doppler velocity 与 GNHPR 使用同一
|
||||||
|
GNSS week/TOW 输出周期。
|
||||||
|
2. 每个日期都录制多段 ≥3 m/s、持续 20–30 s 的正向直线；包含不同方位，
|
||||||
|
避免单一磁环境和单一会话支配。
|
||||||
|
3. 为 R1b 增加可控的 roll/pitch 激励；只有平面 yaw 无法稳定估出横向
|
||||||
|
基线的两个微小倾斜角。
|
||||||
|
4. 每个日期至少录两种车头方位的明确水平静止段，检验 HI13 重力和绝对
|
||||||
|
quaternion 的跨日期稳定性。
|
||||||
|
5. 若 HI13 quaternion 的 yaw 来自磁融合，应获取其导航坐标定义、磁偏角
|
||||||
|
设置和融合状态；否则 R2V 只使用其 roll/pitch，yaw 由 GNSS 基线和速度
|
||||||
|
决定。
|
||||||
|
6. 上述门禁通过前继续冻结杆臂和平移。
|
||||||
|
|
||||||
|
## 实现入口
|
||||||
|
|
||||||
|
- tools/rscap_v2/g90_rtk.py：GGA/GNHPR/BESTNAVA/PVTSLNA 和双 checksum。
|
||||||
|
- tools/rscap_v2/hi13_imu.py：HI91 system_time、惯性、姿态和四元数。
|
||||||
|
- tools/export_rtk_imu_unified.py：55 组配对、原生异步导出和断点续导。
|
||||||
|
- rtk_imu/rtk_imu_multisource.py：R1b/R2V/R2G/R3。
|
||||||
|
- tools/run_rtk_imu_multisource.py：多源旋转命令行入口。
|
||||||
|
- artifacts/rtk_imu_calibration_v3/multisource_result.json：首轮 R3 结果。
|
||||||
@@ -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
@@ -15,6 +15,7 @@ Accepted inputs
|
|||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import csv
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
@@ -36,16 +37,17 @@ def load_imu_samples(path: Path | str) -> ImuSeries:
|
|||||||
|
|
||||||
|
|
||||||
def _load_imu_csv(path: Path) -> ImuSeries:
|
def _load_imu_csv(path: Path) -> ImuSeries:
|
||||||
data = np.genfromtxt(path, delimiter=",", names=True, dtype=float)
|
required_order = ["t", "gx", "gy", "gz", "ax", "ay", "az"]
|
||||||
if data.ndim == 0:
|
with path.open("r", encoding="utf-8-sig", newline="") as handle:
|
||||||
data = np.array([data])
|
header = next(csv.reader(handle), [])
|
||||||
names = set(data.dtype.names or ())
|
names = set(header)
|
||||||
required = {"t", "gx", "gy", "gz", "ax", "ay", "az"}
|
if not set(required_order).issubset(names):
|
||||||
if not required.issubset(names):
|
raise ValueError(f"IMU CSV must contain columns {sorted(required_order)}, got {sorted(names)}")
|
||||||
raise ValueError(f"IMU CSV must contain columns {sorted(required)}, got {sorted(names)}")
|
usecols = [header.index(name) for name in required_order]
|
||||||
t = np.asarray(data["t"], dtype=float).reshape(-1)
|
data = np.loadtxt(path, delimiter=",", skiprows=1, usecols=usecols, ndmin=2)
|
||||||
gyro = np.column_stack([data["gx"], data["gy"], data["gz"]]).astype(float)
|
t = np.asarray(data[:, 0], dtype=float).reshape(-1)
|
||||||
acc = np.column_stack([data["ax"], data["ay"], data["az"]]).astype(float)
|
gyro = np.asarray(data[:, 1:4], dtype=float)
|
||||||
|
acc = np.asarray(data[:, 4:7], dtype=float)
|
||||||
order = np.argsort(t)
|
order = np.argsort(t)
|
||||||
return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])
|
return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])
|
||||||
|
|
||||||
|
|||||||
@@ -158,9 +158,14 @@ def preintegrate_gyro(
|
|||||||
if dt <= 0:
|
if dt <= 0:
|
||||||
continue
|
continue
|
||||||
|
|
||||||
# Exact endpoint gyro via linear interpolation inside the sample interval.
|
# Exact local endpoint interpolation. ``seg0`` and ``seg1`` are inside
|
||||||
g_a = _interp_gyro(times_s, gyro_rad_s, seg0)
|
# this adjacent sample interval, so scanning the full series with
|
||||||
g_b = _interp_gyro(times_s, gyro_rad_s, seg1)
|
# 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
|
omega = 0.5 * (g_a + g_b) - bias
|
||||||
gyro_norms.append(float(np.linalg.norm(omega)))
|
gyro_norms.append(float(np.linalg.norm(omega)))
|
||||||
|
|
||||||
@@ -279,10 +284,13 @@ def preintegrate_imu(
|
|||||||
if dt <= 0:
|
if dt <= 0:
|
||||||
continue
|
continue
|
||||||
|
|
||||||
g_a = _interp_vec(times_s, gyro_rad_s, seg0)
|
sample_dt = max(t_b - t_a, 1e-12)
|
||||||
g_b = _interp_vec(times_s, gyro_rad_s, seg1)
|
u0 = (seg0 - t_a) / sample_dt
|
||||||
a_a = _interp_vec(times_s, acc_m_s2, seg0)
|
u1 = (seg1 - t_a) / sample_dt
|
||||||
a_b = _interp_vec(times_s, acc_m_s2, seg1)
|
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
|
omega = 0.5 * (g_a + g_b) - bg
|
||||||
acc = 0.5 * (a_a + a_b) - ba
|
acc = 0.5 * (a_a + a_b) - ba
|
||||||
gyro_norms.append(float(np.linalg.norm(omega)))
|
gyro_norms.append(float(np.linalg.norm(omega)))
|
||||||
|
|||||||
+2
-2
@@ -1,7 +1,7 @@
|
|||||||
[project]
|
[project]
|
||||||
name = "lidar-imu-calibration"
|
name = "lidar-imu-calibration"
|
||||||
version = "0.3.0"
|
version = "0.3.0"
|
||||||
description = "LiDAR–IMU extrinsic calibration from continuous-motion keyframes (imu_lidar)"
|
description = "LiDAR-IMU and RTK-IMU extrinsic calibration algorithms"
|
||||||
requires-python = ">=3.10"
|
requires-python = ">=3.10"
|
||||||
dependencies = [
|
dependencies = [
|
||||||
"numpy>=1.26",
|
"numpy>=1.26",
|
||||||
@@ -21,7 +21,7 @@ requires = ["setuptools>=68", "wheel"]
|
|||||||
build-backend = "setuptools.build_meta"
|
build-backend = "setuptools.build_meta"
|
||||||
|
|
||||||
[tool.setuptools]
|
[tool.setuptools]
|
||||||
packages = ["imu_lidar", "tools"]
|
packages = ["imu_lidar", "rtk_imu", "tools"]
|
||||||
|
|
||||||
[tool.pytest.ini_options]
|
[tool.pytest.ini_options]
|
||||||
testpaths = ["tests"]
|
testpaths = ["tests"]
|
||||||
|
|||||||
@@ -0,0 +1,13 @@
|
|||||||
|
# RTK-IMU Calibration Package
|
||||||
|
|
||||||
|
`rtk_imu/` contains the independent RTK-IMU calibration pipeline: G90 observation I/O, antenna-baseline rotation, V1 replay, V3 multisource stages, engineering 6DoF, and the node-state factor graph.
|
||||||
|
|
||||||
|
## Boundary
|
||||||
|
|
||||||
|
This package may import only shared `imu_lidar` utilities: `contracts`, `geometry`, `geodesy`, `imu_io`, `imu_preintegration`, and the generic `rotation_handeye` initializer.
|
||||||
|
|
||||||
|
It must not import LiDAR-specific modules such as `lidar_io`, `lidar_deskew`, `registration`, `pipeline`, `phase_a`, or `joint_optimizer`. The LiDAR-IMU pipeline remains in `imu_lidar/`.
|
||||||
|
|
||||||
|
## Entrypoints
|
||||||
|
Use `tools/run_rtk_imu_*.py` and `tools/audit_rtk_imu_*.py`; these scripts import from `rtk_imu.*`.
|
||||||
|
For the engineering procedure, current result, acceptance boundary, and reproduction commands, see [`README_RTK_IMU.md`](../README_RTK_IMU.md).
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
"""RTK-IMU extrinsic calibration algorithms and data interfaces."""
|
||||||
@@ -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
@@ -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)
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -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),
|
||||||
|
)
|
||||||
@@ -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),
|
||||||
|
)
|
||||||
@@ -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])
|
||||||
@@ -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
|
||||||
@@ -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
@@ -5,7 +5,12 @@ from __future__ import annotations
|
|||||||
import struct
|
import struct
|
||||||
|
|
||||||
from tools.rscap_v2.capture_format_v2 import CaptureFile, CaptureHeader, RawChunk
|
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(
|
def _hi91_frame(
|
||||||
@@ -13,6 +18,8 @@ def _hi91_frame(
|
|||||||
device_ms: int = 123456,
|
device_ms: int = 123456,
|
||||||
accel_g=(0.0, 0.0, 1.0),
|
accel_g=(0.0, 0.0, 1.0),
|
||||||
gyro_dps=(1.0, -2.0, 3.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:
|
) -> bytes:
|
||||||
payload = bytearray(76)
|
payload = bytearray(76)
|
||||||
payload[0] = 0x91
|
payload[0] = 0x91
|
||||||
@@ -22,7 +29,8 @@ def _hi91_frame(
|
|||||||
struct.pack_into("<I", payload, 8, device_ms)
|
struct.pack_into("<I", payload, 8, device_ms)
|
||||||
struct.pack_into("<fff", payload, 12, *accel_g)
|
struct.pack_into("<fff", payload, 12, *accel_g)
|
||||||
struct.pack_into("<fff", payload, 24, *gyro_dps)
|
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)
|
payload_length = len(payload)
|
||||||
header = bytearray(6)
|
header = bytearray(6)
|
||||||
header[0] = 0x5A
|
header[0] = 0x5A
|
||||||
@@ -48,6 +56,12 @@ def test_parse_hi91_units():
|
|||||||
assert device_ms == 5000
|
assert device_ms == 5000
|
||||||
assert abs(accel[2] - 9.80665) < 1e-4
|
assert abs(accel[2] - 9.80665) < 1e-4
|
||||||
assert abs(gyro[0] - 1.0) < 1e-5
|
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():
|
def test_iter_hi13_from_capture():
|
||||||
|
|||||||
@@ -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)
|
||||||
@@ -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.)
|
||||||
@@ -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
|
||||||
@@ -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())
|
||||||
@@ -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())
|
||||||
@@ -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())
|
||||||
@@ -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())
|
||||||
@@ -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())
|
||||||
@@ -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())
|
||||||
@@ -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())
|
||||||
@@ -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())
|
||||||
@@ -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())
|
||||||
@@ -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())
|
||||||
@@ -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
@@ -12,6 +12,7 @@ HI91 (preferred for calibration):
|
|||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import struct
|
import struct
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
@@ -22,6 +23,29 @@ G0 = 9.80665
|
|||||||
DEG2RAD = np.pi / 180.0
|
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:
|
def crc16_hi13(frame: bytes, payload_length: int) -> int:
|
||||||
crc = 0
|
crc = 0
|
||||||
for value in frame[:4]:
|
for value in frame[:4]:
|
||||||
@@ -41,8 +65,8 @@ def _update_crc16(crc: int, value: int) -> int:
|
|||||||
return crc
|
return crc
|
||||||
|
|
||||||
|
|
||||||
def parse_hi91_frame(raw: bytes) -> tuple[tuple[float, float, float], tuple[float, float, float], int] | None:
|
def parse_hi91_sample(raw: bytes, host_receive_utc_ticks: int = 0) -> Hi13Sample | None:
|
||||||
"""Return (gyro_rad_s, accel_m_s2, device_timestamp_ms) for a CRC-valid HI91 frame."""
|
"""Decode every calibration-relevant field from a CRC-valid HI91 frame."""
|
||||||
|
|
||||||
if len(raw) < 6 + 76:
|
if len(raw) < 6 + 76:
|
||||||
return None
|
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)
|
expected = raw[4] | (raw[5] << 8)
|
||||||
if crc16_hi13(raw, payload_length) != expected:
|
if crc16_hi13(raw, payload_length) != expected:
|
||||||
return None
|
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)
|
ax, ay, az = struct.unpack_from("<fff", raw, 18)
|
||||||
gx, gy, gz = struct.unpack_from("<fff", raw, 30)
|
gx, gy, gz = struct.unpack_from("<fff", raw, 30)
|
||||||
gyro = (gx * DEG2RAD, gy * DEG2RAD, gz * DEG2RAD)
|
gyro = (gx * DEG2RAD, gy * DEG2RAD, gz * DEG2RAD)
|
||||||
accel = (ax * G0, ay * G0, az * G0)
|
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(
|
def iter_hi13_imu_samples(
|
||||||
@@ -67,14 +113,14 @@ def iter_hi13_imu_samples(
|
|||||||
*,
|
*,
|
||||||
host_utc_ticks_min: int | None = None,
|
host_utc_ticks_min: int | None = None,
|
||||||
host_utc_ticks_max: int | None = None,
|
host_utc_ticks_max: int | None = None,
|
||||||
) -> list[ImuSample]:
|
) -> list[Hi13Sample]:
|
||||||
"""Return CRC-valid HI91 samples sorted by device timestamp.
|
"""Return CRC-valid HI91 samples sorted by device timestamp.
|
||||||
|
|
||||||
Streams chunk-by-chunk (no giant join) and can skip whole chunks outside the
|
Streams chunk-by-chunk (no giant join) and can skip whole chunks outside the
|
||||||
host UTC receive window before parsing.
|
host UTC receive window before parsing.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
samples: list[ImuSample] = []
|
samples: list[Hi13Sample] = []
|
||||||
carry = b""
|
carry = b""
|
||||||
for chunk in capture.chunks:
|
for chunk in capture.chunks:
|
||||||
if host_utc_ticks_min is not None and chunk.receive_utc_ticks < host_utc_ticks_min:
|
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):
|
if end > len(stream):
|
||||||
carry = stream[sync:]
|
carry = stream[sync:]
|
||||||
break
|
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
|
cursor = end
|
||||||
if parsed is None:
|
if parsed is None:
|
||||||
continue
|
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:
|
if host_utc_ticks_min is not None and host_ticks < host_utc_ticks_min:
|
||||||
continue
|
continue
|
||||||
if host_utc_ticks_max is not None and host_ticks > host_utc_ticks_max:
|
if host_utc_ticks_max is not None and host_ticks > host_utc_ticks_max:
|
||||||
continue
|
continue
|
||||||
samples.append(
|
samples.append(parsed)
|
||||||
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,
|
|
||||||
)
|
|
||||||
)
|
|
||||||
else:
|
else:
|
||||||
carry = b""
|
carry = b""
|
||||||
samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us))
|
samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us))
|
||||||
@@ -129,8 +166,10 @@ def iter_hi13_imu_samples(
|
|||||||
|
|
||||||
__all__ = [
|
__all__ = [
|
||||||
"ImuSample",
|
"ImuSample",
|
||||||
|
"Hi13Sample",
|
||||||
"crc16_hi13",
|
"crc16_hi13",
|
||||||
"iter_hi13_imu_samples",
|
"iter_hi13_imu_samples",
|
||||||
"parse_hi91_frame",
|
"parse_hi91_frame",
|
||||||
|
"parse_hi91_sample",
|
||||||
"samples_to_arrays",
|
"samples_to_arrays",
|
||||||
]
|
]
|
||||||
|
|||||||
@@ -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())
|
||||||
@@ -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())
|
||||||
@@ -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())
|
||||||
@@ -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())
|
||||||
@@ -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())
|
||||||
@@ -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)),
|
||||||
|
)
|
||||||
@@ -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())
|
||||||
Reference in New Issue
Block a user