From 32da10bb7b14c0db4490ba333489d97cd81a3728 Mon Sep 17 00:00:00 2001
From: Breelyn Styler <breelyn.styler@picknik.ai>
Date: Tue, 28 Jul 2026 19:06:11 -0400
Subject: [PATCH 1/2] feat(hangar_sim): fuse odometry localization for the
 mobile base

Give hangar_sim's mobile base a realistic localization stack so beluga_amcl is
exercised against fuse-fused odometry (wheel + IMU) with real drift, including
where the scene is degenerate for scan matching (the smooth fuselage, the
unmapped picking boxes).

- Fuse on by default (use_fuse=true) -> /odom_filtered; odom_world_drift injects
  a live odom->world from the fuse estimate so AMCL sees drift to correct while
  world->base stays ground truth for whole-body planning.
- slip_aware_odom (new C++ node): grows wheel-yaw covariance during sustained
  in-place spin (mecanum roller slip) so fuse defers to the IMU while spinning
  and trusts the wheels driving straight; also re-anchors odom across the
  whole-body<->nav controller handoff.
- AMCL tuning: OmniMotionModel; alpha1 0.1->0.4; update_min_a 0.1 with
  resample_interval 3; likelihood relaxed (sigma_hit 0.1->0.25, z_hit 0.9->0.65,
  z_rand 0.1->0.3) so unmapped boxes read as outliers instead of yanking the pose.
- fuse lag_duration 0.5->0.3: shorter smoother window cuts output latency
  (moving-yaw 0.70->0.465deg) while still smoothing transient spikes.
- amcl_odom_gate (new C++ node): sole map->odom publisher (AMCL tf_broadcast
  false). Holds the last good map->odom and coasts on fuse odom where AMCL is
  degenerate, blending back when trustworthy. A large correction is accepted
  only if it persists over a sliding window (gated on position AND yaw) with
  particle-spread hysteresis; spread_accept_max additionally rejects a
  confident-but-WRONG lock (which persists yet stays spread) so a scan-slide
  divergence is coasted through while a real recovery (which converges) is still
  adopted. latency_compensation_sec composes the correction with odom->base from
  one estimator-lag ago (referenced to the cloud stamp), forward-projecting it
  with real buffered odometry -> moving-pose error 13.8->8.2cm, moving-yaw
  1.2->0.9deg, no overshoot on turns; 0 disables, hangar_sim uses 0.30. The pure
  decision logic (detail::updateGate, interpolateOdom, appendOdomSample) has no
  ROS/TF deps and is fully unit-tested.
- Removed the per-objective SetInitialPose reseed from the clicked-point
  Objectives (superseded by slip_aware_odom re-anchoring; the unconditional
  reseed could cement a drifting estimate).
- wz_max 0.6 (below the velocity_smoother cap so it binds) keeps spins within
  AMCL's correction bandwidth; odom_rate 50Hz / tf_publish_rate 30Hz and the
  broadcaster rates decoupled from the control loop give the stack CPU headroom.

Closes #19667.

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
---
 src/hangar_sim/CMakeLists.txt                 |  36 ++
 .../control/picknik_ur.ros2_control.yaml      |   2 +
 src/hangar_sim/config/fuse/fuse.yaml          |   6 +-
 .../picknik_ur_mujoco_ros2_control.xacro      |   4 +-
 src/hangar_sim/description/ur5e_ridgeback.xml |   4 +
 .../hangar_sim/amcl_odom_gate_logic.hpp       | 186 ++++++
 .../hangar_sim/odom_world_drift_logic.hpp     |  76 +++
 src/hangar_sim/include/hangar_sim/se2.hpp     |  61 ++
 .../robot_drivers_to_persist_sim.launch.py    | 132 +++-
 .../objectives/navigate_to_clicked_point.xml  |   1 -
 ...igate_to_clicked_point_with_replanning.xml |   1 -
 src/hangar_sim/package.xml                    |   9 +-
 src/hangar_sim/params/nav2_params.yaml        |  46 +-
 src/hangar_sim/src/amcl_odom_gate.cpp         | 348 +++++++++++
 src/hangar_sim/src/amcl_odom_gate_logic.cpp   | 113 ++++
 src/hangar_sim/src/odom_world_drift.cpp       | 174 ++++++
 src/hangar_sim/src/slip_aware_odom.cpp        | 206 +++++++
 src/hangar_sim/test/test_amcl_odom_gate.cpp   | 583 ++++++++++++++++++
 src/hangar_sim/test/test_odom_world_drift.cpp | 104 ++++
 19 files changed, 2058 insertions(+), 34 deletions(-)
 create mode 100644 src/hangar_sim/include/hangar_sim/amcl_odom_gate_logic.hpp
 create mode 100644 src/hangar_sim/include/hangar_sim/odom_world_drift_logic.hpp
 create mode 100644 src/hangar_sim/include/hangar_sim/se2.hpp
 create mode 100644 src/hangar_sim/src/amcl_odom_gate.cpp
 create mode 100644 src/hangar_sim/src/amcl_odom_gate_logic.cpp
 create mode 100644 src/hangar_sim/src/odom_world_drift.cpp
 create mode 100644 src/hangar_sim/src/slip_aware_odom.cpp
 create mode 100644 src/hangar_sim/test/test_amcl_odom_gate.cpp
 create mode 100644 src/hangar_sim/test/test_odom_world_drift.cpp

diff --git a/src/hangar_sim/CMakeLists.txt b/src/hangar_sim/CMakeLists.txt
index 0d49770e5..c055f0155 100644
--- a/src/hangar_sim/CMakeLists.txt
+++ b/src/hangar_sim/CMakeLists.txt
@@ -3,6 +3,31 @@ project(hangar_sim)
 
 find_package(ament_cmake REQUIRED)
 find_package(picknik_accessories REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+add_executable(slip_aware_odom src/slip_aware_odom.cpp)
+target_include_directories(slip_aware_odom PRIVATE include)
+target_compile_features(slip_aware_odom PRIVATE cxx_std_20)
+ament_target_dependencies(slip_aware_odom rclcpp nav_msgs tf2 tf2_geometry_msgs)
+
+add_executable(amcl_odom_gate src/amcl_odom_gate.cpp src/amcl_odom_gate_logic.cpp)
+target_include_directories(amcl_odom_gate PRIVATE include)
+target_compile_features(amcl_odom_gate PRIVATE cxx_std_20)
+ament_target_dependencies(amcl_odom_gate rclcpp geometry_msgs tf2 tf2_geometry_msgs tf2_ros)
+
+add_executable(odom_world_drift src/odom_world_drift.cpp)
+target_include_directories(odom_world_drift PRIVATE include)
+target_compile_features(odom_world_drift PRIVATE cxx_std_20)
+ament_target_dependencies(odom_world_drift rclcpp nav_msgs sensor_msgs geometry_msgs tf2 tf2_geometry_msgs
+                          tf2_ros)
+
+install(TARGETS slip_aware_odom amcl_odom_gate odom_world_drift DESTINATION lib/${PROJECT_NAME})
 
 install(
   DIRECTORY
@@ -47,6 +72,17 @@ if(BUILD_TESTING)
           ENV MOVEIT_CONFIG_PACKAGE=hangar_sim
               MOVEIT_HOST_USER_WORKSPACE=${CMAKE_SOURCE_DIR}
               ROS_LOG_DIR=${CMAKE_CURRENT_BINARY_DIR}/test_results/${PROJECT_NAME}/ros_logs)
+
+  # Unit tests for the pure amcl_odom_gate decision logic (no ROS/TF).
+  find_package(ament_cmake_gmock REQUIRED)
+  ament_add_gmock(test_amcl_odom_gate test/test_amcl_odom_gate.cpp src/amcl_odom_gate_logic.cpp)
+  target_include_directories(test_amcl_odom_gate PRIVATE include)
+  target_compile_features(test_amcl_odom_gate PRIVATE cxx_std_20)
+
+  # Unit tests for the pure odom_world_drift rail-joint index resolver (no ROS).
+  ament_add_gmock(test_odom_world_drift test/test_odom_world_drift.cpp)
+  target_include_directories(test_odom_world_drift PRIVATE include)
+  target_compile_features(test_odom_world_drift PRIVATE cxx_std_20)
 endif()
 
 ament_package()
diff --git a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml
index ab6e0afb3..a6446f8a1 100644
--- a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml
+++ b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml
@@ -160,6 +160,7 @@ platform_velocity_controller_nav2:
 joint_state_broadcaster:
   ros__parameters:
     use_local_topics: false
+    update_rate: 50  # publish /joint_states at 50 Hz, not the 600 Hz control loop
     joints:
       - shoulder_pan_joint
       - shoulder_lift_joint
@@ -297,6 +298,7 @@ force_torque_sensor_broadcaster:
 
 imu_sensor_broadcaster:
   ros__parameters:
+    update_rate: 100  # 100 Hz IMU is plenty for fuse (optimizes at 10 Hz); was inheriting the 600 Hz control loop
     sensor_name: imu_site
     frame_id: ridgeback_base_link
     # Static covariance values (row-major 3x3 matrices)
diff --git a/src/hangar_sim/config/fuse/fuse.yaml b/src/hangar_sim/config/fuse/fuse.yaml
index 4ad1039f5..22f5135b6 100644
--- a/src/hangar_sim/config/fuse/fuse.yaml
+++ b/src/hangar_sim/config/fuse/fuse.yaml
@@ -3,9 +3,9 @@
 state_estimator:
   ros__parameters:
     # Fixed-lag smoother configuration
-    optimization_frequency: 20.0
+    optimization_frequency: 10.0  # match publish_frequency (10 Hz); optimizing 2x faster than we publish was wasted work
     transaction_timeout: 0.01
-    lag_duration: 0.5
+    lag_duration: 0.3  # chosen: median 0.465deg (<0.5 goal), clean transient tail vs 0.25
 
     # Motion model for mobile base (3D omnidirectional)
     motion_models:
@@ -51,7 +51,7 @@ state_estimator:
     # only the relative change between consecutive messages, preventing
     # accumulated drift from corrupting the estimate.
     wheel_odom_sensor:
-      topic: /platform_velocity_controller_nav2/odom
+      topic: /odom_slip_aware  # slip_aware_odom republishes with spin-aware yaw covariance
       queue_size: 10
       pose_loss:
         type: fuse_loss::HuberLoss
diff --git a/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro b/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro
index 42241108e..05ba84c3f 100644
--- a/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro
+++ b/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro
@@ -31,11 +31,11 @@
           <param name="mujoco_model">${mujoco_model}</param>
           <param name="mujoco_model_package">hangar_sim</param>
           <param name="render_publish_rate">20</param>
-          <param name="tf_publish_rate">60</param>
+          <param name="tf_publish_rate">30</param>
           <param name="lidar_publish_rate">10</param>
           <param name="publish_odom">${publish_odom}</param>
           <param name="odom_child_frame">ridgeback_base_link</param>
-          <param name="odom_rate">150</param>
+          <param name="odom_rate">50</param>
           <!-- Zero z/roll/pitch in /odom for this planar base. The previous tag here,
                odom_zero_z, is not a recognized odom publisher parameter and was silently
                ignored; odom_planar is the real one. -->
diff --git a/src/hangar_sim/description/ur5e_ridgeback.xml b/src/hangar_sim/description/ur5e_ridgeback.xml
index 2c18a2d13..5fe0538e3 100644
--- a/src/hangar_sim/description/ur5e_ridgeback.xml
+++ b/src/hangar_sim/description/ur5e_ridgeback.xml
@@ -313,6 +313,10 @@
             conaffinity="0"
             group="2"
           />
+          <!-- Ground-truth base pose for localization testing and QA. MujocoSystem auto-publishes
+               every site as TF mj_world -> <name> at its true world pose, independent of
+               fuse/AMCL, so it can be compared against the estimate. Sites add no DOF. -->
+          <site name="base_gt" pos="0 0 0" size="0.01" rgba="1 0 0 1" />
           <!-- Front SICK TIM571: mounted at x=+0.45m (front bumper), z=0.15m.
                91 beams x 3 deg = 270 deg FOV, sweeping CCW from beam-0 at -135 deg in robot frame.
                Quat rule: site Z axis = beam-0 firing direction. Site X axis must point in world -Z
diff --git a/src/hangar_sim/include/hangar_sim/amcl_odom_gate_logic.hpp b/src/hangar_sim/include/hangar_sim/amcl_odom_gate_logic.hpp
new file mode 100644
index 000000000..8fe3bfb28
--- /dev/null
+++ b/src/hangar_sim/include/hangar_sim/amcl_odom_gate_logic.hpp
@@ -0,0 +1,186 @@
+// Copyright 2026 PickNik Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the PickNik Inc. nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+// Pure decision logic for the degeneracy-aware map->odom gate. No ROS, no TF, so it
+// is unit-testable in isolation (see test/test_amcl_odom_gate.cpp). The node
+// (src/amcl_odom_gate.cpp) does the ROS I/O and calls detail::updateGate.
+//
+// A large innovation in AMCL's implied map->odom is neither hard-accepted nor
+// hard-rejected: it goes PROVISIONAL and is accepted only if it PERSISTS at the same
+// place for persist_time (a valid correction persists; an ambiguity teleport
+// thrashes). Small innovations track AMCL transparently. Particle spread is a
+// second, independent hold trigger (hysteresis) for the self-reported-lost case.
+
+#include <algorithm>
+#include <cmath>
+#include <numbers>
+#include <optional>
+#include <vector>
+
+#include "hangar_sim/se2.hpp"
+
+namespace amcl_odom_gate
+{
+// SE(2) primitives are shared with slip_aware_odom (se2.hpp); alias them in so the logic below and
+// the tests keep referring to Pose2/compose/invert unqualified.
+using hangar_sim::se2::compose;
+using hangar_sim::se2::invert;
+using hangar_sim::se2::Pose2;
+// Fraction alpha of the way from a to b, wrapping the yaw through the short arc.
+[[nodiscard]] inline Pose2 blend(const Pose2& a, const Pose2& b, double alpha)
+{
+  const double dyaw = std::remainder(b.yaw - a.yaw, 2.0 * std::numbers::pi);
+  return { a.x + alpha * (b.x - a.x), a.y + alpha * (b.y - a.y), a.yaw + alpha * dyaw };
+}
+[[nodiscard]] inline double planarDist(const Pose2& a, const Pose2& b)
+{
+  return std::hypot(a.x - b.x, a.y - b.y);
+}
+// Absolute yaw difference through the short arc [0, pi].
+[[nodiscard]] inline double yawDist(const Pose2& a, const Pose2& b)
+{
+  return std::abs(std::remainder(a.yaw - b.yaw, 2.0 * std::numbers::pi));
+}
+
+// A timestamped odom->base sample, used to reconstruct where odom->base was at a past
+// instant so a latency-lagged AMCL correction can be composed at the CONTENT time, not now.
+struct OdomSample
+{
+  double t;    // seconds
+  Pose2 pose;  // odom->base at t
+};
+
+// Append `s` to a time-ascending odom buffer, then drop samples older than keep_window_sec behind
+// the newest. Ignores a sample that is not strictly newer than the current back, so the buffer
+// stays strictly ascending (which keeps interpolateOdom's span positive). If a sample arrives more
+// than one window OLDER than the back, the clock jumped backward (sim reset / bag loop) -- the
+// strictly-newer rule would otherwise freeze the buffer stale forever, so flush and re-seed from it.
+// Pure -- unit-tested.
+inline void appendOdomSample(std::vector<OdomSample>& buf, const OdomSample& s, double keep_window_sec)
+{
+  if (!buf.empty())
+  {
+    if (s.t < buf.back().t - keep_window_sec)
+    {
+      buf.clear();  // backward time jump: discard the now-unreachable pre-jump samples and re-seed.
+    }
+    else if (s.t <= buf.back().t)
+    {
+      return;  // not strictly newer (duplicate / near-stale replay): keep the buffer ascending.
+    }
+  }
+  buf.push_back(s);
+  const double cutoff = s.t - keep_window_sec;
+  while (buf.size() > 1 && buf.front().t < cutoff)
+  {
+    buf.erase(buf.begin());
+  }
+}
+
+// Linear interpolation of odom->base at `target` [s] from a time-ordered (ascending t) buffer.
+// Clamps to the nearest end if `target` is outside [front, back] (e.g. before the buffer fills).
+// nullopt only if the buffer is empty. Pure -- unit-tested in isolation.
+[[nodiscard]] inline std::optional<Pose2> interpolateOdom(const std::vector<OdomSample>& buf, double target)
+{
+  if (buf.empty())
+  {
+    return std::nullopt;
+  }
+  if (target <= buf.front().t)
+  {
+    return buf.front().pose;
+  }
+  if (target >= buf.back().t)
+  {
+    return buf.back().pose;
+  }
+  for (size_t i = 1; i < buf.size(); ++i)
+  {
+    if (buf[i].t >= target)
+    {
+      const double span = buf[i].t - buf[i - 1].t;
+      // span is always positive here (the front/back clamps above mean buf[i-1].t < target <=
+      // buf[i].t at this point), so the guard is defensive against a hand-built zero-span buffer,
+      // not something reachable through appendOdomSample (which keeps stamps strictly ascending).
+      const double alpha = (span > 1e-9) ? (target - buf[i - 1].t) / span : 0.0;
+      return blend(buf[i - 1].pose, buf[i].pose, alpha);
+    }
+  }
+  return buf.back().pose;  // unreachable given the guards above, but keeps the compiler happy.
+}
+
+// Tunables for the gate decision.
+struct GateParams
+{
+  double spread_hold = 1.5;               // spread [m] above which AMCL is held as untrustworthy
+  double spread_resume = 0.6;             // resume below this (< spread_hold): hysteresis, no flicker
+  double jump_hold = 0.5;                 // position innovation [m] above which a candidate goes provisional
+  double jump_hold_yaw = 0.35;            // yaw innovation [rad, ~20 deg] above which a candidate goes provisional
+  double provisional_tol = 0.3;           // position drift [m] within which successive candidates are the same target
+  double provisional_tol_yaw = 0.2;       // yaw drift [rad] within which successive candidates are the same target
+  double persist_time = 1.0;              // a provisional target must persist this long [s] to be accepted
+  double spread_accept_max = 3.0;         // spread [m] above which a persisted correction is NEVER accepted
+                                          // (keep above spread_hold; default is 2x it). Persistence distinguishes
+                                          // a real correction from a thrashing ambiguity, but a confident-WRONG
+                                          // lock (e.g. plane scan ambiguity) also persists. A legitimate
+                                          // correction converges so its spread drops below this; a divergence
+                                          // stays wide, so the gate keeps coasting on odom instead of adopting it.
+                                          // Guards POSITIONAL trust only -- heading multimodality is caught by
+                                          // jump_hold_yaw + persistence, not by (positional) spread.
+  double latency_compensation_sec = 0.0;  // AMCL estimate lag [s]. beluga's cloud content trails the
+                                          // true pose by ~this long (its stamp under-reports the lag), so
+                                          // during motion the correction is stale by speed*lag. Compose the
+                                          // mean with odom->base from (cloud stamp - this), not the latest, so
+                                          // map->odom is consistent at the CONTENT time and the live odom
+                                          // carries it to now. 0 disables (compose at latest); tune to the
+                                          // measured lag (relative to the cloud stamp).
+  double alpha_slew = 0.05;               // max change in follow fraction per update (smooth ramp)
+};
+
+namespace detail
+{
+// Mutable state carried between updates.
+struct GateState
+{
+  Pose2 held{ 0.0, 0.0, 0.0 };  // current broadcast map->odom
+  double alpha = 1.0;           // follow fraction: 1 transparent, 0 held
+  bool have_held = false;
+  bool spread_holding = false;         // spread hysteresis latch
+  Pose2 provisional{ 0.0, 0.0, 0.0 };  // candidate being watched for persistence
+  double provisional_since = 0.0;      // time the current provisional target began persisting [s]
+  bool have_provisional = false;
+};
+
+// Pure decision. Given AMCL's implied candidate map->odom, the particle spread, the
+// current time, params, and mutable state, update and return the map->odom to
+// broadcast. No ROS, no TF -- unit-tested in test/test_amcl_odom_gate.cpp.
+[[nodiscard]] Pose2 updateGate(const Pose2& candidate, double spread, double now, const GateParams& p, GateState& s);
+}  // namespace detail
+}  // namespace amcl_odom_gate
diff --git a/src/hangar_sim/include/hangar_sim/odom_world_drift_logic.hpp b/src/hangar_sim/include/hangar_sim/odom_world_drift_logic.hpp
new file mode 100644
index 000000000..1714b4382
--- /dev/null
+++ b/src/hangar_sim/include/hangar_sim/odom_world_drift_logic.hpp
@@ -0,0 +1,76 @@
+// Copyright 2026 PickNik Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the PickNik Inc. nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+// Pure /joint_states index-resolution for odom_world_drift, split out so its branches
+// (all present / reordered / joint absent / duplicate name) are unit-testable without ROS.
+// In an internal namespace: NOT part of any supported API (see .claude/rules/cpp-style.md).
+
+#include <algorithm>
+#include <array>
+#include <cstddef>
+#include <optional>
+#include <string>
+#include <string_view>
+#include <vector>
+
+namespace odom_world_drift::internal
+{
+// Virtual-rail joints encoding the ground-truth base pose in `world`, in (x, y, yaw) order.
+//
+// Reading these three values as world->base = Pose2{x, y, yaw} is exact only under the sim's rail
+// kinematics: the chain is prismatic-x -> prismatic-y -> revolute-z with the revolute joint
+// base-most, so the prismatic values are un-rotated world translations and world->base reduces to
+// [R(yaw), (x, y)]. It also assumes ridgeback_base_link IS the revolute joint's child frame with no
+// intervening fixed offset -- the predecessor read world->base via a full SE(3) TF lookup and so was
+// robust to such an offset; this direct read is not. A URDF change that reorders the chain or inserts
+// a carriage offset would silently bias world->base (and thus map->odom). Both hold in the shipped
+// hangar_sim URDF; this note is here so a future edit that breaks either fails loudly in review.
+inline constexpr std::array<std::string_view, 3> kRailJoints = { "linear_x_joint", "linear_y_joint",
+                                                                 "rotational_yaw_joint" };
+
+// Resolve the indices of kRailJoints within a /joint_states name list, in kRailJoints order.
+// Returns nullopt if any rail joint is absent -- /joint_states is multi-publisher, so a given
+// message may omit them entirely. On a duplicate name the first occurrence wins.
+[[nodiscard]] inline std::optional<std::array<std::size_t, kRailJoints.size()>>
+resolveRailIndices(const std::vector<std::string>& names)
+{
+  std::array<std::size_t, kRailJoints.size()> out{};
+  for (std::size_t j = 0; j < kRailJoints.size(); ++j)
+  {
+    const auto it = std::ranges::find(names, kRailJoints[j]);
+    if (it == names.end())
+    {
+      return std::nullopt;
+    }
+    out[j] = static_cast<std::size_t>(std::distance(names.begin(), it));
+  }
+  return out;
+}
+}  // namespace odom_world_drift::internal
diff --git a/src/hangar_sim/include/hangar_sim/se2.hpp b/src/hangar_sim/include/hangar_sim/se2.hpp
new file mode 100644
index 000000000..9ee729a69
--- /dev/null
+++ b/src/hangar_sim/include/hangar_sim/se2.hpp
@@ -0,0 +1,61 @@
+// Copyright 2026 PickNik Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the PickNik Inc. nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+// Shared planar SE(2) pose composition for hangar_sim's odometry/localization nodes. Kept ROS-free
+// so the pure gate decision logic can include it without pulling in ROS. Previously copy-pasted in
+// amcl_odom_gate_logic.hpp and slip_aware_odom.cpp. (For quaternion->yaw use tf2::getYaw, the
+// ecosystem standard -- there is no equally-standard planar-SE(2) compose/invert, tf2::Transform
+// being full SE(3), so these two stay here.)
+
+#include <cmath>
+
+namespace hangar_sim::se2
+{
+// Planar SE(2) pose. Members default to zero so a default-constructed Pose2 is the identity rather
+// than an indeterminate pose.
+struct Pose2
+{
+  double x = 0.0;    // translation along world/parent x [m]
+  double y = 0.0;    // translation along world/parent y [m]
+  double yaw = 0.0;  // heading about z [rad]
+};
+
+[[nodiscard]] inline Pose2 invert(const Pose2& p)
+{
+  const double c = std::cos(p.yaw), s = std::sin(p.yaw);
+  return { -c * p.x - s * p.y, s * p.x - c * p.y, -p.yaw };
+}
+
+[[nodiscard]] inline Pose2 compose(const Pose2& a, const Pose2& b)
+{
+  const double c = std::cos(a.yaw), s = std::sin(a.yaw);
+  return { a.x + c * b.x - s * b.y, a.y + s * b.x + c * b.y, a.yaw + b.yaw };
+}
+}  // namespace hangar_sim::se2
diff --git a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
index b693f7835..35d57e0ad 100644
--- a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
+++ b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
@@ -31,13 +31,15 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
+import launch.logging
 from launch.actions import (
     DeclareLaunchArgument,
     GroupAction,
     IncludeLaunchDescription,
+    OpaqueFunction,
     SetEnvironmentVariable,
 )
-from launch.conditions import IfCondition
+from launch.conditions import IfCondition, UnlessCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import (
     LaunchConfiguration,
@@ -51,6 +53,26 @@
 from nav2_common.launch import RewrittenYaml, ReplaceString
 
 
+def _warn_unsupported_localization(context, *args, **kwargs):
+    """Emit a WARNING for the unsupported use_fuse:=false + localization:=true combination.
+
+    launch has no LogWarn action, so evaluate the condition here and log at warning severity via an
+    OpaqueFunction -- LogInfo would print at INFO and blend into normal launch output, defeating the
+    "fail loudly" intent for a config that silently degrades localization to raw odometry.
+    """
+    if (
+        LaunchConfiguration("use_fuse").perform(context).lower() == "false"
+        and LaunchConfiguration("localization").perform(context).lower() == "true"
+    ):
+        launch.logging.get_logger("hangar_sim").warning(
+            "UNSUPPORTED: use_fuse:=false with localization:=true -- amcl_odom_gate does not launch, "
+            "so AMCL's map->odom correction is discarded and a static identity is used instead; "
+            "localization degrades to raw odometry. Use use_fuse:=true (default) for map-based "
+            "localization, or localization:=false for the intentional static-TF fallback."
+        )
+    return []
+
+
 def generate_launch_description():
     # Get the relevant directories
     # The config directory (Contains the map and the parameters)
@@ -189,7 +211,7 @@ def generate_launch_description():
 
     declare_use_fuse_cmd = DeclareLaunchArgument(
         "use_fuse",
-        default_value="false",
+        default_value="true",  # #19667: fuse drives sim odometry (was "false")
         description="Whether to launch the fuse state estimator",
     )
 
@@ -288,18 +310,25 @@ def generate_launch_description():
         arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "mj_world", "map"],
     )
 
