fix(benchmark): start the dead-reckoning control from the true initial state (T2-4)

CONSTRAINTS.md

@@ -4,7 +4,7 @@ Invariants this project holds itself to. Every entry is either currently true
 or is a stated blocker; nothing here is aspirational. Ratchets are measured,
 not estimated, and the command to re-measure each one is given.
 
-Last measured: <!-- metric:tests_collected -->1629<!-- /metric --> tests collected, all of which pass except the deliberate
+Last measured: <!-- metric:tests_collected -->1631<!-- /metric --> tests collected, all of which pass except the deliberate
 xfails and the skips that `pytest -rs` lists, and <!-- metric:coverage_percent -->94.3<!-- /metric -->%
 line coverage (<!-- metric:coverage_lines_hit -->10033<!-- /metric -->/<!-- metric:coverage_lines_total -->10638<!-- /metric --> executable lines) on **CPython <!-- metric:coverage_python -->3.13<!-- /metric -->**, measured
 with `python scripts/coverage_report.py`. The figures come from `docs/data/metrics.json`, which `scripts/metrics.py` checks in CI. Coverage is quoted from that script alone:
@@ -91,7 +91,7 @@ the floors themselves are enforced by `scripts/coverage_report.py --ratchet`.
 
 | Ratchet | Floor | Current (CPython 3.13) | Re-measure with |
 |---|---:|---:|---|
-| Tests collected | 405 | <!-- metric:tests_collected -->1629<!-- /metric --> | `pytest`, `scripts/metrics.py --check` |
+| Tests collected | 405 | <!-- metric:tests_collected -->1631<!-- /metric --> | `pytest`, `scripts/metrics.py --check` |
 | Tests passing | 300 | all collected, except the deliberate xfails and the listed skips | `pytest -rs` |
 | Line coverage | 75% | <!-- metric:coverage_percent -->94.3<!-- /metric -->% | `scripts/coverage_report.py`, `scripts/metrics.py --check-coverage` |
 | `io/trajectory.py` coverage | 85% | 95.0% | as above |

CONTRIBUTING.md

@@ -52,7 +52,7 @@ mypy src --ignore-missing-imports
 `ruff format src tests scripts` applies formatting; the `--check` form is the
 gate.
 
-The suite collects <!-- metric:tests_collected -->1629<!-- /metric --> tests and all of them pass on a fresh clone, except the deliberate
+The suite collects <!-- metric:tests_collected -->1631<!-- /metric --> tests and all of them pass on a fresh clone, except the deliberate
 xfails and the skips. `pytest -rs` lists the skips: two need TUM VI reference data that is not
 vendored (see below), and the doc-table check runs once `python scripts/run_benchmark.py` has
 written `results/benchmark.json`. The count and the other numbers the docs quote are in

README.md

@@ -104,7 +104,7 @@ filter starts in the reference frame, so there is no offset for a fitted transfo
 | Scenario | ATE RMSE (m) | Claimed 1σ (m) | Mean NEES (exp. 3) | Coverage @ 2σ | Verdict |
 | --- | ---: | ---: | ---: | ---: | --- |
 | GNSS only (control) | 0.528 | 0.253 | 4.3 | 100.0% | mixed: bulk overconfident, tail underconfident |
-| Dead reckoning (no aiding) | 2.359 | n/a | n/a | n/a | no covariance reported |
+| Dead reckoning (no aiding) | 1.248 | n/a | n/a | n/a | no covariance reported |
 | Anchor as measurement noise (defect) | 1.063 | 0.094 | 163.7 | 16.3% | overconfident |
 | Vision only | 2.320 | 0.158 | 264.2 | 0.7% | overconfident |
 | GNSS denied 5–20 s, vision off (control) | 3.768 | 0.567 | 4.1 | 100.0% | mixed: bulk overconfident, tail underconfident |

ROADMAP.md

@@ -5,7 +5,7 @@ Status markers: `[x]` done, `[~]` in progress, `[ ]` not started, `[!]`
 blocked. Checked items record what was measured when the item closed. They are not kept current: the
 current values are in the README tables, `docs/results.md` and `CONSTRAINTS.md` (B1).
 
-Measured baseline: <!-- metric:tests_collected -->1629<!-- /metric --> tests collected and <!-- metric:coverage_percent -->94.3<!-- /metric -->% line coverage on CPython
+Measured baseline: <!-- metric:tests_collected -->1631<!-- /metric --> tests collected and <!-- metric:coverage_percent -->94.3<!-- /metric -->% line coverage on CPython
 <!-- metric:coverage_python -->3.13<!-- /metric --> (`docs/data/metrics.json`, from `scripts/metrics.py` and `scripts/coverage_report.py`). Every
 number below re-measures against that, not against a remembered value.
 

