Improve IMU preintegration stability (#115)

* Improve IMU preintegration stability

* Organize IMU parameters

Author: rsasaki0109

CMakeLists.txt

@@ -471,6 +471,37 @@ if(BUILD_TESTING)
     COMMAND test_imu_preintegration_diagnostics_policy
   )
 
+  add_executable(test_imu_seed_consistency_policy
+    test/test_imu_seed_consistency_policy.cpp)
+  target_include_directories(test_imu_seed_consistency_policy PRIVATE
+    include)
+  add_test(
+    NAME imu_seed_consistency_policy
+    COMMAND test_imu_seed_consistency_policy
+  )
+
+  add_executable(test_imu_queue_policy
+    test/test_imu_queue_policy.cpp)
+  target_include_directories(test_imu_queue_policy PRIVATE
+    include ${EIGEN3_INCLUDE_DIRS})
+  target_link_libraries(test_imu_queue_policy
+    Eigen3::Eigen)
+  add_test(
+    NAME imu_queue_policy
+    COMMAND test_imu_queue_policy
+  )
+
+  add_executable(test_imu_initialization_policy
+    test/test_imu_initialization_policy.cpp)
+  target_include_directories(test_imu_initialization_policy PRIVATE
+    include ${EIGEN3_INCLUDE_DIRS})
+  target_link_libraries(test_imu_initialization_policy
+    Eigen3::Eigen)
+  add_test(
+    NAME imu_initialization_policy
+    COMMAND test_imu_initialization_policy
+  )
+
   add_executable(test_deskew_readiness_policy
     test/test_deskew_readiness_policy.cpp)
   target_include_directories(test_deskew_readiness_policy PRIVATE

docs/imu_estimation.md

@@ -70,43 +70,75 @@ It is registered in `CMakeLists.txt` and runs under `colcon test` as
 A LiDAR localizer must not let a bad IMU integration drag the pose. The guard
 policy (`imu_preintegration_guard_policy.hpp`) enforces that:
 
-- IMU preintegration is used as the registration seed/prediction path; the
-  accepted LiDAR registration measurement remains the published pose anchor;
+- the default-on consistency gate first compares IMU predictions with accepted
+  LiDAR poses without applying them to registration; only the configured number
+  of consecutive translation-and-rotation passes permits the next IMU seed;
+- after permission, IMU preintegration can be used as the registration
+  seed/prediction path while the accepted LiDAR measurement remains the
+  published pose anchor;
 - if the IMU prediction and the NDT correction disagree by more than
   `imu_prediction_correction_guard_translation_m` (2.0 m) or
   `imu_prediction_correction_guard_yaw_deg` (10.0°), or the smoother update is
   non-finite / implausibly large, the node publishes the LiDAR measurement pose
   and resets the IMU smoother to that measurement before continuing;
 - fallback mode is reserved for an already-disabled IMU preintegration state;
 - the guard is itself unit-tested (`test_imu_preintegration_guard_policy.cpp`).
+- finite threshold violations or non-finite predictions immediately revoke
+  seed permission; a temporarily unavailable prediction cannot be used for
+  that scan but does not erase an already established gate
+  (`test_imu_seed_consistency_policy.cpp`).
 
 This is the same design principle as the rest of the stack (see
 [pose_covariance.md](pose_covariance.md), the G3 reinitialization guard): a
 component that feeds itself must not be able to diverge unbounded.
 
 ## Parameters
 
+### Backend and input units
+
 | Parameter | Default | Meaning |
 | --- | --- | --- |
 | `use_imu` | `false` | Enable the legacy IMU correction path |
 | `use_imu_preintegration` | `true` | Use the guarded preintegration smoother from `/imu`, independent of `use_imu` |
 | `imu_preintegration_use_base_frame_transform` | `false` | Transform IMU samples into the base frame before integrating |
+| `imu_accel_scale` | `1.0` | Multiply incoming acceleration before integration; use `9.80665` only for drivers/bags that publish acceleration in `g` rather than m/s² |
+| `imu_dual_queue_enabled` | `true` | LIO-SAM-style optimization/prediction queues with correction-time repropagation and velocity/bias retention; set `false` only for the legacy single-stream A/B baseline |
+
+### Seed safety and correction guards
+
+| Parameter | Default | Meaning |
+| --- | --- | --- |
+| `imu_seed_consistency_gate_enabled` | `true` | Keep IMU prediction diagnostic-only until consecutive LiDAR comparisons pass |
+| `imu_seed_consistency_max_translation_error_m` | `0.5` | Maximum open-loop IMU-vs-LiDAR translation error for one pass |
+| `imu_seed_consistency_max_rotation_error_deg` | `5.0` | Maximum open-loop IMU-vs-LiDAR rotation error for one pass |
+| `imu_seed_consistency_required_consecutive_passes` | `5` | Consecutive passes required before an IMU prediction may seed registration |
+| `imu_prediction_correction_guard_translation_m` | `2.0` | Reset the IMU smoother if IMU-vs-NDT translation disagrees beyond this |
+| `imu_prediction_correction_guard_yaw_deg` | `10.0` | Reset the IMU smoother if IMU-vs-NDT yaw disagrees beyond this |
+| `imu_prediction_correction_guard_warmup_accepts` | `5` | Accepted NDT corrections required before enforcing the correction guard |
+
+### Continuous-time deskew and diagnostics (experimental)
+
+| Parameter | Default | Meaning |
+| --- | --- | --- |
 | `use_continuous_time_deskew` | `false` | Experimental default-off hook that deskews the pre-voxel scan using per-point timing and IMU-predicted relative motion |
 | `continuous_time_deskew_mode` | `relative_motion` | Experimental motion source: `relative_motion`, `imu_pose_history`, or `lidar_constant_velocity` |
 | `continuous_time_cloud_stamp_reference` | `start` | Whether the cloud header is the scan `start` or `end`; Koide Livox uses `start` |
 | `continuous_time_deskew_reference_time_sec` | `0.0` | Scan-relative reference time for the deskewed cloud; `0.0` means earliest point |
 | `continuous_time_pose_history_duration_sec` | `2.0` | Bounded rotation-only IMU pose-history horizon used by `imu_pose_history` |
 | `enable_localizability_guard` | `false` | Experimental XY scan-covariance proxy; suppress the previous-delta seed only below its threshold |
 | `localizability_min_xy_eigen_ratio` | `0.05` | Minimum XY covariance eigenvalue ratio for the proxy guard |
+
+### Preintegration noise and factor model
+
+| Parameter | Default | Meaning |
+| --- | --- | --- |
 | `imu_gyro_noise_density` | `0.01` | rad/s/√Hz |
 | `imu_accel_noise_density` | `0.1` | m/s²/√Hz |
 | `imu_gyro_random_walk` | `0.0001` | rad/s²/√Hz |
 | `imu_accel_random_walk` | `0.001` | m/s³/√Hz |
 | `imu_bias_prior_sigma_gyro` | `0.01` | gyro bias prior σ |
 | `imu_bias_prior_sigma_accel` | `0.1` | accel bias prior σ |
 | `imu_ndt_sigma_z` / `_roll` / `_pitch` | `0.1` / `0.05` / `0.05` | NDT prior σ on the axes IMU constrains most |
-| `imu_prediction_correction_guard_translation_m` | `2.0` | reset IMU smoother if IMU-vs-NDT disagree beyond this |
-| `imu_prediction_correction_guard_yaw_deg` | `10.0` | reset IMU smoother if IMU-vs-NDT yaw disagree beyond this |
 
 Set the noise densities from your IMU datasheet (or an Allan-variance run) for
 best results; the defaults are MEMS-grade ballpark values.

docs/research_driven_development_plan.md

@@ -194,6 +194,19 @@ alignment p95 `<=1.2`とした。全candidateがFAILしたため、既定値はd
 なお全modeでcrash、NaN、再初期化要求は0だった。共有機のCPU使用率が高いため、latency値は
 候補間gateにのみ使い、一般性能claimにはしない。
 
+### R1 IMU/scan時刻同期buffer result (2026-07-14): negative
+
+NDT計算中に未来時刻のIMUがsmootherへ入る可能性を切り分けるため、変換済みIMUをbufferし、
+対象cloud時刻までだけ積分するprototypeを同じ85--112 s区間で実行した。0.65 sのIMU gap後に
+回復する処理を含めても、deskew適用率は`3.8%`、trajectory coverageは`6.3%`、translation
+RMSEは`6.599 m`だった。最初の不正確なIMU seed後にregistrationが連続rejectされ、accepted
+poseからの正しい積分窓が1 sを超えたためである。時刻同期だけでは、初期速度0やbiasを含む
+open-loop seed誤差を解決できない。prototypeはrevertした。測定値と判断は
+[`../experiments/imu_scan_time_sync/README.md`](../experiments/imu_scan_time_sync/README.md)に残す。
+
+次のIMU runtime候補は、初期速度を観測または推定し、NDTへ接続する前のopen-loop seed accuracy
+gateを通すことを前提とする。deskew適用率だけを上げる変更は引き続き採用しない。
+
 ## Phase R2: Localizability診断と退化方向の抑制
 
 仮説: scalar fitnessだけではcorridor aliasや旋回時の弱拘束方向を判別できない。
@@ -325,6 +338,36 @@ Boreasでは閾値sweepを再開する前にdata contractを検証する。
 目安は1人で既存machineを使うengineering effortであり、dataset downloadやidle-machine待ちは
 含まない。各gateで仮説が反証された場合、次phaseへ機械的に進まず本書へnegative resultを追記する。
 
+## IMU seed consistency gate result (2026-07-14)
+
+直接のsmoother state保持やscan時刻queueの試作は、最初の不正確な予測がNDTのfeedback loopへ
+入った後にKoideで大きく悪化した。代わりにopen-loop比較を先行させる。IMU予測と受理済み
+LiDAR poseを比較し、0.5 mかつ5度以内を5回連続で満たすまでseedを禁止する。欠測、非有限値、
+閾値超過または非有限予測ではfail closedする。確立前の欠測は連続列を切るが、確立後の一時的な
+予測欠測は不一致の証拠ではないためpermissionを保持し、そのscanでは予測自体を使用しない。
+
+Koide `outdoor_hard_01a`先頭30秒では、並進誤差中央値0.995 m、回転誤差中央値5.25度、
+最長合格列4/5だった。このため44 alignment rowでIMU seedは0回だった。これは安全gateの
+期待どおりの結果であり、IMU精度改善を意味しない。詳細とmachine-readable値は
+`experiments/imu_seed_consistency/`に保存した。
+
+その後、noise densityの離散化で`density^2 / dt`が欠落していたことと、Koide Livox
+accelerationがm/s²ではなく`g`単位であることを特定した。単位修正、明示scale=9.80665、
+初期速度、状態保持、scan-bounded dual queueを組み合わせた先頭30秒の候補は、並進予測
+中央値0.064 m、回転0.763度、seed 38/47、reject streak 0、coverage 98.0%、並進RMSE
+0.079 mとなった。同じscaleのsingle-stream対照はcoverage 76.0%、並進RMSE 0.242 m、
+reject streak 8だった。詳細は`experiments/imu_dual_queue/`に保存した。
+
+その後、1秒window境界に10 msのtimestamp/scheduler jitter許容を設け（1.1秒の実gapは拒否）、
+IMU coverageが無い受理scanではIMU factorを捏造せずpose-only anchorとしてvelocity/biasを保持した。
+`outdoor_hard_01a`先頭30秒、同bagのoffset 85秒、`outdoor_hard_02a`先頭30秒を各3 repeatした結果、
+dual queueは全9 runでreject streak 0、coverage worst 96.7%以上となった。このmulti-window/
+multi-sequence gateを通過したため`imu_dual_queue_enabled=true`へ昇格した。
+
+dual queueとrelative-motion deskewを組み合わせた追加gateでは、`outdoor_hard_01a`は3/3通過したが、
+`outdoor_hard_02a`のtranslation RMSE中央値がno-deskew 0.095 mから0.146 mへ悪化した。
+したがってdual queue本体のみ採用し、continuous-time deskewはdefault-offを維持する。
+
 ## 直近の着手項目
 
 最初の実装単位はR0である。

experiments/imu_dual_queue/README.md

@@ -0,0 +1,80 @@
+# Scan-bounded dual IMU queue experiment
+
+Date: 2026-07-14
+
+This candidate follows the LIO-SAM separation between an optimization IMU
+queue and a prediction/repropagation queue. It also preserves optimized
+velocity and bias across accepted LiDAR updates, integrates only causal samples
+through the scan timestamp, and keeps the consistency gate in front of the NDT
+seed.
+
+## Bugs found before the passing probe
+
+1. `gyro_noise_density` and `accel_noise_density` are continuous densities in
+   `/sqrt(Hz)`, but the discrete measurement variance omitted division by
+   `dt`. At 200 Hz this made the IMU factor roughly 200 times overconfident.
+2. Koide `outdoor_hard_01a` publishes Livox acceleration in `g`, not m/s².
+   Offline LI-Init-style analysis measured the required scale as `9.80665`.
+3. Resetting velocity to zero at every correction produced metre-scale
+   open-loop errors. The candidate transfers optimized velocity/bias and uses a
+   bounded LiDAR finite-difference velocity only when a reset is unavoidable.
+
+All three issues have analytical or behavioral regression tests. The
+dataset-specific acceleration scale remains an explicit parameter; it is not
+auto-guessed at runtime.
+
+## Same-window result
+
+Dataset: Koide `outdoor_hard_01a`, first 30 seconds. Both IMU modes used
+`imu_accel_scale=9.80665`; only the candidate enabled the dual queue.
+
+| Metric | Current single stream | Dual queue candidate |
+| --- | ---: | ---: |
+| Finite open-loop comparisons | 40 | 43 |
+| Translation prediction median | 0.988 m | 0.064 m |
+| Rotation prediction median | 4.633 deg | 0.763 deg |
+| IMU seed rows | 2/46 | 38/47 |
+| Reject streak max | 8 | 0 |
+| Pose coverage | 22.80/30 s (76.0%) | 29.40/30 s (98.0%) |
+| Translation RMSE | 0.242 m | 0.079 m |
+| Rotation RMSE | 2.486 deg | 1.084 deg |
+| Translation end error | 1.391 m | 0.057 m |
+| Rotation end error | 14.314 deg | 0.354 deg |
+| Alignment latency median | 0.297 s | 0.272 s |
+| Alignment latency p95 | 0.412 s | 0.391 s |
+
+Candidate artifacts are on the external SSD under
+`generated/imu_dual_queue_covariance_probe_20260714`; the single-stream control
+is under `generated/imu_single_queue_covariance_probe_20260714`.
+
+## Decision
+
+The initial probe was followed by three repeats on each of three fixtures.
+
+| Fixture | Candidate median (worst) coverage | Translation RMSE | Rotation RMSE | End translation | End rotation | Reject max | Seed ratio | Latency p95 |
+| --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: |
+| `outdoor_hard_01a`, 0–30 s | 98.0% (98.0%) | 0.085 (0.085) m | 1.134 (1.185) deg | 0.054 (0.057) m | 0.330 (0.350) deg | 0 | 85.4% (84.8%) | 0.323 (0.336) s |
+| `outdoor_hard_01a`, 85–112 s | 96.7% (96.7%) | 0.073 (0.081) m | 0.895 (1.061) deg | 0.031 (0.034) m | 0.517 (0.535) deg | 0 | 69.0% (67.5%) | 0.442 (0.457) s |
+| `outdoor_hard_02a`, 0–30 s | 98.7% (98.3%) | 0.095 (0.096) m | 1.051 (1.141) deg | 0.117 (0.125) m | 1.101 (1.122) deg | 0 | 86.0% (74.5%) | 0.323 (0.375) s |
+
+The remaining nondeterministic failure was traced to a `1.000281 s` scan
+interval being rejected by an exact `1.000000 s` guard. A 10 ms timestamp/
+scheduler tolerance admits this boundary case while still rejecting genuine
+`1.1 s` gaps. Accepted scans without causal IMU coverage now advance through a
+pose-only anchor rather than erasing optimized velocity and bias.
+
+The dual queue passed all nine repeats and is promoted to
+`imu_dual_queue_enabled=true`. The comparison runner still provides
+`imu_preintegration` as the explicit legacy single-stream control.
+
+Continuous-time deskew remains default-off. With the promoted dual queue it
+applied to at least 87.5% of `outdoor_hard_01a` rows and passed three repeats,
+but on `outdoor_hard_02a` its median translation RMSE regressed from `0.095 m`
+to `0.146 m` across three repeats. The `imu_dual_queue_deskew` runner mode is
+retained only for reproducible future experiments.
+
+Repeated artifacts are on the external SSD under `runs/`:
+`imu_jitter_tolerance_ab_20260714`, `imu_jitter_corner_ab_20260714`,
+`imu_jitter_outdoor_hard_02a_ab_20260714`,
+`imu_dual_queue_deskew_ab_20260714`, and
+`imu_dual_queue_deskew_outdoor_hard_02a_20260714`.

experiments/imu_dual_queue/results.json

@@ -0,0 +1,117 @@
+{
+  "date": "2026-07-14",
+  "imu_accel_scale": 9.80665,
+  "repeat_count_per_fixture": 3,
+  "dual_queue_no_deskew": {
+    "outdoor_hard_01a_0_30": {
+      "coverage_ratio_median": 0.979979,
+      "coverage_ratio_worst": 0.979979,
+      "translation_rmse_m_median": 0.084802,
+      "translation_rmse_m_worst": 0.085000,
+      "rotation_rmse_deg_median": 1.134165,
+      "rotation_rmse_deg_worst": 1.185000,
+      "translation_end_error_m_median": 0.054,
+      "translation_end_error_m_worst": 0.057,
+      "rotation_end_error_deg_median": 0.330,
+      "rotation_end_error_deg_worst": 0.350,
+      "reject_streak_max_worst": 0,
+      "imu_seed_ratio_median": 0.85417,
+      "imu_seed_ratio_worst": 0.84783,
+      "alignment_latency_p95_sec_median": 0.323,
+      "alignment_latency_p95_sec_worst": 0.336
+    },
+    "outdoor_hard_01a_85_112": {
+      "coverage_ratio_median": 0.966688,
+      "coverage_ratio_worst": 0.966688,
+      "translation_rmse_m_median": 0.073120,
+      "translation_rmse_m_worst": 0.081406,
+      "rotation_rmse_deg_median": 0.895,
+      "rotation_rmse_deg_worst": 1.060690,
+      "translation_end_error_m_median": 0.031,
+      "translation_end_error_m_worst": 0.034,
+      "rotation_end_error_deg_median": 0.517,
+      "rotation_end_error_deg_worst": 0.535,
+      "reject_streak_max_worst": 0,
+      "imu_seed_ratio_median": 0.69048,
+      "imu_seed_ratio_worst": 0.675,
+      "alignment_latency_p95_sec_median": 0.442,
+      "alignment_latency_p95_sec_worst": 0.457
+    },
+    "outdoor_hard_02a_0_30": {
+      "coverage_ratio_median": 0.986658,
+      "coverage_ratio_worst": 0.983313,
+      "translation_rmse_m_median": 0.094636,
+      "translation_rmse_m_worst": 0.095766,
+      "rotation_rmse_deg_median": 1.050963,
+      "rotation_rmse_deg_worst": 1.140939,
+      "translation_end_error_m_median": 0.117,
+      "translation_end_error_m_worst": 0.125,
+      "rotation_end_error_deg_median": 1.101,
+      "rotation_end_error_deg_worst": 1.122,
+      "reject_streak_max_worst": 0,
+      "imu_seed_ratio_median": 0.86,
+      "imu_seed_ratio_worst": 0.74510,
+      "alignment_latency_p95_sec_median": 0.323,
+      "alignment_latency_p95_sec_worst": 0.375
+    }
+  },
+  "controls": {
+    "outdoor_hard_01a_0_30_lidar_only": {
+      "coverage_ratio_median": 0.75997,
+      "translation_rmse_m_median": 0.336198,
+      "rotation_rmse_deg_median": 3.340312,
+      "reject_streak_max_median": 8
+    },
+    "outdoor_hard_01a_0_30_legacy_single_stream": {
+      "coverage_ratio_median": 0.763311,
+      "translation_rmse_m_median": 0.365202,
+      "rotation_rmse_deg_median": 3.925431,
+      "reject_streak_max_median": 7
+    },
+    "outdoor_hard_02a_0_30_lidar_only_valid_run": {
+      "coverage_ratio": 0.243340,
+      "translation_rmse_m": 0.338117,
+      "rotation_rmse_deg": 3.813385,
+      "reject_streak_max": 31
+    },
+    "outdoor_hard_02a_0_30_legacy_single_stream": {
+      "coverage_ratio": 0.823322,
+      "translation_rmse_m": 0.652040,
+      "rotation_rmse_deg": 4.264858,
+      "reject_streak_max": 6
+    }
+  },
+  "dual_queue_deskew": {
+    "outdoor_hard_01a_0_30": {
+      "coverage_ratio_median": 0.979979,
+      "translation_rmse_m_median": 0.080,
+      "rotation_rmse_deg_median": 0.974,
+      "reject_streak_max_worst": 0,
+      "imu_seed_ratio_worst": 0.80851,
+      "deskew_applied_ratio_median": 0.87755,
+      "deskew_applied_ratio_worst": 0.875,
+      "alignment_latency_p95_sec_worst": 0.363
+    },
+    "outdoor_hard_02a_0_30": {
+      "coverage_ratio_median": 0.983313,
+      "translation_rmse_m_median": 0.145606,
+      "rotation_rmse_deg_median": 1.168211,
+      "reject_streak_max_worst": 0,
+      "imu_seed_ratio_worst": 0.57447,
+      "deskew_applied_ratio_median": 0.81633,
+      "deskew_applied_ratio_worst": 0.76596,
+      "alignment_latency_p95_sec_worst": 0.478
+    }
+  },
+  "decisions": {
+    "imu_dual_queue": "promote_default_on_after_nine_of_nine_multi_window_multi_sequence_runs_pass",
+    "continuous_time_deskew": "keep_default_off_due_to_repeatable_outdoor_hard_02a_accuracy_regression"
+  },
+  "artifact_roots": [
+    "runs/imu_jitter_tolerance_ab_20260714",
+    "runs/imu_jitter_corner_ab_20260714",
+    "runs/imu_jitter_outdoor_hard_02a_ab_20260714",
+    "runs/imu_dual_queue_deskew_ab_20260714",
+    "runs/imu_dual_queue_deskew_outdoor_hard_02a_20260714"
+  ]
+}