-    # Static map->odom TF fallback: only used when neither SLAM nor AMCL is publishing it.
-    # Compare lowercased strings rather than `not <bareword>` so this still works
-    # when slam/localization are passed as ROS-style lowercase booleans.
+    # Static map->odom TF fallback: launched whenever nothing else publishes map->odom.
+    # The amcl_odom_gate is the sole publisher only when BOTH use_fuse and localization are
+    # true (it coasts on fuse odom and consumes AMCL's /particle_cloud); AMCL itself never
+    # broadcasts (tf_broadcast:false). So publish this static identity unless SLAM is on or the
+    # gate is active -- i.e. when slam is false AND (use_fuse is false OR localization is false).
+    # Without this, use_fuse:=false + localization:=true (no gate, AMCL not broadcasting) would
+    # have no map->odom source at all. Compare lowercased strings rather than `not <bareword>`
+    # so this still works when the args are passed as ROS-style lowercase booleans.
     static_tf_map_to_odom = Node(
         condition=IfCondition(
             PythonExpression(
                 [
                     "'",
                     slam,
-                    "'.lower() == 'false' and '",
+                    "'.lower() == 'false' and ('",
+                    LaunchConfiguration("use_fuse"),
+                    "'.lower() == 'false' or '",
                     localization,
-                    "'.lower() == 'false'",
+                    "'.lower() == 'false')",
                 ]
             )
         ),
@@ -319,7 +348,12 @@ def generate_launch_description():
     # MuJoCo broadcast a competing odom->ridgeback_base_link TF — removed in this
     # change.) The UI (pose-utils.ts) hardcodes 'world' for user-clicked poses, so
     # this link also keeps nav2 goals transformable to 'map'.
+    # odom -> world. Without fuse it's a static identity (odom == world). With fuse
+    # (#19667), odom_world_drift replaces it with a live transform so odom -> base
+    # resolves to fuse's drifty estimate, giving AMCL real drift to correct while
+    # world -> base stays ground truth for whole-body.
     static_tf_odom_to_world = Node(
+        condition=UnlessCondition(LaunchConfiguration("use_fuse")),
         package="tf2_ros",
         executable="static_transform_publisher",
         name="static_tf_odom_to_world",
@@ -327,6 +361,68 @@ def generate_launch_description():
         arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "odom", "world"],
     )
 
+    odom_world_drift = Node(
+        condition=IfCondition(LaunchConfiguration("use_fuse")),
+        package="hangar_sim",
+        executable="odom_world_drift",
+        name="odom_world_drift",
+        output="log",
+        respawn=LaunchConfiguration("use_respawn"),
+        respawn_delay=2.0,
+        parameters=[{"use_sim_time": use_sim_time}],
+    )
+
+    # Slip-aware wheel-odom covariance: distrust wheel yaw during spins (mecanum
+    # rollers slip), trust it when straight (bounds gyro drift). fuse reads the
+    # republished topic instead of the raw controller odom.
+    slip_aware_odom = Node(
+        condition=IfCondition(LaunchConfiguration("use_fuse")),
+        package="hangar_sim",
+        executable="slip_aware_odom",
+        name="slip_aware_odom",
+        output="log",
+        respawn=LaunchConfiguration("use_respawn"),
+        respawn_delay=2.0,
+        parameters=[{"use_sim_time": use_sim_time}],
+    )
+
+    # Confidence-gated map->odom: holds AMCL's correction and rides fuse odom when the
+    # particle cloud spreads on unmapped obstacles (AMCL runs with tf_broadcast:=false).
+    # latency_compensation_sec composes the correction with odom->base from ~300 ms ago --
+    # beluga's estimate content trails the true pose by about that (its stamp under-reports it),
+    # so during motion this cancels most of the speed*lag error (measured: moving-pose error
+    # ~14 cm -> ~8 cm, moving yaw ~1.2 deg -> ~0.9 deg on the fuselage route). Tuned to this sim's
+    # pipeline; re-measure per platform. 0 disables.
+    amcl_odom_gate = Node(
+        # Sole map->odom publisher only when slam is off AND both use_fuse and localization are on --
+        # makes the single-publisher invariant structural (matches static_tf_map_to_odom's slam guard)
+        # instead of resting on beluga staying silent. Under slam:=true, slam_toolbox owns map->odom
+        # and AMCL is skipped, so the gate must stand down to avoid two competing publishers.
+        condition=IfCondition(
+            PythonExpression(
+                [
+                    "'",
+                    slam,
+                    "'.lower() == 'false' and '",
+                    LaunchConfiguration("use_fuse"),
+                    "'.lower() == 'true' and '",
+                    localization,
+                    "'.lower() == 'true'",
+                ]
+            )
+        ),
+        package="hangar_sim",
+        executable="amcl_odom_gate",
+        name="amcl_odom_gate",
+        output="log",
+        respawn=LaunchConfiguration("use_respawn"),
+        # Shorter than the other nodes: this is the sole map->odom publisher, and AMCL's
+        # transform_tolerance is 1.0 s, so keep the crash-recovery gap under that to avoid
+        # map->base lookup failures during respawn.
+        respawn_delay=0.5,
+        parameters=[{"use_sim_time": use_sim_time, "latency_compensation_sec": 0.30}],
+    )
+
     # QoS relay to bridge BEST_EFFORT odom and IMU to RELIABLE for fuse
     sensor_qos_relay = Node(
         package="hangar_sim",
@@ -393,13 +489,27 @@ def generate_launch_description():
         package="fuse_optimizers",
         executable="fixed_lag_smoother_node",
         name="state_estimator",
+        respawn=LaunchConfiguration("use_respawn"),
+        respawn_delay=2.0,
         parameters=[
-            PathJoinSubstitution([hangar_sim_pkg, "config", "fuse", "fuse.yaml"])
+            PathJoinSubstitution([hangar_sim_pkg, "config", "fuse", "fuse.yaml"]),
+            {"use_sim_time": use_sim_time},
         ],
         output="screen",
         condition=IfCondition(LaunchConfiguration("use_fuse")),
     )
 
+    # Loud guard for an unsupported combination. amcl_odom_gate is the sole map->odom publisher and
+    # only launches when use_fuse:=true. With use_fuse:=false and localization:=true, AMCL converges
+    # but nothing broadcasts its correction (the gate is absent and static_tf_map_to_odom publishes a
+    # fixed identity instead), so localization silently degrades to raw odometry. Warn loudly rather
+    # than look like healthy AMCL. Supported modes: use_fuse:=true (map-based, default) or
+    # localization:=false (intentional static-TF fallback). See _warn_unsupported_localization: the
+    # OpaqueFunction logs at WARNING (launch has no LogWarn action), so it does not blend into INFO.
+    warn_unsupported_localization = OpaqueFunction(
+        function=_warn_unsupported_localization
+    )
+
     # Create the launch description and populate
     ld = LaunchDescription()
 
@@ -430,7 +540,13 @@ def generate_launch_description():
 
     ld.add_action(static_tf_world_to_map)
     ld.add_action(static_tf_odom_to_world)
+    ld.add_action(odom_world_drift)
+    ld.add_action(slip_aware_odom)
+    ld.add_action(
+        amcl_odom_gate
+    )  # ENABLED: degeneracy gate (spread hysteresis + innovation)
     ld.add_action(static_tf_map_to_odom)
+    ld.add_action(warn_unsupported_localization)
     ld.add_action(sensor_qos_relay)
     ld.add_action(forward_stereo_publisher)
     ld.add_action(laser_filter_front_node)
diff --git a/src/hangar_sim/objectives/navigate_to_clicked_point.xml b/src/hangar_sim/objectives/navigate_to_clicked_point.xml
index c72150fd0..5dba09cdf 100644
--- a/src/hangar_sim/objectives/navigate_to_clicked_point.xml
+++ b/src/hangar_sim/objectives/navigate_to_clicked_point.xml
@@ -7,7 +7,6 @@
     _favorite="true"
   >
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="SetInitialPose" robot_frame_id="ridgeback_base_link" />
       <Action
         ID="CreatePoseStamped"
         reference_frame="base_link"
diff --git a/src/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xml b/src/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xml
index 6292d3d16..e9475dae2 100644
--- a/src/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xml
+++ b/src/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xml
@@ -10,7 +10,6 @@
     _favorite="true"
   >
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="SetInitialPose" robot_frame_id="ridgeback_base_link" />
       <Action
         ID="CreatePoseStamped"
         reference_frame="base_link"
diff --git a/src/hangar_sim/package.xml b/src/hangar_sim/package.xml
index e701dee7c..a1e0411e5 100644
--- a/src/hangar_sim/package.xml
+++ b/src/hangar_sim/package.xml
@@ -14,6 +14,13 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
 
   <exec_depend>admittance_controller</exec_depend>
   <exec_depend>clearpath_mecanum_drive_controller</exec_depend>
@@ -34,7 +41,6 @@
   <exec_depend>realsense2_description</exec_depend>
   <exec_depend>ridgeback_description</exec_depend>
   <exec_depend>robotiq_description</exec_depend>
-  <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>slam_toolbox</exec_depend>
   <exec_depend>ur_description</exec_depend>
   <exec_depend>velocity_force_controller</exec_depend>
@@ -47,6 +53,7 @@
   <test_depend>ament_clang_format</test_depend>
   <test_depend>ament_clang_tidy</test_depend>
   <test_depend>ament_cmake_copyright</test_depend>
+  <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_lint_cmake</test_depend>
   <test_depend>picknik_ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
diff --git a/src/hangar_sim/params/nav2_params.yaml b/src/hangar_sim/params/nav2_params.yaml
index 2ccfc4ee2..79c406c7c 100644
--- a/src/hangar_sim/params/nav2_params.yaml
+++ b/src/hangar_sim/params/nav2_params.yaml
@@ -5,8 +5,9 @@ amcl:
     # activation, so localization starts without a manual 2D pose estimate.
     # beluga_amcl is 2D: only x, y, yaw are declared (no z). The covariances set
     # the initial spread (~0.5 m position, ~15 deg yaw) so the seed is not
-    # overconfident; beluga's default covariance is near-zero. The SetInitialPose
-    # Behavior re-seeds at the robot's current pose at each navigation Objective.
+    # overconfident; beluga's default covariance is near-zero. The navigation
+    # Objectives no longer re-seed per run: slip_aware_odom keeps odom continuous
+    # across controller switches and amcl_odom_gate rides odom through divergences.
     set_initial_pose: true
     initial_pose:
       x: 0.0
@@ -15,11 +16,12 @@ amcl:
       covariance_x: 0.25
       covariance_y: 0.25
       covariance_yaw: 0.0685
-    alpha1: 0.05
-    alpha2: 0.05
-    alpha3: 0.1
-    alpha4: 0.05
-    alpha5: 0.05
+    # Raised ~2x from 0.05/0.1 to cover drifty odom without over-spreading the cloud.
+    alpha1: 0.4  # rotational particle spread to keep yaw lock through spins
+    alpha2: 0.1
+    alpha3: 0.2
+    alpha4: 0.1
+    alpha5: 0.1
     base_frame_id: "ridgeback_base_link"
     beam_skip_distance: 0.5
     beam_skip_error_threshold: 0.9