configs/benchmark.yaml

@@ -27,7 +27,7 @@ defaults:
     # 1 re-commits the anchor pose at every visual update, so each measurement
     # compares consecutive frames. None holds a single anchor for the whole
     # run, which makes the anchor error one unknown shared by every measurement
-    # and measures measurably worse (mean NEES 23 rather than 3).
+    # and measures measurably worse in mean NEES.
     vision_keyframe_interval: 1
     vision_anchor_modelled: true
     rpe_delta_s: 1.0
@@ -53,8 +53,10 @@ cases:
   dead_reckoning:
     name: dead_reckoning
     description: >-
-      Inertial integration with no aiding at all. Reported as the control for
-      "how much of the result is the filter and how much is the motion model".
+      Inertial integration with no aiding at all, started from the true initial
+      state, so its error is drift and not a start offset. Reported as the
+      control for "how much of the result is the filter and how much is the
+      motion model".
     estimator:
       class: DeadReckoning
     scenario:

docs/data/metrics.json

@@ -6,5 +6,5 @@
   "coverage_python": "3.13",
   "outage_visual_window_coverage_percent": 0.0,
   "outage_visual_window_nees": 478.64,
-  "tests_collected": 1629
+  "tests_collected": 1631
 }

docs/results.md

@@ -8,7 +8,7 @@ the strictest of the four conventions the code supports.
 | Scenario | ATE RMSE (m) | Claimed 1σ (m) | Mean NEES (exp. 3) | Coverage @ 2σ | Verdict |
 | --- | ---: | ---: | ---: | ---: | --- |
 | GNSS only (control) | 0.528 | 0.253 | 4.3 | 100.0% | mixed: bulk overconfident, tail underconfident |
-| Dead reckoning (no aiding) | 2.359 | n/a | n/a | n/a | no covariance reported |
+| Dead reckoning (no aiding) | 1.248 | n/a | n/a | n/a | no covariance reported |
 | Anchor as measurement noise (defect) | 1.063 | 0.094 | 163.7 | 16.3% | overconfident |
 | Vision only | 2.320 | 0.158 | 264.2 | 0.7% | overconfident |
 | GNSS denied 5–20 s, vision off (control) | 3.768 | 0.567 | 4.1 | 100.0% | mixed: bulk overconfident, tail underconfident |

docs/scenarios.md

@@ -16,7 +16,7 @@ GNSS at 5 Hz, no outage, no vision. The reference every other case is read again
 
 ## dead_reckoning
 
-Inertial integration with no aiding at all. Reported as the control for "how much of the result is the filter and how much is the motion model".
+Inertial integration with no aiding at all, started from the true initial state, so its error is drift and not a start offset. Reported as the control for "how much of the result is the filter and how much is the motion model".
 
 **Configuration:** GNSS off; vision off; estimator DeadReckoning.
 

src/navkit/benchmark.py

@@ -56,7 +56,7 @@
 from .degrade.config import Scenario, scenario_from_dict
 from .degrade.inject import config_hash, inject
 from .estimators.dead_reckoning import DeadReckoning
