[clmazzac]

Problem

CandidateOutputRuntime compared the latest localization estimate with the latest truth odometry. refreshOracle runs every step and recomputes the error with the newest truth. An estimate is always older than that truth, so its latency counted as position error.

Measured with a perfect estimate that arrives 30 ms late, at 3 m/s in a 0.3 rad/s turn, 10 ms step:

	Reported error
main	0.090 m on every step
This PR	0.000 m

The error is about speed × latency. With the 10–40 ms latencies seen from an external filter through the orchestrator, that is 3–12 cm at 3 m/s.

Fix

• The runtime keeps the last 10 s of truth odometry, keyed by header stamp.
• Each estimate is compared with truth at the estimate's header stamp. Between steps, position uses Three.js Vector3.lerpVectors and orientation uses Quaternion.slerp.
• localization.truth holds that matched truth sample, so the overlay's error line joins the estimate to the truth at the same instant.
• An estimate with no stamp is compared with the newest truth, as before. An estimate outside the 10 s history has no error.
• The error adds headingRad and truthCaptureTimeNs. headingRad is the yaw of the rotation from truth to estimate, from a Three.js ZYX Euler, in (-π, π]. Existing fields are unchanged.

Tests

• Checked with a local script against the real CandidateOutputRuntime, outside the repo:
  • a perfect estimate 30 ms late scores 0 m and 0 rad (0.090 m on main)
  • an estimate off by 0.5 m and 0.1 rad, stamped between steps, scores exactly 0.5 m and 0.1 rad
• npm test on a clean checkout: 1964 pass, 6 skipped, 1 fail. The failure is Python decodes retained RGB, depth, and CameraInfo SFLog messages, which also fails on main.
• No unit test is added in this PR.

Not in this PR

Speed error, and NEES (a check of the estimate's covariance against its actual error).

🤖 Generated with Claude Code