@@ -31,25 +33,33 @@ amcl:
     laser_max_range: 25.0
     laser_min_range: 0.0
     laser_model_type: "likelihood_field"
-    max_beams: 60
-    max_particles: 5000
+    max_beams: 720  # full merged scan (0.5deg) -- max hits
+    max_particles: 2000  # E1: needed for box-cluster convergence
     min_particles: 1000
     odom_frame_id: "odom"
     pf_err: 0.05
     pf_z: 0.99
     recovery_alpha_fast: 0.1
     recovery_alpha_slow: 0.001
-    resample_interval: 1
+    resample_interval: 3  # resample less often than we correct: avoids particle depletion that diverged at update_min_a=0.05/0.1
     robot_model_type: "nav2_amcl::OmniMotionModel"
     save_pose_rate: 0.5
-    sigma_hit: 0.2
-    tf_broadcast: true
+    sigma_hit: 0.25  # E1 keeper: relaxed peak, boxes as outliers
+    tf_broadcast: false  # AMCL never broadcasts map->odom: the amcl_odom_gate publishes it when
+    # use_fuse+localization are on, else a static map->odom fallback does (see the launch file).
+    # Coupled to use_fuse: the gate only launches with use_fuse:=true, so use_fuse:=false +
+    # localization:=true is unsupported (AMCL's correction is discarded) -- the launch file logs a
+    # loud warning for that combination.
     transform_tolerance: 1.0
-    update_min_a: 0.2
-    update_min_d: 0.25
-    z_hit: 0.5
+    update_min_a: 0.1  # correct often for box robustness
+    update_min_d: 0.1
+    # likelihood_field uses only z_hit + z_rand. Their RATIO (0.65:0.30) is the tuned quantity;
+    # the 0.95 sum is deliberate -- a constant scale on every beam's likelihood that cancels in the
+    # particle-weight normalization, so summing to 1.0 would be cosmetic while re-weighting one term
+    # would change the tuned mix. z_short/z_max are ignored in this model.
+    z_hit: 0.65
     z_max: 0.05
-    z_rand: 0.5
+    z_rand: 0.3
     z_short: 0.05
     scan_topic: /scan_merged
 
@@ -120,7 +130,7 @@ controller_server:
       vx_max: 0.5
       vx_min: -0.35
       vy_max: 0.5
-      wz_max: 1.9
+      wz_max: 0.6  # must stay below velocity_smoother max_velocity[2] (1.0) to bind; slower turns keep the base within AMCL's correction bandwidth so map->odom tracks instead of lagging
       iteration_count: 1
       prune_distance: 1.7
       transform_tolerance: 0.1
@@ -404,7 +414,7 @@ velocity_smoother:
     smoothing_frequency: 20.0
     scale_velocities: False
     feedback: "OPEN_LOOP"
-    max_velocity: [1.0, 1.0, 1.0]
+    max_velocity: [1.0, 1.0, 1.0]  # [2] (yaw) caps controller wz_max above; keep >= wz_max (0.6) or the gate's turn-rate assumption drifts
     min_velocity: [-1.0, -1.0, -1.0]
     max_accel: [2.5, 2.5, 3.2]
     max_decel: [-2.5, -2.5, -3.2]