-from .estimators.eskf import VISION_NOISE_PRESETS, ErrorStateKalmanFilter, EskfConfig
+from .estimators.eskf import VISION_NOISE_PRESETS, ErrorStateKalmanFilter, EskfConfig, InitialState
 from .eval.calibration import (
     normalized_error_squared,
     score_mask,
@@ -277,7 +277,10 @@ def run_case(
     # confidently wrong table.
     estimator_name = keys.get("class", "ErrorStateKalmanFilter")
     if estimator_name == "DeadReckoning":
-        result = DeadReckoning().run(imu)
+        # The control starts from the true initial state, so its error is drift and not a start offset. The ESKF
+        # cases keep their declared prior: that is part of what they measure.
+        start = InitialState(R=reference.poses[0, :3, :3], p=reference.poses[0, :3, 3])
+        result = DeadReckoning().run(imu, initial=start)
     elif estimator_name == "ErrorStateKalmanFilter":
         result = ErrorStateKalmanFilter(cfg).run(imu, gnss=gnss, vision=vision)
     else:

src/navkit/estimators/dead_reckoning.py

@@ -22,13 +22,22 @@
 
 import time
 from dataclasses import dataclass, field
+from typing import Protocol
 
 import numpy as np
 
 from ..geometry.rigid import rot_exp
 from ..types import GRAVITY, ImuSample, Trajectory
 
 
+class InitialPose(Protocol):
+    """What a start needs: attitude, position and velocity. :class:`~navkit.estimators.eskf.InitialState` has them."""
+
+    R: np.ndarray
+    p: np.ndarray
+    v: np.ndarray
+
+
 @dataclass
 class EstimatorResult:
     """Output of any estimator: the estimated trajectory plus run metadata."""
@@ -64,7 +73,20 @@ class DeadReckoning:
     def __init__(self, gravity: np.ndarray | None = None) -> None:
         self.g = GRAVITY.copy() if gravity is None else np.asarray(gravity, float)
 
-    def run(self, imu: ImuSample, t0: float | None = None, t_end: float | None = None) -> EstimatorResult:
+    def run(
+        self,
+        imu: ImuSample,
+        t0: float | None = None,
+        t_end: float | None = None,
+        initial: InitialPose | None = None,
+    ) -> EstimatorResult:
+        """Integrate the inertial stream.
+
+        ``initial`` is where the run starts: attitude, position and velocity, as for the filter. The default is the
+        identity pose at rest at the origin. A scene whose first pose is elsewhere needs the true start passed in,
+        or the error mixes that offset with the drift. The biases in ``initial`` are not used: this integrator has no
+        bias state.
+        """
         t_start = time.perf_counter()
         t = imu.t
         if t_end is not None:
@@ -78,9 +100,9 @@ def run(self, imu: ImuSample, t0: float | None = None, t_end: float | None = Non
             raise ValueError("dead reckoning needs at least 2 IMU samples")
 
         poses = np.zeros((n, 4, 4))
-        R = np.eye(3)
-        p = np.zeros(3)
-        v = np.zeros(3)
+        R = np.eye(3) if initial is None else initial.R.copy()
+        p = np.zeros(3) if initial is None else initial.p.copy()
+        v = np.zeros(3) if initial is None else initial.v.copy()
         poses[0, :3, :3] = R
         poses[0, :3, 3] = p
         poses[0, 3, 3] = 1.0

tests/golden/benchmark.json

@@ -12,17 +12,17 @@
      ],
      "position_m": {
       "count": 6001.0,
-      "max": 3.4388039753646056,
-      "mean": 2.307140419411952,
-      "median": 2.080402196974764,
-      "min": 1.8674732534416745,
-      "rmse": 2.358836188063927,
-      "std": 0.49113261674989667
+      "max": 2.594729331065698,
+      "mean": 0.9266604881442663,
+      "median": 0.7032021184509726,
+      "min": 0.0,
+      "rmse": 1.247834265616023,
+      "std": 0.8356977289413378
      },
      "position_percentiles_m": {
-      "p50": 2.080402196974764,
-      "p95": 3.287195961038279,
-      "p99": 3.4083390143190964
+      "p50": 0.7032021184509726,
+      "p95": 2.412046100159873,
+      "p99": 2.557794003964035
      },
      "rotation_deg": {
       "count": 6001.0,
@@ -45,17 +45,17 @@
      ],
      "position_m": {
       "count": 6001.0,
-      "max": 1.722330767093535,
-      "mean": 0.7394499521527442,
-      "median": 0.7311195296359095,
-      "min": 0.016279826200096104,
-      "rmse": 0.8383763243440048,
-      "std": 0.3950805354378627
+      "max": 1.7223307670935317,
+      "mean": 0.7394499521527298,
+      "median": 0.7311195296358778,
+      "min": 0.01627982620010059,
+      "rmse": 0.8383763243439899,
+      "std": 0.39508053543785787
      },
      "position_percentiles_m": {
-      "p50": 0.7311195296359095,
-      "p95": 1.5049818809627311,
-      "p99": 1.67809738535761
+      "p50": 0.7311195296358778,
+      "p95": 1.5049818809627282,
+      "p99": 1.678097385357605
      },
      "rotation_deg": {
       "count": 6001.0,
@@ -78,17 +78,17 @@
      ],
      "position_m": {
       "count": 6001.0,
-      "max": 2.552200901226419,
-      "mean": 0.9035545718053405,
-      "median": 0.6772679239751968,
-      "min": 0.008720228530675805,
-      "rmse": 1.2308984691972957,
-      "std": 0.8358828729205509
+      "max": 2.552200901226397,
+      "mean": 0.9035545718053167,
+      "median": 0.6772679239751673,
+      "min": 0.008720228530676001,
+      "rmse": 1.2308984691972709,
+      "std": 0.8358828729205399
      },
      "position_percentiles_m": {
-      "p50": 0.6772679239751968,
-      "p95": 2.3719015800748964,
-      "p99": 2.5156397046535406
+      "p50": 0.6772679239751673,
+      "p95": 2.3719015800748844,
+      "p99": 2.51563970465352
      },
      "rotation_deg": {
       "count": 6001.0,
@@ -111,17 +111,17 @@
      ],
      "position_m": {
       "count": 6001.0,
-      "max": 1.7556854496889616,
-      "mean": 0.6979294462974219,
-      "median": 0.6679252472587413,
-      "min": 0.07626052510686518,
-      "rmse": 0.8282675249877637,
-      "std": 0.44600625661567767
+      "max": 1.7556854496889562,
+      "mean": 0.6979294462974107,
+      "median": 0.6679252472587202,
+      "min": 0.07626052510686754,
+      "rmse": 0.8282675249877497,
+      "std": 0.4460062566156692
      },
      "position_percentiles_m": {
-      "p50": 0.6679252472587413,
-      "p95": 1.5746735293544103,
-      "p99": 1.7199213221564336
+      "p50": 0.6679252472587202,
+      "p95": 1.574673529354404,
+      "p99": 1.7199213221564271
      },
      "rotation_deg": {
       "count": 6001.0,
@@ -132,28 +132,28 @@
       "rmse": 0.04678473254427496,
       "std": 0.015842861334198828
      },
-     "scale": 0.9612331151882978
+     "scale": 0.9612331151882995
     }
    },
    "claim_type": "MEASUREMENT",
    "config_hash": "94a94d13fc6fcf87",
    "data_class": "synthetic",