diff --git a/src/hangar_sim/src/amcl_odom_gate.cpp b/src/hangar_sim/src/amcl_odom_gate.cpp
new file mode 100644
index 000000000..bdb1a94c1
--- /dev/null
+++ b/src/hangar_sim/src/amcl_odom_gate.cpp
@@ -0,0 +1,348 @@
+// Copyright 2026 PickNik Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the PickNik Inc. nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+// Degeneracy-aware map->odom gate (ROS node).
+//
+// AMCL matches the lidar to a static map. Where the scene is degenerate for that
+// match -- pressed against the large smooth fuselage (slide-along ambiguity), or
+// among unmapped boxes -- the estimate lurches/teleports and the map flips.
+// odom->base (fuse's wheel+IMU dead-reckoning) stays locally accurate through those
+// zones, so this node holds the last good map->odom and coasts on odom until AMCL is
+// trustworthy again, then blends smoothly back. It replaces AMCL's own broadcast:
+// set AMCL's tf_broadcast:=false so this node is the sole map->odom publisher.
+//
+// This file is only the ROS I/O: read /particle_cloud, distil it to (mean, spread),
+// look up odom->base, and hand the implied map->odom to the pure decision function
+// detail::updateGate (amcl_odom_gate_logic.hpp), which is unit-tested in isolation.
+//
+// ASSUMPTION (documented, load-bearing): odom->base is trustworthy for the DURATION
+// of a degenerate zone. This holds because the zones are transient (a few seconds
+// passing the fuselage or boxes) and fuse's drift over that span is small. If the
+// robot were held in a degenerate zone long enough for odom to drift materially, or
+// odometry failed grossly (severe wheel slip) exactly there, the gate would coast on
+// bad data and degrade to "as good as odometry" -- the design's outer limit.
+
+#include <chrono>
+#include <cmath>
+#include <vector>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tf2/utils.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include "hangar_sim/amcl_odom_gate_logic.hpp"
+
+namespace
+{
+constexpr double kPubPeriod = 0.033;            // 30 Hz map->odom broadcast
+constexpr double kTransformTolerance = 0.1;     // future-date the transform so TF can extrapolate [s]
+constexpr double kCoastAlpha = 0.5;             // follow-fraction below which the gate counts as coasting
+constexpr double kCoastWarnSeconds = 5.0;       // warn once a continuous coast exceeds this [s]
+constexpr int64_t kCoastWarnThrottleMs = 2000;  // min interval between coast warnings [ms]
+constexpr double kStaleInputSeconds = 3.0;      // publish() warns if onCloud has not updated in this long:
+                                                // beluga publishes /particle_cloud faster than this even at
+                                                // rest, so a longer gap means input starvation (beluga/odom
+                                                // TF gone) and the node is republishing a frozen map->odom.
+constexpr int64_t kStaleWarnThrottleMs = 2000;  // min interval between stale-input warnings [ms]
+constexpr double kOdomBufferSec = 1.0;          // odom->base history kept for latency compensation [s].
+                                                // Usable compensation ceiling is this minus the newest
+                                                // sample's age (the odom TF lag), not the full window.
+}  // namespace
+
+using amcl_odom_gate::Pose2;
+
+class AmclOdomGate : public rclcpp::Node
+{
+public:
+  AmclOdomGate() : Node("amcl_odom_gate"), tf_buffer_(get_clock()), tf_listener_(tf_buffer_), tf_broadcaster_(*this)
+  {
+    params_.spread_hold = declare_parameter("spread_hold", params_.spread_hold);
+    params_.spread_resume = declare_parameter("spread_resume", params_.spread_resume);
+    params_.jump_hold = declare_parameter("jump_hold", params_.jump_hold);
+    params_.jump_hold_yaw = declare_parameter("jump_hold_yaw", params_.jump_hold_yaw);
+    params_.provisional_tol = declare_parameter("provisional_tol", params_.provisional_tol);
+    params_.provisional_tol_yaw = declare_parameter("provisional_tol_yaw", params_.provisional_tol_yaw);
+    params_.persist_time = declare_parameter("persist_time", params_.persist_time);
+    params_.spread_accept_max = declare_parameter("spread_accept_max", params_.spread_accept_max);
+    params_.latency_compensation_sec = declare_parameter("latency_compensation_sec", params_.latency_compensation_sec);
+    params_.alpha_slew = declare_parameter("alpha_slew", params_.alpha_slew);
+    if (params_.latency_compensation_sec >= kOdomBufferSec)
+    {
+      // interpolateOdom would clamp to the oldest sample, silently degrading compensation. The two
+      // constants live apart (a parameter vs. a hardcoded window), so warn rather than fail silently.
+      RCLCPP_WARN(get_logger(),
+                  "latency_compensation_sec (%.2f s) >= odom buffer window (%.2f s); compensation will "
+                  "clamp to the oldest sample. Reduce it or enlarge kOdomBufferSec.",
+                  params_.latency_compensation_sec, kOdomBufferSec);
+    }
+    // alpha_slew <= 0 freezes the gate: once s.alpha snaps to 0 on a hold, min(s.alpha + alpha_slew, 1)
+    // can never climb back, so the node would broadcast a frozen map->odom for the rest of the run.
+    // Fall back to the default rather than run permanently stuck.
+    if (params_.alpha_slew <= 0.0)
+    {
+      const double default_alpha_slew = amcl_odom_gate::GateParams{}.alpha_slew;
+      RCLCPP_WARN(get_logger(),
+                  "alpha_slew (%.3f) must be > 0; the gate could never resume after a hold. Using the default (%.3f).",
+                  params_.alpha_slew, default_alpha_slew);
+      params_.alpha_slew = default_alpha_slew;
+    }
+    // spread_resume must sit below spread_hold or the hysteresis loses its dead band and the hold latch
+    // can flicker every update.
+    if (params_.spread_resume >= params_.spread_hold)
+    {
+      RCLCPP_WARN(get_logger(),
+                  "spread_resume (%.2f) >= spread_hold (%.2f); the hold hysteresis has no dead band and may flicker.",
+                  params_.spread_resume, params_.spread_hold);
+    }
+    // Negative persist_time makes the persistence check vacuous (now - provisional_since >= negative is
+    // always true), accepting every large innovation at once -- the teleport guard is effectively off.
+    if (params_.persist_time < 0.0)
+    {
+      RCLCPP_WARN(get_logger(), "persist_time (%.2f s) is negative; the teleport-persistence guard is disabled.",
+                  params_.persist_time);
+    }
+    // beluga publishes /particle_cloud BEST_EFFORT; match it or we receive nothing.
+    cloud_sub_ = create_subscription<geometry_msgs::msg::PoseArray>(
+        "/particle_cloud", rclcpp::SensorDataQoS(),
+        [this](geometry_msgs::msg::PoseArray::ConstSharedPtr m) { onCloud(*m); });
+    // Node-clock timer (not wall): the node runs use_sim_time, and publish()'s coast/stale windows
+    // are measured in now() (sim time), so the tick must advance on sim time too or the two drift
+    // apart when the sim is not real-time.
+    timer_ =
+        rclcpp::create_timer(this, get_clock(), rclcpp::Duration::from_seconds(kPubPeriod), [this]() { publish(); });
+  }
+
+private:
+  void onCloud(const geometry_msgs::msg::PoseArray& cloud)
+  {
+    const size_t n = cloud.poses.size();
+    if (n < 2)
+    {
+      return;
+    }
+    // Refresh the odom buffer here too (not only in the publish timer): onCloud fires asynchronously
+    // to that 30 Hz timer, so without this the newest buffered sample could be up to a publish period
+    // stale relative to now, and interpolateOdom at (now - 0) would clamp to a stale pose -- making
+    // even latency_compensation_sec = 0 differ from a fresh latest lookup. Sampling now keeps the
+    // buffer as fresh as this callback and preserves the "0 == old compose-at-latest" invariant.
+    sampleOdomForBuffer();
+    // Circular mean (AMCL's estimate) and RMS spread (its confidence) over the cloud.
+    double sx = 0.0, sy = 0.0, ss = 0.0, sc = 0.0;
+    for (const auto& p : cloud.poses)
+    {
+      sx += p.position.x;
+      sy += p.position.y;
+      const double y = tf2::getYaw(p.orientation);
+      ss += std::sin(y);
+      sc += std::cos(y);
+    }
+    const Pose2 mean{ sx / n, sy / n, std::atan2(ss, sc) };
+    double var = 0.0;
+    for (const auto& p : cloud.poses)
+    {
+      const double dx = p.position.x - mean.x, dy = p.position.y - mean.y;
+      var += dx * dx + dy * dy;
+    }
+    const double spread = std::sqrt(var / n);
+
+    // odom->base is fuse's estimate. Compose the AMCL mean with odom->base from the cloud's own
+    // content time (cloud stamp - latency_compensation_sec), NOT the latest: beluga's cloud CONTENT
+    // trails the true pose by ~that lag during motion (its stamp under-reports it -- a lookup at the
+    // cloud stamp itself fails as future extrapolation), so composing with the latest odom leaves
+    // map->odom stale by speed*lag. Referencing the cloud's OWN header stamp (not now()) keeps the
+    // compensation immune to onCloud transport/scheduling jitter, so the parameter measures one
+    // physical thing. The live odom->base then carries the consistent map->odom forward to now.
+    // odom_buf_ is filled at the publish rate and at the top of this callback; interpolateOdom clamps
+    // if the target predates the buffer (startup) and returns nullopt only if it is empty, in which
+    // case fall back to the latest TF lookup. lag = 0 reproduces compose-at-latest.
+    // INVARIANT: mean (from beluga's /particle_cloud, in ITS base_frame) and this odom->base lookup
+    // must refer to the SAME base link (ridgeback_base_link); a frame mismatch would bias every
+    // candidate by a constant offset that persistence would then accept as a real correction.
+    const double now_sec = this->now().seconds();
+    const double odom_target = rclcpp::Time(cloud.header.stamp).seconds() - params_.latency_compensation_sec;
+    Pose2 odom_base{ 0.0, 0.0, 0.0 };
+    if (const auto interp = amcl_odom_gate::interpolateOdom(odom_buf_, odom_target); interp.has_value())
+    {
+      odom_base = interp.value();
+      if (params_.latency_compensation_sec > 0.0 && !odom_buf_.empty() && odom_target > odom_buf_.back().t)
+      {
+        // Compensation was requested but the target landed AFTER the newest odom sample, so
+        // interpolateOdom clamped to it -- i.e. the odom->base TF latency exceeds
+        // latency_compensation_sec and the feature is silently under-compensating. Surface it.
+        RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), kStaleWarnThrottleMs,
+                             "amcl_odom_gate: latency-compensation target is newer than the odom buffer "
+                             "(odom TF lag exceeds latency_compensation_sec=%.2f s); clamped, under-compensating.",
+                             params_.latency_compensation_sec);
+      }
+    }
+    else
+    {
+      // interpolateOdom returns nullopt only when odom_buf_ is empty, which -- given the
+      // sampleOdomForBuffer() at the top of this callback -- only happens in the narrow startup
+      // window where that lookup just failed. Retry once here; if it still fails, skip this cloud.
+      try
+      {
+        const auto t = tf_buffer_.lookupTransform("odom", "ridgeback_base_link", tf2::TimePointZero);
+        odom_base = { t.transform.translation.x, t.transform.translation.y, tf2::getYaw(t.transform.rotation) };
+      }
+      catch (const tf2::TransformException&)
+      {
+        return;  // odom->base not available yet: skip this cloud (timer keeps rebroadcasting last good).
+      }
+    }
+
+    // AMCL's implied map->odom = (map->base) o inv(odom->base); gate decides what to broadcast.
+    // The persist_time clock uses now() (wall/sim), not the cloud stamp; under steady AMCL
+    // latency the constant offset leaves elapsed-duration measurement correct.
+    const Pose2 candidate = amcl_odom_gate::compose(mean, amcl_odom_gate::invert(odom_base));
+    map_odom_ = amcl_odom_gate::detail::updateGate(candidate, spread, now_sec, params_, state_);
+    have_map_odom_ = true;
+
+    // Watchdog: the gate is the SOLE map->odom source, so a prolonged hold (coasting on odom
+    // through a severe-spread wrong lock) silently degrades localization to unbounded dead
+    // reckoning. Surface a long coast so it does not quietly move the base's map pose out from
+    // under the nav stack -- the state_.alpha follow-fraction is near 0 whenever the gate holds.
+    if (state_.alpha < kCoastAlpha)
+    {
+      const auto t_now = now();  // one read: keep the logged elapsed consistent with the check
+      if (!coasting_)
+      {
+        coasting_ = true;
+        coast_start_ = t_now;
+      }
+      else if (const double coast_s = (t_now - coast_start_).seconds(); coast_s > kCoastWarnSeconds)
+      {
+        RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), kCoastWarnThrottleMs,
+                             "amcl_odom_gate coasting on odom for %.1f s (AMCL untrusted, spread=%.2f); "
+                             "map->odom is dead-reckoning and will drift until AMCL re-locks.",
+                             coast_s, spread);
+      }
+    }
+    else
+    {
+      coasting_ = false;
+    }
+    last_update_ = now();  // liveness: publish() warns if onCloud stops updating (input starvation).
+  }
+
+  // Sample odom->base at the latest available TF and append to the ring buffer keyed by the TF's
+  // OWN stamp (which trails now by the odom publish latency, so labelling by now would mis-time it),
+  // trimming samples older than kOdomBufferSec. Runs at the publish rate; feeds interpolateOdom() in
+  // onCloud so a latency-lagged AMCL correction can be composed at its content time (now - lag).
+  void sampleOdomForBuffer()
+  {
+    try
+    {
+      const auto tf = tf_buffer_.lookupTransform("odom", "ridgeback_base_link", tf2::TimePointZero);
+      amcl_odom_gate::appendOdomSample(
+          odom_buf_,
+          { rclcpp::Time(tf.header.stamp).seconds(),
+            { tf.transform.translation.x, tf.transform.translation.y, tf2::getYaw(tf.transform.rotation) } },
+          kOdomBufferSec);
+    }
+    catch (const tf2::TransformException&)
+    {
+      // odom->base not ready yet; leave the buffer as-is.
+    }
+  }
+
+  void publish()
+  {
+    sampleOdomForBuffer();  // keep the latency-compensation buffer filled at the publish rate
+
+    if (!have_map_odom_)
+    {
+      // No seed yet: /particle_cloud hasn't arrived or the odom->base lookup keeps failing, so this
+      // sole map->odom publisher is emitting nothing. The starvation watchdog below sits after this
+      // return and can't see a never-started gate, so surface it here (throttled).
+      RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), kStaleWarnThrottleMs,
+                           "amcl_odom_gate: no map->odom yet -- waiting for /particle_cloud and the "
+                           "odom->ridgeback_base_link TF; nothing is being broadcast.");
+      return;
+    }
+    // Liveness watchdog: this timer keeps firing regardless of input, so it is the only place
+    // that can see onCloud STARVATION -- if beluga stops publishing /particle_cloud or odom->base
+    // disappears, updateGate never runs, state_.alpha freezes, and the coast watchdog in onCloud
+    // is never reached, yet we keep broadcasting a frozen (future-dated) map->odom. Surface that
+    // here so the worst silent failure for a sole broadcaster does not pass unnoticed.
+    if (const double stale_s = (now() - last_update_).seconds(); stale_s > kStaleInputSeconds)
+    {
+      RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), kStaleWarnThrottleMs,
+                           "amcl_odom_gate: no /particle_cloud update for %.1f s -- broadcasting a frozen "
+                           "map->odom. Check that beluga_amcl and the odom->base TF are alive.",
+                           stale_s);
+    }
+    geometry_msgs::msg::TransformStamped t;
+    t.header.stamp = now() + rclcpp::Duration::from_seconds(kTransformTolerance);
+    t.header.frame_id = "map";
+    t.child_frame_id = "odom";
+    t.transform.translation.x = map_odom_.x;
+    t.transform.translation.y = map_odom_.y;
+    t.transform.rotation.z = std::sin(map_odom_.yaw / 2.0);
+    t.transform.rotation.w = std::cos(map_odom_.yaw / 2.0);
+    tf_broadcaster_.sendTransform(t);
+  }
+
+  // onCloud (subscription) writes state_/map_odom_/coasting_/coast_start_/last_update_ and reads
+  // odom_buf_; publish (timer) reads map_odom_/last_update_ and writes odom_buf_. main() spins on the
+  // default SingleThreadedExecutor, so this node's two callbacks never run concurrently and the
+  // access is safe without locking. (tf2_ros::TransformListener spins its own thread that writes
+  // tf_buffer_, but that is internally synchronized and touches none of these members.) If this node
+  // is ever moved to a MultiThreadedExecutor or separate callback groups, guard the shared state
+  // (state_/map_odom_/last_update_/odom_buf_).
+  amcl_odom_gate::GateParams params_;
+  amcl_odom_gate::detail::GateState state_;
+  Pose2 map_odom_{ 0.0, 0.0, 0.0 };
+  bool have_map_odom_ = false;
+  bool coasting_ = false;  // watchdog: is the gate currently holding (alpha < kCoastAlpha)?
+  // coast_start_/last_update_ are default-constructed RCL_SYSTEM_TIME; now() is RCL_ROS_TIME under
+  // use_sim_time and subtracting mismatched clock types throws. Both are only subtracted AFTER being
+  // assigned now() on a prior tick (coast_start_ behind coasting_, last_update_ behind have_map_odom_),
+  // so the clock types always match at the subtraction sites -- keep that ordering if refactoring.
+  rclcpp::Time coast_start_;  // when the current continuous coast began (valid only while coasting_)
+  rclcpp::Time last_update_;  // time of the last successful onCloud update (valid once have_map_odom_)
+  std::vector<amcl_odom_gate::OdomSample> odom_buf_;  // recent odom->base samples for latency compensation
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+  tf2_ros::TransformBroadcaster tf_broadcaster_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr cloud_sub_;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<AmclOdomGate>());
+  rclcpp::shutdown();
+  return 0;
+}
diff --git a/src/hangar_sim/src/amcl_odom_gate_logic.cpp b/src/hangar_sim/src/amcl_odom_gate_logic.cpp
new file mode 100644
index 000000000..8d584efab
--- /dev/null
+++ b/src/hangar_sim/src/amcl_odom_gate_logic.cpp
@@ -0,0 +1,113 @@
+// Copyright 2026 PickNik Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the PickNik Inc. nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include "hangar_sim/amcl_odom_gate_logic.hpp"
+
+#include <algorithm>
+
+namespace amcl_odom_gate::detail
+{
+Pose2 updateGate(const Pose2& candidate, double spread, double now, const GateParams& p, GateState& s)
+{
+  if (!s.have_held)
+  {
+    s.held = candidate;
+    s.have_held = true;
+    s.alpha = 1.0;
+    return s.held;
+  }
+
+  // Trigger 1: spread hysteresis (filter self-reported uncertainty).
+  if (spread > p.spread_hold)
+  {
+    s.spread_holding = true;
+  }
+  else if (spread < p.spread_resume)
+  {
+    s.spread_holding = false;
+  }
+
+  // Innovation is gated on BOTH position and yaw: a heading teleport (e.g. a 180 deg
+  // flip in a symmetric corridor) can leave x/y nearly fixed, so position alone would
+  // miss it and follow the flip transparently.
+  const bool large_innov = planarDist(candidate, s.held) > p.jump_hold || yawDist(candidate, s.held) > p.jump_hold_yaw;
+  bool follow;  // follow (accept) the candidate this update, vs hold and coast on odom
+
+  if (!large_innov)
+  {
+    // Small innovation: normal tracking. Transparent unless spread says hold.
+    s.have_provisional = false;
+    follow = !s.spread_holding;
+  }
+  else
+  {
+    // Trigger 2: large innovation -> provisional persistence check (the way to tell a
+    // valid correction from a teleport: valid persists at one pose, ambiguity thrashes).
+    // "Same target" is measured against the ANCHOR captured when the clock started, not
+    // the latest candidate, so a target creeping just under the tolerance each step
+    // still eventually resets rather than being accepted after drifting far.
+    const bool same_target = s.have_provisional && planarDist(candidate, s.provisional) <= p.provisional_tol &&
+                             yawDist(candidate, s.provisional) <= p.provisional_tol_yaw;
+    // A confident-WRONG lock (e.g. scan ambiguity sliding along a smooth surface) persists
+    // at one pose while its cloud stays SEVERELY spread -- persistence alone cannot tell it
+    // from a real correction. Fold the spread check INTO the persistence requirement rather
+    // than testing it only at the acceptance instant: re-anchoring the clock whenever spread
+    // exceeds spread_accept_max means persist_time must elapse with the cloud continuously
+    // tight. A single-frame spread dip during a wrong lock then cannot ratchet the held pose
+    // toward it (the blend below is stateful and never reverts). Persistence still overrides
+    // MODERATE spread by design (a bad-seed recovery converges below the cap and is accepted).
+    const bool trustworthy = spread <= p.spread_accept_max;
+    // A backward clock jump (sim reset / bag loop) leaves provisional_since in the future, so
+    // now - provisional_since goes negative and acceptance stalls until the clock passes the old
+    // value. Treat it like a new target: re-anchor and restart the clock. (appendOdomSample flushes
+    // its buffer on the same condition, so the two stay consistent.)
+    const bool clock_went_backward = s.have_provisional && now < s.provisional_since;
+    if (!same_target || !trustworthy || clock_went_backward)
+    {
+      // New/moving target, or cloud too spread to trust: (re)anchor and restart the clock, hold.
+      s.provisional = candidate;
+      s.provisional_since = now;
+      s.have_provisional = true;
+      follow = false;
+    }
+    else
+    {
+      // Candidate has persisted near the anchor with a tight-enough cloud for persist_time.
+      follow = (now - s.provisional_since >= p.persist_time);
+    }
+  }
+
+  // Asymmetric slew: clamp to a hold IMMEDIATELY (react fast the instant a jump or
+  // high spread is detected, so the held pose is not dragged toward a bad candidate),
+  // but ease back to transparent GRADUALLY on resume (so re-engaging never teleports).
+  const double target = follow ? 1.0 : 0.0;
+  s.alpha = (target < s.alpha) ? target : std::min(s.alpha + p.alpha_slew, target);
+  s.held = blend(s.held, candidate, s.alpha);
+  return s.held;
+}
+}  // namespace amcl_odom_gate::detail
diff --git a/src/hangar_sim/src/odom_world_drift.cpp b/src/hangar_sim/src/odom_world_drift.cpp
new file mode 100644
index 000000000..7ada16452
--- /dev/null
+++ b/src/hangar_sim/src/odom_world_drift.cpp
@@ -0,0 +1,174 @@
+// Copyright 2026 PickNik Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the PickNik Inc. nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+// Publish odom -> world so that odom -> base equals fuse's (drifty) estimate,
+// while robot_state_publisher keeps world -> base as ground truth (#19667).
+//
+//     odom -> world = fuse_estimate(odom->base) (+) inverse(world->base_true)
+// makes the TF lookup odom -> base resolve to fuse's estimate; AMCL then has real
+// drift to correct. Replaces the static odom -> world identity when use_fuse:=true.
+//
+// Sim-only. The drift lives in the ground plane, so everything here is 2D pose
+// algebra (x, y, yaw), shared with the other hangar_sim nodes via se2.hpp. The
+// ground-truth base pose is read straight from the virtual-rail joints on
+// /joint_states rather than a tf2 listener: the sim floods /tf at ~700 Hz across
+// ~140 frames, and buffering that just to read one pose is wasteful. (The Python
+// predecessor also avoided numpy here to dodge OpenBLAS's spin-waiting worker
+// threads; in C++ there is no such dependency to avoid.)
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+#include <cstddef>
+#include <optional>
+#include <string>
+#include <vector>
+
+#include <tf2_ros/transform_broadcaster.h>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <tf2/utils.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include "hangar_sim/odom_world_drift_logic.hpp"
+#include "hangar_sim/se2.hpp"
+
+namespace
+{
+using hangar_sim::se2::compose;
+using hangar_sim::se2::invert;
+using hangar_sim::se2::Pose2;
+// Rail-joint list + index resolver live in an internal-namespace header so their branches are
+// unit-tested (test_odom_world_drift); the frame/kinematics assumptions are documented there.
+using odom_world_drift::internal::kRailJoints;
+using odom_world_drift::internal::resolveRailIndices;
+
+constexpr double kPubPeriod = 0.02;  // 50 Hz, keeps odom->world fresh for AMCL
+}  // namespace
+
+class OdomWorldDrift : public rclcpp::Node
+{
+public:
+  OdomWorldDrift() : Node("odom_world_drift"), tf_broadcaster_(*this)
+  {
+    // Depth-10 default-reliable on both: fuse publishes /odom_filtered reliable, and
+    // joint_state_broadcaster publishes /joint_states reliable. The node only ever uses the latest
+    // sample of each (no history), so the small queue is intentional -- it just tolerates a brief
+    // callback-scheduling burst.
+    est_sub_ = create_subscription<nav_msgs::msg::Odometry>(
+        "/odom_filtered", 10, [this](nav_msgs::msg::Odometry::ConstSharedPtr m) { onEst(*m); });
+    joints_sub_ = create_subscription<sensor_msgs::msg::JointState>(
+        "/joint_states", 10, [this](sensor_msgs::msg::JointState::ConstSharedPtr m) { onJoints(*m); });
+    // Node-clock timer (not wall): the node runs use_sim_time, so the 50 Hz tick must advance on
+    // sim time too, matching the stamp taken from the same clock in publish().
+    timer_ =
+        rclcpp::create_timer(this, get_clock(), rclcpp::Duration::from_seconds(kPubPeriod), [this]() { publish(); });
+  }
+
+private:
+  void onEst(const nav_msgs::msg::Odometry& m)
+  {
+    const auto& p = m.pose.pose;
+    est_ = Pose2{ p.position.x, p.position.y, tf2::getYaw(p.orientation) };
+  }
+
+  void onJoints(const sensor_msgs::msg::JointState& msg)
+  {
+    // /joint_states is multi-publisher: messages can omit the rail joints or order them
+    // differently, so re-resolve the indices whenever the name list changes rather than trusting
+    // a stale cache. Cache the name list unconditionally -- including on a failed resolve -- so a
+    // repeated rail-less list is recognized and skipped without re-searching every message.
+    if (msg.name != cached_names_)
+    {
+      cached_names_ = msg.name;
+      idx_ = resolveRailIndices(msg.name);
+    }
+    if (!idx_.has_value())
+    {
+      return;
+    }
+    // position and name normally align, but position may be shorter than a stale index implies;
+    // bounds-check before operator[] (unlike Python, an out-of-range read here is UB, not an
+    // exception that merely drops the callback).
+    const auto& idx = idx_.value();
+    if (std::any_of(idx.begin(), idx.end(), [&](std::size_t i) { return i >= msg.position.size(); }))
+    {
+      return;
+    }
+    // Read the rail joints directly as world->base (x, y, yaw). Exact only under the sim's rail
+    // kinematics and zero base-link offset -- see kRailJoints in odom_world_drift_logic.hpp.
+    base_ = Pose2{ msg.position[idx[0]], msg.position[idx[1]], msg.position[idx[2]] };
+  }
+
+  void publish()
+  {
+    if (!est_.has_value() || !base_.has_value())
+    {
+      return;
+    }
+    // odom->world = odom->base(est) (+) inverse(world->base(true))
+    // Note: est (fuse, ~10 Hz) and base (rail joints, ~50 Hz) are the latest of each, sampled at
+    // different instants, so during motion this carries up to ~speed*0.1s of timing-skew noise
+    // (~5 cm at 0.5 m/s). Second-order vs the 300 ms lag the gate's latency-comp corrects, and it
+    // just presents as extra apparent drift for AMCL to absorb -- left as-is intentionally.
+    const Pose2 t = compose(est_.value(), invert(base_.value()));
+
+    geometry_msgs::msg::TransformStamped tf;
+    tf.header.stamp = get_clock()->now();
+    tf.header.frame_id = "odom";
+    tf.child_frame_id = "world";
+    tf.transform.translation.x = t.x;
+    tf.transform.translation.y = t.y;
+    tf.transform.rotation.z = std::sin(t.yaw / 2.0);
+    tf.transform.rotation.w = std::cos(t.yaw / 2.0);
+    tf_broadcaster_.sendTransform(tf);
+  }
+
+  // All state below is written and read only from the two subscription callbacks and the timer,
+  // which share the node's default mutually-exclusive callback group -- so they never run
+  // concurrently and the state needs no locking.
+  std::optional<Pose2> est_;                                        // fuse estimate odom->base
+  std::optional<Pose2> base_;                                       // ground-truth world->base
+  std::vector<std::string> cached_names_;                           // /joint_states name list idx_ resolves against
+  std::optional<std::array<std::size_t, kRailJoints.size()>> idx_;  // cached rail-joint indices into that list
+  // ROS entities declared last so they destruct (and stop firing callbacks) before the state above.
+  tf2_ros::TransformBroadcaster tf_broadcaster_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr est_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joints_sub_;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<OdomWorldDrift>());
+  rclcpp::shutdown();
+  return 0;
+}
diff --git a/src/hangar_sim/src/slip_aware_odom.cpp b/src/hangar_sim/src/slip_aware_odom.cpp
new file mode 100644
index 000000000..9406a027d
--- /dev/null
+++ b/src/hangar_sim/src/slip_aware_odom.cpp
@@ -0,0 +1,206 @@
+// Copyright 2026 PickNik Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the PickNik Inc. nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+// Forward the active platform controller's odometry as one continuous stream with
+// a yaw covariance that grows during spin, for fuse's wheel_odom_sensor.
+//
+// Two mecanum controllers drive the base -- platform_velocity_controller_nav2 for
+// Nav2, platform_velocity_controller for whole-body -- and only the active one
+// publishes odom. Fuse needs a single unbroken input across every nav <-> whole-
+// body switch, so this forwards whichever controller is publishing and stitches
+// the handoff: each source's pose is rigidly offset to continue from the last
+// output, so the stream never jumps (fuse consumes it differentially). Forwarding
+// the controllers' own ~600 Hz odometry keeps their accuracy rather than
+// re-deriving it from wheel states. The yaw covariance ramps from kBaseCov (trust
+// wheel yaw, bounding the IMU gyro's drift) toward kSpinCov (defer to the IMU) as
+// spin accumulates, because mecanum rollers slip in place.
+//
+// The deactivating controller stops publishing a beat before the activating one
+// starts, so at each switch there is a brief window where neither source is live.
+// A timer bridges it by holding the last pose at zero velocity (see hold()): fuse
+// then never loses its wheel constraint and cannot coast, which otherwise jumped
+// the estimate ~1 m at the switch and flipped the map during whole-body motion.
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tf2/utils.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include "hangar_sim/se2.hpp"
+
+namespace
+{
+constexpr double kBaseCov = 0.03;     // straight-line yaw covariance
+constexpr double kSpinCov = 10.0;     // fully-slipping yaw covariance: large enough to
+                                      // effectively drop the wheel yaw so fuse rides the
+                                      // IMU (its orientation tracks truth to ~0.1 deg even
+                                      // mid-spin, where the mecanum wheels slip badly)
+constexpr double kTau = 0.5;          // slip relaxation time constant [s]
+constexpr double kSlipFull = 0.5;     // leaky-integrated |yaw rate| for full distrust
+constexpr double kMaxDt = 0.1;        // clamp dt so a stale gap can't spike the integral
+constexpr double kSourceGap = 0.2;    // a source quiet longer than this counts as switched out [s]
+constexpr int kYaw = 35;              // (yaw, yaw) pose element and (vyaw, vyaw) twist element
+constexpr double kHoldPeriod = 0.02;  // 50 Hz: rate at which to check for a silent source
+constexpr double kHoldGap = 0.015;    // hold the last pose once the source is quiet this long [s]
+
+// SE(2) primitives shared with amcl_odom_gate (se2.hpp).
+using hangar_sim::se2::compose;
+using hangar_sim::se2::invert;
+using hangar_sim::se2::Pose2;
+}  // namespace
+
+class SlipAwareOdom : public rclcpp::Node
+{
+public:
+  SlipAwareOdom() : Node("slip_aware_odom")
+  {
+    odom_.header.frame_id = "odom";
+    odom_.child_frame_id = "ridgeback_base_link";
+    // The published pose is a re-anchored stream seeded at the origin, not a true
+    // absolute odom->base -- fuse consumes it differentially, so only its deltas are
+    // meaningful. No TF is published from it. An absolute consumer would be wrong.
+    pub_ = create_publisher<nav_msgs::msg::Odometry>("/odom_slip_aware", 10);
+    // source 0 = nav2 controller, source 1 = whole-body controller.
+    nav_sub_ = create_subscription<nav_msgs::msg::Odometry>("/platform_velocity_controller_nav2/odom", 10,
+                                                            [this](nav_msgs::msg::Odometry::ConstSharedPtr m) {
+                                                              relay(*m, 0);
+                                                            });
+    body_sub_ = create_subscription<nav_msgs::msg::Odometry>(
+        "/platform_velocity_controller/odom", 10, [this](nav_msgs::msg::Odometry::ConstSharedPtr m) { relay(*m, 1); });
+    last_relay_ = now();
+    // Node-clock timer (not wall): the hold-gap check in hold() compares against now() (sim time),
+    // so the tick must advance on sim time too, not wall time, under use_sim_time.
+    hold_timer_ =
+        rclcpp::create_timer(this, get_clock(), rclcpp::Duration::from_seconds(kHoldPeriod), [this]() { hold(); });
+  }
+
+private:
+  void relay(const nav_msgs::msg::Odometry& m, int source)
+  {
+    // Forward every message: fuse consumes each one as a differential yaw constraint,
+    // and that full density is what holds AMCL locked through the fast base turns.
+    // Throttling (tested at 50/200 Hz) starves those constraints and AMCL diverges
+    // mid-turn, so the controller's native rate is the necessary rate here. That rate
+    // is ~500 Hz (measured): the forked clearpath_mecanum_drive_controller publishes
+    // odom every update() and ignores its `publish_rate: 50.0` param, so kHoldGap is
+    // sized for ~500 Hz, not the 50 Hz the config implies.
+    const double t = rclcpp::Time(m.header.stamp).seconds();
+    const auto& q = m.pose.pose.orientation;
+    const Pose2 p{ m.pose.pose.position.x, m.pose.pose.position.y, tf2::getYaw(q) };
+
+    // Only the active controller publishes odom (the inactive one is silent), so the
+    // two sources never interleave and a source change always means a genuine handoff.
+    // On that switch or a stale gap, re-anchor so the output continues seamlessly:
+    // pick the offset for which compose(offset, p) == the last output pose.
+    if (source != active_ || last_t_ <= 0.0 || t - last_t_ > kSourceGap)
+    {
+      offset_ = compose(out_, invert(p));
+      active_ = source;
+    }
+    out_ = compose(offset_, p);
+
+    // Full elapsed time drives the decay -- a long gap (e.g. a nav <-> whole-body handoff silence)
+    // SHOULD relax the integral toward kBaseCov. The clamped value drives only the growth term, so a
+    // stale gap cannot spike |yaw rate| * dt. Under normal ~500 Hz relay the two are equal (dt << kMaxDt).
+    const double elapsed = (last_t_ > 0.0) ? std::max(t - last_t_, 0.0) : 0.0;
+    const double dt = std::min(elapsed, kMaxDt);
+    last_t_ = t;
+    // Leaky integral of |yaw rate|: grows over sustained spin, decays with kTau.
+    slip_ = slip_ * std::exp(-elapsed / kTau) + std::abs(m.twist.twist.angular.z) * dt;
+    const double cov = kBaseCov + (kSpinCov - kBaseCov) * std::min(1.0, slip_ / kSlipFull);
+
+    odom_.header.stamp = m.header.stamp;
+    odom_.pose.pose.position.x = out_.x;
+    odom_.pose.pose.position.y = out_.y;
+    odom_.pose.pose.orientation.z = std::sin(out_.yaw / 2.0);
+    odom_.pose.pose.orientation.w = std::cos(out_.yaw / 2.0);
+    odom_.pose.covariance = m.pose.covariance;  // keep the controller's x/y covariance
+    odom_.twist = m.twist;                      // body-frame twist is source-independent
+    // Grow both the pose-yaw and twist-yaw-rate covariance so fuse defers to the IMU
+    // gyro -- for orientation and yaw rate alike -- while the mecanum wheels slip.
+    odom_.pose.covariance[kYaw] = cov;
+    odom_.twist.covariance[kYaw] = cov;
+    pub_->publish(odom_);
+    last_relay_ = now();
+  }
+
+  // Bridge the brief source silence at a controller handoff: once the active
+  // controller has been quiet for kHoldGap, republish the last pose at zero
+  // velocity so fuse keeps a live wheel constraint and cannot coast. During
+  // normal driving the ~500 Hz stream keeps last_relay_ fresh and this no-ops.
+  void hold()
+  {
+    if (active_ < 0)
+    {
+      return;
+    }
+    const double gap = (now() - last_relay_).seconds();
+    if (gap < kHoldGap)
+    {
+      return;
+    }
+    // Stamp = last relayed message stamp (last_t_, on the controllers' clock) advanced
+    // by the elapsed gap, so held samples stay on the relayed stream's clock base and
+    // strictly increase -- independent of whether the node clock matches it.
+    const double held = last_t_ + gap;
+    odom_.header.stamp.sec = static_cast<int32_t>(held);
+    odom_.header.stamp.nanosec = static_cast<uint32_t>((held - std::floor(held)) * 1e9);
+    odom_.twist.twist.linear.x = 0.0;
+    odom_.twist.twist.linear.y = 0.0;
+    odom_.twist.twist.angular.z = 0.0;
+    pub_->publish(odom_);
+  }
+
+  // All shared state below is written and read only from relay() and hold(), which
+  // share the node's default mutually-exclusive callback group -- so they never run
+  // concurrently and the state needs no locking. Do NOT move either callback to a
+  // separate or reentrant callback group without adding synchronization.
+  nav_msgs::msg::Odometry odom_;
+  Pose2 out_{ 0.0, 0.0, 0.0 };     // continuous output pose
+  Pose2 offset_{ 0.0, 0.0, 0.0 };  // current source -> output-frame offset
+  int active_ = -1;                // source id currently being forwarded
+  double slip_ = 0.0, last_t_ = 0.0;
+  rclcpp::Time last_relay_;  // node-clock time of the last forwarded message
+  // ROS entities declared last so they destruct (and stop firing callbacks) before
+  // the state above that those callbacks read.
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr nav_sub_, body_sub_;
+  rclcpp::TimerBase::SharedPtr hold_timer_;
+};
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<SlipAwareOdom>());
+  rclcpp::shutdown();
+  return 0;
+}
diff --git a/src/hangar_sim/test/test_amcl_odom_gate.cpp b/src/hangar_sim/test/test_amcl_odom_gate.cpp
new file mode 100644
index 000000000..7ca5f478e
--- /dev/null
+++ b/src/hangar_sim/test/test_amcl_odom_gate.cpp
@@ -0,0 +1,583 @@
+// Copyright 2026 PickNik Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the PickNik Inc. nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+// Unit tests for the pure gate-decision function detail::updateGate. Each test
+// encodes a failure mode observed in sim as a regression: run a candidate stream
+// through the gate and assert the broadcast map->odom behaves. No ROS, no TF.
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+#include "hangar_sim/amcl_odom_gate_logic.hpp"
+
+namespace
+{
+using amcl_odom_gate::appendOdomSample;
+using amcl_odom_gate::blend;
+using amcl_odom_gate::compose;
+using amcl_odom_gate::GateParams;
+using amcl_odom_gate::interpolateOdom;
+using amcl_odom_gate::invert;
+using amcl_odom_gate::OdomSample;
+using amcl_odom_gate::planarDist;
+using amcl_odom_gate::Pose2;
+using amcl_odom_gate::yawDist;
+using amcl_odom_gate::detail::GateState;
+using amcl_odom_gate::detail::updateGate;
+
+constexpr double kDt = 0.033;  // 30 Hz, matching the node's broadcast period.
+
+// Drive `count` updates of the same candidate/spread through the gate and return the
+// final broadcast pose. `t` advances by kDt each step so persistence timers elapse.
+Pose2 run(const Pose2& candidate, double spread, int count, const GateParams& p, GateState& s, double& t)
+{
+  Pose2 out = s.held;
+  for (int i = 0; i < count; ++i)
+  {
+    out = updateGate(candidate, spread, t, p, s);
+    t += kDt;
+  }
+  return out;
+}
+}  // namespace
+
+// SE(2) math: compose(a, invert(a)) is the identity, and compose/invert round-trip a
+// pose. Guards the transform algebra the gate's candidate computation relies on.
+TEST(AmclOdomGate, ComposeInvertRoundTrips)
+{
+  const Pose2 a{ 1.5, -2.0, 0.9 };
+  const Pose2 b{ -0.3, 0.7, -1.2 };
+  // GIVEN a pose a; WHEN composed with its inverse; THEN the result is the identity.
+  const Pose2 id = compose(a, invert(a));
+  EXPECT_NEAR(id.x, 0.0, 1e-9);
+  EXPECT_NEAR(id.y, 0.0, 1e-9);
+  EXPECT_NEAR(std::remainder(id.yaw, 2.0 * M_PI), 0.0, 1e-9);
+  // AND applying b then removing it via inverse-compose recovers b.
+  const Pose2 rt = compose(invert(a), compose(a, b));
+  EXPECT_NEAR(rt.x, b.x, 1e-9);
+  EXPECT_NEAR(rt.y, b.y, 1e-9);
+  EXPECT_NEAR(yawDist(rt, b), 0.0, 1e-9);
+}
+
+// blend wraps yaw through the SHORT arc: blending 170 deg and -170 deg lands near
+// 180 deg, not near 0. A long-way-around implementation would fail this.
+TEST(AmclOdomGate, BlendYawTakesShortArc)
+{
+  const Pose2 a{ 0.0, 0.0, 170.0 * M_PI / 180.0 };
+  const Pose2 b{ 0.0, 0.0, -170.0 * M_PI / 180.0 };
+  const Pose2 mid = blend(a, b, 0.5);
+  EXPECT_NEAR(yawDist(mid, Pose2{ 0.0, 0.0, M_PI }), 0.0, 1e-6);
+}
+
+// interpolateOdom: empty buffer yields nullopt (caller falls back to a live TF lookup).
+TEST(AmclOdomGate, InterpolateOdomEmptyIsNullopt)
+{
+  // GIVEN an empty odom buffer; WHEN interpolating; THEN there is no value.
+  const std::vector<OdomSample> buf;
+  EXPECT_FALSE(interpolateOdom(buf, 1.0).has_value());
+}
+
+// interpolateOdom: a target strictly between two samples is linearly interpolated in x/y/yaw.
+TEST(AmclOdomGate, InterpolateOdomLinearBetweenSamples)
+{
+  // GIVEN two samples 1 s apart moving +2 m in x and +0.4 rad in yaw.
+  const std::vector<OdomSample> buf{ { 10.0, { 0.0, 0.0, 0.0 } }, { 11.0, { 2.0, 0.0, 0.4 } } };
+
+  // WHEN interpolating at the midpoint (t = 10.5).
+  const auto out = interpolateOdom(buf, 10.5);
+
+  // THEN it is exactly halfway.
+  ASSERT_TRUE(out.has_value());
+  EXPECT_NEAR(out->x, 1.0, 1e-9);
+  EXPECT_NEAR(out->y, 0.0, 1e-9);
+  EXPECT_NEAR(out->yaw, 0.2, 1e-9);
+}
+
+// interpolateOdom: a target at or past the ends clamps to the nearest sample (no extrapolation) --
+// this is what happens at startup before the buffer spans (now - lag).
+TEST(AmclOdomGate, InterpolateOdomClampsOutOfRange)
+{
+  const std::vector<OdomSample> buf{ { 10.0, { 1.0, 2.0, 0.1 } }, { 11.0, { 3.0, 4.0, 0.2 } } };
+
+  // GIVEN a target BEFORE the buffer; THEN it clamps to the front.
+  const auto before = interpolateOdom(buf, 9.0);
+  ASSERT_TRUE(before.has_value());
+  EXPECT_NEAR(before->x, 1.0, 1e-9);
+  EXPECT_NEAR(before->y, 2.0, 1e-9);
+
+  // AND a target AFTER the buffer clamps to the back (never extrapolates forward).
+  const auto after = interpolateOdom(buf, 99.0);
+  ASSERT_TRUE(after.has_value());
+  EXPECT_NEAR(after->x, 3.0, 1e-9);
+  EXPECT_NEAR(after->y, 4.0, 1e-9);
+}
+
+// interpolateOdom: yaw interpolates through the SHORT arc across the +/-pi wrap, not the long way.
+TEST(AmclOdomGate, InterpolateOdomYawWrapsShortArc)
+{
+  const std::vector<OdomSample> buf{ { 0.0, { 0.0, 0.0, 170.0 * M_PI / 180.0 } },
+                                     { 1.0, { 0.0, 0.0, -170.0 * M_PI / 180.0 } } };
+  const auto mid = interpolateOdom(buf, 0.5);
+  ASSERT_TRUE(mid.has_value());
+  EXPECT_NEAR(yawDist(*mid, Pose2{ 0.0, 0.0, M_PI }), 0.0, 1e-6);
+}
+
+// interpolateOdom: a single-element buffer returns that element for any target (front() == back(),
+// exercising both clamp branches against the same sample).
+TEST(AmclOdomGate, InterpolateOdomSingleElement)
+{
+  const std::vector<OdomSample> buf{ { 5.0, { 1.0, 2.0, 0.3 } } };
+  for (const double target : { 4.0, 5.0, 6.0 })
+  {
+    const auto out = interpolateOdom(buf, target);
+    ASSERT_TRUE(out.has_value());
+    EXPECT_NEAR(out->x, 1.0, 1e-9);
+    EXPECT_NEAR(out->y, 2.0, 1e-9);
+    EXPECT_NEAR(out->yaw, 0.3, 1e-9);
+  }
+}
+
+// interpolateOdom with duplicate trailing timestamps returns cleanly (no NaN) via the
+// `target >= back().t` clamp -- NOT via the loop's zero-span guard, which is unreachable for any
+// input (the loop returns at the first buf[i].t >= target, so buf[i-1].t < target <= buf[i].t
+// always implies a positive span; the guard is purely defensive). appendOdomSample forbids
+// duplicate stamps in production anyway; this only pins the pure function's clamp behaviour.
+TEST(AmclOdomGate, InterpolateOdomDuplicateTrailingStampsClamp)
+{
+  const std::vector<OdomSample> buf{ { 0.0, { 0.0, 0.0, 0.0 } },
+                                     { 1.0, { 1.0, 0.0, 0.0 } },
+                                     { 1.0, { 9.0, 0.0, 0.0 } } };
+  const auto out = interpolateOdom(buf, 1.0);  // clamps to back() since target == back().t
+  ASSERT_TRUE(out.has_value());
+  EXPECT_FALSE(std::isnan(out->x));
+  EXPECT_NEAR(out->x, 9.0, 1e-9);  // the back sample, via the clamp
+}
+
+// appendOdomSample: newer samples append and the buffer stays trimmed to the keep window behind
+// the newest, leaving the buffer time-ascending.
+TEST(AmclOdomGate, AppendOdomSampleTrimsToWindow)
+{
+  std::vector<OdomSample> buf;
+  // GIVEN samples 0.0..1.5 s appended with a 1.0 s keep window.
+  for (int i = 0; i <= 15; ++i)
+  {
+    appendOdomSample(buf, { 0.1 * i, { 0.1 * i, 0.0, 0.0 } }, 1.0);
+  }
+  // THEN nothing older than (1.5 - 1.0) survives, and the buffer is strictly ascending.
+  ASSERT_FALSE(buf.empty());
+  EXPECT_GE(buf.front().t, 0.5 - 1e-9);
+  EXPECT_NEAR(buf.back().t, 1.5, 1e-9);
+  for (size_t i = 1; i < buf.size(); ++i)
+  {
+    EXPECT_GT(buf[i].t, buf[i - 1].t);
+  }
+}
+
+// appendOdomSample: a stale/duplicate stamp (not strictly newer than the back) is ignored, so the
+// buffer never goes out of order even if TF replays an old or repeated transform.
+TEST(AmclOdomGate, AppendOdomSampleIgnoresNonNewer)
+{
+  std::vector<OdomSample> buf;
+  appendOdomSample(buf, { 10.0, { 1.0, 0.0, 0.0 } }, 5.0);
+  appendOdomSample(buf, { 10.0, { 2.0, 0.0, 0.0 } }, 5.0);  // equal stamp -> ignored
+  appendOdomSample(buf, { 9.5, { 3.0, 0.0, 0.0 } }, 5.0);   // older stamp -> ignored
+  ASSERT_EQ(buf.size(), 1u);
+  EXPECT_NEAR(buf.front().pose.x, 1.0, 1e-9);  // still the original sample
+}
+
+// appendOdomSample: a sample far older than the newest (a backward clock jump: sim reset / bag loop)
+// flushes the stale buffer and re-seeds, rather than freezing forever under the strictly-newer rule.
+TEST(AmclOdomGate, AppendOdomSampleFlushesOnBackwardJump)
+{
+  std::vector<OdomSample> buf;
+  // GIVEN a buffer around t=100 with a 1 s window.
+  for (int i = 0; i <= 10; ++i)
+  {
+    appendOdomSample(buf, { 100.0 + 0.1 * i, { 0.1 * i, 0.0, 0.0 } }, 1.0);
+  }
+  ASSERT_GT(buf.size(), 1u);
+
+  // WHEN the clock jumps back to t=5 (far more than one window older than the back).
+  appendOdomSample(buf, { 5.0, { 42.0, 0.0, 0.0 } }, 1.0);
+
+  // THEN the stale pre-jump samples are gone and the buffer re-seeds from the post-jump sample.
+  ASSERT_EQ(buf.size(), 1u);
+  EXPECT_NEAR(buf.front().t, 5.0, 1e-9);
+  EXPECT_NEAR(buf.front().pose.x, 42.0, 1e-9);
+}
+
+// GIVEN a fresh gate; WHEN the first candidate arrives; THEN it is adopted as-is
+// (nothing to hold onto yet).
+TEST(AmclOdomGate, FirstCandidateAdoptedImmediately)
+{
+  GateParams p;
+  GateState s;
+  const Pose2 first{ 1.0, 2.0, 0.3 };
+  const Pose2 out = updateGate(first, 0.2, 0.0, p, s);
+  EXPECT_NEAR(out.x, 1.0, 1e-9);
+  EXPECT_NEAR(out.y, 2.0, 1e-9);
+  EXPECT_NEAR(out.yaw, 0.3, 1e-9);
+}
+
+// Case 3 (normal tracking, no jitter/accuracy loss): small confident innovations are
+// followed transparently -- the gate output equals AMCL within a hair.
+TEST(AmclOdomGate, SmallInnovationsTrackTransparently)
+{
+  GateParams p;
+  GateState s;
+  double t = 0.0;
+  run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t);
+  // WHEN AMCL drifts slowly (each step well under jump_hold) at low spread
+  Pose2 out{ 0.0, 0.0, 0.0 };
+  for (int i = 1; i <= 30; ++i)
+  {
+    const Pose2 c{ 0.01 * i, 0.0, 0.0 };  // 1 cm/step, far below jump_hold
+    out = updateGate(c, 0.2, t, p, s);
+    t += kDt;
+  }
+  // THEN the gate has followed AMCL essentially exactly (no held lag, no jitter).
+  EXPECT_NEAR(out.x, 0.30, 0.02);
+}
+
+// Case 2 (plane/box ambiguity): a candidate that THRASHES (jumps to a different far
+// pose every update) never persists, so the gate holds -- the robot coasts on odom.
+TEST(AmclOdomGate, ThrashingTeleportsAreHeld)
+{
+  GateParams p;
+  GateState s;
+  double t = 0.0;
+  const Pose2 good{ 0.0, 0.0, 0.0 };
+  run(good, 0.2, 1, p, s, t);  // establish a confident held pose
+
+  // WHEN AMCL teleports to a DIFFERENT far pose each update (ambiguity artifact),
+  // with spread also elevated as it does near unmapped/degenerate geometry.
+  Pose2 out{ 0.0, 0.0, 0.0 };
+  const double far = 5.0;
+  for (int i = 0; i < 60; ++i)
+  {
+    const double ang = 0.7 * i;  // walk the teleport around so it never repeats a spot
+    const Pose2 c{ far * std::cos(ang), far * std::sin(ang), 0.0 };
+    out = updateGate(c, 2.0, t, p, s);  // spread 2.0 > spread_hold
+    t += kDt;
+  }
+  // THEN the broadcast pose stayed near the last good pose (held), NOT chasing the
+  // teleports -- otherwise the map would flip.
+  EXPECT_LT(planarDist(out, good), 0.5);
+}
+
+// Case 2b (yaw teleport): a heading flip with x/y essentially unchanged must still be
+// caught -- position innovation alone would miss it. The flip thrashes (never
+// persists), so the gate holds the last good heading.
+TEST(AmclOdomGate, YawTeleportWithStablePositionIsHeld)
+{
+  GateParams p;
+  GateState s;
+  double t = 0.0;
+  const Pose2 good{ 0.0, 0.0, 0.0 };
+  run(good, 0.2, 1, p, s, t);
+
+  // WHEN yaw flips by ~+/-160 deg each update while x/y barely move, at LOW spread
+  // (position variance stays small -- the spread trigger cannot see a yaw-only flip).
+  Pose2 out{ 0.0, 0.0, 0.0 };
+  for (int i = 0; i < 60; ++i)
+  {
+    const double yaw = ((i % 2) == 0) ? 2.8 : -2.8;  // ~160 deg, alternating
+    out = updateGate({ 0.02, 0.0, yaw }, 0.2, t, p, s);
+    t += kDt;
+  }
+  // THEN the broadcast heading stayed near the last good heading (0), not chasing flips.
+  EXPECT_LT(yawDist(out, good), 0.35);
+}
+
+// Case 1 (bad seed / genuinely-lost recovery): a large correction that PERSISTS at
+// the same place is accepted after persist_time -- the gate must NOT lock in the
+// wrong pose forever (the limitation the first gate hit).
+TEST(AmclOdomGate, PersistentCorrectionIsAccepted)
+{
+  GateParams p;
+  GateState s;
+  double t = 0.0;
+  run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t);  // held starts at origin (a bad seed)
+
+  // WHEN AMCL consistently reports the true pose 8 m away (a real, stable correction),
+  // with the cloud converged there (low spread).
+  const Pose2 truth{ 8.0, 0.0, 0.0 };
+  Pose2 out{ 0.0, 0.0, 0.0 };
+  // Enough steps for persist_time to elapse AND the alpha ramp to converge.
+  for (int i = 0; i < 120; ++i)
+  {
+    out = updateGate(truth, 0.2, t, p, s);
+    t += kDt;
+  }
+  // THEN the gate eventually accepts and converges to the true pose (recovers),
+  // rather than holding the wrong seed indefinitely.
+  EXPECT_LT(planarDist(out, truth), 0.3);
+}
+
+// Boundary: a persistent correction is NOT accepted before persist_time has elapsed
+// (pins the timer -- proves it waits, and that persist_time is load-bearing).
+TEST(AmclOdomGate, PersistentCorrectionHeldUntilTimerElapses)
+{
+  GateParams p;
+  p.persist_time = 1.0;
+  GateState s;
+  double t = 0.0;
+  run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t);
+
+  const Pose2 truth{ 8.0, 0.0, 0.0 };
+  // Just UNDER persist_time worth of updates (0.9 s of 0.033 s steps ~= 27).
+  Pose2 out{ 0.0, 0.0, 0.0 };
+  for (int i = 0; i < 27; ++i)
+  {
+    out = updateGate(truth, 0.2, t, p, s);
+    t += kDt;
+  }
+  // THEN it is still held near origin -- the correction has not yet proven persistent.
+  EXPECT_LT(planarDist(out, Pose2{ 0.0, 0.0, 0.0 }), 0.5);
+}
+
+// Regression: a backward clock jump (sim reset / bag loop) must re-anchor the persistence timer.
+// Without re-anchoring, provisional_since stays ahead of `now`, so now - provisional_since is
+// negative and a valid persistent correction can never be accepted until the clock climbs back
+// past the pre-jump timestamp -- the gate would strand itself holding a stale pose.
+TEST(AmclOdomGate, BackwardClockJumpReanchorsPersistence)
+{
+  GateParams p;
+  p.persist_time = 1.0;
+  GateState s;
+  double t = 100.0;  // start with a large clock so the jump below is unambiguously backward
+  run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t);
+
+  // GIVEN a true correction that has begun persisting (anchor captured near t = 100), not yet accepted
+  const Pose2 truth{ 8.0, 0.0, 0.0 };
+  for (int i = 0; i < 10; ++i)
+  {
+    updateGate(truth, 0.2, t, p, s);
+    t += kDt;
+  }
+
+  // WHEN the clock jumps backward and the same true pose keeps persisting from there for > persist_time
+  t = 0.0;
+  Pose2 out{ 0.0, 0.0, 0.0 };
+  for (int i = 0; i < 120; ++i)
+  {
+    out = updateGate(truth, 0.2, t, p, s);
+    t += kDt;
+  }
+
+  // THEN the gate re-anchored on the jump and accepts after persist_time from the NEW clock, rather
+  // than staying stranded waiting for the pre-jump timestamp (which would leave it near origin).
+  EXPECT_LT(planarDist(out, truth), 0.3);
+}
+
+// Case 1b (confident-WRONG lock): a large candidate that PERSISTS at one pose but with
+// a SEVERELY spread cloud is a divergence (e.g. scan ambiguity sliding along a smooth
+// surface), not a real correction. Persistence alone would accept it; spread_accept_max
+// must reject it so the gate keeps coasting on odom instead of adopting the wrong pose.
+TEST(AmclOdomGate, SevereSpreadPersistentPoseNotAccepted)
+{
+  GateParams p;
+  GateState s;
+  double t = 0.0;
+  run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t);  // established a confident held pose
+
+  // WHEN AMCL sits at the same far pose well past persist_time, but the cloud stays
+  // severely spread (above spread_accept_max) the whole time -- the wrong-lock signature.
+  const Pose2 wrong{ 8.0, 0.0, 0.0 };
+  const double severe_spread = 5.0;  // > spread_accept_max (3.0)
+  Pose2 out{ 0.0, 0.0, 0.0 };
+  for (int i = 0; i < 120; ++i)
+  {
+    out = updateGate(wrong, severe_spread, t, p, s);
+    t += kDt;
+  }
+  // THEN the gate never adopts it -- it stays held near the last good pose (coasting on
+  // odom), NOT snapping to the persistent-but-wrong pose.
+  EXPECT_LT(planarDist(out, Pose2{ 0.0, 0.0, 0.0 }), 0.5);
+}
+
+// Case 1c (right case preserved): a genuine recovery whose cloud starts severely spread
+// then CONVERGES (spread drops below the cap) must still be accepted once it tightens --
+// the cap waits for spread to fall, it does not reject legitimate corrections.
+TEST(AmclOdomGate, RecoveryAcceptedOnceSpreadDrops)
+{
+  GateParams p;
+  GateState s;
+  double t = 0.0;
+  run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t);
+
+  const Pose2 truth{ 8.0, 0.0, 0.0 };
+  Pose2 out{ 0.0, 0.0, 0.0 };
+  // Phase 1: persistent at truth but spread still severe -> held (not yet accepted).
+  for (int i = 0; i < 60; ++i)
+  {
+    out = updateGate(truth, 5.0, t, p, s);  // severe spread > spread_accept_max
+    t += kDt;
+  }
+  EXPECT_LT(planarDist(out, Pose2{ 0.0, 0.0, 0.0 }), 0.5) << "must not accept while spread severe";
+
+  // Phase 2: cloud converges (spread drops below the cap) while the pose stays put.
+  for (int i = 0; i < 120; ++i)
+  {
+    out = updateGate(truth, 0.2, t, p, s);  // spread now tight
+    t += kDt;
+  }
+  // THEN the now-converged correction is accepted and the gate recovers to truth.
+  EXPECT_LT(planarDist(out, truth), 0.3);
+}
+
+// Boundary: pin spread_accept_max. A persisted correction just BELOW the cap is accepted;
+// just ABOVE it is held. Probes threshold +/- a small epsilon so the constant cannot
+// drift without this test failing (per test-design threshold+/-1 rule).
+TEST(AmclOdomGate, SpreadAcceptMaxBoundary)
+{
+  constexpr double kEps = 0.1;
+
+  // GIVEN spread just UNDER spread_accept_max: a persistent correction is accepted.
+  {
+    GateParams p;  // spread_accept_max = 3.0
+    // Push the spread-hysteresis band above the probed spread so this case isolates
+    // spread_accept_max alone -- otherwise the 2.9 probe also trips the default spread_hold (1.5),
+    // latching a hold that freezes the alpha ramp part-way and leaves only a ~0.04 m margin.
+    p.spread_hold = p.spread_accept_max + 1.0;
+    p.spread_resume = p.spread_accept_max;
+    GateState s;
+    double t = 0.0;
+    run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t);
+    const Pose2 truth{ 8.0, 0.0, 0.0 };
+    Pose2 out{ 0.0, 0.0, 0.0 };
+    for (int i = 0; i < 120; ++i)
+    {
+      out = updateGate(truth, p.spread_accept_max - kEps, t, p, s);
+      t += kDt;
+    }
+    EXPECT_LT(planarDist(out, truth), 0.3) << "just below the cap must be accepted";
+  }
+
+  // GIVEN spread just OVER spread_accept_max: the same persistent correction is held.
+  {
+    GateParams p;
+    GateState s;
+    double t = 0.0;
+    run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t);
+    const Pose2 truth{ 8.0, 0.0, 0.0 };
+    Pose2 out{ 0.0, 0.0, 0.0 };
+    for (int i = 0; i < 120; ++i)
+    {
+      out = updateGate(truth, p.spread_accept_max + kEps, t, p, s);
+      t += kDt;
+    }
+    EXPECT_LT(planarDist(out, Pose2{ 0.0, 0.0, 0.0 }), 0.5) << "just above the cap must be held";
+  }
+}
+
+// A persistent-but-wrong pose at severe spread must stay bounded near the last good pose
+// for the FULL divergence window -- the gate never drifts toward it (regression for the
+// organic plane-nose cascade where the gate snapped to a 6.7 m wrong lock after ~20 s).
+TEST(AmclOdomGate, SevereSpreadWrongLockStaysBoundedThroughout)
+{
+  GateParams p;
+  GateState s;
+  double t = 0.0;
+  const Pose2 good{ 0.0, 0.0, 0.0 };
+  run(good, 0.2, 1, p, s, t);
+
+  // WHEN AMCL sits at a far wrong pose with severe spread for a long window (~20 s),
+  // far longer than persist_time.
+  const Pose2 wrong{ 6.7, 0.0, 0.0 };
+  double worst = 0.0;
+  for (int i = 0; i < 600; ++i)  // 600 * 0.033 s ~= 20 s
+  {
+    const Pose2 out = updateGate(wrong, 5.0, t, p, s);
+    worst = std::max(worst, planarDist(out, good));
+    t += kDt;
+  }
+  // THEN the broadcast pose never crept toward the wrong lock at any point in the window.
+  EXPECT_LT(worst, 0.5);
+}
+
+// The spread check is part of the persistence requirement, not an instantaneous gate: a
+// wrong pose that persists while spread NOISILY dips below the cap on alternating frames
+// must never ratchet the held pose toward it (the blend is stateful and never reverts).
+TEST(AmclOdomGate, OscillatingSpreadAcrossCapDoesNotRatchet)
+{
+  GateParams p;
+  GateState s;
+  double t = 0.0;
+  run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t);
+
+  // WHEN AMCL sits at a far wrong pose whose cloud spread flickers ABOVE and BELOW the cap
+  // (severe on even frames, just under on odd) -- so the longest continuously-tight run is a
+  // single frame, never persist_time.
+  const Pose2 wrong{ 8.0, 0.0, 0.0 };
+  double worst = 0.0;
+  for (int i = 0; i < 300; ++i)
+  {
+    const double spread = ((i % 2) == 0) ? 5.0 : 2.0;  // 2.0 < spread_accept_max(3.0) < 5.0
+    const Pose2 out = updateGate(wrong, spread, t, p, s);
+    worst = std::max(worst, planarDist(out, Pose2{ 0.0, 0.0, 0.0 }));
+    t += kDt;
+  }
+  // THEN intermittent sub-cap dips never accumulate persist_time, so the held pose stays put.
+  EXPECT_LT(worst, 0.5);
+}
+
+// Case 4 (spread hysteresis): high spread alone holds even for a small innovation,
+// and it resumes only after spread drops below the lower resume threshold.
+TEST(AmclOdomGate, HighSpreadHoldsAndResumesWithHysteresis)
+{
+  GateParams p;
+  GateState s;
+  double t = 0.0;
+  run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t);
+
+  // WHEN spread is high, a small nearby candidate is held (not followed).
+  Pose2 out{ 0.0, 0.0, 0.0 };
+  for (int i = 0; i < 20; ++i)
+  {
+    out = updateGate({ 0.2, 0.0, 0.0 }, 2.0, t, p, s);  // spread 2.0 > spread_hold
+    t += kDt;
+  }
+  EXPECT_LT(planarDist(out, Pose2{ 0.0, 0.0, 0.0 }), 0.15);
+
+  // AND between spread_resume and spread_hold it stays held (hysteresis: no flicker).
+  for (int i = 0; i < 20; ++i)
+  {
+    out = updateGate({ 0.2, 0.0, 0.0 }, 1.0, t, p, s);  // resume(0.6) < 1.0 < hold(1.5)
+    t += kDt;
+  }
+  EXPECT_LT(planarDist(out, Pose2{ 0.0, 0.0, 0.0 }), 0.2);
+
+  // THEN once spread drops below spread_resume it follows AMCL again.
+  for (int i = 0; i < 40; ++i)
+  {
+    out = updateGate({ 0.2, 0.0, 0.0 }, 0.2, t, p, s);
+    t += kDt;
+  }
+  EXPECT_NEAR(out.x, 0.2, 0.03);
+}
diff --git a/src/hangar_sim/test/test_odom_world_drift.cpp b/src/hangar_sim/test/test_odom_world_drift.cpp
new file mode 100644
index 000000000..b1b602d38
--- /dev/null
+++ b/src/hangar_sim/test/test_odom_world_drift.cpp
@@ -0,0 +1,104 @@
+// Copyright 2026 PickNik Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the PickNik Inc. nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+#include <string>
+#include <vector>
+
+#include "hangar_sim/odom_world_drift_logic.hpp"
+
+using odom_world_drift::internal::resolveRailIndices;
+using ::testing::ElementsAre;
+
+// The three rail joints in canonical (x, y, yaw) order, plus decoys the real /joint_states carries.
+TEST(ResolveRailIndices, AllPresentInCanonicalOrder)
+{
+  // GIVEN a name list with the rail joints first, in kRailJoints order
+  const std::vector<std::string> names = { "linear_x_joint", "linear_y_joint", "rotational_yaw_joint" };
+
+  // WHEN resolving
+  const auto idx = resolveRailIndices(names);
+
+  // THEN each rail joint maps to its own position
+  ASSERT_TRUE(idx.has_value());
+  EXPECT_THAT(idx.value(), ElementsAre(0u, 1u, 2u));
+}
+
+TEST(ResolveRailIndices, ResolvesByNameNotColumnOrder)
+{
+  // GIVEN the rail joints interleaved with arm joints and reordered vs kRailJoints
+  const std::vector<std::string> names = { "shoulder_pan_joint", "rotational_yaw_joint", "elbow_joint",
+                                           "linear_x_joint",     "wrist_1_joint",        "linear_y_joint" };
+
+  // WHEN resolving
+  const auto idx = resolveRailIndices(names);
+
+  // THEN indices follow kRailJoints order (x, y, yaw), pointing at the actual columns
+  ASSERT_TRUE(idx.has_value());
+  EXPECT_THAT(idx.value(), ElementsAre(3u, 5u, 1u));
+}
+
+TEST(ResolveRailIndices, MissingAnyRailJointReturnsNullopt)
+{
+  // GIVEN a list missing rotational_yaw_joint (e.g. an arm-only /joint_states publisher)
+  const std::vector<std::string> names = { "linear_x_joint", "linear_y_joint", "elbow_joint" };
+
+  // WHEN resolving
+  const auto idx = resolveRailIndices(names);
+
+  // THEN the whole resolve fails rather than returning a partial/garbage index
+  EXPECT_FALSE(idx.has_value());
+}
+
+TEST(ResolveRailIndices, EmptyNameListReturnsNullopt)
+{
+  // GIVEN an empty name list
+  const std::vector<std::string> names = {};
+
+  // WHEN resolving
+  const auto idx = resolveRailIndices(names);
+
+  // THEN it fails cleanly
+  EXPECT_FALSE(idx.has_value());
+}
+
+TEST(ResolveRailIndices, DuplicateNameTakesFirstOccurrence)
+{
+  // GIVEN linear_x_joint appearing twice
+  const std::vector<std::string> names = { "linear_x_joint", "linear_x_joint", "linear_y_joint",
+                                           "rotational_yaw_joint" };
+
+  // WHEN resolving
+  const auto idx = resolveRailIndices(names);
+
+  // THEN the first occurrence wins (index 0, not 1)
+  ASSERT_TRUE(idx.has_value());
+  EXPECT_THAT(idx.value(), ElementsAre(0u, 2u, 3u));
+}

From 84e170888c79d022b91f81f0cf258167e7963663 Mon Sep 17 00:00:00 2001
From: Breelyn Styler <breelyn.styler@picknik.ai>
Date: Thu, 6 Aug 2026 15:04:41 -0400
Subject: [PATCH 2/2] fix(hangar_sim): extract slip_aware_odom logic, add
 localization staleness guards

Extracts slip_aware_odom's re-anchor/covariance-ramp logic into a pure,
unit-tested header (mirroring amcl_odom_gate's existing pattern), and adds
staleness guards flagged by review: odom_world_drift now withholds
odom->world when /odom_filtered goes stale instead of broadcasting a frozen
estimate with a fresh timestamp; amcl_odom_gate now treats a stale odom
buffer as unavailable (with a dedicated warning) instead of silently
clamping to an old sample; and slip_aware_odom's hold() now stops (and
warns) after a bounded gap instead of fabricating "parked" odometry forever
if a controller dies. Routes the three new nodes' logs to console
(output=both) so their watchdog warnings are actually visible, and drops
internal issue-number references from code comments per
.claude/rules/cpp-style.md.

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
---
 src/hangar_sim/CMakeLists.txt                 |   5 +
 .../control/picknik_ur.ros2_control.yaml      |  16 +-
 .../hangar_sim/slip_aware_odom_logic.hpp      | 115 ++++++++
 .../robot_drivers_to_persist_sim.launch.py    |  18 +-
 src/hangar_sim/src/amcl_odom_gate.cpp         |  33 ++-
 src/hangar_sim/src/odom_world_drift.cpp       |  25 +-
 src/hangar_sim/src/slip_aware_odom.cpp        | 100 +++----
 src/hangar_sim/test/test_slip_aware_odom.cpp  | 260 ++++++++++++++++++
 8 files changed, 497 insertions(+), 75 deletions(-)
 create mode 100644 src/hangar_sim/include/hangar_sim/slip_aware_odom_logic.hpp
 create mode 100644 src/hangar_sim/test/test_slip_aware_odom.cpp