-   "description": "Inertial integration with no aiding at all. Reported as the control for \"how much of the result is the filter and how much is the motion model\".",
+   "description": "Inertial integration with no aiding at all, started from the true initial state, so its error is drift and not a start offset. Reported as the control for \"how much of the result is the filter and how much is the motion model\".",
    "drift": {
     "duration_s": 30.0,
-    "final_drift_pct_path_length": 10.042306206384247,
-    "final_drift_pct_time": 11.462679917882019,
+    "final_drift_pct_path_length": 7.577363133205508,
+    "final_drift_pct_time": 8.649097770218994,
     "growth": {
-     "intercept_m": 0.6091892884008449,
-     "r_squared": 0.9957236079731048,
-     "slope_m_per_s": 0.09312605493188454,
+     "intercept_m": -1.2267807565318194,
+     "r_squared": 0.9998629244542112,
+     "slope_m_per_s": 0.12787061309613057,
      "window_s": [
       15.0,
       30.0
      ]
     },
-    "max_drift_pct_path_length": 9126380.702733766,
-    "max_drift_pct_time": 37736.0133459977,
+    "max_drift_pct_path_length": 50.897399222500816,
+    "max_drift_pct_time": 8.649097770218994,
     "total_path_length_m": 34.2431698923743
    },
    "estimator": {
@@ -208,9 +208,9 @@
    },
    "headline": {
     "alignment": "none",
-    "ate_median_m": 2.080402196974764,
-    "ate_p95_m": 3.287195961038279,
-    "ate_rmse_m": 2.358836188063927,
+    "ate_median_m": 0.7032021184509726,
+    "ate_p95_m": 2.412046100159873,
+    "ate_rmse_m": 1.247834265616023,
     "rotation_rmse_deg": 0.04678473254427496
    },
    "manifest": {
@@ -278,12 +278,12 @@
     },
     "translation_m": {
      "count": 5801.0,
-     "max": 0.1347054038363281,
-     "mean": 0.08939891121728478,
-     "median": 0.11487537390280449,
-     "min": 0.0016173357971738626,
-     "rmse": 0.10051160959177144,
-     "std": 0.04593928967553499
+     "max": 0.1347054038363282,
+     "mean": 0.08939891121728345,
+     "median": 0.11487537390280465,
+     "min": 0.0016173357971723255,
+     "rmse": 0.1005116095917705,
+     "std": 0.04593928967553547
     }
    },
    "scenario": {
@@ -332,7 +332,7 @@
    "summary": {
     "final_speed_m_s": 1.1908382151065695,
     "name": "dead_reckoning",
-    "path_length_m": 34.04669886639325,
+    "path_length_m": 34.04669886639324,
     "poses": 6001,
     "reports_covariance": false,
     "trajectory_duration_s": 30.0
@@ -9081,7 +9081,7 @@
  },
  "claim_type": "MEASUREMENT",
  "config_file": "configs/benchmark.yaml",
- "config_sha256": "20b4e7a1ed97bdf6deb4977543be7dd33974bc9c0fe351de84809ea16e79e3cc",
+ "config_sha256": "d6f88bf7acfc4bc263df86510fa8e3710b32f78d64b0d5b1cf6cef02b7eafe4c",
  "data_class": "synthetic",
  "disclaimer": "All numbers are from a known-answer synthetic fixture. They are not field measurements and must not be presented as real-sensor performance.",
  "schema": "navkit-benchmark/1"