diff --git a/src/hangar_sim/CMakeLists.txt b/src/hangar_sim/CMakeLists.txt
index c055f0155..61482d938 100644
--- a/src/hangar_sim/CMakeLists.txt
+++ b/src/hangar_sim/CMakeLists.txt
@@ -83,6 +83,11 @@ if(BUILD_TESTING)
   ament_add_gmock(test_odom_world_drift test/test_odom_world_drift.cpp)
   target_include_directories(test_odom_world_drift PRIVATE include)
   target_compile_features(test_odom_world_drift PRIVATE cxx_std_20)
+
+  # Unit tests for the pure slip_aware_odom re-anchor/stitch and covariance-ramp logic (no ROS).
+  ament_add_gmock(test_slip_aware_odom test/test_slip_aware_odom.cpp)
+  target_include_directories(test_slip_aware_odom PRIVATE include)
+  target_compile_features(test_slip_aware_odom PRIVATE cxx_std_20)
 endif()
 
 ament_package()
diff --git a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml
index a6446f8a1..eea8e6e45 100644
--- a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml
+++ b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml
@@ -212,19 +212,19 @@ joint_trajectory_controller:
     action_monitor_rate: 20.0
     allow_partial_joints_goal: true
     open_loop_control: false # this is closed loop
-    # Workaround for PickNikRobotics/moveit_pro#20766: trajectory generation
-    # leaves sub-epsilon terminal-velocity residuals that Jazzy's JTC rejects
-    # by default — measured ~1e-4 on `linear_x_joint` from the MTC Cartesian
-    # approach in "ML Move Boxes to Loading Zone", and ~4e-7 from joint
-    # interpolation in "Point-to-Point Trajectory" (previously skipped in
-    # test/objectives_integration_test.py for exactly this rejection).
+    # Workaround: trajectory generation leaves sub-epsilon terminal-velocity
+    # residuals that Jazzy's JTC rejects by default — measured ~1e-4 on
+    # `linear_x_joint` from the MTC Cartesian approach in "ML Move Boxes to
+    # Loading Zone", and ~4e-7 from joint interpolation in "Point-to-Point
+    # Trajectory" (previously skipped in test/objectives_integration_test.py
+    # for exactly this rejection).
     # CAUTION: this flag is binary and controller-wide — it disables the
     # endpoint-still-moving check for every magnitude on every joint,
     # including the mecanum base, where a genuinely nonzero terminal velocity
     # means the base is still translating at trajectory end and then snaps to
     # a position hold. Acceptable for this sim config; re-evaluate before
-    # copying to real hardware. Remove when moveit_pro#20766 clamps the
-    # residuals at trajectory generation.
+    # copying to real hardware. Remove once trajectory generation clamps the
+    # residuals upstream.
     allow_nonzero_velocity_at_trajectory_end: true
     gains:
       shoulder_pan_joint:
diff --git a/src/hangar_sim/include/hangar_sim/slip_aware_odom_logic.hpp b/src/hangar_sim/include/hangar_sim/slip_aware_odom_logic.hpp
new file mode 100644
index 000000000..415d67a11
--- /dev/null
+++ b/src/hangar_sim/include/hangar_sim/slip_aware_odom_logic.hpp
@@ -0,0 +1,115 @@
+// Copyright 2026 PickNik Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the PickNik Inc. nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+// Pure re-anchor/stitch and slip-covariance-ramp logic for slip_aware_odom, split out so it is
+// unit-testable without ROS (see test/test_slip_aware_odom.cpp). The node (src/slip_aware_odom.cpp)
+// does the ROS I/O and calls updateRelay/computeHoldStamp.
+
+#include <algorithm>
+#include <cmath>
+#include <optional>
+
+#include "hangar_sim/se2.hpp"
+
+namespace slip_aware_odom
+{
+using hangar_sim::se2::compose;
+using hangar_sim::se2::invert;
+using hangar_sim::se2::Pose2;
+
+// Tunables for the relay/covariance-ramp decision.
+struct RelayParams
+{
+  double base_cov = 0.03;   // straight-line yaw covariance
+  double spin_cov = 10.0;   // fully-slipping yaw covariance: large enough to effectively drop the
+                            // wheel yaw so fuse rides the IMU
+  double tau = 0.5;         // slip relaxation time constant [s]
+  double slip_full = 0.5;   // leaky-integrated |yaw rate| for full distrust
+  double max_dt = 0.1;      // clamp dt so a stale gap can't spike the integral
+  double source_gap = 0.2;  // a source quiet longer than this counts as switched out [s]
+};
+
+// Mutable state carried between relay updates.
+struct RelayState
+{
+  Pose2 out{ 0.0, 0.0, 0.0 };     // continuous output pose
+  Pose2 offset{ 0.0, 0.0, 0.0 };  // current source -> output-frame offset
+  int active = -1;                // source id currently being forwarded
+  double slip = 0.0;
+  double last_t = 0.0;
+};
+
+struct RelayResult
+{
+  Pose2 pose;      // odom pose to publish this update
+  double yaw_cov;  // pose-yaw / twist-yaw-rate covariance to publish alongside it
+};
+
+// Re-anchor `p` (the active source's pose at time `t`, source id `source`) onto the continuous
+// output stream, and ramp the yaw covariance from base_cov toward spin_cov as |yaw_rate| accumulates
+// (leaky integral, decaying with tau). On a source switch -- or a stale gap longer than
+// params.source_gap -- re-anchor so the output continues seamlessly: pick the offset for which
+// compose(offset, p) equals the last output pose. Pure -- unit-tested in isolation.
+[[nodiscard]] inline RelayResult updateRelay(const Pose2& p, double t, double yaw_rate, int source,
+                                             const RelayParams& params, RelayState& s)
+{
+  if (source != s.active || s.last_t <= 0.0 || t - s.last_t > params.source_gap)
+  {
+    s.offset = compose(s.out, invert(p));
+    s.active = source;
+  }
+  s.out = compose(s.offset, p);
+
+  // Full elapsed time drives the decay -- a long gap (e.g. a nav <-> whole-body handoff silence)
+  // SHOULD relax the integral toward base_cov. The clamped value drives only the growth term, so a
+  // stale gap cannot spike |yaw rate| * dt. Under normal high-rate relay the two are equal.
+  const double elapsed = (s.last_t > 0.0) ? std::max(t - s.last_t, 0.0) : 0.0;
+  const double dt = std::min(elapsed, params.max_dt);
+  s.last_t = t;
+  s.slip = s.slip * std::exp(-elapsed / params.tau) + std::abs(yaw_rate) * dt;
+  const double cov = params.base_cov + (params.spin_cov - params.base_cov) * std::min(1.0, s.slip / params.slip_full);
+
+  return { s.out, cov };
+}
+
+// If the active source has been quiet for at least hold_gap [s] (gap = now - last relayed message
+// time, on the node clock), return the stamp [s] to publish a held (zero-velocity) sample at:
+// last_t advanced by the elapsed gap, so held samples stay on the relayed stream's clock base and
+// strictly increase. nullopt if nothing has been relayed yet (active < 0) or the gap hasn't reached
+// hold_gap. Pure -- unit-tested in isolation.
+[[nodiscard]] inline std::optional<double> computeHoldStamp(int active, double last_t, double gap, double hold_gap)
+{
+  if (active < 0 || gap < hold_gap)
+  {
+    return std::nullopt;
+  }
+  return last_t + gap;
+}
+}  // namespace slip_aware_odom
diff --git a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
index 35d57e0ad..ce6eebfcb 100644
--- a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
+++ b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
@@ -211,7 +211,7 @@ def generate_launch_description():
 
     declare_use_fuse_cmd = DeclareLaunchArgument(
         "use_fuse",
-        default_value="true",  # #19667: fuse drives sim odometry (was "false")
+        default_value="true",
         description="Whether to launch the fuse state estimator",
     )
 
@@ -348,8 +348,8 @@ def generate_launch_description():
     # MuJoCo broadcast a competing odom->ridgeback_base_link TF — removed in this
     # change.) The UI (pose-utils.ts) hardcodes 'world' for user-clicked poses, so
     # this link also keeps nav2 goals transformable to 'map'.
-    # odom -> world. Without fuse it's a static identity (odom == world). With fuse
-    # (#19667), odom_world_drift replaces it with a live transform so odom -> base
+    # odom -> world. Without fuse it's a static identity (odom == world). With fuse,
+    # odom_world_drift replaces it with a live transform so odom -> base
     # resolves to fuse's drifty estimate, giving AMCL real drift to correct while
     # world -> base stays ground truth for whole-body.
     static_tf_odom_to_world = Node(
@@ -366,7 +366,9 @@ def generate_launch_description():
         package="hangar_sim",
         executable="odom_world_drift",
         name="odom_world_drift",
-        output="log",
+        # "both": this node's warnings are operator-facing signal, not noise -- log-only would
+        # route them to ~/.ros/log/ on the (per this package's harness notes) doomed container fs.
+        output="both",
         respawn=LaunchConfiguration("use_respawn"),
         respawn_delay=2.0,
         parameters=[{"use_sim_time": use_sim_time}],
@@ -380,7 +382,8 @@ def generate_launch_description():
         package="hangar_sim",
         executable="slip_aware_odom",
         name="slip_aware_odom",
-        output="log",
+        # "both": see odom_world_drift's comment above.
+        output="both",
         respawn=LaunchConfiguration("use_respawn"),
         respawn_delay=2.0,
         parameters=[{"use_sim_time": use_sim_time}],
@@ -414,7 +417,10 @@ def generate_launch_description():
         package="hangar_sim",
         executable="amcl_odom_gate",
         name="amcl_odom_gate",
-        output="log",
+        # "both": the gate's coast/starvation watchdogs and parameter-validation warnings are
+        # exactly the operator-facing signal this node exists to surface -- see odom_world_drift's
+        # comment above.
+        output="both",
         respawn=LaunchConfiguration("use_respawn"),
         # Shorter than the other nodes: this is the sole map->odom publisher, and AMCL's
         # transform_tolerance is 1.0 s, so keep the crash-recovery gap under that to avoid
diff --git a/src/hangar_sim/src/amcl_odom_gate.cpp b/src/hangar_sim/src/amcl_odom_gate.cpp
index bdb1a94c1..810a22041 100644
--- a/src/hangar_sim/src/amcl_odom_gate.cpp
+++ b/src/hangar_sim/src/amcl_odom_gate.cpp
@@ -49,6 +49,7 @@
 
 #include <chrono>
 #include <cmath>
+#include <optional>
 #include <vector>
 
 #include <tf2_ros/buffer.h>
@@ -77,6 +78,13 @@ constexpr int64_t kStaleWarnThrottleMs = 2000;  // min interval between stale-in
 constexpr double kOdomBufferSec = 1.0;          // odom->base history kept for latency compensation [s].
                                                 // Usable compensation ceiling is this minus the newest
                                                 // sample's age (the odom TF lag), not the full window.
+constexpr double kOdomBufferStaleSec = 0.5;     // treat the odom buffer as unavailable once its newest
+                                                // sample is older than this: appendOdomSample never empties
+                                                // the buffer on a running TF failure (it only clears on a
+                                                // backward clock jump), so without this age check a
+                                                // persistently-failing odom->base lookup would silently
+                                                // clamp interpolateOdom to an ever-staler sample forever
+                                                // instead of falling through to the retry/skip path below.
 }  // namespace
 
 using amcl_odom_gate::Pose2;
@@ -190,8 +198,14 @@ class AmclOdomGate : public rclcpp::Node
     // candidate by a constant offset that persistence would then accept as a real correction.
     const double now_sec = this->now().seconds();
     const double odom_target = rclcpp::Time(cloud.header.stamp).seconds() - params_.latency_compensation_sec;
+    // A non-empty buffer whose newest sample is stale means the odom->base TF lookup has been
+    // failing (see kOdomBufferStaleSec above), not that a fresh sample is simply pending -- treat
+    // it the same as an empty buffer and fall through to the retry/skip path instead of silently
+    // clamping to an arbitrarily old pose.
+    const bool odom_buf_fresh = !odom_buf_.empty() && (now_sec - odom_buf_.back().t) <= kOdomBufferStaleSec;
     Pose2 odom_base{ 0.0, 0.0, 0.0 };
-    if (const auto interp = amcl_odom_gate::interpolateOdom(odom_buf_, odom_target); interp.has_value())
+    if (const auto interp = odom_buf_fresh ? amcl_odom_gate::interpolateOdom(odom_buf_, odom_target) : std::nullopt;
+        interp.has_value())
     {
       odom_base = interp.value();
       if (params_.latency_compensation_sec > 0.0 && !odom_buf_.empty() && odom_target > odom_buf_.back().t)
@@ -207,9 +221,20 @@ class AmclOdomGate : public rclcpp::Node
     }
     else
     {
-      // interpolateOdom returns nullopt only when odom_buf_ is empty, which -- given the
-      // sampleOdomForBuffer() at the top of this callback -- only happens in the narrow startup
-      // window where that lookup just failed. Retry once here; if it still fails, skip this cloud.
+      // Reached when the buffer is empty (narrow startup window) or its newest sample is stale
+      // (odom->base TF lookup has been failing -- see kOdomBufferStaleSec above). Distinguish the
+      // two: an empty buffer at startup is expected and not worth warning about, but a non-empty,
+      // stale buffer means the TF lookup was working and then stopped -- surface that specifically
+      // rather than relying on the generic starvation watchdog in publish() to eventually notice.
+      if (!odom_buf_.empty())
+      {
+        RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), kStaleWarnThrottleMs,
+                             "amcl_odom_gate: odom->base buffer is stale (newest sample is %.2fs old, > "
+                             "kOdomBufferStaleSec=%.2fs) -- the odom->ridgeback_base_link TF lookup has "
+                             "stopped succeeding.",
+                             now_sec - odom_buf_.back().t, kOdomBufferStaleSec);
+      }
+      // Retry once here; if it still fails, skip this cloud.
       try
       {
         const auto t = tf_buffer_.lookupTransform("odom", "ridgeback_base_link", tf2::TimePointZero);
diff --git a/src/hangar_sim/src/odom_world_drift.cpp b/src/hangar_sim/src/odom_world_drift.cpp
index 7ada16452..f44ced44d 100644
--- a/src/hangar_sim/src/odom_world_drift.cpp
+++ b/src/hangar_sim/src/odom_world_drift.cpp
@@ -27,7 +27,7 @@
 // POSSIBILITY OF SUCH DAMAGE.
 
 // Publish odom -> world so that odom -> base equals fuse's (drifty) estimate,
-// while robot_state_publisher keeps world -> base as ground truth (#19667).
+// while robot_state_publisher keeps world -> base as ground truth.
 //
 //     odom -> world = fuse_estimate(odom->base) (+) inverse(world->base_true)
 // makes the TF lookup odom -> base resolve to fuse's estimate; AMCL then has real
@@ -70,7 +70,11 @@ using hangar_sim::se2::Pose2;
 using odom_world_drift::internal::kRailJoints;
 using odom_world_drift::internal::resolveRailIndices;
 
-constexpr double kPubPeriod = 0.02;  // 50 Hz, keeps odom->world fresh for AMCL
+constexpr double kPubPeriod = 0.02;   // 50 Hz, keeps odom->world fresh for AMCL
+constexpr double kEstStaleSec = 0.5;  // ~5x fuse's ~10 Hz publish period: treat /odom_filtered as
+                                      // stale (fuse crashed/stalled) beyond this and stop
+                                      // broadcasting, rather than replaying a frozen estimate
+                                      // against a still-advancing ground-truth base_ stamp.
 }  // namespace
 
 class OdomWorldDrift : public rclcpp::Node
@@ -97,6 +101,8 @@ class OdomWorldDrift : public rclcpp::Node
   {
     const auto& p = m.pose.pose;
     est_ = Pose2{ p.position.x, p.position.y, tf2::getYaw(p.orientation) };
+    est_stamp_ = get_clock()->now();  // arrival time, not the sender's stamp: staleness is judged
+                                      // against how long WE have gone without a fresh sample.
   }
 
   void onJoints(const sensor_msgs::msg::JointState& msg)
@@ -133,6 +139,20 @@ class OdomWorldDrift : public rclcpp::Node
     {
       return;
     }
+    // base_ keeps updating from /joint_states at 50 Hz independent of fuse's health, so a stale
+    // est_ (fuse crashed or stalled -- now recoverable via respawn) would otherwise broadcast
+    // odom->world with an advancing stamp but frozen content: the TF lookup stays "available" and
+    // AMCL silently localizes against a base that appears not to be moving. Withhold instead.
+    const double est_age = (get_clock()->now() - est_stamp_).seconds();
+    if (est_age > kEstStaleSec)
+    {
+      RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000,
+                           "odom_world_drift: /odom_filtered is %.2fs stale (fuse down?) -- "
+                           "withholding odom->world so downstream TF lookups fail loudly instead "
+                           "of localizing against a frozen estimate.",
+                           est_age);
+      return;
+    }
     // odom->world = odom->base(est) (+) inverse(world->base(true))
     // Note: est (fuse, ~10 Hz) and base (rail joints, ~50 Hz) are the latest of each, sampled at
     // different instants, so during motion this carries up to ~speed*0.1s of timing-skew noise
@@ -155,6 +175,7 @@ class OdomWorldDrift : public rclcpp::Node
   // which share the node's default mutually-exclusive callback group -- so they never run
   // concurrently and the state needs no locking.
   std::optional<Pose2> est_;                                        // fuse estimate odom->base
+  rclcpp::Time est_stamp_;                                          // arrival time of the last est_
   std::optional<Pose2> base_;                                       // ground-truth world->base
   std::vector<std::string> cached_names_;                           // /joint_states name list idx_ resolves against
   std::optional<std::array<std::size_t, kRailJoints.size()>> idx_;  // cached rail-joint indices into that list
diff --git a/src/hangar_sim/src/slip_aware_odom.cpp b/src/hangar_sim/src/slip_aware_odom.cpp
index 9406a027d..d17af4974 100644
--- a/src/hangar_sim/src/slip_aware_odom.cpp
+++ b/src/hangar_sim/src/slip_aware_odom.cpp
@@ -46,8 +46,6 @@
 // then never loses its wheel constraint and cannot coast, which otherwise jumped
 // the estimate ~1 m at the switch and flipped the map during whole-body motion.
 
-#include <algorithm>
-#include <chrono>
 #include <cmath>
 
 #include <nav_msgs/msg/odometry.hpp>
@@ -55,27 +53,24 @@
 #include <tf2/utils.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
-#include "hangar_sim/se2.hpp"
+#include "hangar_sim/slip_aware_odom_logic.hpp"
 
 namespace
 {
-constexpr double kBaseCov = 0.03;     // straight-line yaw covariance
-constexpr double kSpinCov = 10.0;     // fully-slipping yaw covariance: large enough to
-                                      // effectively drop the wheel yaw so fuse rides the
-                                      // IMU (its orientation tracks truth to ~0.1 deg even
-                                      // mid-spin, where the mecanum wheels slip badly)
-constexpr double kTau = 0.5;          // slip relaxation time constant [s]
-constexpr double kSlipFull = 0.5;     // leaky-integrated |yaw rate| for full distrust
-constexpr double kMaxDt = 0.1;        // clamp dt so a stale gap can't spike the integral
-constexpr double kSourceGap = 0.2;    // a source quiet longer than this counts as switched out [s]
-constexpr int kYaw = 35;              // (yaw, yaw) pose element and (vyaw, vyaw) twist element
-constexpr double kHoldPeriod = 0.02;  // 50 Hz: rate at which to check for a silent source
-constexpr double kHoldGap = 0.015;    // hold the last pose once the source is quiet this long [s]
+constexpr int kYaw = 35;                          // (yaw, yaw) pose element and (vyaw, vyaw) twist element
+constexpr double kHoldPeriod = 0.02;              // 50 Hz: rate at which to check for a silent source
+constexpr double kHoldGap = 0.015;                // hold the last pose once the source is quiet this long [s]
+constexpr double kHoldMaxSec = 0.5;               // stop holding (and warn) once the source has been quiet this
+                                                  // long: well beyond the ~15 ms handoff beat kHoldGap bridges,
+                                                  // so a hold reaching this means the active controller died,
+                                                  // not a handoff -- keep publishing a fabricated "parked" state
+                                                  // forever would tell fuse the base is stationary indefinitely.
+constexpr int64_t kHoldMaxWarnThrottleMs = 2000;  // min interval between hold-timeout warnings [ms]
 
-// SE(2) primitives shared with amcl_odom_gate (se2.hpp).
-using hangar_sim::se2::compose;
-using hangar_sim::se2::invert;
-using hangar_sim::se2::Pose2;
+using slip_aware_odom::Pose2;
+using slip_aware_odom::RelayParams;
+using slip_aware_odom::RelayState;
+using slip_aware_odom::updateRelay;
 }  // namespace
 
 class SlipAwareOdom : public rclcpp::Node
@@ -119,36 +114,23 @@ class SlipAwareOdom : public rclcpp::Node
 
     // Only the active controller publishes odom (the inactive one is silent), so the
     // two sources never interleave and a source change always means a genuine handoff.
-    // On that switch or a stale gap, re-anchor so the output continues seamlessly:
-    // pick the offset for which compose(offset, p) == the last output pose.
-    if (source != active_ || last_t_ <= 0.0 || t - last_t_ > kSourceGap)
-    {
-      offset_ = compose(out_, invert(p));
-      active_ = source;
-    }
-    out_ = compose(offset_, p);
-
-    // Full elapsed time drives the decay -- a long gap (e.g. a nav <-> whole-body handoff silence)
-    // SHOULD relax the integral toward kBaseCov. The clamped value drives only the growth term, so a
-    // stale gap cannot spike |yaw rate| * dt. Under normal ~500 Hz relay the two are equal (dt << kMaxDt).
-    const double elapsed = (last_t_ > 0.0) ? std::max(t - last_t_, 0.0) : 0.0;
-    const double dt = std::min(elapsed, kMaxDt);
-    last_t_ = t;
-    // Leaky integral of |yaw rate|: grows over sustained spin, decays with kTau.
-    slip_ = slip_ * std::exp(-elapsed / kTau) + std::abs(m.twist.twist.angular.z) * dt;
-    const double cov = kBaseCov + (kSpinCov - kBaseCov) * std::min(1.0, slip_ / kSlipFull);
+    // updateRelay re-anchors on that switch (or a stale gap) so the output continues
+    // seamlessly, and ramps the yaw covariance toward spin_cov as the leaky-integrated
+    // |yaw rate| accumulates. Pure logic lives in slip_aware_odom_logic.hpp -- unit-tested
+    // in test/test_slip_aware_odom.cpp.
+    const auto result = updateRelay(p, t, m.twist.twist.angular.z, source, relay_params_, relay_state_);
 
     odom_.header.stamp = m.header.stamp;
-    odom_.pose.pose.position.x = out_.x;
-    odom_.pose.pose.position.y = out_.y;
-    odom_.pose.pose.orientation.z = std::sin(out_.yaw / 2.0);
-    odom_.pose.pose.orientation.w = std::cos(out_.yaw / 2.0);
+    odom_.pose.pose.position.x = result.pose.x;
+    odom_.pose.pose.position.y = result.pose.y;
+    odom_.pose.pose.orientation.z = std::sin(result.pose.yaw / 2.0);
+    odom_.pose.pose.orientation.w = std::cos(result.pose.yaw / 2.0);
     odom_.pose.covariance = m.pose.covariance;  // keep the controller's x/y covariance
     odom_.twist = m.twist;                      // body-frame twist is source-independent
     // Grow both the pose-yaw and twist-yaw-rate covariance so fuse defers to the IMU
     // gyro -- for orientation and yaw rate alike -- while the mecanum wheels slip.
-    odom_.pose.covariance[kYaw] = cov;
-    odom_.twist.covariance[kYaw] = cov;
+    odom_.pose.covariance[kYaw] = result.yaw_cov;
+    odom_.twist.covariance[kYaw] = result.yaw_cov;
     pub_->publish(odom_);
     last_relay_ = now();
   }
@@ -159,19 +141,29 @@ class SlipAwareOdom : public rclcpp::Node
   // normal driving the ~500 Hz stream keeps last_relay_ fresh and this no-ops.
   void hold()
   {
-    if (active_ < 0)
+    const double gap = (now() - last_relay_).seconds();
+    if (gap > kHoldMaxSec)
     {
+      // The active source has been quiet far longer than a handoff beat -- it died (crashed
+      // controller, deactivated with nothing taking over) rather than switched. Stop fabricating
+      // zero-velocity odometry: let fuse's wheel constraint lapse loudly instead of asserting
+      // "parked" indefinitely.
+      RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), kHoldMaxWarnThrottleMs,
+                           "slip_aware_odom: active source has been silent for %.2fs (> %.2fs) -- "
+                           "treating it as dead, not a handoff. Withholding held odometry.",
+                           gap, kHoldMaxSec);
       return;
     }
-    const double gap = (now() - last_relay_).seconds();
-    if (gap < kHoldGap)
+    // computeHoldStamp returns the stamp (last relayed message time, on the controllers'
+    // clock, advanced by the elapsed gap) to publish a held sample at, or nullopt if
+    // nothing has been relayed yet or the gap hasn't reached kHoldGap. Pure logic in
+    // slip_aware_odom_logic.hpp -- unit-tested in test/test_slip_aware_odom.cpp.
+    const auto held_opt = slip_aware_odom::computeHoldStamp(relay_state_.active, relay_state_.last_t, gap, kHoldGap);
+    if (!held_opt.has_value())
     {
       return;
     }
-    // Stamp = last relayed message stamp (last_t_, on the controllers' clock) advanced
-    // by the elapsed gap, so held samples stay on the relayed stream's clock base and
-    // strictly increase -- independent of whether the node clock matches it.
-    const double held = last_t_ + gap;
+    const double held = held_opt.value();
     odom_.header.stamp.sec = static_cast<int32_t>(held);
     odom_.header.stamp.nanosec = static_cast<uint32_t>((held - std::floor(held)) * 1e9);
     odom_.twist.twist.linear.x = 0.0;
@@ -185,11 +177,9 @@ class SlipAwareOdom : public rclcpp::Node
   // concurrently and the state needs no locking. Do NOT move either callback to a
   // separate or reentrant callback group without adding synchronization.
   nav_msgs::msg::Odometry odom_;
-  Pose2 out_{ 0.0, 0.0, 0.0 };     // continuous output pose
-  Pose2 offset_{ 0.0, 0.0, 0.0 };  // current source -> output-frame offset
-  int active_ = -1;                // source id currently being forwarded
-  double slip_ = 0.0, last_t_ = 0.0;
-  rclcpp::Time last_relay_;  // node-clock time of the last forwarded message
+  RelayParams relay_params_;  // relay/covariance-ramp tunables (defaults match the prior constants)
+  RelayState relay_state_;    // mutable re-anchor/slip state, updated in relay()
+  rclcpp::Time last_relay_;   // node-clock time of the last forwarded message
   // ROS entities declared last so they destruct (and stop firing callbacks) before
   // the state above that those callbacks read.
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_;
diff --git a/src/hangar_sim/test/test_slip_aware_odom.cpp b/src/hangar_sim/test/test_slip_aware_odom.cpp
new file mode 100644
index 000000000..7b1b4a5e1
--- /dev/null
+++ b/src/hangar_sim/test/test_slip_aware_odom.cpp
@@ -0,0 +1,260 @@
+// Copyright 2026 PickNik Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the PickNik Inc. nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+#include "hangar_sim/slip_aware_odom_logic.hpp"
+
+using slip_aware_odom::computeHoldStamp;
+using slip_aware_odom::Pose2;
+using slip_aware_odom::RelayParams;
+using slip_aware_odom::RelayState;
+using slip_aware_odom::updateRelay;
+using ::testing::DoubleNear;
+
+namespace
+{
+constexpr double kTol = 1e-6;
+}  // namespace
+
+// -------------------- updateRelay: first sample / continuity --------------------
+
+TEST(UpdateRelay, FirstSampleSeedsOutputAtOriginRegardlessOfSourcePose)
+{
+  // GIVEN fresh state (out_ default-constructed at the origin, nothing relayed yet)
+  RelayState state;
+  const RelayParams params;
+
+  // WHEN relaying the first sample, from an arbitrary non-origin source pose
+  const auto result = updateRelay(Pose2{ 1.0, 2.0, 0.5 }, /*t=*/10.0, /*yaw_rate=*/0.0, /*source=*/0, params, state);
+
+  // THEN the output stream is seeded at the origin, not the raw source pose: with no prior output
+  // to preserve continuity from, the re-anchor (offset = compose(out=identity, invert(p))) makes
+  // compose(offset, p) collapse back to identity. This is the documented contract -- the published
+  // stream is re-anchored, not absolute -- and is why a single relayed message can never be trusted
+  // as an absolute pose.
+  EXPECT_THAT(result.pose.x, DoubleNear(0.0, kTol));
+  EXPECT_THAT(result.pose.y, DoubleNear(0.0, kTol));
+  EXPECT_THAT(result.pose.yaw, DoubleNear(0.0, kTol));
+  EXPECT_EQ(state.active, 0);
+}
+
+TEST(UpdateRelay, SameSourceContinuesWithoutReanchor)
+{
+  // GIVEN state anchored by an initial sample from source 0
+  RelayState state;
+  const RelayParams params;
+  updateRelay(Pose2{ 0.0, 0.0, 0.0 }, 10.0, 0.0, 0, params, state);
+
+  // WHEN a second sample from the SAME source arrives, shifted by 2 m
+  const auto result = updateRelay(Pose2{ 2.0, 0.0, 0.0 }, 10.1, 0.0, 0, params, state);
+
+  // THEN the output tracks the source's own delta directly (offset is unchanged)
+  EXPECT_THAT(result.pose.x, DoubleNear(2.0, kTol));
+}
+
+TEST(UpdateRelay, SourceSwitchReanchorsSeamlessly)
+{
+  // GIVEN a stream anchored at the origin by source 0, then driven to output x=5
+  RelayState state;
+  const RelayParams params;
+  updateRelay(Pose2{ 0.0, 0.0, 0.0 }, 10.0, 0.0, 0, params, state);
+  updateRelay(Pose2{ 5.0, 0.0, 0.0 }, 10.02, 0.0, 0, params, state);
+
+  // WHEN source 1 takes over publishing its own pose (a different coordinate origin, e.g. x=0)
+  const auto result = updateRelay(Pose2{ 0.0, 0.0, 0.0 }, 10.05, 0.0, /*source=*/1, params, state);
+
+  // THEN the output continues from the last output pose (no jump) -- re-anchored at the switch
+  EXPECT_THAT(result.pose.x, DoubleNear(5.0, kTol));
+  EXPECT_EQ(state.active, 1);
+}
+
+TEST(UpdateRelay, StaleGapPastSourceGapReanchorsEvenOnSameSource)
+{
+  // GIVEN a stream anchored at the origin by source 0, then driven to output x=5
+  RelayState state;
+  RelayParams params;
+  params.source_gap = 0.2;
+  updateRelay(Pose2{ 0.0, 0.0, 0.0 }, 10.0, 0.0, 0, params, state);
+  updateRelay(Pose2{ 5.0, 0.0, 0.0 }, 10.02, 0.0, 0, params, state);
+
+  // WHEN the SAME source resumes after a gap longer than source_gap, at a pose that jumped (e.g. the
+  // controller reset its own internal odom)
+  const auto result = updateRelay(Pose2{ 100.0, 0.0, 0.0 }, 10.02 + params.source_gap + 0.01, 0.0, 0, params, state);
+
+  // THEN the output re-anchors instead of jumping to 100
+  EXPECT_THAT(result.pose.x, DoubleNear(5.0, kTol));
+}
+
+TEST(UpdateRelay, GapAtExactlySourceGapDoesNotReanchor)
+{
+  // GIVEN a stream anchored at the origin by source 0, then driven to output x=5
+  RelayState state;
+  RelayParams params;
+  params.source_gap = 0.2;
+  updateRelay(Pose2{ 0.0, 0.0, 0.0 }, 10.0, 0.0, 0, params, state);
+  updateRelay(Pose2{ 5.0, 0.0, 0.0 }, 10.02, 0.0, 0, params, state);
+
+  // WHEN the same source resumes at EXACTLY the source_gap threshold (not longer than it)
+  const auto result = updateRelay(Pose2{ 105.0, 0.0, 0.0 }, 10.02 + params.source_gap, 0.0, 0, params, state);
+
+  // THEN no re-anchor happens (the gap must be strictly greater than source_gap) -- output tracks
+  // the source's own delta through the unchanged offset: source moved +100 from its anchor pose
+  // (5,0,0), so output moves +100 from its own anchor (5,0,0) too.
+  EXPECT_THAT(result.pose.x, DoubleNear(105.0, kTol));
+}
+
+// -------------------- updateRelay: yaw-covariance ramp --------------------
+
+TEST(UpdateRelay, NoYawRateKeepsCovarianceAtBase)
+{
+  // GIVEN fresh state
+  RelayState state;
+  const RelayParams params;
+
+  // WHEN relaying several samples with zero yaw rate (driving straight)
+  auto result = updateRelay(Pose2{}, 10.0, 0.0, 0, params, state);
+  result = updateRelay(Pose2{}, 10.1, 0.0, 0, params, state);
+  result = updateRelay(Pose2{}, 10.2, 0.0, 0, params, state);
+
+  // THEN the yaw covariance stays at base_cov
+  EXPECT_THAT(result.yaw_cov, DoubleNear(params.base_cov, kTol));
+}
+
+TEST(UpdateRelay, SustainedSpinRampsCovarianceTowardSpinCov)
+{
+  // GIVEN fresh state and a spin fast/long enough to fully saturate the leaky integral
+  // (|yaw_rate| * dt accumulated over many updates exceeds slip_full)
+  RelayState state;
+  const RelayParams params;
+  double t = 10.0;
+
+  // WHEN relaying many samples at a sustained high yaw rate
+  auto result = updateRelay(Pose2{}, t, 0.0, 0, params, state);
+  for (int i = 0; i < 200; ++i)
+  {
+    t += 0.05;
+    result = updateRelay(Pose2{}, t, /*yaw_rate=*/5.0, 0, params, state);
+  }
+
+  // THEN the covariance saturates at spin_cov (clamped, not overshooting)
+  EXPECT_THAT(result.yaw_cov, DoubleNear(params.spin_cov, 1e-3));
+}
+
+TEST(UpdateRelay, CovarianceDecaysAfterSpinStops)
+{
+  // GIVEN state that has ramped up to a high slip covariance from sustained spin
+  RelayState state;
+  const RelayParams params;
+  double t = 10.0;
+  auto result = updateRelay(Pose2{}, t, 0.0, 0, params, state);
+  for (int i = 0; i < 200; ++i)
+  {
+    t += 0.05;
+    result = updateRelay(Pose2{}, t, 5.0, 0, params, state);
+  }
+  const double spiking_cov = result.yaw_cov;
+  ASSERT_GT(spiking_cov, params.base_cov + 1.0);
+
+  // WHEN the spin stops and several tau-scale gaps of straight driving follow
+  for (int i = 0; i < 20; ++i)
+  {
+    t += params.tau;
+    result = updateRelay(Pose2{}, t, 0.0, 0, params, state);
+  }
+
+  // THEN the covariance decays back toward base_cov
+  EXPECT_THAT(result.yaw_cov, DoubleNear(params.base_cov, 1e-2));
+}
+
+TEST(UpdateRelay, MaxDtClampsGrowthButNotDecayOnStaleGap)
+{
+  // GIVEN state with a moderate slip level
+  RelayState state;
+  RelayParams params;
+  params.max_dt = 0.1;
+  double t = 10.0;
+  auto result = updateRelay(Pose2{}, t, 0.0, 0, params, state);
+  result = updateRelay(Pose2{}, t + 0.05, 5.0, 0, params, state);
+  const double slip_before = state.slip;
+
+  // WHEN a stale gap much longer than max_dt arrives (e.g. a handoff silence) with a high yaw rate
+  result = updateRelay(Pose2{}, t + 5.05, /*yaw_rate=*/5.0, 0, params, state);
+
+  // THEN the growth contribution is bounded by max_dt (slip cannot spike by yaw_rate * 5.0s), even
+  // though the decay term used the full 5.0s elapsed
+  const double max_possible_growth = 5.0 * params.max_dt;
+  EXPECT_LE(state.slip, slip_before * std::exp(-5.0 / params.tau) + max_possible_growth + kTol);
+}
+
+// -------------------- computeHoldStamp --------------------
+
+TEST(ComputeHoldStamp, NothingRelayedYetReturnsNullopt)
+{
+  // GIVEN no source has been relayed yet (active < 0)
+  // WHEN checking whether to hold
+  const auto held = computeHoldStamp(/*active=*/-1, /*last_t=*/0.0, /*gap=*/1.0, /*hold_gap=*/0.015);
+
+  // THEN nothing is published
+  EXPECT_FALSE(held.has_value());
+}
+
+TEST(ComputeHoldStamp, GapBelowThresholdReturnsNullopt)
+{
+  // GIVEN an active source and a gap shorter than hold_gap
+  // WHEN checking whether to hold
+  const auto held = computeHoldStamp(/*active=*/0, /*last_t=*/10.0, /*gap=*/0.01, /*hold_gap=*/0.015);
+
+  // THEN no held sample is published yet
+  EXPECT_FALSE(held.has_value());
+}
+
+TEST(ComputeHoldStamp, GapAtExactlyThresholdHolds)
+{
+  // GIVEN a gap exactly at hold_gap
+  // WHEN checking whether to hold
+  const auto held = computeHoldStamp(0, 10.0, /*gap=*/0.015, /*hold_gap=*/0.015);
+
+  // THEN it holds (>= threshold triggers)
+  ASSERT_TRUE(held.has_value());
+  EXPECT_THAT(held.value(), DoubleNear(10.015, kTol));
+}
+
+TEST(ComputeHoldStamp, GapPastThresholdAdvancesStampByFullGap)
+{
+  // GIVEN a source quiet well past hold_gap
+  // WHEN checking whether to hold
+  const auto held = computeHoldStamp(0, /*last_t=*/10.0, /*gap=*/0.5, /*hold_gap=*/0.015);
+
+  // THEN the held stamp continues the relayed stream's clock base: last_t + gap
+  ASSERT_TRUE(held.has_value());
+  EXPECT_THAT(held.value(), DoubleNear(10.5, kTol));
+}