Sync the verified Rift S r25 runtime and tracking fixes

src/xrt/drivers/CMakeLists.txt

@@ -350,6 +350,8 @@ if(XRT_BUILD_DRIVER_RIFT_S)
 		rift_s/rift_s_controller.h
 		rift_s/rift_s_position_filter.c
 		rift_s/rift_s_position_filter.h
+		rift_s/rift_s_motion.cpp
+		rift_s/rift_s_motion.h
 		rift_s/rift_s_slam_guard.c
 		rift_s/rift_s_slam_guard.h
 		rift_s/rift_s_firmware.c

src/xrt/drivers/rift_s/rift_s.c

@@ -197,6 +197,12 @@ rift_s_system_create(struct xrt_prober *xp,
 		goto cleanup;
 	}
 
+	sys->motion = rift_s_motion_create();
+	if (sys->motion == NULL) {
+		RIFT_S_ERROR("Failed to allocate controller motion cache");
+		goto cleanup;
+	}
+
 	if (read_hmd_config(hid_hmd, &sys->hmd_config) < 0) {
 		RIFT_S_ERROR("Failed to read HMD configuration");
 		goto cleanup;
@@ -282,6 +288,11 @@ rift_s_system_free(struct rift_s_system *sys)
 	/* Stop the packet reading thread */
 	os_thread_helper_destroy(&sys->oth);
 
+	/* The world worker reads SLAM and camera buffers owned by the frame context. */
+	if (sys->tracker != NULL) {
+		rift_s_tracker_stop(sys->tracker);
+	}
+
 	/* Stop all the frame processing (has to happen before the cameras
 	 * and tracker are destroyed */
 	xrt_frame_context_destroy_nodes(&sys->xfctx);
@@ -310,6 +321,7 @@ rift_s_system_free(struct rift_s_system *sys)
 
 	os_mutex_destroy(&sys->dev_mutex);
 
+	rift_s_motion_destroy(&sys->motion);
 	free(sys);
 }
 

src/xrt/drivers/rift_s/rift_s.h

@@ -29,6 +29,7 @@
 #include "rift_s_protocol.h"
 #include "rift_s_radio.h"
 #include "rift_s_tracker.h"
+#include "rift_s_motion.h"
 
 #ifndef RIFT_S_H
 #define RIFT_S_H
@@ -111,6 +112,8 @@ struct rift_s_system
 
 	/* Controller devices */
 	struct rift_s_controller *controllers[MAX_TRACKED_DEVICES];
+	//! Shared controller motion cache, fed at IMU capture time.
+	struct rift_s_motion *motion;
 
 	/* Video feed handling */
 	struct xrt_frame_context xfctx;

src/xrt/drivers/rift_s/rift_s_controller.c

@@ -188,6 +188,16 @@ attitude_sample(struct rift_s_controller *ctrl, unsigned int index)
 	return &ctrl->attitude_history[(ctrl->attitude_history_start + index) % RIFT_S_ATTITUDE_HISTORY_CAPACITY];
 }
 
+static void
+suspend_position_tracking(struct rift_s_controller *ctrl)
+{
+	rift_s_position_filter_suspend(&ctrl->position_filter);
+	if (ctrl->sys != NULL) {
+		rift_s_motion_reset_hand(ctrl->sys->motion,
+		                         ctrl->base.device_type == XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER ? 1 : 0);
+	}
+}
+
 /* Forget the device-to-host clock mapping and everything captured against it. The next sample
  * starts a new mapping. Caller holds the mutex. */
 static void
@@ -202,7 +212,7 @@ reset_clock_mapping(struct rift_s_controller *ctrl)
 	ctrl->clock_stable_samples = 0;
 	ctrl->optical_heading_valid = false;
 	// Retained IMU times no longer match the optical capture times: hold the position.
-	rift_s_position_filter_freeze(&ctrl->position_filter);
+	suspend_position_tracking(ctrl);
 }
 
 bool
@@ -262,7 +272,7 @@ rift_s_controller_record_attitude(struct rift_s_controller *ctrl,
 		ctrl->attitude_history_count = ctrl->clock_stable_samples = 0;
 		ctrl->optical_heading_valid = false;
 		// Retained IMU times no longer match the optical capture times: hold the position.
-		rift_s_position_filter_freeze(&ctrl->position_filter);
+		suspend_position_tracking(ctrl);
 		ctrl->diagnostics.reject_clock++;
 		ctrl->diagnostics.reject_clock_by[RIFT_S_CLOCK_DISCONTINUITY]++;
 	}
@@ -651,7 +661,7 @@ reset_fusion_interval(struct rift_s_controller *ctrl, const struct xrt_quat *ori
 	ctrl->gravity_accel_sum = ctrl->gravity_gyro_sum = (struct xrt_vec3){0};
 	ctrl->gravity_moving = false;
 	ctrl->rest = (struct rift_s_rest_stats){0};
-	rift_s_position_filter_freeze(&ctrl->position_filter);
+	suspend_position_tracking(ctrl);
 }
 
 static bool
@@ -813,8 +823,6 @@ static void
 reset_position_filter(struct rift_s_controller *ctrl)
 {
 	rift_s_position_filter_reset(&ctrl->position_filter);
-	ctrl->reacquire_offset = (struct xrt_vec3){0};
-	ctrl->reacquire_started_ns = 0;
 	float accel_noise = debug_get_float_option_rift_s_position_accel_noise();
 	float bias_walk = debug_get_float_option_rift_s_position_bias_walk();
 	if (accel_noise > 0) {
@@ -833,15 +841,13 @@ reset_position_filter(struct rift_s_controller *ctrl)
 #define RIFT_S_POSITION_RESET_DISTANCE_M 0.15f
 // Longest render-time extrapolation of aim/grip.
 #define RIFT_S_MAX_PREDICTION_NS (100 * U_TIME_1MS_IN_NS)
-// Resume the optical filter immediately, but remove the held-pose display offset over 100 ms.
-#define RIFT_S_REACQUIRE_BLEND_NS (100 * U_TIME_1MS_IN_NS)
 
 enum position_tracking
 {
 	POSITION_NONE,     // never fixed: no position estimate
 	POSITION_TRACKED,  // fresh optical fix
 	POSITION_COASTING, // IMU-only, within the position duration of the loss ladder
-	POSITION_LOST,     // held at the last estimate, waiting for an optical reset
+	POSITION_LOST,     // no reliable position confidence
 };
 
 static enum position_tracking
@@ -851,17 +857,13 @@ position_tracking_at(const struct rift_s_controller *ctrl, timepoint_ns t, bool
 	if (!ctrl->position_filter.initialized) {
 		return POSITION_NONE;
 	}
-	if (ctrl->position_filter.frozen) {
+	if (ctrl->position_filter.suspended) {
 		return POSITION_LOST;
 	}
 	time_duration_ns position_duration, velocity_duration;
 	rift_s_tracking_loss_durations(ctrl->track_vision_updates, ctrl->track_reliable_updates, &position_duration,
 	                               &velocity_duration);
 	time_duration_ns age = t > ctrl->last_fix_ns ? t - ctrl->last_fix_ns : 0;
-	if (position_duration > RIFT_S_POSITION_FILTER_BRIDGE_NS)
-		position_duration = RIFT_S_POSITION_FILTER_BRIDGE_NS;
-	if (velocity_duration > position_duration)
-		velocity_duration = position_duration;
 	*velocity_tracked = age <= velocity_duration;
 	if (age > position_duration) {
 		return POSITION_LOST;
@@ -869,23 +871,40 @@ position_tracking_at(const struct rift_s_controller *ctrl, timepoint_ns t, bool
 	return age > RIFT_S_COAST_AFTER_NS ? POSITION_COASTING : POSITION_TRACKED;
 }
 
-// Apply the loss ladder at IMU time: coast, then hold the position once it is lost.
-static void
-update_position_tracking(struct rift_s_controller *ctrl, timepoint_ns capture_ns)
+// Adapt the Monado filter's current uncertainty to the model reliability gate. Normal
+// vision delivery latency must not activate a model merely because capture time is old.
+void
+rift_s_controller_update_motion(struct rift_s_controller *ctrl, timepoint_ns capture_ns)
 {
+	if (ctrl->sys == NULL || ctrl->sys->motion == NULL || !ctrl->position_filter.initialized) {
+		return;
+	}
+	struct xrt_device *head = rift_s_system_get_hmd(ctrl->sys);
+	struct xrt_space_relation head_relation = XRT_SPACE_RELATION_ZERO;
+	if (head == NULL ||
+	    head->get_tracked_pose(head, XRT_INPUT_GENERIC_HEAD_POSE, capture_ns, &head_relation) != XRT_SUCCESS ||
+	    (head_relation.relation_flags &
+	     (XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT)) !=
+	        (XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT)) {
+		return;
+	}
 	bool velocity_tracked;
 	enum position_tracking state = position_tracking_at(ctrl, capture_ns, &velocity_tracked);
-	struct rift_s_position_filter *f = &ctrl->position_filter;
-	f->coasting = state == POSITION_COASTING;
-	if (state == POSITION_LOST && !f->frozen) {
-		rift_s_position_filter_freeze(f);
-		RIFT_S_DEBUG("Controller position hand=%s state=lost age_ms=%.1f vision_updates=%u reliable_updates=%u"
-		             " held=%.3f,%.3f,%.3f",
-		             hand_alias(ctrl), (double)(capture_ns - ctrl->last_fix_ns) / U_TIME_1MS_IN_NS,
-		             ctrl->track_vision_updates, ctrl->track_reliable_updates, f->state.x[0][0],
-		             f->state.x[1][0], f->state.x[2][0]);
-		ctrl->track_vision_updates = ctrl->track_reliable_updates = 0;
+	struct rift_s_motion_sample sample = {0};
+	sample.timestamp_ns = capture_ns;
+	sample.pose.orientation = ctrl->pose.orientation;
+	rift_s_position_filter_predict(&ctrl->position_filter, capture_ns, 0, &sample.pose.position,
+	                               &sample.linear_velocity);
+	sample.linear_acceleration = ctrl->world_accel;
+	sample.angular_velocity = ctrl->fusion.last.gyro;
+	for (int axis = 0; axis < 3; ++axis) {
+		sample.position_variance[axis] =
+		    state != POSITION_LOST && !ctrl->position_filter.suspended && ctrl->track_vision_updates >= 3
+		        ? ctrl->position_filter.state.P[axis][0][0]
+		        : -1;
 	}
+	unsigned int hand = ctrl->base.device_type == XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER ? 1 : 0;
+	rift_s_motion_update(ctrl->sys->motion, hand, &sample, &head_relation.pose);
 }
 
 bool
@@ -1018,8 +1037,8 @@ handle_imu_update(struct rift_s_controller *ctrl,
 		timepoint_ns capture_ns = attitude_sample(ctrl, ctrl->attitude_history_count - 1)->capture_ns;
 		ctrl->last_imu_capture_ns = capture_ns;
 		if (ctrl->world_accel_valid) {
-			update_position_tracking(ctrl, capture_ns);
 			rift_s_position_filter_imu(&ctrl->position_filter, capture_ns, &ctrl->world_accel);
+			rift_s_controller_update_motion(ctrl, capture_ns);
 		}
 	}
 	if (log_imu) {
@@ -1629,22 +1648,8 @@ rift_s_controller_get_fusion_pose(struct rift_s_controller *ctrl,
 	return true;
 }
 
-// Translation smoothing is only for presentation, never the capture-time association prior.
-static bool
-apply_reacquire_offset(const struct rift_s_controller *ctrl, timepoint_ns t, struct xrt_vec3 *position)
-{
-	if (ctrl->reacquire_started_ns == 0 || t >= ctrl->reacquire_started_ns + RIFT_S_REACQUIRE_BLEND_NS) {
-		return false;
-	}
-	double u =
-	    t > ctrl->reacquire_started_ns ? (double)(t - ctrl->reacquire_started_ns) / RIFT_S_REACQUIRE_BLEND_NS : 0;
-	float weight = (float)(1 - u * u * (3 - 2 * u));
-	*position = m_vec3_add(*position, m_vec3_mul_scalar(ctrl->reacquire_offset, weight));
-	return true;
-}
-
-// Aim/grip: gyro orientation and the unchanged IMU/optical position filter. After the bounded
-// bridge, hold a valid estimated position indefinitely with optical confidence cleared.
+// Motion-model estimates are separate from the optical association prior and are applied
+// at the IMU origin. The device/aim/grip offsets are applied afterwards.
 static bool
 predict_aim_grip(struct rift_s_controller *ctrl, int64_t at_timestamp_ns, struct xrt_space_relation *rel)
 {
@@ -1665,20 +1670,27 @@ predict_aim_grip(struct rift_s_controller *ctrl, int64_t at_timestamp_ns, struct
 		struct xrt_vec3 velocity;
 		rift_s_position_filter_predict(&ctrl->position_filter, at_timestamp_ns, RIFT_S_MAX_PREDICTION_NS,
 		                               &rel->pose.position, &velocity);
-		rel->relation_flags |= XRT_SPACE_RELATION_POSITION_VALID_BIT;
 		if (state != POSITION_LOST) {
-			rel->relation_flags |= XRT_SPACE_RELATION_POSITION_TRACKED_BIT;
+			rel->relation_flags |=
+			    XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT;
 		}
 		if (state != POSITION_LOST && velocity_tracked) {
 			rel->linear_velocity = velocity;
 			rel->relation_flags |= XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT;
 		}
-		if (apply_reacquire_offset(ctrl, at_timestamp_ns, &rel->pose.position)) {
-			// SteamVR must not extrapolate the filter velocity across this presentation correction.
-			rel->linear_velocity = (struct xrt_vec3){0};
-			rel->relation_flags &= ~XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT;
+		source = state == POSITION_TRACKED ? "optical" : state == POSITION_COASTING ? "inertial" : "unreliable";
+		if (ctrl->sys != NULL && ctrl->sys->motion != NULL) {
+			struct xrt_space_relation estimate = XRT_SPACE_RELATION_ZERO;
+			unsigned int hand = ctrl->base.device_type == XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER ? 1 : 0;
+			enum rift_s_motion_model model =
+			    rift_s_motion_get(ctrl->sys->motion, hand, at_timestamp_ns, &estimate);
+			if (model != RIFT_S_MOTION_NONE) {
+				*rel = estimate;
+				static const char *const names[] = {"none",           "at_rest",     "near_head",
+				                                    "hand_over_hand", "behind_back", "out_of_fov"};
+				source = names[model];
+		}
 		}
-		source = state == POSITION_TRACKED ? "optical" : state == POSITION_COASTING ? "coast" : "held";
 	}
 	if (source != ctrl->position_source) {
 		bool fallback = state == POSITION_NONE;
@@ -1751,16 +1763,23 @@ rift_s_controller_get_tracked_pose(struct xrt_device *xdev,
 	} else {
 		have_position = predict_aim_grip(ctrl, at_timestamp_ns, rel);
 	}
-	bool position_valid = name == XRT_INPUT_GENERIC_TRACKER_POSE ||
-	                      (rel->relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0;
+	const enum xrt_space_relation_flags position_flags =
+	    rel->relation_flags & (XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
+	// Transform estimated coordinates through the lever arms without promoting confidence.
+	if (name != XRT_INPUT_GENERIC_TRACKER_POSE && have_position) {
+		rel->relation_flags |= XRT_SPACE_RELATION_POSITION_VALID_BIT;
+	}
 	os_mutex_unlock(&ctrl->mutex);
 
 	m_relation_chain_resolve(&xrc, out_relation);
 
-	if (!have_position || !position_valid) {
-		out_relation->relation_flags &=
-		    ~(XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT |
-		      XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
+	if (name != XRT_INPUT_GENERIC_TRACKER_POSE) {
+		out_relation->relation_flags =
+		    (out_relation->relation_flags &
+		     ~(XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT)) |
+		    position_flags;
+		if (!have_position)
+			out_relation->relation_flags &= ~XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT;
 	}
 
 	return XRT_SUCCESS;
@@ -1934,17 +1953,14 @@ fuse_optical_position(struct rift_s_controller *ctrl,
                       timepoint_ns processing_ns)
 {
 	struct xrt_vec3 variance = optical_position_variance(obs->position_covariance);
-	bool was_held = ctrl->position_filter.initialized && ctrl->position_filter.frozen;
-	struct xrt_vec3 held_position = {0}, velocity;
-	if (was_held) {
-		rift_s_position_filter_predict(&ctrl->position_filter, processing_ns, RIFT_S_MAX_PREDICTION_NS,
-		                               &held_position, &velocity);
-		apply_reacquire_offset(ctrl, processing_ns, &held_position);
-	}
-	bool was_lost = !ctrl->position_filter.initialized || ctrl->position_filter.frozen;
+	bool velocity_tracked;
+	bool age_lost = ctrl->position_filter.initialized && !ctrl->position_filter.suspended &&
+	                position_tracking_at(ctrl, obs->capture_ns, &velocity_tracked) == POSITION_LOST;
+	bool was_lost = age_lost || !ctrl->position_filter.initialized || ctrl->position_filter.suspended;
 	enum rift_s_position_fix_result result =
-	    rift_s_position_filter_fix(&ctrl->position_filter, obs->capture_ns, &obs->pose.position, &variance,
-	                               RIFT_S_POSITION_RESET_DISTANCE_M);
+	    (age_lost ? rift_s_position_filter_reacquire : rift_s_position_filter_fix)(
+	    &ctrl->position_filter, obs->capture_ns, &obs->pose.position, &variance,
+	    RIFT_S_POSITION_RESET_DISTANCE_M);
 	if (result == RIFT_S_POSITION_FIX_STALE) {
 		return result;
 	}
@@ -1963,13 +1979,6 @@ fuse_optical_position(struct rift_s_controller *ctrl,
 		return result;
 	}
 	if (result == RIFT_S_POSITION_FIX_RESET) {
-		if (was_held) {
-			struct xrt_vec3 resumed_position;
-			rift_s_position_filter_predict(&ctrl->position_filter, processing_ns, RIFT_S_MAX_PREDICTION_NS,
-			                               &resumed_position, &velocity);
-			ctrl->reacquire_offset = m_vec3_sub(held_position, resumed_position);
-			ctrl->reacquire_started_ns = processing_ns;
-		}
 		// A new track: the loss ladder counts from here.
 		ctrl->track_vision_updates = ctrl->track_reliable_updates = 0;
 		RIFT_S_DEBUG("Controller position hand=%s state=reset reason=%s capture_ns=%" PRIi64
@@ -2016,7 +2025,7 @@ apply_optical_tilt(struct rift_s_controller *ctrl, timepoint_ns capture_ns, cons
 	ctrl->last_optical_tilt_ns = capture_ns;
 }
 
-void
+bool
 rift_s_controller_push_pose_observation(struct xrt_device *xdev, const struct t_constellation_pose_observation *obs)
 {
 	struct rift_s_controller *ctrl = (struct rift_s_controller *)(xdev);
@@ -2039,7 +2048,7 @@ rift_s_controller_push_pose_observation(struct xrt_device *xdev, const struct t_
 	if (reject != NULL) {
 		log_fusion(ctrl, frame_mono_ns, processing_ns, reject, obs);
 		os_mutex_unlock(&ctrl->mutex);
-		return;
+		return false;
 	}
 	// Candidates must pass position confirmation before changing the IMU heading or
 	// the association prior. Otherwise a rejected fit can steer the next search.
@@ -2047,7 +2056,7 @@ rift_s_controller_push_pose_observation(struct xrt_device *xdev, const struct t_
 	if (position_result == RIFT_S_POSITION_FIX_PENDING || position_result == RIFT_S_POSITION_FIX_STALE) {
 		log_fusion(ctrl, frame_mono_ns, processing_ns, "reject_position", obs);
 		os_mutex_unlock(&ctrl->mutex);
-		return;
+		return false;
 	}
 	struct xrt_quat optical = pose->orientation;
 	math_quat_normalize(&optical);
@@ -2091,6 +2100,7 @@ rift_s_controller_push_pose_observation(struct xrt_device *xdev, const struct t_
 	ctrl->diagnostics.optical_accepted++;
 	log_fusion(ctrl, frame_mono_ns, processing_ns, "accepted", obs);
 	os_mutex_unlock(&ctrl->mutex);
+	return true;
 }
 
 void
@@ -2303,6 +2313,8 @@ rift_s_controller_update_configuration(struct rift_s_controller *ctrl, uint64_t
 		ctrl->irled_requested = false;
 		reset_position_filter(ctrl);
 		ctrl->track_vision_updates = ctrl->track_reliable_updates = 0;
+		rift_s_motion_reset_hand(ctrl->sys->motion,
+		                         ctrl->base.device_type == XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER ? 1 : 0);
 		ctrl->last_fix_ns = 0;
 		ctrl->imu_descriptor[0] = '\0';
 		ctrl->gravity_initialized = false;

src/xrt/drivers/rift_s/rift_s_controller.h

@@ -197,9 +197,6 @@ struct rift_s_controller
 	float last_position_covariance[9];
 	//! Aim/grip position: IMU acceleration integrated at IMU rate, corrected by optical fixes.
 	struct rift_s_position_filter position_filter;
-	//! Presentation-only translation correction after held position reacquires optics.
-	struct xrt_vec3 reacquire_offset;
-	timepoint_ns reacquire_started_ns;
 	//! Successful and reliable optical fixes in the current track (inputs of the loss ladder).
 	uint32_t track_vision_updates, track_reliable_updates;
 	//! Capture time of the newest optical fix given to position_filter.
@@ -405,9 +402,14 @@ rift_s_controller_push_observed_pose(struct xrt_device *xdev, timepoint_ns frame
 bool
 rift_s_controller_get_led_model(struct xrt_device *xdev, struct t_constellation_led_model *led_model);
 // Accept an optical fit. Yaw is corrected only when the fit itself observed it.
-void
+bool
 rift_s_controller_push_pose_observation(struct xrt_device *xdev, const struct t_constellation_pose_observation *obs);
 
+// Feed motion models from the current capture-time filter state. Caller holds the controller mutex
+// and keeps the system devices alive, as the radio dispatch does.
+void
+rift_s_controller_update_motion(struct rift_s_controller *ctrl, timepoint_ns capture_ns);
+
 // Controller pose callback. Optical loss preserves IMU orientation and estimates hand position.
 xrt_result_t
 rift_s_controller_get_tracked_pose(struct xrt_device *xdev,

src/xrt/drivers/rift_s/rift_s_motion.cpp

@@ -0,0 +1,737 @@
+// Copyright 2026, lbgos
+// SPDX-License-Identifier: BSL-1.0
+#include "rift_s_motion.h"
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+#include <deque>
+#include <mutex>
+#include <new>
+
+namespace {
+struct Vec
+{
+	double x = 0, y = 0, z = 0;
+	Vec
+	operator+(Vec b) const
+	{
+		return {x + b.x, y + b.y, z + b.z};
+	}
+	Vec
+	operator-(Vec b) const
+	{
+		return {x - b.x, y - b.y, z - b.z};
+	}
+	Vec
+	operator*(double s) const
+	{
+		return {x * s, y * s, z * s};
+	}
+	double
+	length() const
+	{
+		return std::sqrt(x * x + y * y + z * z);
+	}
+	bool
+	inside(double t) const
+	{
+		return -t < x && x < t && -t < y && y < t && -t < z && z < t;
+	}
+};
+static Vec
+cross(Vec a, Vec b)
+{
+	return {a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x};
+}
+static double
+dot(Vec a, Vec b)
+{
+	return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+static Vec
+components(Vec a, Vec b)
+{
+	return {a.x * b.x, a.y * b.y, a.z * b.z};
+}
+
+struct Quat
+{
+	double x = 0, y = 0, z = 0, w = 1;
+	Quat
+	normalized() const
+	{
+		double n = std::sqrt(x * x + y * y + z * z + w * w);
+		return {x / n, y / n, z / n, w / n};
+	}
+	Quat
+	inverse() const
+	{
+		return Quat{-x, -y, -z, w}.normalized();
+	}
+	Quat
+	operator*(Quat b) const
+	{
+		return Quat{w * b.x + x * b.w + y * b.z - z * b.y, w * b.y - x * b.z + y * b.w + z * b.x,
+		            w * b.z + x * b.y - y * b.x + z * b.w, w * b.w - x * b.x - y * b.y - z * b.z}
+		    .normalized();
+	}
+	Vec
+	rotate(Vec v) const
+	{
+		Vec q{x, y, z}, t = cross(q, v) * 2;
+		return v + t * w + cross(q, t);
+	}
+};
+static Quat
+exp_rotation(Vec v)
+{
+	double angle = v.length();
+	double scale = angle > 1e-12 ? std::sin(angle / 2) / angle : 0.5;
+	return Quat{v.x * scale, v.y * scale, v.z * scale, std::cos(angle / 2)}.normalized();
+}
+static Vec
+log_rotation(Quat q)
+{
+	q = q.normalized();
+	double n = Vec{q.x, q.y, q.z}.length();
+	if (n * n < 1e-20)
+		return Vec{q.x, q.y, q.z} * (2 / q.w);
+	double angle = std::abs(q.w) < 1e-10 ? (q.w > 0 ? M_PI : -M_PI) : 2 * std::atan(n / q.w);
+	return Vec{q.x, q.y, q.z} * (angle / n);
+}
+struct Pose
+{
+	Quat q;
+	Vec p;
+	Vec
+	apply(Vec v) const
+	{
+		return p + q.rotate(v);
+	}
+	Pose
+	inverse() const
+	{
+		Quat qi = q.inverse();
+		return {qi, qi.rotate(p * -1)};
+	}
+	Pose
+	operator*(Pose b) const
+	{
+		return {q * b.q, apply(b.p)};
+	}
+};
+static Pose
+pose_from(const xrt_pose &p)
+{
+	return {{p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w},
+	        {p.position.x, p.position.y, p.position.z}};
+}
+static xrt_pose
+pose_to(Pose p)
+{
+	return {{float(p.q.x), float(p.q.y), float(p.q.z), float(p.q.w)}, {float(p.p.x), float(p.p.y), float(p.p.z)}};
+}
+static Vec
+vec_from(xrt_vec3 v)
+{
+	return {v.x, v.y, v.z};
+}
+static xrt_vec3
+vec_to(Vec v)
+{
+	return {float(v.x), float(v.y), float(v.z)};
+}
+
+struct Sample
+{
+	double t = -1;
+	Pose pose;
+	Vec v, a, w;
+	std::array<double, 3> variance = {-1, -1, -1};
+};
+static Sample
+sample_from(const rift_s_motion_sample &s)
+{
+	return {double(s.timestamp_ns) / U_TIME_1S_IN_NS,
+	        pose_from(s.pose),
+	        vec_from(s.linear_velocity),
+	        vec_from(s.linear_acceleration),
+	        vec_from(s.angular_velocity),
+	        {s.position_variance[0], s.position_variance[1], s.position_variance[2]}};
+}
+static Pose
+predict(const Sample &s, double t)
+{
+	// Model prediction uses constant rates; acceleration is not a quadratic position term.
+	double dt = t - s.t;
+	return {s.pose.q * exp_rotation(s.w * dt), s.pose.p + s.v * dt};
+}
+struct Record
+{
+	Sample sample, behavior;
+	double reliable_t = -1;
+	Vec reliable_position, relative_position;
+	Quat current_orientation;
+	int model = 0;
+	void
+	store(Sample s, const Pose &head)
+	{
+		sample = s;
+		current_orientation = s.pose.q;
+		if (std::all_of(s.variance.begin(), s.variance.end(), [](double v) { return 0 < v && v < 0.001; })) {
+			reliable_t = s.t;
+			reliable_position = s.pose.p;
+			relative_position = head.inverse().apply(s.pose.p);
+		}
+	}
+	bool
+	fresh(double epsilon) const
+	{
+		return sample.t <= reliable_t + epsilon;
+	}
+	Pose
+	source() const
+	{
+		return behavior.t == -1 ? Pose{current_orientation, reliable_position} : predict(behavior, sample.t);
+	}
+};
+
+struct PairSample
+{
+	double t;
+	Vec a, b;
+};
+struct PairWindow
+{
+	std::deque<PairSample> items;
+	bool ready = false;
+	size_t allocated = 0;
+	void
+	push(PairSample s, double seconds, size_t capacity)
+	{
+		if (allocated == 0)
+			allocated = capacity;
+		if (items.size() == allocated) {
+			if (items.front().t + seconds < items.back().t + 0.0005) {
+				items.pop_front();
+				ready = ready && items.size() >= 2;
+			} else
+				allocated += capacity;
+		}
+		items.push_back(s);
+		while (!items.empty() && !(s.t < items.front().t + seconds + 0.0005)) {
+			items.pop_front();
+			ready = items.size() >= 2;
+		}
+	}
+	std::array<Vec, 5>
+	stats() const
+	{
+		Vec sa, sb, saa, sbb, sab;
+		for (auto &s : items) {
+			sa = sa + s.a;
+			sb = sb + s.b;
+			saa = saa + components(s.a, s.a);
+			sbb = sbb + components(s.b, s.b);
+			sab = sab + components(s.a, s.b);
+		}
+		double n = double(items.size());
+		if (n < 2)
+			return {};
+		return {sa * (1 / n), sb * (1 / n), (saa - components(sa, sa) * (1 / n)) * (1 / (n - 1)),
+		        (sbb - components(sb, sb) * (1 / n)) * (1 / (n - 1)),
+		        (sab - components(sa, sb) * (1 / n)) * (1 / (n - 1))};
+	}
+	void
+	reset()
+	{
+		items.clear();
+		ready = false;
+	}
+};
+
+struct Rest
+{
+	int state = 0;
+	PairWindow history;
+	Vec mean_a, mean_b;
+	Pose latch;
+	bool
+	update(const Record &r, Sample &out)
+	{
+		history.push({r.sample.t, r.sample.w, r.sample.a}, 0.1, 300);
+		double factor = state == 3 ? 3 : 1;
+		auto s = history.stats();
+		bool variance_ok = history.ready && s[2].inside(factor * 0.000076) && s[3].inside(factor * 0.005);
+		if (variance_ok) {
+			mean_a = s[0];
+			mean_b = s[1];
+		}
+		bool mean_ok =
+		    (r.sample.w - mean_a).inside(factor * 0.005) && (r.sample.a - mean_b).inside(factor * 0.065);
+		bool fresh = r.fresh(0.01), far = r.relative_position.length() > 0.8;
+		int next = state;
+		switch (state) {
+		case 0:
+			if (variance_ok)
+				next = 1;
+			break;
+		case 1:
+			if (fresh && far)
+				next = 2;
+			break;
+		case 2:
+			if (mean_ok)
+				next = 3;
+			else if (fresh && !far)
+				next = 1;
+			break;
+		case 3:
+			if (!mean_ok)
+				next = 0;
+			break;
+		}
+		if (state != 3 && next == 3)
+			latch = r.sample.pose;
+		state = next;
+		if (state != 3)
+			return false;
+		out = {};
+		out.t = r.sample.t;
+		out.pose = latch;
+		return true;
+	}
+	void
+	reset()
+	{
+		state = 0;
+		history.reset();
+		mean_a = {};
+		mean_b = {};
+	}
+};
+
+struct RotationModel
+{
+	int state = 0;
+	Sample output;
+	Pose correction;
+	Vec anchor;
+	double cap = 0.25;
+	std::deque<double> ratios;
+	bool
+	update(const Record &r, const Pose &head, const Pose &torso, bool torso_valid, bool near, Sample &out)
+	{
+		bool fresh = r.fresh(0.0003);
+		double radius = r.relative_position.length(), height = r.reliable_position.y - head.p.y;
+		bool in_range = near ? radius < 0.25 : 0.25 < radius && radius < 1.1 && height > -0.6;
+		Vec delta = r.reliable_position - output.pose.p;
+		double distance = delta.length(), previous_t = output.t;
+		// Compute the return rate from the previous cache before replacing that cache.
+		Vec return_rate = r.sample.v;
+		if (distance > 0)
+			return_rate = return_rate + delta * (std::min(std::max(distance * 50, 0.25), cap) / distance);
+		int next = state;
+		switch (state) {
+		case 0:
+			if (!torso_valid)
+				break;
+			next = 1;
+			[[fallthrough]];
+		case 1:
+			if (in_range && (near || fresh))
+				next = fresh ? 2 : 3;
+			break;
+		case 2:
+			if (!fresh)
+				next = 3;
+			else if (!in_range)
+				next = 0;
+			break;
+		case 3:
+			if (fresh)
+				next = 4;
+			break;
+		case 4:
+			if (distance <= 0.001)
+				next = 0;
+			break;
+		}
+		if (next == 3 && state != 3) {
+			double seed = 0.2;
+			if (!near) {
+				if (height < -0.2)
+					seed = 0.5;
+				else if (!ratios.empty()) {
+					seed = 0;
+					for (double v : ratios)
+						seed += v;
+					seed /= ratios.size();
+				}
+			}
+			correction = {exp_rotation({-0.087, 0, 0}), {0, 0.025, near ? 0.2 : -seed}};
+			Pose source = r.source();
+			anchor = torso.inverse().apply(source.p + source.q.rotate(correction.inverse().p));
+			cap = 0.25;
+		}
+		state = next;
+		if (!near && state == 2) {
+			double w = r.sample.w.length();
+			double ratio = w > 0 ? std::clamp(r.sample.v.length() / w, 0.2, 0.4) : 0.2;
+			if (ratios.size() == 500)
+				ratios.pop_front();
+			ratios.push_back(ratio);
+		}
+		if (state == 3) {
+			output = r.sample;
+			output.variance = {-1, -1, -1};
+			output.pose.p = torso.p + (r.sample.pose.q * correction.inverse().q).rotate(correction.p) +
+			                torso.q.rotate(anchor);
+			cap = std::max(cap, 0.2 * r.sample.a.length());
+		} else if (state == 4) {
+			Vec previous = output.pose.p;
+			output = r.sample;
+			output.variance = {-1, -1, -1};
+			output.v = return_rate;
+			output.pose.p = previous + return_rate * (r.sample.t - previous_t);
+		}
+		if (state != 3 && state != 4)
+			return false;
+		out = output;
+		return true;
+	}
+	void
+	reset()
+	{
+		state = 0;
+	}
+};
+
+struct Behind
+{
+	int state = 0;
+	bool
+	update(const Record &r, const Pose &head, Sample &out)
+	{
+		bool fresh = r.fresh(0.0003);
+		Vec look = head.q.rotate({0, 0, 1}), delta = r.sample.pose.p - head.p;
+		look.y = delta.y = 0;
+		if (state == 0 && !fresh) {
+			bool behind = dot(look, delta) > 0 && r.reliable_position.y - head.p.y < -0.3 &&
+			              (r.reliable_position - head.p).length() < 1.2;
+			state = behind ? 1 : 2;
+		} else if (state != 0 && fresh)
+			state = 0;
+		if (state != 1)
+			return false;
+		out = r.sample;
+		out.pose.p = head.apply(r.relative_position);
+		return true;
+	}
+	void
+	reset()
+	{
+		state = 0;
+	}
+};
+
+struct HandOverHand
+{
+	int state = 0;
+	bool latch = false;
+	PairWindow history;
+	Sample output;
+	Vec other_support, torso_support;
+	bool
+	update(const Record &p, const Record &q, const Pose &head, const Pose &torso, Sample &out)
+	{
+		history.push({p.sample.t, p.sample.a, q.sample.a}, 0.1, 100);
+		auto s = history.stats();
+		bool all_above = false, all_below = false;
+		if (history.items.size() >= 2 && s[2].x > 0.004 && s[2].y > 0.004 && s[2].z > 0.004 &&
+		    s[3].x >= 0.004 && s[3].y >= 0.004 && s[3].z >= 0.004) {
+			Vec corr{s[4].x / std::sqrt(s[2].x * s[3].x), s[4].y / std::sqrt(s[2].y * s[3].y),
+			         s[4].z / std::sqrt(s[2].z * s[3].z)};
+			all_above = corr.x > 0.25 && corr.y > 0.25 && corr.z > 0.25;
+			all_below = corr.x < 0.25 && corr.y < 0.25 && corr.z < 0.25;
+		}
+		bool lost = !p.fresh(0.05), other_fresh = q.fresh(0.05);
+		bool close = (q.sample.pose.inverse() * p.sample.pose).p.length() < 0.2;
+		Vec look = head.q.rotate({0, 0, 1});
+		Vec dp = p.sample.pose.p - head.p, dq = q.sample.pose.p - head.p;
+		dp.y = dq.y = 0;
+		bool both_gate = dot(look, dp) > 0 && dot(look, dq) > 0;
+		bool mismatch = p.model != 0 && p.model != 3, other_model = q.model != 0;
+		if (!other_fresh)
+			latch = false;
+		int next = state;
+		switch (state) {
+		case 0:
+			if (!other_fresh)
+				break;
+			next = 1;
+			[[fallthrough]];
+		case 1:
+			latch = true;
+			if (!other_fresh)
+				next = 0;
+			else if (!lost && close && !both_gate)
+				next = 2;
+			break;
+		case 2:
+			if (!other_fresh)
+				next = 0;
+			else if (lost)
+				next = all_above ? 3 : 4;
+			else if (!close)
+				next = 1;
+			break;
+		case 3:
+			if (mismatch || other_model)
+				next = 0;
+			else if (!lost || (other_fresh && all_below))
+				next = 4;
+			break;
+		case 4:
+			if (mismatch || other_model || !lost || !other_fresh)
+				next = 0;
+			else if (latch && all_above)
+				next = 3;
+			break;
+		}
+		if (state != next && (next == 3 || next == 4)) {
+			Pose basis = next == 3 ? q.sample.pose : torso;
+			Vec support = basis.inverse().apply(state == 3 || state == 4 ? output.pose.p : p.sample.pose.p);
+			if (next == 3)
+				other_support = support;
+			else
+				torso_support = support;
+		}
+		state = next;
+		if (state != 3 && state != 4)
+			return false;
+		output = p.sample;
+		output.pose.p = state == 3 ? q.sample.pose.apply(other_support) : torso.apply(torso_support);
+		out = output;
+		return true;
+	}
+	void
+	reset()
+	{
+		state = 0;
+		history.reset();
+	}
+};
+
+static bool
+project_yaw(Quat q, Quat &yaw)
+{
+	Vec x = q.rotate({1, 0, 0});
+	x.y = 0;
+	if (x.length() < 0.1)
+		return false;
+	yaw = exp_rotation({0, std::atan2(-x.z, x.x), 0});
+	return true;
+}
+
+struct Torso
+{
+	bool valid = false;
+	Pose output;
+	Quat previous_controller, previous_head;
+	PairWindow history;
+	void
+	update(const Sample &s, const Pose &head)
+	{
+		Vec position = head.apply({0, -0.075, 0.0805});
+		Quat controller_yaw, head_yaw;
+		if (!project_yaw(head.q, head_yaw) || !project_yaw(s.pose.q, controller_yaw))
+			return;
+		history.push({s.t,
+		              {log_rotation(controller_yaw).y, log_rotation(head_yaw).y,
+		               log_rotation(head_yaw.inverse() * controller_yaw).y},
+		              {}},
+		             0.1, 100);
+		if (!history.ready) {
+			if (!valid) {
+				output = {head_yaw, position};
+				valid = true;
+			}
+			return;
+		}
+		Vec variance = history.stats()[2];
+		Quat result = output.q;
+		if (variance.z >= 0.001) {
+			if (std::abs(log_rotation(head_yaw.inverse() * result).y) >= 0.523 && variance.x >= 0.0027) {
+				double weight = std::abs(variance.y) / (std::abs(variance.x) + std::abs(variance.y));
+				Quat target = controller_yaw *
+				              exp_rotation(log_rotation(controller_yaw.inverse() * head_yaw) * weight);
+				Vec residual = log_rotation(result.inverse() * target);
+				result = result * exp_rotation({0, (residual.y > 0 ? 1 : -1) * 0.018 * 0.523, 0});
+			}
+		} else {
+			result = result * (std::abs(variance.x) <= std::abs(variance.y)
+			                       ? previous_controller.inverse() * controller_yaw
+			                       : previous_head.inverse() * head_yaw);
+		}
+		// The output is itself projected, just like the source yaw filters.
+		Quat projected;
+		if (project_yaw(result, projected)) {
+			output = {projected, position};
+			valid = true;
+		}
+		previous_controller = controller_yaw;
+		previous_head = head_yaw;
+	}
+};
+
+struct Hand
+{
+	Record record;
+	Torso torso;
+	Rest rest;
+	RotationModel near, rotation;
+	HandOverHand together;
+	Behind behind;
+	void
+	update(const Record &other, const Pose &head, const Pose &body, bool body_valid)
+	{
+		if (record.reliable_t == -1)
+			return;
+		Sample estimate;
+		int model = 0;
+		if (rest.update(record, estimate))
+			model = 1;
+		else if (near.update(record, head, body, body_valid, true, estimate))
+			model = 2;
+		else if (other.reliable_t != -1 && together.update(record, other, head, body, estimate))
+			model = 3;
+		else if (behind.update(record, head, estimate))
+			model = 4;
+		else if (rotation.update(record, head, body, body_valid, false, estimate))
+			model = 5;
+		// Lower priorities must acquire again after another model has owned the output.
+		if (model != 0) {
+			if (model < 2)
+				near.reset();
+			if (model < 3)
+				together.reset();
+			if (model < 4)
+				behind.reset();
+			if (model < 5)
+				rotation.reset();
+		}
+		record.model = model;
+		record.behavior = model ? estimate : Sample{};
+	}
+};
+
+static bool
+finite(Vec v)
+{
+	return std::isfinite(v.x) && std::isfinite(v.y) && std::isfinite(v.z);
+}
+static bool
+finite(Pose p)
+{
+	double n = p.q.x * p.q.x + p.q.y * p.q.y + p.q.z * p.q.z + p.q.w * p.q.w;
+	return finite(p.p) && std::isfinite(n) && n > 1e-12;
+}
+} // namespace
+
+struct rift_s_motion
+{
+	std::mutex mutex;
+	std::array<Hand, 2> hands;
+	Pose body;
+	bool body_valid = false;
+};
+
+extern "C" struct rift_s_motion *
+rift_s_motion_create(void)
+{
+	return new (std::nothrow) rift_s_motion;
+}
+
+extern "C" void
+rift_s_motion_destroy(struct rift_s_motion **motion)
+{
+	if (motion == nullptr)
+		return;
+	delete *motion;
+	*motion = nullptr;
+}
+
+extern "C" void
+rift_s_motion_reset_hand(struct rift_s_motion *motion, unsigned int hand)
+{
+	if (motion == nullptr || hand >= 2)
+		return;
+	std::lock_guard<std::mutex> lock(motion->mutex);
+	motion->hands[hand] = Hand{};
+	motion->body_valid = motion->hands[1 - hand].torso.valid;
+	if (motion->body_valid)
+		motion->body = motion->hands[1 - hand].torso.output;
+}
+
+extern "C" void
+rift_s_motion_update(struct rift_s_motion *motion,
+                     unsigned int hand,
+                     const struct rift_s_motion_sample *sample,
+                     const struct xrt_pose *reference)
+{
+	if (motion == nullptr || hand >= 2 || sample == nullptr || reference == nullptr || sample->timestamp_ns <= 0)
+		return;
+	Sample s = sample_from(*sample);
+	Pose head = pose_from(*reference);
+	if (!finite(s.pose) || !finite(head) || !finite(s.v) || !finite(s.a) || !finite(s.w))
+		return;
+	s.pose.q = s.pose.q.normalized();
+	head.q = head.q.normalized();
+	std::lock_guard<std::mutex> lock(motion->mutex);
+	Hand &p = motion->hands[hand], &q = motion->hands[1 - hand];
+	if (s.t <= p.record.sample.t)
+		return;
+	p.record.store(s, head);
+	p.torso.update(s, head);
+	if (p.torso.valid) {
+		motion->body = p.torso.output;
+		motion->body_valid = true;
+		if (q.torso.valid) {
+			motion->body.q = motion->body.q *
+			                 exp_rotation(log_rotation(motion->body.q.inverse() * q.torso.output.q) * 0.5);
+		}
+	}
+	p.update(q.record, head, motion->body, motion->body_valid);
+}
+
+extern "C" enum rift_s_motion_model
+rift_s_motion_get(struct rift_s_motion *motion,
+                  unsigned int hand,
+                  timepoint_ns timestamp_ns,
+                  struct xrt_space_relation *out_relation)
+{
+	if (motion == nullptr || hand >= 2 || out_relation == nullptr)
+		return RIFT_S_MOTION_NONE;
+	std::lock_guard<std::mutex> lock(motion->mutex);
+	const Record &r = motion->hands[hand].record;
+	if (r.model == 0)
+		return RIFT_S_MOTION_NONE;
+	const Sample &s = r.behavior;
+	double time = std::clamp(double(timestamp_ns) / U_TIME_1S_IN_NS, s.t, s.t + 1.0);
+	*out_relation = {};
+	out_relation->pose = pose_to(predict(s, time));
+	out_relation->linear_velocity = vec_to(s.v);
+	out_relation->angular_velocity = vec_to(s.pose.q.rotate(s.w));
+	out_relation->relation_flags = static_cast<xrt_space_relation_flags>(
+	    XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
+	    XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT);
+	// Attachment models pass the input matrix through. Rest and rotation models emit -I.
+	if (std::all_of(s.variance.begin(), s.variance.end(), [](double v) { return v > 0; })) {
+		out_relation->relation_flags = static_cast<xrt_space_relation_flags>(
+		    out_relation->relation_flags | XRT_SPACE_RELATION_POSITION_VALID_BIT |
+		    XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
+	}
+	return static_cast<rift_s_motion_model>(r.model);
+}

src/xrt/drivers/rift_s/rift_s_motion.h

@@ -0,0 +1,66 @@
+// Copyright 2026, lbgos
+// SPDX-License-Identifier: BSL-1.0
+#pragma once
+
+#include "xrt/xrt_defines.h"
+#include "util/u_time.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct rift_s_motion;
+
+// Samples are at the controller IMU origin, before the device/aim/grip lever arms.
+struct rift_s_motion_sample
+{
+	timepoint_ns timestamp_ns;
+	struct xrt_pose pose;
+	struct xrt_vec3 linear_velocity;
+	struct xrt_vec3 linear_acceleration;
+	// Angular rate in the controller frame. Relations returned by get use tracking-frame rates.
+	struct xrt_vec3 angular_velocity;
+	// A negative diagonal marks a position estimate without a reliable filter position.
+	double position_variance[3];
+};
+
+enum rift_s_motion_model
+{
+	RIFT_S_MOTION_NONE,
+	RIFT_S_MOTION_AT_REST,
+	RIFT_S_MOTION_NEAR_HEAD,
+	RIFT_S_MOTION_HAND_OVER_HAND,
+	RIFT_S_MOTION_BEHIND_BACK,
+	RIFT_S_MOTION_OUT_OF_FOV,
+};
+
+struct rift_s_motion *
+rift_s_motion_create(void);
+
+void
+rift_s_motion_destroy(struct rift_s_motion **motion);
+
+void
+rift_s_motion_reset_hand(struct rift_s_motion *motion, unsigned int hand);
+
+// hand: 0 left, 1 right. The shared cache retains the other hand's most recent sample.
+// The reference pose must share the controller's tracking frame. A missing reference leaves
+// the previous cache intact; it cannot initialize a new hand model.
+void
+rift_s_motion_update(struct rift_s_motion *motion,
+                     unsigned int hand,
+                     const struct rift_s_motion_sample *sample,
+                     const struct xrt_pose *reference);
+
+// Query is read-only: neither render prediction nor lever-arm transforms enter the model cache.
+// Rest/rotation estimates have no position confidence; attachment models retain the input confidence.
+// The caller keeps its optical result whenever no model is active.
+enum rift_s_motion_model
+rift_s_motion_get(struct rift_s_motion *motion,
+                  unsigned int hand,
+                  timepoint_ns timestamp_ns,
+                  struct xrt_space_relation *out_relation);
+
+#ifdef __cplusplus
+}
+#endif

src/xrt/drivers/rift_s/rift_s_position_filter.c

@@ -15,8 +15,6 @@
 #include <math.h>
 #include <string.h>
 
-//! Velocity decay time constant while coasting without optical fixes.
-#define COAST_VELOCITY_TAU_S 0.5
 #define INITIAL_VELOCITY_VARIANCE (0.5 * 0.5)
 #define INITIAL_BIAS_VARIANCE (0.5 * 0.5)
 //! Longest single integration step; larger gaps are dropped radio packets.
@@ -34,7 +32,6 @@ propagate(const struct rift_s_position_filter *f,
           const struct xrt_vec3 *accel,
           timepoint_ns t)
 {
-	const bool coasting = f->coasting;
 	double dt = (double)(t - s->t) / U_TIME_1S_IN_NS;
 	s->t = t;
 	if (dt <= 0) {
@@ -43,21 +40,13 @@ propagate(const struct rift_s_position_filter *f,
 	if (dt > MAX_STEP_S) {
 		dt = MAX_STEP_S;
 	}
-	if (coasting && f->track_updates < RIFT_S_POSITION_FILTER_MATURE_UPDATES) {
-		// A young track's velocity comes from a handful of fixes, possibly a wrong one: hold.
-		for (int axis = 0; axis < 3; axis++) {
-			s->x[axis][1] = 0;
-		}
-		return;
-	}
 	const double a[3] = {accel->x, accel->y, accel->z};
-	const double d = coasting ? exp(-dt / COAST_VELOCITY_TAU_S) : 1.0;
-	const double F[3][3] = {{1, dt, -0.5 * dt * dt}, {0, d, -d * dt}, {0, 0, 1}};
+	const double F[3][3] = {{1, dt, -0.5 * dt * dt}, {0, 1, -dt}, {0, 0, 1}};
 	for (int axis = 0; axis < 3; axis++) {
 		double *x = s->x[axis];
 		double u = a[axis] - x[2];
 		x[0] += x[1] * dt + 0.5 * u * dt * dt;
-		x[1] = d * (x[1] + u * dt);
+		x[1] += u * dt;
 
 		double FP[3][3] = {{0}};
 		double (*P)[3] = s->P[axis];
@@ -133,28 +122,12 @@ rift_s_position_filter_imu(struct rift_s_position_filter *f, timepoint_ns t, con
 		f->last_accel = *accel_world;
 		return;
 	}
-	if (f->frozen) {
+	if (f->suspended) {
 		f->state.t = t;
 	} else {
 		propagate(f, &f->state, accel_world, t);
 	}
 	f->last_accel = *accel_world;
-	if (f->initialized && t - f->last_fix_ns > 120 * U_TIME_1MS_IN_NS) {
-		// No measurement can correct acceleration/tilt error while occluded. Bound the
-		// displacement from the last optical fix, including before coasting was noticed.
-		double delta[3] = {f->state.x[0][0] - f->last_fix_position.x, f->state.x[1][0] - f->last_fix_position.y,
-		                   f->state.x[2][0] - f->last_fix_position.z};
-		double distance = sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
-		if (distance > 0.1) {
-			const double base[3] = {f->last_fix_position.x, f->last_fix_position.y, f->last_fix_position.z};
-			for (int axis = 0; axis < 3; ++axis) {
-				f->state.x[axis][0] = base[axis] + delta[axis] * 0.1 / distance;
-				f->state.x[axis][1] = 0;
-			}
-		}
-		if (t - f->last_fix_ns > RIFT_S_POSITION_FILTER_BRIDGE_NS)
-			rift_s_position_filter_freeze(f);
-	}
 	if (f->history_count == RIFT_S_POSITION_FILTER_HISTORY) {
 		f->history_start = (f->history_start + 1) % RIFT_S_POSITION_FILTER_HISTORY;
 		f->history_count--;
@@ -197,13 +170,17 @@ confirm_candidate(struct rift_s_position_filter *f,
 	return f->candidate_count >= RIFT_S_POSITION_FILTER_CONFIRM_FIXES;
 }
 
-enum rift_s_position_fix_result
-rift_s_position_filter_fix(struct rift_s_position_filter *f,
+static enum rift_s_position_fix_result
+position_filter_fix(struct rift_s_position_filter *f,
                            timepoint_ns capture,
                            const struct xrt_vec3 *position,
                            const struct xrt_vec3 *variance,
-                           float reset_distance_m)
+                           float reset_distance_m,
+                           bool confirmed_reacquire)
 {
+	// Optical age loss has already passed Constellation's track confirmation.
+	// A clock discontinuity still needs independent filter confirmation.
+	confirmed_reacquire = confirmed_reacquire && !f->suspended;
 	// First retained input after the capture time.
 	unsigned int next = 0;
 	while (next < f->history_count && input_at(f, next)->t <= capture) {
@@ -211,7 +188,7 @@ rift_s_position_filter_fix(struct rift_s_position_filter *f,
 	}
 	// A fix needs the state at or before its capture time; the oldest retained input only has
 	// the state after it.
-	bool reset = !f->initialized || f->frozen;
+	bool reset = !f->initialized || f->suspended || confirmed_reacquire;
 	bool stale = f->history_count != 0 ? next == 0 : capture < f->state.t;
 	if (f->initialized && stale) {
 		f->stale++;
@@ -235,7 +212,8 @@ rift_s_position_filter_fix(struct rift_s_position_filter *f,
 			f->innovation_count++;
 		}
 	}
-	if (reset && f->initialized && !confirm_candidate(f, capture, position, reset_distance_m)) {
+	if (reset && f->initialized && !confirmed_reacquire &&
+	    !confirm_candidate(f, capture, position, reset_distance_m)) {
 		f->rejected++;
 		return RIFT_S_POSITION_FIX_PENDING;
 	}
@@ -252,20 +230,39 @@ rift_s_position_filter_fix(struct rift_s_position_filter *f,
 	f->initialized = true;
 	f->last_fix_ns = capture;
 	f->last_fix_position = *position;
-	f->frozen = false;
-	f->coasting = false;
+	f->suspended = false;
 	// Inputs before the capture keep their states; later ones are replayed from the fused state.
 	replay(f, s, next);
 	return reset ? RIFT_S_POSITION_FIX_RESET : RIFT_S_POSITION_FIX_UPDATED;
 }
 
-void
-rift_s_position_filter_freeze(struct rift_s_position_filter *f)
+enum rift_s_position_fix_result
+rift_s_position_filter_fix(struct rift_s_position_filter *f,
+                           timepoint_ns capture,
+                           const struct xrt_vec3 *position,
+                           const struct xrt_vec3 *variance,
+                           float reset_distance_m)
 {
-	if (!f->initialized || f->frozen) {
+	return position_filter_fix(f, capture, position, variance, reset_distance_m, false);
+}
+
+enum rift_s_position_fix_result
+rift_s_position_filter_reacquire(struct rift_s_position_filter *f,
+                                 timepoint_ns capture,
+                                 const struct xrt_vec3 *position,
+                                 const struct xrt_vec3 *variance,
+                                 float reset_distance_m)
+{
+	return position_filter_fix(f, capture, position, variance, reset_distance_m, true);
+}
+
+void
+rift_s_position_filter_suspend(struct rift_s_position_filter *f)
+{
+	if (!f->initialized || f->suspended) {
 		return;
 	}
-	f->frozen = true;
+	f->suspended = true;
 	for (int axis = 0; axis < 3; axis++) {
 		f->state.x[axis][1] = 0;
 	}
@@ -279,7 +276,7 @@ rift_s_position_filter_predict(const struct rift_s_position_filter *f,
                                struct xrt_vec3 *out_velocity)
 {
 	time_duration_ns ahead = t - f->state.t;
-	if (ahead < 0 || f->frozen || (f->initialized && t - f->last_fix_ns > RIFT_S_POSITION_FILTER_BRIDGE_NS)) {
+	if (ahead < 0 || f->suspended) {
 		ahead = 0;
 	} else if (ahead > max_horizon_ns) {
 		ahead = max_horizon_ns;
@@ -292,21 +289,6 @@ rift_s_position_filter_predict(const struct rift_s_position_filter *f,
 	out_position->x = (float)(s->x[0][0] + s->x[0][1] * dt);
 	out_position->y = (float)(s->x[1][0] + s->x[1][1] * dt);
 	out_position->z = (float)(s->x[2][0] + s->x[2][1] * dt);
-	if (f->initialized && t - f->last_fix_ns > 120 * U_TIME_1MS_IN_NS) {
-		// Queries can run beyond the last IMU sample, including during a radio gap.
-		double delta[3] = {out_position->x - f->last_fix_position.x, out_position->y - f->last_fix_position.y,
-		                   out_position->z - f->last_fix_position.z};
-		double distance = sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
-		if (distance > 0.1) {
-			out_position->x = (float)(f->last_fix_position.x + delta[0] * 0.1 / distance);
-			out_position->y = (float)(f->last_fix_position.y + delta[1] * 0.1 / distance);
-			out_position->z = (float)(f->last_fix_position.z + delta[2] * 0.1 / distance);
-			*out_velocity = (struct xrt_vec3){0};
-		}
-		if (t - f->last_fix_ns > RIFT_S_POSITION_FILTER_BRIDGE_NS) {
-			*out_velocity = (struct xrt_vec3){0};
-		}
-	}
 }
 
 void

src/xrt/drivers/rift_s/rift_s_position_filter.h

@@ -9,6 +9,7 @@
 
 #include "xrt/xrt_defines.h"
 #include "util/u_time.h"
+#include "tracking/t_constellation_motion.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -16,7 +17,7 @@ extern "C" {
 
 /*!
  * Defaults chosen on the synthetic cases in tests_rift_s_position_filter (noise at rest, delayed
- * fixes during a hand swing, slowly varying acceleration error); no Meta values are used.
+ * fixes during a hand swing, slowly varying acceleration error); no Meta values were recovered.
  * Continuous white acceleration noise density, (m/s^2)^2 per Hz, and bias random walk,
  * (m/s^2)^2 per second.
  */
@@ -25,25 +26,12 @@ extern "C" {
 
 /*!
  * Mutually consistent fixes needed before a fix that disagrees with the estimate (a gated
- * innovation, or any fix while the position is held after loss) replaces it. A single wrong
+ * innovation, or any fix after an IMU clock discontinuity) replaces it. A single wrong
  * constellation fit, e.g. a partial ring seen behind the back, must not move the hand.
  */
 #define RIFT_S_POSITION_FILTER_CONFIRM_FIXES 3
 //! Longest gap between consecutive candidate fixes of one confirmation.
-#define RIFT_S_POSITION_FILTER_CONFIRM_GAP_NS (250 * U_TIME_1MS_IN_NS)
-/*!
- * Fixes before a track's velocity is trusted for coasting; younger tracks hold their position
- * when fixes stop. Matches the "more than 10 updates" step of the loss ladder.
- */
-#define RIFT_S_POSITION_FILTER_MATURE_UPDATES 11
-
-/*!
- * Longest optical gap the IMU bridges before the position is frozen and reported lost: the
- * 0.5 s step of the loss ladder. Fixes arrive every 33 ms, so in view only real occlusion
- * reaches it; displacement while bridging stays bounded to 10 cm from the last fix.
- */
-#define RIFT_S_POSITION_FILTER_BRIDGE_NS (500 * U_TIME_1MS_IN_NS)
-
+#define RIFT_S_POSITION_FILTER_CONFIRM_GAP_NS T_CONSTELLATION_TRACK_MAX_GAP_NS
 //! IMU inputs retained for replaying after a delayed optical fix: 1 s at 500 Hz.
 #define RIFT_S_POSITION_FILTER_HISTORY 512
 
@@ -68,10 +56,8 @@ struct rift_s_position_filter_input
 struct rift_s_position_filter
 {
 	bool initialized;
-	//! Velocity damping and no bias update: no optical fix for longer than the coast threshold.
-	bool coasting;
-	//! Position frozen and velocity zero: tracking was lost; waits for an optical reset.
-	bool frozen;
+	//! IMU clock or device discontinuity: stop propagation until a confirmed optical reset.
+	bool suspended;
 	//! Process noise; set by reset(), may be overridden afterwards.
 	double accel_noise_density, bias_random_walk;
 	struct rift_s_position_filter_state state;
@@ -114,7 +100,7 @@ rift_s_position_filter_imu(struct rift_s_position_filter *f, timepoint_ns t, con
 /*!
  * Fuse an optical position at its capture time. @p variance is the per-axis measurement variance
  * in m^2. The first fix initializes the filter. An innovation beyond @p reset_distance_m, or any fix
- * while frozen, is a candidate: it re-initializes position and velocity (the Windows "6DoF reset")
+ * while suspended, is a candidate: it re-initializes position and velocity (the Windows "6DoF reset")
  * only once RIFT_S_POSITION_FILTER_CONFIRM_FIXES consecutive candidates agree within
  * @p reset_distance_m of each other; until then the estimate is kept and PENDING returned.
  */
@@ -125,9 +111,18 @@ rift_s_position_filter_fix(struct rift_s_position_filter *f,
                            const struct xrt_vec3 *variance,
                            float reset_distance_m);
 
-//! Stop integrating: hold the current position with zero velocity until the next reset.
+//! Seed a Constellation-confirmed track after optical age loss, retaining IMU history and noise.
+//! Stale samples and clock/device discontinuities still use the normal rejection/confirmation gates.
+enum rift_s_position_fix_result
+rift_s_position_filter_reacquire(struct rift_s_position_filter *f,
+                                 timepoint_ns capture,
+                                 const struct xrt_vec3 *position,
+                                 const struct xrt_vec3 *variance,
+                                 float reset_distance_m);
+
+//! Suspend integration after an IMU clock/device discontinuity, until the next optical reset.
 void
-rift_s_position_filter_freeze(struct rift_s_position_filter *f);
+rift_s_position_filter_suspend(struct rift_s_position_filter *f);
 
 /*!
  * Position and velocity at @p t. Times after the last IMU sample extrapolate with constant
@@ -141,7 +136,7 @@ rift_s_position_filter_predict(const struct rift_s_position_filter *f,
                                struct xrt_vec3 *out_velocity);
 
 /*!
- * Windows controller loss ladder (its default configuration): how long position and velocity
+ * Controller loss duration configuration: how long position and velocity
  * stay tracked after the last optical update, by the number of successful and reliable optical
  * updates in the current track.
  */

src/xrt/drivers/rift_s/rift_s_slam_guard.c

@@ -6,7 +6,7 @@
  *
  * Validate source samples, separately from historical and predicted queries. A lost pose holds
  * position and follows IMU attitude. Ordinary loss retains the map alignment; a backend reset
- * invalidates it. A saved world requires visual relocalization, never a held-head realignment.
+ * invalidates it. Temporary tracking does not confirm a saved boundary's world frame.
  * @ingroup drv_rift_s
  */
 #include "rift_s_slam_guard.h"
@@ -47,13 +47,21 @@ rift_s_slam_guard_pose_usable(const struct xrt_pose *p, const struct xrt_quat *f
 void
 rift_s_slam_guard_set_world(struct rift_s_slam_guard *g, const struct xrt_pose *world_from_slam)
 {
+	bool confirmed = g->boundary_confirmed;
 	g->boundary_confirmed = true;
-	g->world_transition = g->world_valid && g->have_good && g->state == RIFT_S_SLAM_GUARD_TRACKING;
+	g->world_transition = confirmed && g->world_valid && g->have_good &&
+	                      g->state == RIFT_S_SLAM_GUARD_TRACKING;
 	if (g->world_transition) {
 		g->transition_from = g->align;
 		g->transition_to = *world_from_slam;
 		g->transition_start_ns = g->good_ns;
 	} else {
+		if (g->world_valid && g->have_good) {
+			struct xrt_pose inverse, raw_good;
+			math_pose_invert(&g->align, &inverse);
+			math_pose_transform(&inverse, &g->good, &raw_good);
+			math_pose_transform(world_from_slam, &raw_good, &g->good);
+		}
 		g->align = *world_from_slam;
 	}
 	g->world_valid = true;
@@ -100,7 +108,6 @@ rift_s_slam_guard_query(const struct rift_s_slam_guard *g, const struct xrt_pose
                          struct xrt_pose *out)
 {
 	if (g->state != RIFT_S_SLAM_GUARD_TRACKING || !g->world_valid ||
-	    (g->require_relocalization && !g->boundary_confirmed) ||
 	    !rift_s_slam_guard_pose_usable(raw, fusion)) return false;
 	struct xrt_pose aligned;
 	apply_align(&g->align, raw, &aligned);
@@ -293,14 +300,6 @@ rift_s_slam_guard_update(struct rift_s_slam_guard *g,
                          struct xrt_pose *out_pose,
                          enum rift_s_slam_guard_reason *out_reason)
 {
-	if (g->require_relocalization && !g->boundary_confirmed && g->world_valid) {
-		// A failed/cancelled setup can restore saved bounds while the fresh frame is still
-		// active. Revoke that frame before either source or predicted poses are published.
-		g->world_valid = false;
-		g->world_transition = false;
-		if (g->state == RIFT_S_SLAM_GUARD_TRACKING)
-			enter_diverged(g, ts_ns, fusion_orientation, false, 0, 0);
-	}
 	bool usable = slam_valid && rift_s_slam_guard_pose_usable(slam_pose, fusion_orientation);
 	if (usable) {
 		g->invalid_since_ns = 0;
@@ -387,14 +386,8 @@ rift_s_slam_guard_update(struct rift_s_slam_guard *g,
 	}
 
 	if (!g->world_valid) {
-		if (g->require_relocalization && !g->boundary_confirmed) {
-			// SteamVR's saved standing origin belongs to the persisted map, not this VIO
-			// run's arbitrary yaw/translation. Only a visual match can join those frames.
-			*out_reason = RIFT_S_SLAM_GUARD_RELOCALIZING;
-			*out_pose = fallback;
-			return false;
-		}
-		// An explicitly new room can start a fresh frame before it has saved landmarks.
+		// Track in a session frame while matching the saved room. SteamVR must keep
+		// its saved bounds in a separate universe until boundary_confirmed is true.
 		struct xrt_quat inv_raw, delta;
 		math_quat_invert(&slam_pose->orientation, &inv_raw);
 		math_quat_rotate(&fallback.orientation, &inv_raw, &delta);

src/xrt/drivers/rift_s/rift_s_slam_guard.h

@@ -57,7 +57,7 @@ struct rift_s_slam_guard
 	bool world_transition;
 	struct xrt_pose transition_from, transition_to;
 	int64_t transition_start_ns;
-	//! A saved standing frame requires visual confirmation before publishing position tracking.
+	//! Saved bounds require visual confirmation; session position tracking remains available.
 	bool require_relocalization;
 	enum rift_s_slam_guard_state state;
 

src/xrt/drivers/rift_s/rift_s_tracker.c

@@ -603,12 +603,27 @@ rift_s_tracker_create(struct xrt_tracking_origin *origin,
 }
 
 void
-rift_s_tracker_destroy(struct rift_s_tracker *t)
+rift_s_tracker_stop(struct rift_s_tracker *t)
 {
+	// Drain an in-flight camera submission and prevent subsequent callbacks from queuing work.
+	os_mutex_lock(&t->slam_mutex);
+	os_mutex_lock(&t->mutex);
+	t->ready_for_data = false;
+	os_mutex_unlock(&t->mutex);
+	os_mutex_unlock(&t->slam_mutex);
+
 	if (t->world_thread_initialized)
-		os_thread_helper_destroy(&t->world_thread);
+		os_thread_helper_stop_and_wait(&t->world_thread);
 	for (int eye = 0; eye < RIFT_S_CAMERA_COUNT; eye++)
 		xrt_frame_reference(&t->world_frames[eye], NULL);
+}
+
+void
+rift_s_tracker_destroy(struct rift_s_tracker *t)
+{
+	rift_s_tracker_stop(t);
+	if (t->world_thread_initialized)
+		os_thread_helper_destroy(&t->world_thread);
 	rift_s_world_anchor_destroy(t->world_anchor);
 	rift_s_passthrough_destroy(t->base.camera);
 	t_stereo_camera_calibration_reference(&t->stereo_calib, NULL);
@@ -1207,11 +1222,21 @@ world_write_status(struct rift_s_tracker *t)
 	                    : (t->slam_guard.boundary_confirmed && !t->slam_guard.world_transition) ? "localized"
 	                    : t->world_map_available                                                ? "relocalizing"
 	                                                                                            : "unanchored";
+	struct rift_s_world_diagnostics diagnostics = t->world_diagnostics;
+	bool standby = t->standby, paused = t->slam_paused, restarting = t->slam_restart_pending;
+	uint64_t generation = t->slam_generation;
+	uint32_t confirmations = t->world_confirmations;
 	os_mutex_unlock(&t->mutex);
 	static timepoint_ns last_write_ns;
 	static const char *last_state;
 	timepoint_ns now_ns = os_monotonic_get_ns();
 	if (state == last_state && now_ns - last_write_ns < 500 * U_TIME_1MS_IN_NS) return;
+	if (state != last_state && strcmp(state, "relocalizing") == 0) {
+		RIFT_S_WARN("Guardian not found: saved room is not localized; boundary unavailable. "
+		            "Uncover headset cameras and look around. If the room stays unavailable, run Room Setup.");
+	} else if (state != last_state && strcmp(state, "unanchored") == 0) {
+		RIFT_S_WARN("Guardian not found: no usable saved room map. Run Room Setup to anchor the boundary.");
+	}
 	last_write_ns = now_ns;
 	last_state = state;
 	char path[1024], temp[1050];
@@ -1219,7 +1244,21 @@ world_write_status(struct rift_s_tracker *t)
 	snprintf(temp, sizeof(temp), "%s.tmp", path);
 	FILE *file = fopen(temp, "w");
 	if (!file) return;
-	fprintf(file, "%lld %s\n", (long long)os_monotonic_get_ns(), state);
+	// The first two fields remain the guard's status protocol. Diagnostics are
+	// available without a debug launch or stopping tracking with a debugger.
+	const struct rift_s_world_anchor_result *result = &diagnostics.result;
+	fprintf(file, "%lld %s attempts=%" PRIu64 " usable=%" PRIu64
+	              " capture_ns=%lld sample_ns=%lld pose_valid=%d flags=%u standby=%d paused=%d restart=%d"
+	              " generation=%" PRIu64 " confirmations=%u keyframes=%u"
+	              " features=%u/%u/%u/%u/%u matches=%u/%u/%u/%u/%u inliers=%u/%u/%u/%u/%u"
+	              " matched=%d rejection=%s\n",
+	        (long long)os_monotonic_get_ns(), state, diagnostics.attempts, diagnostics.usable,
+	        (long long)diagnostics.capture_ns, (long long)diagnostics.sample_ns, diagnostics.pose_valid,
+	        diagnostics.pose_flags, standby, paused, restarting, generation, confirmations, result->keyframes,
+	        result->features[0], result->features[1], result->features[2], result->features[3], result->features[4],
+	        result->matches[0], result->matches[1], result->matches[2], result->matches[3], result->matches[4],
+	        result->inliers[0], result->inliers[1], result->inliers[2], result->inliers[3], result->inliers[4],
+	        result->relocalized, result->rejection ? result->rejection : "none");
 	if (fclose(file) == 0) rename(temp, path);
 }
 
@@ -1352,6 +1391,11 @@ rift_s_world_thread(void *ptr)
 			                     valid ? &capture.pose.orientation : NULL, &result);
 			attempts++;
 			usable += valid;
+			os_mutex_lock(&t->mutex);
+			t->world_diagnostics = (struct rift_s_world_diagnostics){
+			    .attempts = attempts, .usable = usable, .capture_ns = capture_ns, .sample_ns = source_ns,
+			    .pose_valid = valid, .pose_flags = capture.relation_flags, .result = result};
+			os_mutex_unlock(&t->mutex);
 			if (capture_ns - last_attempt_log_ns >= U_TIME_1S_IN_NS) {
 				last_attempt_log_ns = capture_ns;
 				RIFT_S_INFO("World attempt=%" PRIu64 " usable=%" PRIu64

src/xrt/drivers/rift_s/rift_s_tracker.h

@@ -35,6 +35,15 @@
 
 struct rift_s_hmd_config;
 
+struct rift_s_world_diagnostics
+{
+	uint64_t attempts, usable;
+	timepoint_ns capture_ns, sample_ns;
+	bool pose_valid;
+	uint32_t pose_flags;
+	struct rift_s_world_anchor_result result;
+};
+
 enum rift_s_tracker_pose
 {
 	RIFT_S_TRACKER_POSE_IMU,
@@ -65,6 +74,7 @@ struct rift_s_tracker
 	struct xrt_pose world_candidate;
 	uint64_t world_candidate_generation;
 	timepoint_ns world_candidate_ns;
+	struct rift_s_world_diagnostics world_diagnostics;
 
 	//! Camera arrival watchdog uses the host clock, not predicted pose time.
 	timepoint_ns last_camera_arrival_ns;
@@ -181,6 +191,9 @@ rift_s_tracker_create(struct xrt_tracking_origin *origin,
                       struct rift_s_hmd_config *hmd_config);
 void
 rift_s_tracker_start(struct rift_s_tracker *t);
+//! Join consumers and release queued camera frames before destroying the frame context.
+void
+rift_s_tracker_stop(struct rift_s_tracker *t);
 void
 rift_s_tracker_set_standby(struct rift_s_tracker *t, bool standby);
 void

src/xrt/drivers/rift_s/rift_s_world_anchor.cpp

@@ -183,6 +183,29 @@ spread(const std::vector<cv::Point2d> &pixels, const rift_s_camera_calibration &
 	return occupied >= 4 && maxx - minx > 0.25 * c.roi.extent.w && maxy - miny > 0.2 * c.roi.extent.h;
 }
 
+// OpenCV seeds PnP in the perspective plane. A native pixel has a different
+// scale there at each fisheye bearing. Only the final native-pixel score decides support.
+double
+perspective_pixel_scale(const rift_s_camera_calibration &c, cv::Vec3d ray)
+{
+	double radius = std::hypot(ray[0], ray[1]), theta = std::atan2(radius, ray[2]);
+	cv::Vec2d angular = radius > 1e-12 ? cv::Vec2d{ray[0], ray[1]} * (theta / radius)
+	                                  : cv::Vec2d{0, 0};
+	constexpr double eps = 1e-6;
+	auto dx = (distort(c, angular + cv::Vec2d{eps, 0}) - distort(c, angular - cv::Vec2d{eps, 0})) / (2 * eps);
+	auto dy = (distort(c, angular + cv::Vec2d{0, eps}) - distort(c, angular - cv::Vec2d{0, eps})) / (2 * eps);
+	cv::Matx22d distortion{dx[0], dy[0], dx[1], dy[1]};
+	double gain = theta > 1e-12 ? std::tan(theta) / theta : 1;
+	double radial = theta > 1e-8 ? (1 / (ray[2] * ray[2]) - gain) / (theta * theta) : 2.0 / 3;
+	cv::Matx22d perspective = gain * cv::Matx22d::eye() + radial * cv::Matx22d{
+	    angular[0] * angular[0], angular[0] * angular[1], angular[0] * angular[1], angular[1] * angular[1]};
+	cv::Matx22d jacobian = perspective * distortion.inv() * cv::Matx22d{1 / c.projection.fx, 0, 0, 1 / c.projection.fy};
+	auto squared = jacobian.t() * jacobian;
+	double eigenvalue = 0.5 * (squared(0, 0) + squared(1, 1) +
+	                          std::hypot(squared(0, 0) - squared(1, 1), 2 * squared(0, 1)));
+	return std::sqrt(eigenvalue);
+}
+
 // PnP supplies the robust seed. All subsequent fitting is on the four parameters
 // allowed by gravity, using the calibrated fisheye residual rather than perspective pixels.
 bool
@@ -628,7 +651,7 @@ struct rift_s_world_anchor
 	cv::Mat reference_images[RIFT_S_WORLD_MAX_CAMERAS];
 	Features reference_features[RIFT_S_WORLD_MAX_CAMERAS];
 	std::string record_dir;
-	unsigned recorded_pairs = 0;
+	unsigned recorded_pairs = 0, record_limit = 600;
 	cv::Ptr<cv::ORB> orb = cv::ORB::create(1400, 1.2f, 8, 19, 0, 2, cv::ORB::HARRIS_SCORE, 31, 12);
 };
 
@@ -702,6 +725,7 @@ rift_s_world_anchor_solve_rig_gravity(const rift_s_world_anchor_config *config,
 				const auto &c = config->camera[eye];
 				std::vector<cv::Point3d> world;
 				std::vector<cv::Point2d> normalized, pixels;
+				double threshold = 0;
 				for (uint32_t i = 0; i < counts[eye]; ++i) {
 					const auto &o = obs[eye][i];
 					cv::Vec3d ray;
@@ -712,10 +736,10 @@ rift_s_world_anchor_solve_rig_gravity(const rift_s_world_anchor_config *config,
 					world.emplace_back(o.world_point.x, o.world_point.y, o.world_point.z);
 					normalized.emplace_back(ray[0] / ray[2], ray[1] / ray[2]);
 					pixels.emplace_back(o.pixel.x, o.pixel.y);
+					threshold = std::max(threshold, MAX_ERROR_PX * perspective_pixel_scale(c, ray));
 				}
 				cv::Vec3d rv, tv;
 				cv::Mat inliers;
-				double threshold = MAX_ERROR_PX / std::max(c.projection.fx, c.projection.fy);
 				result->rejection = "pnp";
 				bool solved =
 				    cv::solvePnPRansac(world, normalized, cv::Matx33d::eye(), cv::noArray(), rv, tv,
@@ -950,9 +974,15 @@ rift_s_world_anchor_create(const rift_s_world_anchor_config *config, const char
 		a->config = *config;
 		resolve_boundary_path(a->config);
 		a->path = path ? path : "";
-		const char *record = std::getenv("RIFT_S_WORLD_RECORD_DIR"),
-		           *bench = std::getenv("MONADO_STEAMVR_BENCH");
-		if (record && record[0] && bench && (std::strcmp(bench, "1") == 0 || std::strcmp(bench, "true") == 0)) {
+		const char *record = std::getenv("RIFT_S_WORLD_RECORD_DIR");
+		if (record && record[0]) {
+			// Diagnostics must not enable synthetic poses or override headset proximity.
+			if (const char *limit = std::getenv("RIFT_S_WORLD_RECORD_LIMIT")) {
+				char *end = nullptr;
+				unsigned long value = std::strtoul(limit, &end, 10);
+				if (end != limit && *end == 0 && value > 0 && value <= 600)
+					a->record_limit = unsigned(value);
+			}
 			if (mkdir(record, 0700) == 0 || errno == EEXIST) {
 				a->record_dir = record;
 				FILE *file = std::fopen((a->record_dir + "/calibration.bin").c_str(), "wb");
@@ -984,7 +1014,7 @@ rift_s_world_anchor_record_rig_frames(rift_s_world_anchor *a,
                                       unsigned count,
                                       const xrt_pose *raw)
 {
-	if (!a || a->record_dir.empty() || a->recorded_pairs >= 600 || !frames || count < 2 ||
+	if (!a || a->record_dir.empty() || a->recorded_pairs >= a->record_limit || !frames || count < 2 ||
 	    count > RIFT_S_WORLD_MAX_CAMERAS || !frames[0])
 		return;
 	for (unsigned eye = 0; eye < count; ++eye) {

src/xrt/drivers/rift_s/rift_s_world_anchor.h

@@ -37,8 +37,8 @@ struct rift_s_world_anchor_result
 	bool recorded, relocalized;
 };
 
-// Opt-in bench recorder. Writes at most 600 synchronized camera sets and capture-time SLAM poses.
-// RIFT_S_WORLD_RECORD_DIR is honored only with MONADO_STEAMVR_BENCH enabled.
+// Opt-in diagnostics recorder. Writes synchronized camera sets and capture-time SLAM poses.
+// RIFT_S_WORLD_RECORD_DIR does not change tracking. RIFT_S_WORLD_RECORD_LIMIT caps sets at 1..600.
 void
 rift_s_world_anchor_record_frames(struct rift_s_world_anchor *anchor,
                                   const struct xrt_frame *left,

src/xrt/include/tracking/t_constellation_motion.h

@@ -0,0 +1,8 @@
+// Copyright 2026, lbgos
+// SPDX-License-Identifier: BSL-1.0
+#pragma once
+
+#include <stdint.h>
+
+//! Maximum capture gap for consecutive optical tracking candidates.
+#define T_CONSTELLATION_TRACK_MAX_GAP_NS INT64_C(300000000)

src/xrt/include/tracking/t_constellation_tracking.h

@@ -107,8 +107,8 @@ struct t_constellation_tracked_device_callbacks
 	void (*push_brightness_update)(struct xrt_device *xdev, uint8_t average_brightness);
 	//! True only when optical heading and capture-time attitude support a rotation prior.
 	bool (*has_heading_prior)(struct xrt_device *xdev, timepoint_ns capture_ns);
-	//! Optional: preferred over push_observed_pose when set, carries fit quality and yaw observability.
-	void (*push_pose_observation)(struct xrt_device *xdev, const struct t_constellation_pose_observation *obs);
+	//! Optional: preferred over push_observed_pose. False releases the hypothesis and its blobs.
+	bool (*push_pose_observation)(struct xrt_device *xdev, const struct t_constellation_pose_observation *obs);
 };
 
 struct t_constellation_tracked_device_connection *

src/xrt/state_trackers/steamvr_drv/ovrd_driver.cpp

@@ -19,6 +19,7 @@
 #include <cstdio>
 #include <cmath>
 #include <algorithm>
+#include <fstream>
 
 #include "math/m_api.h"
 #include "ovrd_log.hpp"
@@ -26,6 +27,8 @@
 #include "ovrd_display.hpp"
 #include "ovrd_camera.hpp"
 #include "ovrd_roomsetup_guard_process.hpp"
+#include "ovrd_world_universe.hpp"
+#include "../../drivers/rift_s/rift_s_world_control.h"
 #include <memory>
 #include "openvr_driver.h"
 
@@ -81,6 +84,28 @@ is_rift_s_touch(const struct xrt_device *xdev)
 	return xdev->name == XRT_DEVICE_TOUCH_CONTROLLER && strncmp(xdev->str, prefix, sizeof(prefix) - 1) == 0;
 }
 
+static void
+update_rift_s_universe(vr::PropertyContainerHandle_t container, uint64_t &universe, int64_t &last_poll)
+{
+	int64_t now = os_monotonic_get_ns();
+	if (now - last_poll < 500000000LL) return;
+	last_poll = now;
+	char path[1024];
+	rift_s_world_control_path("status", path, sizeof(path));
+	std::ifstream stream(path);
+	int64_t stamp = 0;
+	std::string status;
+	stream >> stamp >> status;
+	static const int64_t started = now;
+	uint64_t next = rift_s_world_universe(status, stamp, now, started);
+	if (next == universe) return;
+	auto error = vr::VRProperties()->SetUint64Property(container, vr::Prop_CurrentUniverseId_Uint64, next);
+	if (error == vr::TrackedProp_Success) {
+		universe = next;
+		ovrd_log("Rift S room universe=%llu status=%s\n", (unsigned long long)next, status.c_str());
+	}
+}
+
 // DebugRequest bench commands expire so a stopped script cannot hold a trigger.
 // Commands: bench x y z pitch trigger; bench6 x y z pitch yaw roll trigger.
 // Angles are radians. "bench off" returns to tracking.
@@ -896,6 +921,8 @@ public:
 			std::this_thread::sleep_for(std::chrono::milliseconds(1));
 
 			if (m_unObjectId != vr::k_unTrackedDeviceIndexInvalid) {
+				if (is_rift_s_touch(m_xdev))
+					update_rift_s_universe(m_ulPropertyContainer, m_worldUniverse, m_worldPoll);
 				vr::VRServerDriverHost()->TrackedDevicePoseUpdated(m_unObjectId, GetPose(),
 				                                                   sizeof(vr::DriverPose_t));
 			}
@@ -928,7 +955,8 @@ public:
 		m_ulPropertyContainer = vr::VRProperties()->TrackedDeviceToPropertyContainer(m_unObjectId);
 
 		// return a constant that's not 0 (invalid) or 1 (reserved for Oculus)
-		vr::VRProperties()->SetUint64Property(m_ulPropertyContainer, vr::Prop_CurrentUniverseId_Uint64, 2);
+		m_worldUniverse = is_rift_s_touch(m_xdev) ? 3 : 2;
+		vr::VRProperties()->SetUint64Property(m_ulPropertyContainer, vr::Prop_CurrentUniverseId_Uint64, m_worldUniverse);
 		vr::VRProperties()->SetInt32Property(m_ulPropertyContainer, vr::Prop_DeviceClass_Int32, vr::TrackedDeviceClass_Controller);
 
 		if (m_hand == XRT_HAND_LEFT) {
@@ -1260,6 +1288,8 @@ public:
 	vr::DriverPose_t m_pose;
 	vr::TrackedDeviceIndex_t m_unObjectId;
 	vr::PropertyContainerHandle_t m_ulPropertyContainer;
+	uint64_t m_worldUniverse = 3;
+	int64_t m_worldPoll = 0;
 
 	bool m_emulate_index_controller = false;
 
@@ -1370,6 +1400,8 @@ private:
 
 	vr::TrackedDeviceIndex_t m_trackedDeviceIndex = 0;
 	vr::PropertyContainerHandle_t m_ulPropertyContainer = vr::k_ulInvalidPropertyContainer;
+	uint64_t m_worldUniverse = 3;
+	int64_t m_worldPoll = 0;
 
 	float m_flSecondsFromVsyncToPhotons = -1;
 	float m_flDisplayFrequency = -1;
@@ -1448,6 +1480,8 @@ CDeviceDriver_Monado::PoseUpdateThreadFunction()
 		std::this_thread::sleep_for(std::chrono::milliseconds(1));
 		if (m_benchProximity)
 			vr::VRDriverInput()->UpdateBooleanComponent(m_benchProximity, true, 0);
+		if (strncmp(m_xdev->str, "Oculus Rift S", 13) == 0)
+			update_rift_s_universe(m_ulPropertyContainer, m_worldUniverse, m_worldPoll);
 		vr::VRServerDriverHost()->TrackedDevicePoseUpdated(m_trackedDeviceIndex, GetPose(),
 		                                                   sizeof(vr::DriverPose_t));
 	}
@@ -1482,7 +1516,8 @@ CDeviceDriver_Monado::Activate(vr::TrackedDeviceIndex_t unObjectId)
 		ovrd_log("MONADO_STEAMVR_BENCH enabled: synthetic fallback and awake HMD\n");
 	}
 		// return a constant that's not 0 (invalid) or 1 (reserved for Oculus)
-	vr::VRProperties()->SetUint64Property( m_ulPropertyContainer, vr::Prop_CurrentUniverseId_Uint64, 2);
+	m_worldUniverse = strncmp(m_xdev->str, "Oculus Rift S", 13) == 0 ? 3 : 2;
+	vr::VRProperties()->SetUint64Property(m_ulPropertyContainer, vr::Prop_CurrentUniverseId_Uint64, m_worldUniverse);
 
     if (m_xdev->camera) {
         vr::EVRInitError queue_error = vr::VRInitError_None;
@@ -1672,12 +1707,8 @@ CDeviceDriver_Monado::GetPose()
 	    .shouldApplyHeadModel = !m_xdev->supported.position_tracking,
 	    .deviceIsConnected = true,
 	};
-	// A loss of positional tracking still has a finite held position. Dropping it sent Y=0 to games.
-	apply_pose(&rel, &t, XRT_SPACE_RELATION_POSITION_VALID_BIT);
-	if (!(rel.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT)) {
-		t.result = vr::TrackingResult_Running_OutOfRange;
-		t.poseIsValid = false;
-	}
+	// Rift S keeps a finite fallback while VIO recovers or the saved room awaits visual confirmation.
+	apply_head_pose(&rel, &t, strcmp(m_xdev->str, "Oculus Rift S") == 0);
 
 #ifdef DUMP_POSE
 	ovrd_log("get hmd pose %f %f %f %f, %f %f %f\n", t.qRotation.x, t.qRotation.y, t.qRotation.z, t.qRotation.w,

src/xrt/state_trackers/steamvr_drv/ovrd_pose.hpp

@@ -96,6 +96,22 @@ apply_pose(struct xrt_space_relation *rel,
 	copy_pose_velocities(rel, m_pose);
 }
 
+inline void
+apply_head_pose(struct xrt_space_relation *rel, vr::DriverPose_t *pose, bool allow_position_estimate)
+{
+	apply_pose(rel, pose, XRT_SPACE_RELATION_POSITION_VALID_BIT, true);
+	if (allow_position_estimate) {
+		const auto valid = XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT;
+		pose->poseIsValid = finite_pose(rel->pose) && (rel->relation_flags & valid) == valid;
+		if (pose->poseIsValid) copy_quat(&rel->pose.orientation, &pose->qRotation);
+	}
+	if ((rel->relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT) == 0 ||
+	    (rel->relation_flags & XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT) == 0) {
+		pose->result = vr::TrackingResult_Running_OutOfRange;
+		if (!allow_position_estimate) pose->poseIsValid = false;
+	}
+}
+
 inline void
 transform_rift_s_touch_relation(struct xrt_space_relation *rel,
                                 const struct xrt_pose *grip_to_model,

src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_floor.hpp

@@ -84,9 +84,9 @@ public:
 		frame.write(active);
 	}
 	bool
-	publish(bool initialize_manual = false)
+	publish(bool initialize_manual = false, bool saved_plane_trusted = true)
 	{
-		if (!handle || !paths)
+		if (!saved_plane_trusted || !handle || !paths)
 			return false;
 		vr::HmdMatrix34_t standing{}, setup{};
 		if (!transforms(standing, setup))

src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_guard.cpp

@@ -105,6 +105,7 @@ static bool update_world_notification(vr::IVRNotifications &notifications,
 		    warning.c_str(), vr::EVRNotificationStyle_Application, nullptr, &created) !=
 		    vr::VRNotificationError_OK) return false;
 		id = created;
+		fprintf(stderr, "monado-roomsetup-guard: Guardian notification %u: %s\n", created, warning.c_str());
 	}
 	delivered = warning;
 	return true;
@@ -261,6 +262,10 @@ int main(int argc, char **argv)
 	fs::path path = calibration_path(log_path);
 	OverflowLog diagnostics(log_path);
 	if (!system || !setup || path.empty() || !snapshot(*setup, path)) { shutdown(); return 1; }
+	char model[128] = {};
+	system->GetStringTrackedDeviceProperty(vr::k_unTrackedDeviceIndex_Hmd, vr::Prop_ModelNumber_String,
+	                                     model, sizeof(model));
+	const bool is_rift_s = std::strstr(model, "Oculus Rift S") != nullptr;
 	vr::VROverlayHandle_t overlay = vr::k_ulOverlayHandleInvalid;
 	if (overlays) overlays->CreateOverlay("monado.roomsetup.guard", "Monado playspace setup", &overlay);
 	if (write(ready, "1", 1) != 1) { shutdown(); return 1; }
@@ -301,6 +306,7 @@ int main(int argc, char **argv)
 	auto floor_deadline = std::chrono::steady_clock::time_point{};
 	bool running = true;
 	bool world_setup = false;
+	bool saved_plane_trusted = !is_rift_s;
 	auto world_cancel_deadline = std::chrono::steady_clock::time_point{};
 	std::string world_warning;
 	vr::VRNotificationId world_notification = 0;
@@ -338,6 +344,10 @@ int main(int argc, char **argv)
 					if (saved && compositor_failed()) failed = true;
 				}
 				if ((next == 2 || next == 3 || drawing(next)) && !world_setup && !failed) {
+					// Capture trust before begin switches the tracker into a new setup frame.
+					saved_plane_trusted = !is_rift_s || (world_status() == "localized" &&
+					    system->GetUint64TrackedDeviceProperty(vr::k_unTrackedDeviceIndex_Hmd,
+					        vr::Prop_CurrentUniverseId_Uint64) == 2);
 					world_setup = true;
 					world_cancel_deadline = {};
 					world_command("begin");
@@ -383,9 +393,9 @@ int main(int argc, char **argv)
 		if (status == "save_failed")
 			warning = "Visual boundary map could not be saved. Check Monado config write access before restarting VR.";
 		else if (status == "unanchored")
-			warning = "Boundary has no visual anchor. Complete room setup once to save its physical location.";
+			warning = "Guardian not found: no usable saved room map. Run Room Setup to anchor the boundary.";
 		else if (status == "relocalizing")
-			warning = "Boundary not confirmed, look around. Temporary tracking is active.";
+			warning = "Guardian not found. Uncover headset cameras and look around. If the room stays unavailable, run Room Setup.";
 		auto now = std::chrono::steady_clock::now();
 		if (warning != world_warning && notifications && overlays && now >= next_notification_attempt) {
 			next_notification_attempt = now + std::chrono::seconds(1);
@@ -395,7 +405,8 @@ int main(int argc, char **argv)
 				update_world_notification(*notifications, overlay, warning, world_warning, world_notification);
 		}
 		if (floor_pending) {
-			if (step == 0 || floor->publish(step == 2) || std::chrono::steady_clock::now() > floor_deadline)
+			if (step == 0 || floor->publish(step == 2, saved_plane_trusted) ||
+			    std::chrono::steady_clock::now() > floor_deadline)
 				floor_pending = false;
 		}
 		floor->update_frame(step == 2 || step == 3 || step == 4 || step == 5 || step == 7);

src/xrt/state_trackers/steamvr_drv/ovrd_world_universe.hpp

@@ -0,0 +1,15 @@
+// Copyright 2026, lbgos
+// SPDX-License-Identifier: BSL-1.0
+#pragma once
+
+#include <cstdint>
+#include <string_view>
+
+// Universe 2 contains the saved room. Universe 3 has no saved room calibration.
+// A session frame must never use universe 2's floor, heading or collision bounds.
+static inline uint64_t
+rift_s_world_universe(std::string_view status, int64_t stamp, int64_t now, int64_t started = 0)
+{
+	bool fresh = stamp > 0 && stamp >= started && stamp <= now && now - stamp <= 2000000000LL;
+	return fresh && (status == "localized" || status == "setup") ? 2 : 3;
+}

src/xrt/targets/common/target_builder_rift_s.c

@@ -44,7 +44,7 @@ enum u_logging_level rift_s_log_level;
  *
  */
 
-DEBUG_GET_ONCE_LOG_OPTION(rift_s_log, "RIFT_S_LOG", U_LOGGING_INFO)
+DEBUG_GET_ONCE_LOG_OPTION(rift_s_log, "RIFT_S_LOG", U_LOGGING_WARN)
 
 #ifdef XRT_BUILD_DRIVER_HANDTRACKING
 DEBUG_GET_ONCE_BOOL_OPTION(rift_s_hand_tracking_as_controller, "RIFT_S_HAND_TRACKING_AS_CONTROLLERS", false)

src/xrt/targets/steamvr_drv/CMakeLists.txt

@@ -86,7 +86,6 @@ install(
 if(TARGET monado-roomsetup-guard)
     install(TARGETS monado-roomsetup-guard RUNTIME DESTINATION ${CMAKE_INSTALL_DATADIR}/steamvr-monado/bin/linux64)
 endif()
-
 if(CMAKE_SYSTEM_NAME STREQUAL "Linux")
     configure_file(${PROJECT_SOURCE_DIR}/scripts/rifts-setup.in ${PROJECT_BINARY_DIR}/rifts-setup @ONLY)
     configure_file(${PROJECT_SOURCE_DIR}/scripts/rifts-session-install.in ${PROJECT_BINARY_DIR}/rifts-session-install @ONLY)

src/xrt/tracking/constellation/internal/constrained_pose.cpp

@@ -1,6 +1,7 @@
 // Copyright 2026, lbgos
 // SPDX-License-Identifier: BSL-1.0
 #include "constrained_pose.h"
+#include "tracking/t_constellation_motion.h"
 
 #include <Eigen/Core>
 #include <Eigen/Eigenvalues>
@@ -918,8 +919,8 @@ prior_pairs(Context &ctx, const Pose &prior)
 void
 constellation_constrained_default_config(constellation_constrained_config *c)
 {
-	/* Score/association gates and the angular separation follow the stock Windows defaults.
-	 * Everything else is Monado policy chosen with the synthetic
+	/* Score/association gates and the angular separation are compiled defaults named in the
+	 * handoff specification. Everything else is Monado policy chosen with the synthetic
 	 * partial-ring tests in tests_constellation_constrained: object-space ray noise 1 mm,
 	 * position std 3 cm, yaw std 20 degrees, ambiguity gap chi-square 9, at least 4 observations
 	 * of 3 distinct LEDs (cold and tracking), at most one unmatched blob within 8 mm (twice the
@@ -1362,7 +1363,7 @@ void
 constellation_constrained_default_motion_limits(constellation_motion_limits *limits)
 {
 	// Proposed values: fast hand motion stays below about 3 m/s; 2 cm covers fit noise.
-	*limits = {3.0, 0.02, 10 * pi / 180, 300000000};
+	*limits = {3.0, 0.02, 10 * pi / 180, T_CONSTELLATION_TRACK_MAX_GAP_NS};
 }
 
 bool

src/xrt/tracking/constellation/internal/constrained_pose.h

@@ -66,7 +66,7 @@ enum constellation_constrained_solver_status
 
 struct constellation_constrained_config
 {
-	//! Object-space score gate in metres (stock default, not an active setting).
+	//! Object-space score gate in metres (compiled default from the handoff spec, not an active setting).
 	double score_gate_m;
 	//! Broad object-space association gate in metres for candidate edges.
 	double association_gate_m;
@@ -88,18 +88,18 @@ struct constellation_constrained_config
 	double minimum_bearing_separation_rad;
 	double minimum_depth_m;
 	double maximum_depth_m;
-	/* Proposed noise/uncertainty policy, not device calibration. */
+	/* Proposed noise/uncertainty policy, not recovered device calibration. */
 	double ray_noise_std_m;
 	double maximum_position_std_m;
 	double maximum_yaw_std_rad;
 	//! Minimum likelihood gap (chi-square units of ray_noise_std_m) to the best competing assignment.
 	double ambiguity_chi_square;
-	/* Monado verification policy, beyond the stock scorer count (inliers > sample size):
+	/* Monado verification policy, beyond the recovered scorer count (inliers > sample size):
 	 * accepted observations required when acquiring without a trusted position prior, and the
 	 * distinct physical LEDs among them. Repeated views of one LED do not fix its identity. */
 	unsigned minimum_cold_inliers;
 	unsigned minimum_cold_distinct_leds;
-	/* The same counts for searches around a trusted prior. The stock minimum (inliers > sample
+	/* The same counts for searches around a trusted prior. The recovered minimum (inliers > sample
 	 * size, so 2 for the retained prior) let one stale prior keep re-verifying itself on two blobs
 	 * in worn use; a published pose now needs this much evidence on every path. */
 	unsigned minimum_tracking_inliers;

src/xrt/tracking/constellation/t_constellation_tracking.c

@@ -33,7 +33,7 @@
 #include "internal/ransac_pnp.h"
 #include "internal/sample.h"
 
-DEBUG_GET_ONCE_LOG_OPTION(ct_log, "CONSTELLATION_LOG", U_LOGGING_INFO)
+DEBUG_GET_ONCE_LOG_OPTION(ct_log, "CONSTELLATION_LOG", U_LOGGING_WARN)
 DEBUG_GET_ONCE_FLOAT_OPTION(ct_score_gate, "CONSTELLATION_SCORE_GATE_M", 0.004)
 DEBUG_GET_ONCE_FLOAT_OPTION(ct_association_gate, "CONSTELLATION_ASSOCIATION_GATE_M", 0.15)
 DEBUG_GET_ONCE_FLOAT_OPTION(ct_ray_noise, "CONSTELLATION_RAY_NOISE_M", 0.001)
@@ -371,19 +371,22 @@ constellation_tracked_device_connection_has_heading(struct t_constellation_track
 	return ret;
 }
 
-static void
+static bool
 constellation_tracked_device_connection_notify_observation(struct t_constellation_tracked_device_connection *ctdc,
                                                            const struct t_constellation_pose_observation *obs)
 {
+	bool accepted = false;
 	os_mutex_lock(&ctdc->lock);
 	if (!ctdc->disconnected) {
 		if (ctdc->cb->push_pose_observation) {
-			ctdc->cb->push_pose_observation(ctdc->xdev, obs);
+			accepted = ctdc->cb->push_pose_observation(ctdc->xdev, obs);
 		} else if (ctdc->cb->push_observed_pose) {
 			ctdc->cb->push_observed_pose(ctdc->xdev, obs->capture_ns, &obs->pose);
+			accepted = true;
 		}
 	}
 	os_mutex_unlock(&ctdc->lock);
+	return accepted;
 }
 
 static const char *ct_stage_names[CT_STAGE_COUNT] = {"fast", "constrained", "p3p"};
@@ -798,10 +801,10 @@ track_gate_locked(struct t_constellation_tracker *ct,
 		d->last_failure_capture_ns = capture_ns;
 		d->last_failure_processing_ns = os_monotonic_get_ns();
 	}
-	log_track_transition_locked(ct, device, before, capture_ns,
-	                            verdict == CONSTELLATION_TRACK_HOLD_RESTART ? "jumps"
-	                            : verdict == CONSTELLATION_TRACK_PUBLISH    ? "confirmed"
-	                                                                        : "candidate");
+	if (verdict != CONSTELLATION_TRACK_PUBLISH) {
+		log_track_transition_locked(ct, device, before, capture_ns,
+		                            verdict == CONSTELLATION_TRACK_HOLD_RESTART ? "jumps" : "candidate");
+	}
 	return verdict == CONSTELLATION_TRACK_PUBLISH ? NULL : constellation_track_verdict_name(verdict);
 }
 
@@ -833,7 +836,14 @@ observation_from_result(const struct constellation_constrained_result *r,
 
 /* Publish one rig pose per exposure. Labels are evidence from this accepted hypothesis,
  * never independent ownership carried from an earlier frame. */
-static bool
+enum joint_pose_submission
+{
+	JOINT_POSE_HELD,
+	JOINT_POSE_FUSION_REJECTED,
+	JOINT_POSE_PUBLISHED,
+};
+
+static enum joint_pose_submission
 submit_joint_pose(struct t_constellation_tracker *ct,
                   struct tracking_sample_device_state *state,
                   struct constellation_tracking_sample *sample,
@@ -842,6 +852,7 @@ submit_joint_pose(struct t_constellation_tracker *ct,
 	struct constellation_tracker_device *device = &ct->devices[state->dev_index];
 	struct constellation_constrained_result *r = &state->joint_result;
 	os_mutex_lock(&ct->tracked_device_lock);
+	const struct constellation_track previous_track = device->track;
 	const char *held = NULL;
 	if (device->have_last_seen_pose && sample->timestamp <= device->last_seen_pose_ts) {
 		device->diagnostics.stale++;
@@ -856,8 +867,25 @@ submit_joint_pose(struct t_constellation_tracker *ct,
 		log_device_summary_locked(ct, device, false);
 		os_mutex_unlock(&ct->tracked_device_lock);
 		log_joint_result(ct, sample, state, stage, held);
-		return false;
+		return JOINT_POSE_HELD;
 	}
+	struct xrt_pose P_xrworld_model, P_xrworld_device;
+	pose_flip_YZ(&r->P_world_model, &P_xrworld_model);
+	math_pose_transform(&P_xrworld_model, &device->led_model.P_model_device, &P_xrworld_device);
+	struct t_constellation_pose_observation obs;
+	observation_from_result(r, sample->timestamp, &P_xrworld_device, &obs);
+	if (!constellation_tracked_device_connection_notify_observation(device->connection, &obs)) {
+		// Fusion declined this pose. Keep its accepted prior and leave the blobs available.
+		device->track = previous_track;
+		device->diagnostics.last_failure = "fusion_rejected";
+		device->diagnostics.last_failure_capture_ns = sample->timestamp;
+		device->diagnostics.last_failure_processing_ns = os_monotonic_get_ns();
+		log_device_summary_locked(ct, device, false);
+		os_mutex_unlock(&ct->tracked_device_lock);
+		log_joint_result(ct, sample, state, stage, "fusion_rejected");
+		return JOINT_POSE_FUSION_REJECTED;
+	}
+	log_track_transition_locked(ct, device, previous_track.state, sample->timestamp, "confirmed");
 	device->have_last_seen_pose = true;
 	device->last_seen_pose_ts = sample->timestamp;
 	device->last_seen_pose = r->P_world_model;
@@ -914,11 +942,6 @@ submit_joint_pose(struct t_constellation_tracker *ct,
 		os_mutex_unlock(&ct->cam[v].bw_lock);
 	}
 
-	struct xrt_pose P_xrworld_model, P_xrworld_device;
-	pose_flip_YZ(&state->final_pose, &P_xrworld_model);
-	math_pose_transform(&P_xrworld_model, &device->led_model.P_model_device, &P_xrworld_device);
-	struct t_constellation_pose_observation obs;
-	observation_from_result(r, sample->timestamp, &P_xrworld_device, &obs);
 	struct xrt_vec3 head = {NAN, NAN, NAN};
 	if (head_relative(sample, device, &r->P_world_model, &head)) {
 		device->diagnostics.last_published_head = head;
@@ -964,7 +987,6 @@ submit_joint_pose(struct t_constellation_tracker *ct,
 		         projected ? maximum_pixel_error : NAN, r->fit_chi_square, r->fit_chi_square_limit, r->cost_m);
 	}
 	constellation_tracked_device_connection_notify_brightness_update(device->connection, brightness / r->inliers);
-	constellation_tracked_device_connection_notify_observation(device->connection, &obs);
 
 	if (ct->controller_masks_sink) {
 		for (unsigned v = 0; v < sample->n_views; v++) {
@@ -987,7 +1009,7 @@ submit_joint_pose(struct t_constellation_tracker *ct,
 	log_device_summary_locked(ct, device, false);
 	os_mutex_unlock(&ct->tracked_device_lock);
 	log_joint_result(ct, sample, state, stage, "none");
-	return true;
+	return JOINT_POSE_PUBLISHED;
 }
 
 /* A competing candidate keeps a shared blob only with this much more inlier support. */
@@ -1004,17 +1026,45 @@ arbitrate_and_submit(struct t_constellation_tracker *ct,
 {
 	const struct constellation_constrained_result *claims[CONSTELLATION_MAX_DEVICES];
 	bool published[CONSTELLATION_MAX_DEVICES] = {0};
+	bool viable[CONSTELLATION_MAX_DEVICES] = {0};
+	bool attempted[CONSTELLATION_MAX_DEVICES] = {0};
 	enum constellation_constrained_claim blocked[CONSTELLATION_MAX_DEVICES];
 	unsigned blockers[CONSTELLATION_MAX_DEVICES];
 	for (unsigned d = 0; d < sample->n_devices; d++) {
 		claims[d] = &sample->devices[d].joint_result;
 		published[d] = sample->devices[d].found_device_pose;
+		viable[d] = candidates[d];
+	}
+	for (unsigned round = 0; round < sample->n_devices; round++) {
+		// Held optical candidates remain competitors. Only fusion rejection releases a claim.
+		bool competitors[CONSTELLATION_MAX_DEVICES] = {0};
+		for (unsigned d = 0; d < sample->n_devices; d++) {
+			competitors[d] = viable[d] && !published[d] && claims[d] != NULL;
+		}
+		constellation_constrained_arbitrate_with_owners(claims, published, competitors, blocked, blockers,
+		                                                sample->n_devices, ARBITRATION_SUPPORT_MARGIN);
+		bool released = false;
+		for (unsigned d = 0; d < sample->n_devices; d++) {
+			candidates[d] = competitors[d];
+			if (!competitors[d] || attempted[d]) {
+				continue;
+			}
+			attempted[d] = true;
+			enum joint_pose_submission result = submit_joint_pose(ct, &sample->devices[d], sample, stage);
+			if (result == JOINT_POSE_FUSION_REJECTED) {
+				claims[d] = NULL;
+				released = true;
+			} else if (result == JOINT_POSE_PUBLISHED) {
+				published[d] = true;
+			}
+		}
+		if (!released) {
+			break;
+		}
 	}
-	constellation_constrained_arbitrate_with_owners(claims, published, candidates, blocked, blockers,
-	                                                sample->n_devices, ARBITRATION_SUPPORT_MARGIN);
 	for (unsigned d = 0; d < sample->n_devices; d++) {
 		struct tracking_sample_device_state *state = &sample->devices[d];
-		if (blocked[d] != CONSTELLATION_CLAIM_NONE) {
+		if (!attempted[d] && blocked[d] != CONSTELLATION_CLAIM_NONE) {
 			os_mutex_lock(&ct->tracked_device_lock);
 			ct->devices[state->dev_index].diagnostics.shared_blob++;
 			ct->devices[state->dev_index].diagnostics.shared_by[blocked[d]]++;
@@ -1061,9 +1111,6 @@ arbitrate_and_submit(struct t_constellation_tracker *ct,
 			                 : blocked[d] == CONSTELLATION_CLAIM_CANDIDATE ? "shared_blob_candidate"
 			                                                               : "shared_blob_rejected");
 		}
-		if (candidates[d]) {
-			submit_joint_pose(ct, state, sample, stage);
-		}
 	}
 }
 
@@ -1459,7 +1506,7 @@ constellation_tracker_process_frame_fast(struct xrt_frame_sink *sink, struct xrt
 	os_mutex_unlock(&ct->tracked_device_lock);
 
 	/* Fast path: a trusted position prior (recent optical fix) bounds association, so the
-	 * stock low-count gates apply. Everything else goes to the long search. */
+	 * recovered low-count gates apply. Everything else goes to the long search. */
 	struct constellation_constrained_view views[CONSTELLATION_MAX_CAMERAS];
 	unsigned n_views = joint_views(ct, sample, views);
 	unsigned blob_count = sample_blob_count(sample);

tests/CMakeLists.txt

@@ -142,9 +142,20 @@ if(XRT_MODULE_CONSTELLATION_TRACKING)
 	target_include_directories(tests_constellation PRIVATE ../src/xrt/tracking/constellation)
 	target_link_libraries(tests_constellation PRIVATE xrt-external-catch2 constellation_tracker aux_math)
 	add_test(NAME tests_constellation COMMAND tests_constellation)
+
+	add_executable(tests_constellation_publication tests_constellation_publication.c)
+	target_include_directories(tests_constellation_publication PRIVATE ../src/xrt/tracking/constellation ${OpenCV_INCLUDE_DIRS})
+	target_link_libraries(tests_constellation_publication PRIVATE constellation_tracker aux_math aux_util_sink)
+	set_target_properties(tests_constellation_publication PROPERTIES LINKER_LANGUAGE CXX)
+	add_test(NAME tests_constellation_publication COMMAND tests_constellation_publication)
 endif()
 
 if(XRT_BUILD_DRIVER_RIFT_S)
+	add_executable(tests_rift_s_motion tests_rift_s_motion.cpp ../src/xrt/drivers/rift_s/rift_s_motion.cpp)
+	target_include_directories(tests_rift_s_motion PRIVATE ../src/xrt/drivers/rift_s)
+	target_link_libraries(tests_rift_s_motion PRIVATE xrt-external-catch2 aux_math aux_util)
+	add_test(NAME tests_rift_s_motion COMMAND tests_rift_s_motion)
+
 	add_executable(tests_rift_s_tracking tests_rift_s_tracking.cpp)
 	target_include_directories(tests_rift_s_tracking PRIVATE ../src/xrt/drivers/rift_s)
 	target_link_libraries(tests_rift_s_tracking PRIVATE xrt-external-catch2 drv_rift_s aux_math aux_tracking)

tests/tests_constellation_constrained.cpp

@@ -88,7 +88,7 @@ struct Scene
 			math_pose_invert(&v.P_world_cam, &v.P_cam_world);
 		constellation_constrained_default_config(&config);
 		config.time_budget_us = 2000000;
-		// These fixtures exercise the stock scorer minimum (inliers > sample size) with three
+		// These fixtures exercise the recovered scorer minimum (inliers > sample size) with three
 		// LEDs, so they relax Monado's stricter cold-acquisition and tracking policy.
 		config.minimum_cold_inliers = 3;
 		config.minimum_cold_distinct_leds = 0;
@@ -877,7 +877,7 @@ TEST_CASE("Cold low-count acquisition never accepts a wrong pose in randomized p
 	CHECK(arbitrated_same_tilt.wrong == 0);
 }
 
-TEST_CASE("Stock minimum count alone admits wrong identities that the cold policy rejects")
+TEST_CASE("Recovered minimum count alone admits wrong identities that the cold policy rejects")
 {
 	// Documents why the cold policy exists: inliers > sample size is necessary, not sufficient.
 	constellation_constrained_config minimum;

tests/tests_constellation_publication.c

@@ -0,0 +1,213 @@
+// Copyright 2026, lbgos
+// SPDX-License-Identifier: BSL-1.0
+#include <stdio.h>
+#include <string.h>
+
+// Include the production publication path to check ownership before and after fusion acceptance.
+#include "../src/xrt/tracking/constellation/t_constellation_tracking.c"
+
+#define CHECK(condition)                                                                                             \
+	do {                                                                                                               \
+		if (!(condition)) {                                                                                         \
+			fprintf(stderr, "CHECK failed at %s:%d: %s\n", __FILE__, __LINE__, #condition);                     \
+			return 1;                                                                                              \
+		}                                                                                                          \
+	} while (0)
+
+static bool accept_observation;
+static unsigned observation_calls;
+
+static xrt_result_t
+fake_get_tracked_pose(struct xrt_device *xdev,
+                      enum xrt_input_name name,
+                      int64_t at_timestamp_ns,
+                      struct xrt_space_relation *out_relation)
+{
+	(void)xdev;
+	(void)name;
+	(void)at_timestamp_ns;
+	*out_relation = (struct xrt_space_relation)XRT_SPACE_RELATION_ZERO;
+	out_relation->pose.orientation.w = 1.0f;
+	out_relation->relation_flags = XRT_SPACE_RELATION_ORIENTATION_VALID_BIT;
+	return XRT_SUCCESS;
+}
+
+static bool
+fake_push_pose_observation(struct xrt_device *xdev, const struct t_constellation_pose_observation *obs)
+{
+	(void)xdev;
+	if (obs->capture_ns != INT64_C(1100000000)) {
+		return false;
+	}
+	observation_calls++;
+	return accept_observation;
+}
+
+static int
+run_submit_acceptance_contract(enum constellation_track_state initial_state)
+{
+	struct t_constellation_tracker tracker = {0};
+	struct t_constellation_tracked_device_connection connection = {0};
+	struct t_constellation_tracked_device_callbacks callbacks = {0};
+	struct xrt_device xdev = {0};
+	struct constellation_tracking_sample sample = {0};
+	struct constellation_tracker_device *device = &tracker.devices[0];
+	struct tracking_sample_device_state *state = &sample.devices[0];
+
+	CHECK(os_mutex_init(&tracker.tracked_device_lock) == 0);
+	CHECK(os_mutex_init(&connection.lock) == 0);
+	tracker.log_level = U_LOGGING_ERROR;
+	connection.xdev = &xdev;
+	connection.cb = &callbacks;
+	connection.tracker = &tracker;
+	callbacks.push_pose_observation = fake_push_pose_observation;
+	xdev.get_tracked_pose = fake_get_tracked_pose;
+	device->connection = &connection;
+	device->alias = "fixture";
+	device->last_matched_cam = -1;
+	device->led_model.P_device_model.orientation.w = 1.0f;
+	device->led_model.P_model_device.orientation.w = 1.0f;
+
+	constellation_track_init(&device->track);
+	device->track.state = initial_state;
+	device->track.ref.P_world_model.orientation.w = 1.0f;
+	device->track.ref.imu_world.w = 1.0f;
+	device->track.ref.capture_ns = INT64_C(1000000000);
+	device->track.published = device->track.ref;
+	device->track.have_published = true;
+	device->track.ref_orientation_observed = false;
+	device->have_last_seen_pose = true;
+	device->last_seen_pose_ts = INT64_C(1000000000);
+	device->last_seen_pose.position.x = -7.0f;
+	device->last_matched_blobs = 17;
+	device->diagnostics.published = 23;
+
+	sample.timestamp = INT64_C(1100000000);
+	sample.n_devices = 1;
+	sample.n_views = 0;
+	sample.views[0].P_cam_world.orientation.w = 1.0f;
+	state->dev_index = 0;
+	state->joint_result.P_world_model.orientation.w = 1.0f;
+	state->joint_result.inliers = 1;
+	state->final_pose.position.x = -9.0f;
+	state->found_device_pose = false;
+
+	const struct constellation_track before_reject = device->track;
+	accept_observation = false;
+	observation_calls = 0;
+	CHECK(submit_joint_pose(&tracker, state, &sample, "fixture") == JOINT_POSE_FUSION_REJECTED);
+	CHECK(observation_calls == 1);
+	CHECK(device->track.state == before_reject.state);
+	CHECK(device->track.ref.capture_ns == before_reject.ref.capture_ns);
+	CHECK(device->track.ref.P_world_model.position.x == before_reject.ref.P_world_model.position.x);
+	CHECK(device->track.ref.imu_world.w == before_reject.ref.imu_world.w);
+	CHECK(device->track.published.capture_ns == before_reject.published.capture_ns);
+	CHECK(device->track.have_published == before_reject.have_published);
+	CHECK(device->track.ref_orientation_observed == before_reject.ref_orientation_observed);
+	CHECK(device->track.consecutive_jumps == before_reject.consecutive_jumps);
+	CHECK(device->have_last_seen_pose);
+	CHECK(device->last_seen_pose_ts == INT64_C(1000000000));
+	CHECK(device->last_seen_pose.position.x == -7.0f);
+	CHECK(device->last_matched_blobs == 17);
+	CHECK(device->diagnostics.published == 23);
+	CHECK(device->diagnostics.confirmed == 0);
+	CHECK(!state->found_device_pose);
+	CHECK(state->final_pose.position.x == -9.0f);
+
+	accept_observation = true;
+	CHECK(submit_joint_pose(&tracker, state, &sample, "fixture") == JOINT_POSE_PUBLISHED);
+	CHECK(observation_calls == 2);
+	CHECK(state->found_device_pose);
+	CHECK(device->last_seen_pose_ts == INT64_C(1100000000));
+	CHECK(device->track.ref.capture_ns == INT64_C(1100000000));
+	CHECK(device->diagnostics.published == 24);
+	CHECK(device->diagnostics.confirmed == (initial_state == CONSTELLATION_TRACK_CONFIRMING));
+
+	os_mutex_destroy(&connection.lock);
+	os_mutex_destroy(&tracker.tracked_device_lock);
+	return 0;
+}
+
+struct arbitration_device
+{
+	struct xrt_device base;
+	bool accept;
+	unsigned calls;
+};
+
+static bool
+arbitration_push_pose(struct xrt_device *xdev, const struct t_constellation_pose_observation *obs)
+{
+	struct arbitration_device *device = (struct arbitration_device *)xdev;
+	device->calls++;
+	return device->accept && obs->capture_ns == INT64_C(1100000000);
+}
+
+static int
+run_arbitration_contract(unsigned stronger_index, bool accept_stronger, bool hold_stronger)
+{
+	struct t_constellation_tracker tracker = {0};
+	struct constellation_tracking_sample sample = {0};
+	struct t_constellation_tracked_device_connection connections[2] = {0};
+	struct t_constellation_tracked_device_callbacks callbacks = {.push_pose_observation = arbitration_push_pose};
+	struct arbitration_device devices[2] = {0};
+	bool candidates[CONSTELLATION_MAX_DEVICES] = {true, true};
+	const unsigned weaker_index = 1 - stronger_index;
+	CHECK(os_mutex_init(&tracker.tracked_device_lock) == 0);
+	tracker.log_level = U_LOGGING_ERROR;
+	sample.n_devices = 2;
+	sample.timestamp = INT64_C(1100000000);
+	sample.views[0].P_cam_world.orientation.w = 1.0f;
+	for (unsigned d = 0; d < 2; d++) {
+		CHECK(os_mutex_init(&connections[d].lock) == 0);
+		connections[d].xdev = &devices[d].base;
+		connections[d].cb = &callbacks;
+		connections[d].tracker = &tracker;
+		devices[d].base.get_tracked_pose = fake_get_tracked_pose;
+		devices[d].accept = d == weaker_index || accept_stronger;
+		struct constellation_tracker_device *device = &tracker.devices[d];
+		device->connection = &connections[d];
+		device->alias = d == stronger_index ? "stronger" : "weaker";
+		device->led_model.P_device_model.orientation.w = 1.0f;
+		device->led_model.P_model_device.orientation.w = 1.0f;
+		constellation_track_init(&device->track);
+		device->track.state = d == stronger_index && hold_stronger ? CONSTELLATION_TRACK_LOST
+		                                                          : CONSTELLATION_TRACK_TRACKING;
+		device->track.ref.capture_ns = INT64_C(1000000000);
+		device->track.ref.P_world_model.orientation.w = 1.0f;
+		device->track.ref.imu_world.w = 1.0f;
+		struct tracking_sample_device_state *state = &sample.devices[d];
+		state->dev_index = d;
+		state->joint_result.P_world_model.orientation.w = 1.0f;
+		state->joint_result.inliers = d == stronger_index ? 8 : 4;
+		state->joint_result.distinct_leds = 4;
+		state->joint_result.num_assignments = 1;
+		state->joint_result.assignments[0].camera_index = 0;
+		state->joint_result.assignments[0].blob_index = 0;
+	}
+	arbitrate_and_submit(&tracker, &sample, candidates, "fixture");
+	CHECK(devices[stronger_index].calls == (hold_stronger ? 0 : 1));
+	CHECK(devices[weaker_index].calls == (!accept_stronger && !hold_stronger ? 1 : 0));
+	CHECK(sample.devices[stronger_index].found_device_pose == (accept_stronger && !hold_stronger));
+	CHECK(sample.devices[weaker_index].found_device_pose == (!accept_stronger && !hold_stronger));
+	CHECK(tracker.devices[stronger_index].diagnostics.published == (accept_stronger && !hold_stronger));
+	CHECK(tracker.devices[weaker_index].diagnostics.published == (!accept_stronger && !hold_stronger));
+	if (hold_stronger) {
+		CHECK(tracker.devices[stronger_index].track.state == CONSTELLATION_TRACK_CONFIRMING);
+	}
+	for (unsigned d = 0; d < 2; d++) {
+		os_mutex_destroy(&connections[d].lock);
+	}
+	os_mutex_destroy(&tracker.tracked_device_lock);
+	return 0;
+}
+
+int
+main(void)
+{
+	return run_submit_acceptance_contract(CONSTELLATION_TRACK_TRACKING) ||
+	       run_submit_acceptance_contract(CONSTELLATION_TRACK_CONFIRMING) ||
+	       run_arbitration_contract(0, false, false) || run_arbitration_contract(1, false, false) ||
+	       run_arbitration_contract(0, true, false) || run_arbitration_contract(1, true, false) ||
+	       run_arbitration_contract(0, false, true) || run_arbitration_contract(1, false, true);
+}

tests/tests_rift_s_controller_geometry.cpp

@@ -16,7 +16,7 @@ struct FactoryCamera
 	std::array<float, 2> tangential;
 	xrt_matrix_4x4 device_from_camera;
 };
-// Factory camera calibration read from a Rift S headset.
+// VM124 factory camera calibration recorded in the real r14 session.
 const std::array<FactoryCamera, 5> factory = {{
     {{190.499924f, 190.499924f, 312.359253f, 233.292801f},
      {0.274657339f, 0.072651282f, -0.284994245f, 0.224762872f, -0.0842394754f, 0.0130622024f},

tests/tests_rift_s_metadata.cpp

@@ -237,7 +237,7 @@ TEST_CASE("Rift S controller IR LED register matches the Windows radio requests"
 	REQUIRE(rift_s_encode_controller_irled_read(read.data()) == 4);
 	CHECK(read == std::array<uint8_t, 4>{0x28, 0x00, 0xe8, 0x03});
 
-	// The Windows runtime wrote 19/40000 to both controllers.
+	// The Windows session wrote 19/40000 to both controllers (OVRService input.cpp:2277).
 	rift_s_controller_irled_config cfg{40000, 19};
 	std::array<uint8_t, 12> write{};
 	REQUIRE(rift_s_encode_controller_irled_write(&cfg, write.data()) == 12);

tests/tests_rift_s_motion.cpp

@@ -0,0 +1,236 @@
+// Copyright 2026, lbgos
+// SPDX-License-Identifier: BSL-1.0
+#include "catch_amalgamated.hpp"
+#include "rift_s_motion.h"
+#include "math/m_api.h"
+#include "math/m_vec3.h"
+#include <memory>
+
+namespace {
+constexpr timepoint_ns MS = U_TIME_1MS_IN_NS;
+struct Motion
+{
+	rift_s_motion *ctx = rift_s_motion_create();
+	xrt_pose head = {XRT_QUAT_IDENTITY, {0, 1.6f, 0}};
+	rift_s_motion_sample sample = {};
+	Motion()
+	{
+		sample.pose.orientation = XRT_QUAT_IDENTITY;
+		sample.timestamp_ns = 1000 * MS;
+		for (double &v : sample.position_variance)
+			v = 1e-5;
+	}
+	~Motion()
+	{
+		rift_s_motion_destroy(&ctx);
+	}
+	void
+	step(unsigned hand, xrt_vec3 p, bool reliable, time_duration_ns dt = 2 * MS)
+	{
+		sample.timestamp_ns += dt;
+		sample.pose.position = p;
+		for (double &v : sample.position_variance)
+			v = reliable ? 1e-5 : -1;
+		rift_s_motion_update(ctx, hand, &sample, &head);
+	}
+	rift_s_motion_model
+	get(unsigned hand, xrt_space_relation &rel, time_duration_ns ahead = 0)
+	{
+		return rift_s_motion_get(ctx, hand, sample.timestamp_ns + ahead, &rel);
+	}
+};
+void
+check_vec(xrt_vec3 a, xrt_vec3 b, double margin = 1e-6)
+{
+	CHECK(a.x == Catch::Approx(b.x).margin(margin));
+	CHECK(a.y == Catch::Approx(b.y).margin(margin));
+	CHECK(a.z == Catch::Approx(b.z).margin(margin));
+}
+} // namespace
+
+TEST_CASE("Behind-back loss follows the saved head-relative position for either hand")
+{
+	for (unsigned hand : {0u, 1u}) {
+		Motion m;
+		xrt_space_relation rel{};
+		const xrt_vec3 hand_position = {hand ? 0.3f : -0.3f, 1.1f, 0.5f};
+		m.step(hand, hand_position, true);
+		CHECK(m.get(hand, rel) == RIFT_S_MOTION_NONE);
+		m.step(hand, hand_position, false);
+		REQUIRE(m.get(hand, rel) == RIFT_S_MOTION_BEHIND_BACK);
+		check_vec(rel.pose.position, hand_position);
+		const xrt_vec3 up = {0, 1, 0};
+		math_quat_from_angle_vector(1.57079632679f, &up, &m.head.orientation);
+		m.head.position = {1, 1.7f, 2};
+		// The unobserved filter position can be nonsense. It must not replace the reliable support.
+		m.step(hand, {100, -100, 100}, false, 800 * MS);
+		REQUIRE(m.get(hand, rel) == RIFT_S_MOTION_BEHIND_BACK);
+		const xrt_vec3 expected = {1.5f, 1.2f, 2 + (hand ? -0.3f : 0.3f)};
+		check_vec(rel.pose.position, expected);
+		CHECK_FALSE(rel.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT);
+		CHECK(rel.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT);
+		m.step(hand, expected, true);
+		CHECK(m.get(hand, rel) != RIFT_S_MOTION_BEHIND_BACK);
+	}
+}
+
+TEST_CASE("Out-of-view wrist rotation uses an arm pivot and returns through the correction-rate state")
+{
+	for (unsigned hand : {0u, 1u}) {
+		Motion m;
+		xrt_space_relation rel{};
+		const xrt_vec3 start = {hand ? 0.3f : -0.3f, 1.45f, -0.5f};
+		m.step(hand, start, true);
+		m.step(hand, start, true);
+		m.step(hand, start, false);
+		REQUIRE(m.get(hand, rel) == RIFT_S_MOTION_OUT_OF_FOV);
+		check_vec(rel.pose.position, start);
+		const xrt_vec3 up = {0, 1, 0};
+		math_quat_from_angle_vector(1.57079632679f, &up, &m.sample.pose.orientation);
+		m.step(hand, {5, -20, -8}, false);
+		REQUIRE(m.get(hand, rel) == RIFT_S_MOTION_OUT_OF_FOV);
+		// Rotation of a 20 cm forearm pivot changes the IMU position; it is not a frozen translation.
+		CHECK(m_vec3_len(m_vec3_sub(rel.pose.position, start)) > 0.25f);
+		CHECK(rel.pose.position.y > 1.3f);
+		CHECK_FALSE(rel.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT);
+		xrt_vec3 estimate = rel.pose.position;
+		xrt_vec3 target = m_vec3_add(estimate, {0.03f, 0, 0});
+		m.sample.linear_velocity = {0, 0, 0};
+		m.step(hand, target, true);
+		REQUIRE(m.get(hand, rel) == RIFT_S_MOTION_OUT_OF_FOV);
+		CHECK(rel.pose.position.x == Catch::Approx(estimate.x + 0.0005f).margin(2e-6));
+		CHECK(rel.linear_velocity.x == Catch::Approx(0.25f).margin(1e-5));
+		// The optical return drives the model with a bounded correction rate, with no fixed blend timer.
+		for (unsigned i = 0; i < 100 && m.get(hand, rel) == RIFT_S_MOTION_OUT_OF_FOV; ++i)
+			m.step(hand, target, true);
+		CHECK(m.get(hand, rel) == RIFT_S_MOTION_NONE);
+	}
+}
+
+TEST_CASE("Motion freshness is a covariance-qualified filter sample, independent of vision cadence")
+{
+	Motion m;
+	xrt_space_relation rel{};
+	const xrt_vec3 start = {0.3f, 1.45f, -0.5f};
+	for (unsigned i = 0; i < 100; ++i) {
+		m.step(1, start, true);
+		CHECK(m.get(1, rel) == RIFT_S_MOTION_NONE);
+	}
+	// The gate is strict; equality with the reliability limit cannot refresh the support.
+	for (double &v : m.sample.position_variance)
+		v = 0.001;
+	m.sample.timestamp_ns += 2 * MS;
+	rift_s_motion_update(m.ctx, 1, &m.sample, &m.head);
+	CHECK(m.get(1, rel) == RIFT_S_MOTION_OUT_OF_FOV);
+	const auto old = rel;
+	// Queries do not change the cache or another hand's model.
+	for (int i = 0; i < 10; ++i) {
+		CHECK(m.get(1, rel, 10 * MS) == RIFT_S_MOTION_OUT_OF_FOV);
+		CHECK(m.get(0, rel) == RIFT_S_MOTION_NONE);
+	}
+	REQUIRE(m.get(1, rel) == RIFT_S_MOTION_OUT_OF_FOV);
+	check_vec(rel.pose.position, old.pose.position);
+}
+
+TEST_CASE("Near-head loss follows wrist rotation and expires only after spatial convergence")
+{
+	Motion m;
+	xrt_space_relation rel{};
+	const xrt_vec3 start = {0.1f, 1.6f, -0.05f};
+	m.step(0, start, true);
+	m.step(0, start, false);
+	REQUIRE(m.get(0, rel) == RIFT_S_MOTION_NEAR_HEAD);
+	check_vec(rel.pose.position, start);
+	const xrt_vec3 up = {0, 1, 0};
+	math_quat_from_angle_vector(0.5f, &up, &m.sample.pose.orientation);
+	m.step(0, {10, -30, 4}, false, 600 * MS);
+	REQUIRE(m.get(0, rel) == RIFT_S_MOTION_NEAR_HEAD);
+	CHECK(m_vec3_len(m_vec3_sub(rel.pose.position, start)) > 0.05f);
+	CHECK(rel.pose.position.y > 1.5f);
+}
+
+TEST_CASE("At-rest wins priority until measured motion releases its latch")
+{
+	Motion m;
+	xrt_space_relation rel{};
+	const xrt_vec3 start = {0.9f, 1.6f, -0.1f};
+	for (int i = 0; i < 60; ++i)
+		m.step(0, start, true);
+	REQUIRE(m.get(0, rel) == RIFT_S_MOTION_AT_REST);
+	m.step(0, {40, -20, 30}, false, 700 * MS);
+	REQUIRE(m.get(0, rel) == RIFT_S_MOTION_AT_REST);
+	check_vec(rel.pose.position, start);
+	check_vec(rel.linear_velocity, {0, 0, 0});
+	// Lower models were reset by the latch and cannot publish an old estimate on release.
+	m.sample.angular_velocity = {1, 0, 0};
+	m.step(0, start, false);
+	CHECK(m.get(0, rel) == RIFT_S_MOTION_NONE);
+	m.step(0, start, true);
+	m.step(0, start, false);
+	CHECK(m.get(0, rel) == RIFT_S_MOTION_OUT_OF_FOV);
+}
+
+TEST_CASE("Correlated close hands attach to the observed hand and preserve the attachment on handover")
+{
+	for (unsigned hand : {0u, 1u}) {
+		Motion m;
+		xrt_space_relation rel{};
+		const xrt_vec3 p = {0.2f, 1.4f, -0.5f}, q = {0.3f, 1.4f, -0.5f};
+		for (int i = 0; i < 60; ++i) {
+			const float a = i % 2 ? 0.3f : -0.3f;
+			m.sample.linear_acceleration = {a, 2 * a, 3 * a};
+			m.step(1 - hand, q, true);
+			m.step(hand, p, true);
+		}
+		for (int i = 0; i < 16; ++i) {
+			const float a = i % 2 ? 0.3f : -0.3f;
+			m.sample.linear_acceleration = {a, 2 * a, 3 * a};
+			m.step(1 - hand, q, true);
+			m.step(hand, p, false);
+		}
+		REQUIRE(m.get(hand, rel) == RIFT_S_MOTION_HAND_OVER_HAND);
+		check_vec(rel.pose.position, p);
+		m.step(1 - hand, {q.x + 0.1f, q.y + 0.05f, q.z}, true);
+		m.step(hand, p, false);
+		REQUIRE(m.get(hand, rel) == RIFT_S_MOTION_HAND_OVER_HAND);
+		check_vec(rel.pose.position, {p.x + 0.1f, p.y + 0.05f, p.z});
+		const auto before = rel.pose.position;
+		// Opposite acceleration correlations switch the support, without resetting position.
+		for (int i = 0; i < 60; ++i) {
+			float a = i % 2 ? 0.3f : -0.3f;
+			m.sample.linear_acceleration = {-a, -2 * a, -3 * a};
+			m.step(1 - hand, {q.x + 0.1f, q.y + 0.05f, q.z}, true);
+			m.sample.linear_acceleration = {a, 2 * a, 3 * a};
+			m.step(hand, p, false);
+		}
+		REQUIRE(m.get(hand, rel) == RIFT_S_MOTION_HAND_OVER_HAND);
+		check_vec(rel.pose.position, before);
+		rift_s_motion_reset_hand(m.ctx, hand);
+		CHECK(m.get(hand, rel) == RIFT_S_MOTION_NONE);
+	}
+}
+
+TEST_CASE("Model prediction uses body angular rate and queries cannot seed an unobserved hand")
+{
+	Motion m;
+	xrt_space_relation rel{};
+	m.sample.pose.orientation = {0, 0, 0, 1};
+	m.step(0, {0.1f, 1.6f, -0.1f}, false);
+	CHECK(m.get(0, rel) == RIFT_S_MOTION_NONE);
+	const xrt_vec3 axis = {1, 0, 0};
+	math_quat_from_angle_vector(0.7f, &axis, &m.sample.pose.orientation);
+	m.sample.angular_velocity = {0, 1, 0};
+	m.step(0, {0.1f, 1.6f, -0.1f}, true);
+	m.step(0, {0.1f, 1.6f, -0.1f}, false);
+	REQUIRE(m.get(0, rel, 10 * MS) == RIFT_S_MOTION_NEAR_HEAD);
+	xrt_quat expected;
+	math_quat_integrate_velocity(&m.sample.pose.orientation, &m.sample.angular_velocity, 0.01f, &expected);
+	CHECK(rel.pose.orientation.x == Catch::Approx(expected.x).margin(1e-6));
+	CHECK(rel.pose.orientation.y == Catch::Approx(expected.y).margin(1e-6));
+	CHECK(rel.pose.orientation.z == Catch::Approx(expected.z).margin(1e-6));
+	CHECK(rel.pose.orientation.w == Catch::Approx(expected.w).margin(1e-6));
+	xrt_vec3 world_rate;
+	math_quat_rotate_vec3(&m.sample.pose.orientation, &m.sample.angular_velocity, &world_rate);
+	check_vec(rel.angular_velocity, world_rate);
+	CHECK(m.get(1, rel) == RIFT_S_MOTION_NONE);
+}

tests/tests_rift_s_position_filter.cpp

@@ -29,7 +29,7 @@ position_at(const rift_s_position_filter &f, timepoint_ns t)
 	return p;
 }
 
-TEST_CASE("Rift S loss ladder matches the Windows defaults")
+TEST_CASE("Rift S configured loss durations retain independent position and velocity gates")
 {
 	time_duration_ns p, v;
 	rift_s_tracking_loss_durations(0, 0, &p, &v);
@@ -114,50 +114,27 @@ TEST_CASE("Rift S position filter follows motion between delayed optical fixes")
 	CHECK(f->gated == 0);
 }
 
-TEST_CASE("Rift S position filter coasts with bounded drift, holds when frozen and resets on reacquire")
+TEST_CASE("Rift S position propagation preserves measured motion outside the camera view")
 {
 	auto f = make_filter();
-	const xrt_vec3 zero = {0, 0, 0};
 	const xrt_vec3 here = {0.3f, 1.0f, -0.4f};
-	timepoint_ns t = MS;
-	for (; t < 1000 * MS; t += IMU_STEP) {
-		rift_s_position_filter_imu(f.get(), t, &zero);
-		if ((t / IMU_STEP) % 16 == 0) {
-			rift_s_position_filter_fix(f.get(), t, &here, &OPTICAL_VARIANCE, 0.15f);
-		}
-	}
-	// Optical loss with a 0.5 m/s^2 uncorrected acceleration error: coast for the 0.5 s ladder step.
-	f->coasting = true;
-	const xrt_vec3 error = {0.5f, 0, 0};
-	for (timepoint_ns end = t + 500 * MS; t < end; t += IMU_STEP) {
-		rift_s_position_filter_imu(f.get(), t, &error);
-	}
-	xrt_vec3 coasted = {(float)f->state.x[0][0], (float)f->state.x[1][0], (float)f->state.x[2][0]};
-	// The 500 ms bridge with decaying velocity: a(tau t - tau^2 (1 - e^-t/tau)) = 4.6 cm.
-	CHECK(fabsf(coasted.x - here.x) < 0.05f);
-	CHECK(fabsf(coasted.x - here.x) > 0.0f);
-
-	rift_s_position_filter_freeze(f.get());
-	for (timepoint_ns end = t + 2000 * MS; t < end; t += IMU_STEP) {
-		rift_s_position_filter_imu(f.get(), t, &error);
-	}
-	xrt_vec3 held = position_at(*f, t + 50 * MS);
-	CHECK(held.x == Catch::Approx(coasted.x).margin(1e-6));
+	REQUIRE(rift_s_position_filter_fix(f.get(), MS, &here, &OPTICAL_VARIANCE, 0.15f) == RIFT_S_POSITION_FIX_RESET);
+	// A hand accelerated at 1 m/s^2 for a second travels 0.5 m. Optical loss does not
+	// introduce an arbitrary displacement clamp, velocity damping, or reset maturity.
+	const xrt_vec3 accel = {1, 0, 0};
+	for (timepoint_ns t = MS + IMU_STEP; t <= 1001 * MS; t += IMU_STEP)
+		rift_s_position_filter_imu(f.get(), t, &accel);
 	xrt_vec3 p, v;
-	rift_s_position_filter_predict(f.get(), t, 100 * MS, &p, &v);
-	CHECK(v.x == 0.0f);
-
-	// Reacquire somewhere else: three consistent fixes re-initialize.
-	const xrt_vec3 there = {0.6f, 1.1f, -0.2f};
-	CHECK(rift_s_position_filter_fix(f.get(), t - 96 * MS, &there, &OPTICAL_VARIANCE, 0.15f) ==
-	      RIFT_S_POSITION_FIX_PENDING);
-	CHECK(rift_s_position_filter_fix(f.get(), t - 63 * MS, &there, &OPTICAL_VARIANCE, 0.15f) ==
-	      RIFT_S_POSITION_FIX_PENDING);
-	CHECK(f->frozen);
-	CHECK(rift_s_position_filter_fix(f.get(), t - 30 * MS, &there, &OPTICAL_VARIANCE, 0.15f) ==
-	      RIFT_S_POSITION_FIX_RESET);
-	CHECK_FALSE(f->frozen);
-	CHECK(position_at(*f, t).x == Catch::Approx(there.x).margin(0.01));
+	rift_s_position_filter_predict(f.get(), 1001 * MS, 100 * MS, &p, &v);
+	CHECK(p.x == Catch::Approx(here.x + 0.5).margin(1e-6));
+	CHECK(v.x == Catch::Approx(1).margin(1e-6));
+	CHECK(f->track_updates == 1);
+	CHECK_FALSE(f->suspended);
+	// A clock discontinuity is separate: suspend propagation until a new accepted track.
+	rift_s_position_filter_suspend(f.get());
+	rift_s_position_filter_imu(f.get(), 1003 * MS, &accel);
+	CHECK(position_at(*f, 1003 * MS).x == Catch::Approx(p.x).margin(1e-6));
+	CHECK(f->state.x[0][1] == 0);
 }
 
 TEST_CASE("Rift S position filter resets on a confirmed large innovation and rejects fixes older than its history")
@@ -227,7 +204,7 @@ TEST_CASE("Rift S position filter stays close to optical under a time-varying ac
 
 // r13 worn log, left hand behind the back: after a long track was held, single wrong fits ~1 m
 // away re-initialized the filter, and coasting on their velocity drifted it further.
-TEST_CASE("Rift S held position ignores isolated wrong fits and needs a consistent reacquire")
+TEST_CASE("Rift S suspended filter ignores isolated wrong fits and needs a consistent reacquire")
 {
 	auto f = make_filter();
 	const xrt_vec3 zero = {0, 0, 0};
@@ -239,7 +216,7 @@ TEST_CASE("Rift S held position ignores isolated wrong fits and needs a consiste
 			rift_s_position_filter_fix(f.get(), t - 20 * MS, &here, &OPTICAL_VARIANCE, 0.15f);
 		}
 	}
-	rift_s_position_filter_freeze(f.get());
+	rift_s_position_filter_suspend(f.get());
 	// Scattered single fits, each one frame apart, never two in agreement.
 	const xrt_vec3 wrong[] = {{-0.99f, 0.29f, 0.40f},
 	                          {-1.02f, -0.18f, 0.71f},
@@ -255,7 +232,7 @@ TEST_CASE("Rift S held position ignores isolated wrong fits and needs a consiste
 		CHECK(p.x == Catch::Approx(here.x).margin(1e-3));
 		CHECK(p.z == Catch::Approx(here.z).margin(1e-3));
 	}
-	CHECK(f->frozen);
+	CHECK(f->suspended);
 	// The hand comes back: three consistent fixes take over.
 	const xrt_vec3 back = {0.25f, 0.05f, 0.1f};
 	enum rift_s_position_fix_result r = RIFT_S_POSITION_FIX_PENDING;
@@ -269,60 +246,22 @@ TEST_CASE("Rift S held position ignores isolated wrong fits and needs a consiste
 	CHECK(position_at(*f, t).x == Catch::Approx(back.x + 0.02f).margin(0.01));
 }
 
-TEST_CASE("Rift S young track holds instead of coasting on velocity from a few fixes")
+TEST_CASE("Replacement fixes confirm throughout the Constellation capture cadence")
 {
-	auto f = make_filter();
-	const xrt_vec3 zero = {0, 0, 0};
-	timepoint_ns t = MS;
-	// Two fixes 33 ms apart and 3 cm apart (a wrong pair implies ~1 m/s), then nothing.
-	const xrt_vec3 a = {0.07f, 0.17f, 0.59f}, b = {0.10f, 0.17f, 0.59f};
-	rift_s_position_filter_fix(f.get(), t, &a, &OPTICAL_VARIANCE, 0.15f);
-	for (timepoint_ns end = t + 33 * MS; t < end; t += IMU_STEP)
-		rift_s_position_filter_imu(f.get(), t, &zero);
-	rift_s_position_filter_fix(f.get(), t, &b, &OPTICAL_VARIANCE, 0.15f);
-	CHECK(f->track_updates == 2);
-	const double fused = f->state.x[0][0];
-	f->coasting = true;
-	for (timepoint_ns end = t + 250 * MS; t < end; t += IMU_STEP)
-		rift_s_position_filter_imu(f.get(), t, &zero);
-	CHECK(f->state.x[0][0] == Catch::Approx(fused).margin(1e-6));
-	CHECK(f->state.x[0][1] == 0.0);
-}
-
-TEST_CASE("Optical loss bounds IMU displacement and freezes after the bridge")
-{
-	auto f = make_filter();
-	xrt_vec3 here = {0.5f, 1.2f, -0.9f}, error = {-20, 0, 0};
-	REQUIRE(rift_s_position_filter_fix(f.get(), MS, &here, &OPTICAL_VARIANCE, 0.15f) == RIFT_S_POSITION_FIX_RESET);
-	f->track_updates = 30;
-	for (timepoint_ns t = 3 * MS; t <= 1000 * MS; t += IMU_STEP) {
-		f->coasting = t > 121 * MS;
-		rift_s_position_filter_imu(f.get(), t, &error);
-		if (t > 121 * MS)
-			CHECK(fabs(position_at(*f, t).x - here.x) <= 0.101);
+	for (const time_duration_ns gap : {240 * MS, 280 * MS, 300 * MS, 301 * MS}) {
+		CAPTURE(gap);
+		auto f = make_filter();
+		const xrt_vec3 here = {0, 1, -0.5f}, replacement = {0.5f, 1, -0.5f}, zero = {};
+		timepoint_ns t = 1000 * MS;
+		REQUIRE(rift_s_position_filter_fix(f.get(), t, &here, &OPTICAL_VARIANCE, 0.15f) ==
+		        RIFT_S_POSITION_FIX_RESET);
+		for (unsigned i = 0; i < RIFT_S_POSITION_FILTER_CONFIRM_FIXES; ++i) {
+			t += gap;
+			rift_s_position_filter_imu(f.get(), t, &zero);
+			const auto result = rift_s_position_filter_fix(f.get(), t, &replacement, &OPTICAL_VARIANCE, 0.15f);
+			CHECK(result == (i + 1 == RIFT_S_POSITION_FILTER_CONFIRM_FIXES &&
+			                 gap <= T_CONSTELLATION_TRACK_MAX_GAP_NS
+			                     ? RIFT_S_POSITION_FIX_RESET : RIFT_S_POSITION_FIX_PENDING));
+		}
 	}
-	CHECK(f->frozen);
-	xrt_vec3 held = position_at(*f, 1000 * MS);
-	for (timepoint_ns t = 1001 * MS; t < 2000 * MS; t += IMU_STEP)
-		rift_s_position_filter_imu(f.get(), t, &error);
-	CHECK(position_at(*f, 2000 * MS).x == held.x);
-}
-
-TEST_CASE("Optical-loss queries bound prediction during a radio gap")
-{
-	auto f = make_filter();
-	xrt_vec3 here = {0.5f, 1.2f, -0.9f}, zero = {0, 0, 0};
-	REQUIRE(rift_s_position_filter_fix(f.get(), MS, &here, &OPTICAL_VARIANCE, 0.15f) == RIFT_S_POSITION_FIX_RESET);
-	f->track_updates = 30;
-	rift_s_position_filter_imu(f.get(), 100 * MS, &zero);
-	f->state.x[0][0] = here.x - 0.09;
-	f->state.x[0][1] = -4;
-	xrt_vec3 p, v;
-	rift_s_position_filter_predict(f.get(), 150 * MS, 50 * MS, &p, &v);
-	CHECK(p.x == Catch::Approx(here.x - 0.1).margin(1e-5));
-	CHECK(v.x == 0);
-	// Past the bridge the held estimate is not extrapolated.
-	rift_s_position_filter_predict(f.get(), RIFT_S_POSITION_FILTER_BRIDGE_NS + 100 * MS, 50 * MS, &p, &v);
-	CHECK(p.x == Catch::Approx(here.x - 0.09).margin(1e-5));
-	CHECK(v.x == 0);
 }

tests/tests_rift_s_slam_guard.cpp

@@ -1,6 +1,7 @@
 // Copyright 2026, lbgos
 // SPDX-License-Identifier: BSL-1.0
 #include "catch_amalgamated.hpp"
+#include "../src/xrt/state_trackers/steamvr_drv/ovrd_world_universe.hpp"
 #include "rift_s/rift_s_slam_guard.h"
 #include "math/m_api.h"
 #include "math/m_vec3.h"
@@ -364,20 +365,27 @@ TEST_CASE("SLAM cannot publish invalid quaternions or a gravity flip")
 	CHECK(rift_s_slam_guard_pose_usable(&inverted, &inverted.orientation));
 }
 
-TEST_CASE("Saved boundary publishes only its recovered world frame after restart")
+TEST_CASE("Failed room localization keeps HMD tracking in an uncalibrated universe")
 {
 	Sim s;
 	s.no_pose = {0, 1.6f, 0};
 	s.g.require_relocalization = true;
 	xrt_pose raw = {yaw_quat(-0.8f), {0.4f, 0.2f, -0.3f}}, out;
-	rift_s_slam_guard_reason reason;
-	for (unsigned i = 0; i < 160; ++i)
-		CHECK_FALSE(s.step(true, raw, &out, &reason));
-	CHECK(reason == RIFT_S_SLAM_GUARD_RELOCALIZING);
-	CHECK_FALSE(s.g.world_valid);
+	REQUIRE(s.settle(raw) > 0);
+	REQUIRE(s.g.world_valid);
 	CHECK_FALSE(s.g.boundary_confirmed);
+	CHECK(rift_s_world_universe("relocalizing", s.ts, s.ts) == 3);
 	CHECK(s.g.resets == 0);
+	xrt_vec3 first = s.g.good.position;
+	for (unsigned i = 0; i < 200; ++i) {
+		raw.position.x += 0.002f;
+		REQUIRE(s.step(true, raw, &out));
+	}
+	CHECK(dist(first, out.position) == Catch::Approx(0.4f).margin(1e-5));
+	CHECK_FALSE(s.g.boundary_confirmed);
+	CHECK(rift_s_slam_guard_query(&s.g, &raw, s.ts, &s.fusion, &s.no_pose, &out));
 
+	// A later room match switches directly to the saved coordinates before enabling bounds.
 	xrt_pose world = {yaw_quat(0.6f), {-0.7f, 1.3f, 0.3f}};
 	rift_s_slam_guard_set_world(&s.g, &world);
 	REQUIRE(s.step(true, raw, &out));
@@ -387,42 +395,48 @@ TEST_CASE("Saved boundary publishes only its recovered world frame after restart
 	CHECK(yaw_of(out.orientation) == Catch::Approx(-0.2f).margin(1e-6));
 	CHECK_FALSE(s.g.world_transition);
 	CHECK(s.g.divergences == 0);
+	CHECK(rift_s_world_universe("localized", s.ts, s.ts) == 2);
 
-	// The replacement VIO gauge must be matched again, while the saved standing origin stays put.
+	// A backend reset loses the room binding, but cannot strand a stable replacement VIO.
 	s.g.reset_pending = true;
 	REQUIRE(rift_s_slam_guard_take_reset(&s.g, s.ts));
 	rift_s_slam_guard_suspend(&s.g, &s.fusion);
 	raw = {yaw_quat(1.2f), {-0.3f, 0.1f, 0.2f}};
-	for (unsigned i = 0; i < 160; ++i)
-		CHECK_FALSE(s.step(true, raw, &out, &reason));
-	CHECK(reason == RIFT_S_SLAM_GUARD_RELOCALIZING);
-	CHECK_FALSE(s.g.world_valid);
+	REQUIRE(s.settle(raw) > 0);
 	CHECK_FALSE(s.g.boundary_confirmed);
+	CHECK(rift_s_world_universe("relocalizing", s.ts, s.ts) == 3);
 	xrt_pose inverse, recovered;
 	math_pose_invert(&raw, &inverse);
 	math_pose_transform(&expected, &inverse, &recovered);
 	rift_s_slam_guard_set_world(&s.g, &recovered);
 	REQUIRE(s.step(true, raw, &out));
 	CHECK(dist(out.position, expected.position) < 1e-6);
-	CHECK(yaw_of(out.orientation) == Catch::Approx(yaw_of(expected.orientation)).margin(1e-6));
 	CHECK_FALSE(s.g.world_transition);
 }
 
-TEST_CASE("Restoring saved bounds revokes a room setup's unconfirmed frame")
+TEST_CASE("Room calibration is enabled only by a fresh confirmed room or explicit setup")
 {
+	const int64_t now = 10000000000LL;
+	CHECK(rift_s_world_universe("setup", now, now) == 2);
+	CHECK(rift_s_world_universe("localized", now, now) == 2);
+	for (auto status : {"", "unanchored", "relocalizing", "save_failed"})
+		CHECK(rift_s_world_universe(status, now, now) == 3);
+	CHECK(rift_s_world_universe("localized", 0, now) == 3);
+	CHECK(rift_s_world_universe("localized", now + 1, now) == 3);
+	CHECK(rift_s_world_universe("localized", now - 2000000001LL, now) == 3);
+	CHECK(rift_s_world_universe("localized", now - 1, now, now) == 3);
+	CHECK(rift_s_world_universe("localized", now, now + 1, now) == 2);
+
 	Sim s;
 	s.no_pose = {0, 1.6f, 0};
 	xrt_pose raw = XRT_POSE_IDENTITY, out;
 	REQUIRE(s.settle(raw) > 0);
-	REQUIRE(s.g.world_valid);
 	s.g.require_relocalization = true;
-	CHECK_FALSE(rift_s_slam_guard_query(&s.g, &raw, s.ts, &s.fusion, &s.no_pose, &out));
-	CHECK_FALSE(s.step(true, raw, &out));
-	CHECK_FALSE(s.g.world_valid);
-	CHECK_FALSE(s.g.world_transition);
+	CHECK(rift_s_slam_guard_query(&s.g, &raw, s.ts, &s.fusion, &s.no_pose, &out));
+	REQUIRE(s.step(true, raw, &out));
+	CHECK_FALSE(s.g.boundary_confirmed);
 	xrt_pose saved{yaw_quat(0.4f), {-0.2f, 1.6f, 0.3f}};
 	rift_s_slam_guard_set_world(&s.g, &saved);
-	REQUIRE(s.settle(raw) > 0);
 	REQUIRE(s.step(true, raw, &out));
 	CHECK(dist(out.position, saved.position) < 1e-6);
 	CHECK(yaw_of(out.orientation) == Catch::Approx(0.4f).margin(1e-6));

tests/tests_rift_s_tracking.cpp

@@ -16,6 +16,47 @@
 #include <cstring>
 #include <limits>
 #include <random>
+#include <atomic>
+
+TEST_CASE("Rift S tracker stop joins the world consumer and drains borrowed camera frames")
+{
+	rift_s_tracker tracker{};
+	os_mutex_init(&tracker.mutex);
+	os_mutex_init(&tracker.slam_mutex);
+	os_thread_helper_init(&tracker.world_thread);
+	tracker.world_thread_initialized = true;
+	tracker.ready_for_data = true;
+	struct Worker {
+		rift_s_tracker *tracker;
+		std::atomic<bool> exited{false};
+	} worker{&tracker};
+	auto consume = [](void *ptr) -> void * {
+		auto &worker = *static_cast<Worker *>(ptr);
+		auto &thread = worker.tracker->world_thread;
+		os_thread_helper_lock(&thread);
+		while (os_thread_helper_is_running_locked(&thread))
+			os_thread_helper_wait_locked(&thread);
+		os_thread_helper_unlock(&thread);
+		worker.exited = true;
+		return nullptr;
+	};
+	REQUIRE(os_thread_helper_start(&tracker.world_thread, consume, &worker) == 0);
+	xrt_frame frame{};
+	frame.reference.count = 1;
+	frame.destroy = [](xrt_frame *) { FAIL("The camera owner must still own its buffer"); };
+	xrt_frame_reference(&tracker.world_frames[0], &frame);
+	REQUIRE(frame.reference.count == 2);
+	rift_s_tracker_stop(&tracker);
+	CHECK(worker.exited);
+	CHECK_FALSE(tracker.ready_for_data);
+	CHECK(tracker.world_frames[0] == nullptr);
+	CHECK(frame.reference.count == 1);
+	// Teardown calls stop again after frame nodes have been destroyed.
+	rift_s_tracker_stop(&tracker);
+	os_thread_helper_destroy(&tracker.world_thread);
+	os_mutex_destroy(&tracker.slam_mutex);
+	os_mutex_destroy(&tracker.mutex);
+}
 
 TEST_CASE("Rift S optical yaw takes ten percent of the shortest world yaw")
 {
@@ -77,7 +118,7 @@ get_test_head_pose(xrt_device *, xrt_input_name, int64_t, xrt_space_relation *ou
 	return XRT_SUCCESS;
 }
 
-TEST_CASE("Rift S controller coordinates hold after loss and stay invalid before acquisition")
+TEST_CASE("Rift S controller estimated coordinates survive loss of position confidence")
 {
 	rift_s_hmd hmd = {};
 	hmd.base.get_tracked_pose = get_test_head_pose;
@@ -114,8 +155,8 @@ TEST_CASE("Rift S controller coordinates hold after loss and stay invalid before
 				CHECK((rel.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0);
 				CHECK((rel.relation_flags & XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT) != 0);
 				CHECK(((rel.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0) ==
-				      (optical_ts != 0));
-				// Optical confidence expires, but the held position remains usable.
+				      (optical_ts == 1999000000));
+				// Expired confidence does not discard the numeric estimate.
 				const bool tracked = optical_ts == 1999000000;
 				const bool held = optical_ts == 1000000000;
 				CHECK(((rel.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT) != 0) == tracked);
@@ -125,12 +166,12 @@ TEST_CASE("Rift S controller coordinates hold after loss and stay invalid before
 				vr::DriverPose_t pose = {};
 				pose.poseIsValid = true;
 				pose.result = vr::TrackingResult_Running_OK;
-				apply_pose(&rel, &pose, XRT_SPACE_RELATION_POSITION_VALID_BIT, true);
-				CHECK(pose.poseIsValid == (tracked || held));
-				CHECK(pose.result == (tracked || held ? vr::TrackingResult_Running_OK
+				apply_rift_s_touch_pose(&rel, &pose);
+				CHECK(pose.poseIsValid);
+				CHECK(pose.result == (tracked ? vr::TrackingResult_Running_OK
 				                                      : vr::TrackingResult_Running_OutOfRange));
-				CHECK(pose.vecPosition[0] == Catch::Approx(tracked || held ? rel.pose.position.x : 0));
-				CHECK(pose.vecPosition[2] == Catch::Approx(tracked || held ? rel.pose.position.z : 0));
+				CHECK(pose.vecPosition[0] == Catch::Approx(rel.pose.position.x));
+				CHECK(pose.vecPosition[2] == Catch::Approx(rel.pose.position.z));
 				CHECK(pose.qRotation.w == Catch::Approx(1));
 				// HMD call sites retain their original tracked-position requirement.
 				vr::DriverPose_t head_pose = {};
@@ -176,7 +217,8 @@ TEST_CASE("Rift S mature track bridges a short optical gap and clears optical co
 	};
 	CHECK(valid_after(30, 400 * U_TIME_1MS_IN_NS));
 	CHECK(valid_after(30, 490 * U_TIME_1MS_IN_NS));
-	CHECK_FALSE(valid_after(30, 510 * U_TIME_1MS_IN_NS));
+	CHECK(valid_after(30, 510 * U_TIME_1MS_IN_NS));
+	CHECK_FALSE(valid_after(30, 3001LL * U_TIME_1MS_IN_NS));
 	CHECK_FALSE(valid_after(1, 300 * U_TIME_1MS_IN_NS));
 	os_mutex_destroy(&ctrl.mutex);
 	os_mutex_destroy(&sys.dev_mutex);
@@ -458,7 +500,7 @@ TEST_CASE("Rift S IMU clock re-anchors after a stale first timestamp instead of
 	CHECK(ctrl.attitude_history_count == 0);
 	CHECK(ctrl.clock_stable_samples == 0);
 	CHECK_FALSE(ctrl.optical_heading_valid);
-	CHECK(ctrl.position_filter.frozen);
+	CHECK(ctrl.position_filter.suspended);
 	// After the resync, ordinary late resends are still rejected without another resync.
 	imu_report(base_us + 499 * 2000u, arrival + 1000000);
 	CHECK(ctrl.diagnostics.clock_resyncs == 1);
@@ -1166,95 +1208,6 @@ check_vec(xrt_vec3 actual, xrt_vec3 expected)
 }
 } // namespace
 
-TEST_CASE("Rift S loss hold and reacquire keep the published hand valid and continuous")
-{
-	const timepoint_ns ms = U_TIME_1MS_IN_NS;
-	const xrt_vec3 here = {0.3f, 1.4f, -0.4f}, error = {4, -6, 2};
-	for (bool right : {false, true}) {
-		PublishedPoseController fixture(right);
-		auto &ctrl = fixture.ctrl;
-		const timepoint_ns start = os_monotonic_get_ns() - 2000 * ms;
-		fixture.measured(XRT_QUAT_IDENTITY, here, start);
-		ctrl.track_vision_updates = ctrl.track_reliable_updates = ctrl.position_filter.track_updates = 30;
-		// Optical loss while the IMU keeps rotating the wrist. Its acceleration error must
-		// never move the IMU origin farther than the existing 10 cm coast bound.
-		for (timepoint_ns t = start + 2 * ms; t <= start + 2000 * ms; t += 2 * ms) {
-			const xrt_vec3 up = {0, 1, 0};
-			math_quat_from_angle_vector(float(t - start) / U_TIME_1S_IN_NS, &up, &ctrl.pose.orientation);
-			ctrl.fusion.rot = ctrl.pose.orientation;
-			ctrl.last_imu_capture_ns = t;
-			REQUIRE(rift_s_controller_record_attitude(&ctrl, t, t));
-			ctrl.position_filter.coasting = t - start > 120 * ms;
-			rift_s_position_filter_imu(&ctrl.position_filter, t, &error);
-			for (auto input : {XRT_INPUT_TOUCH_AIM_POSE, XRT_INPUT_TOUCH_GRIP_POSE}) {
-				auto rel = fixture.query(input, t);
-				REQUIRE((rel.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0);
-				if (t - start > 500 * ms) {
-					CHECK_FALSE((rel.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT) !=
-					            0);
-					CHECK_FALSE(
-					    (rel.relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) != 0);
-				}
-				const auto &offset =
-				    input == XRT_INPUT_TOUCH_AIM_POSE ? ctrl.P_device_aim : ctrl.P_device_grip;
-				xrt_pose imu_device_offset, expected;
-				math_pose_transform(&ctrl.P_imu_device, &offset, &imu_device_offset);
-				const xrt_pose anchor = {ctrl.pose.orientation, here};
-				math_pose_transform(&anchor, &imu_device_offset, &expected);
-				CHECK(m_vec3_len(m_vec3_sub(rel.pose.position, expected.position)) <= 0.10001f);
-				CHECK(rel.pose.orientation.y == Catch::Approx(expected.orientation.y).margin(2e-6));
-#ifdef TEST_RIFT_S_STEAMVR_POSE
-				const auto model_offset =
-				    rift_s_touch_grip_to_model(right ? XRT_HAND_RIGHT : XRT_HAND_LEFT);
-				if (input == XRT_INPUT_TOUCH_GRIP_POSE)
-					math_pose_transform(&rel.pose, &model_offset, &rel.pose);
-				vr::DriverPose_t pose{};
-				pose.poseIsValid = true;
-				pose.result = vr::TrackingResult_Running_OK;
-				apply_pose(&rel, &pose, XRT_SPACE_RELATION_POSITION_VALID_BIT, true);
-				REQUIRE(pose.poseIsValid);
-				CHECK(pose.result == vr::TrackingResult_Running_OK);
-				CHECK(pose.vecPosition[1] > 1.0);
-#endif
-			}
-		}
-		REQUIRE(ctrl.position_filter.frozen);
-		// The first two fits remain candidates. The third resumes the unchanged optical
-		// filter, while only the displayed translation blends out over 100 ms.
-		const auto held = fixture.query(XRT_INPUT_TOUCH_GRIP_POSE, os_monotonic_get_ns());
-		t_constellation_pose_observation obs{};
-		obs.pose = {ctrl.pose.orientation, {-0.2f, 1.5f, -0.5f}};
-		obs.position_covariance[0] = obs.position_covariance[4] = obs.position_covariance[8] = 1e-5f;
-		for (int i = 0; i < 3; ++i) {
-			obs.capture_ns = start + (1894 + 33 * i) * ms;
-			rift_s_controller_push_pose_observation(&ctrl.base, &obs);
-			CHECK(ctrl.position_filter.frozen == (i < 2));
-			const auto rel = fixture.query(XRT_INPUT_TOUCH_GRIP_POSE, os_monotonic_get_ns());
-			CHECK(m_vec3_len(m_vec3_sub(rel.pose.position, held.pose.position)) < 0.01f);
-		}
-		REQUIRE(ctrl.diagnostics.optical_accepted == 1);
-		const timepoint_ns recovered = os_monotonic_get_ns();
-		auto previous = fixture.query(XRT_INPUT_TOUCH_GRIP_POSE, recovered);
-		for (int i = 1; i <= 12; ++i) {
-			auto rel = fixture.query(XRT_INPUT_TOUCH_GRIP_POSE, recovered + i * 10 * ms);
-			CHECK(m_vec3_len(m_vec3_sub(rel.pose.position, previous.pose.position)) < 0.09f);
-			REQUIRE((rel.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0);
-			previous = rel;
-		}
-		// At the end the presentation offset is exactly gone, including future queries.
-		xrt_vec3 p, v;
-		rift_s_position_filter_predict(&ctrl.position_filter, recovered + 120 * ms, 100 * ms, &p, &v);
-		xrt_pose imu_device_offset, expected;
-		math_pose_transform(&ctrl.P_imu_device, &ctrl.P_device_grip, &imu_device_offset);
-		const xrt_pose target = {ctrl.pose.orientation, p};
-		math_pose_transform(&target, &imu_device_offset, &expected);
-		check_vec(previous.pose.position, expected.position);
-		// Presentation queries never replace the capture-time optical association prior.
-		const auto prior = fixture.query(XRT_INPUT_GENERIC_TRACKER_POSE, obs.capture_ns);
-		check_vec(prior.pose.position, obs.pose.position);
-	}
-}
-
 TEST_CASE("Rift S grip stays at its center during rotation for either hand")
 {
 	const xrt_vec3 center = {0.3f, 1.2f, -0.5f};
@@ -1722,6 +1675,66 @@ TEST_CASE("HMD capture-time IMU attitude predicts with a world-space gyro")
 }
 
 #ifdef TEST_RIFT_S_STEAMVR_POSE
+TEST_CASE("Rift S motion estimates pass through production grip/model offsets without changing optical association")
+{
+	for (bool left:{true,false}) {
+		rift_s_hmd hmd{};
+		hmd.base.get_tracked_pose=get_test_head_pose;
+		rift_s_system sys{};
+		sys.hmd=&hmd;
+		sys.motion=rift_s_motion_create();
+		REQUIRE(sys.motion != nullptr);
+		rift_s_controller ctrl{};
+		ctrl.sys=&sys;
+		ctrl.base.device_type=left ? XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER : XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER;
+		ctrl.gravity_initialized=true;
+		ctrl.pose.orientation=XRT_QUAT_IDENTITY;
+		ctrl.P_imu_device={XRT_QUAT_IDENTITY,{0.015f,0.02f,-0.01f}};
+		rift_s_controller_pose_frames(left,&ctrl.P_device_aim,&ctrl.P_device_grip);
+		os_mutex_init(&ctrl.mutex);
+		const timepoint_ns t=1000000000;
+		const xrt_vec3 position={left ? 0.7f : 1.3f,1.1f,2.5f}, variance={1e-5f,1e-5f,1e-5f};
+		rift_s_position_filter_reset(&ctrl.position_filter);
+		rift_s_position_filter_fix(&ctrl.position_filter,t,&position,&variance,0.15f);
+		ctrl.last_fix_ns=ctrl.last_imu_capture_ns=t;
+		ctrl.track_vision_updates=3;
+		ctrl.last_tracked_pose={XRT_QUAT_IDENTITY,position};
+		ctrl.last_tracked_pose_ts=t;
+		ctrl.attitude_history_count=2;
+		ctrl.attitude_history[0]={t-2000000,t-2000000,t-2000000,XRT_QUAT_IDENTITY,{0,0,0},true};
+		ctrl.attitude_history[1]={t,t,t,XRT_QUAT_IDENTITY,{0,0,0},true};
+		rift_s_controller_update_motion(&ctrl,t);
+		xrt_pose imu={XRT_QUAT_IDENTITY,position}, device, expected;
+		math_pose_transform(&imu,&ctrl.P_imu_device,&device);
+		math_pose_transform(&device,&ctrl.P_device_grip,&expected);
+		xrt_space_relation grip{};
+		REQUIRE(rift_s_controller_get_tracked_pose(&ctrl.base,XRT_INPUT_TOUCH_GRIP_POSE,t,&grip)==XRT_SUCCESS);
+		check_vec(grip.pose.position,expected.position);
+		CHECK(grip.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
+		const xrt_vec3 zero={0,0,0};
+		rift_s_position_filter_imu(&ctrl.position_filter,t+2000000,&zero);
+		ctrl.last_fix_ns=t-U_TIME_1S_IN_NS;
+		rift_s_controller_update_motion(&ctrl,t+2000000);
+		REQUIRE(rift_s_controller_get_tracked_pose(&ctrl.base,XRT_INPUT_TOUCH_GRIP_POSE,t+2000000,&grip)==XRT_SUCCESS);
+		check_vec(grip.pose.position,expected.position);
+		CHECK_FALSE(grip.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT);
+		xrt_space_relation prior{};
+		REQUIRE(rift_s_controller_get_tracked_pose(&ctrl.base,XRT_INPUT_GENERIC_TRACKER_POSE,t+2000000,&prior)==XRT_SUCCESS);
+		check_vec(prior.pose.position,position);
+		CHECK(prior.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT);
+		const auto grip_model=rift_s_touch_grip_to_model(left ? XRT_HAND_LEFT : XRT_HAND_RIGHT);
+		const xrt_pose origin=XRT_POSE_IDENTITY;
+		transform_rift_s_touch_relation(&grip,&grip_model,&origin);
+		vr::DriverPose_t pose{};
+		apply_rift_s_touch_pose(&grip,&pose);
+		CHECK(pose.poseIsValid);
+		CHECK(pose.result==vr::TrackingResult_Running_OutOfRange);
+		CHECK(pose.vecPosition[1]>1.0);
+		rift_s_motion_destroy(&sys.motion);
+		os_mutex_destroy(&ctrl.mutex);
+	}
+}
+
 TEST_CASE("Rift S Touch model and world transforms preserve estimated coordinates and confidence")
 {
 	for (bool position_valid : {false, true}) {
@@ -1807,6 +1820,36 @@ TEST_CASE("Rift S Touch OpenVR adapter rejects a nonfinite estimate")
 	CHECK(pose.vecPosition[0] == 0);
 }
 
+TEST_CASE("Rift S HMD fallback remains usable during relocalization without claiming tracked position")
+{
+	xrt_space_relation rel=XRT_SPACE_RELATION_ZERO;
+	rel.pose={XRT_QUAT_IDENTITY,{0.2f,1.6f,-0.1f}};
+	rel.relation_flags=static_cast<xrt_space_relation_flags>(XRT_SPACE_RELATION_POSITION_VALID_BIT |
+	    XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
+	vr::DriverPose_t pose{};
+	pose.poseIsValid=true;
+	pose.result=vr::TrackingResult_Running_OK;
+	apply_head_pose(&rel,&pose,true);
+	CHECK(pose.poseIsValid);
+	CHECK(pose.result==vr::TrackingResult_Running_OutOfRange);
+	CHECK(pose.vecPosition[1]==Catch::Approx(1.6f));
+	CHECK_FALSE(rel.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
+	apply_head_pose(&rel,&pose,false);
+	CHECK_FALSE(pose.poseIsValid);
+	// Standby keeps the finite held pose usable, while still reporting OutOfRange.
+	rel.relation_flags=static_cast<xrt_space_relation_flags>(rel.relation_flags & ~XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
+	pose.poseIsValid=true;
+	apply_head_pose(&rel,&pose,true);
+	CHECK(pose.poseIsValid);
+	CHECK(pose.vecPosition[1]==Catch::Approx(1.6f));
+	CHECK(pose.result==vr::TrackingResult_Running_OutOfRange);
+	rel.relation_flags=static_cast<xrt_space_relation_flags>(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
+	    XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
+	pose.poseIsValid=true;
+	apply_head_pose(&rel,&pose,true);
+	CHECK_FALSE(pose.poseIsValid);
+}
+
 TEST_CASE("SteamVR held pose clears velocity retained from the last tracked pose")
 {
 	xrt_space_relation rel = XRT_SPACE_RELATION_ZERO;
@@ -1903,7 +1946,7 @@ TEST_CASE("Rift S unconfirmed position candidates cannot rotate or replace the a
 	obs.inliers = obs.distinct_leds = 6;
 	obs.position_covariance[0] = obs.position_covariance[4] = obs.position_covariance[8] = 1e-6f;
 	obs.hypothesis = "test";
-	rift_s_controller_push_pose_observation(&ctrl.base, &obs);
+	REQUIRE(rift_s_controller_push_pose_observation(&ctrl.base, &obs));
 	const xrt_pose trusted = ctrl.last_tracked_pose;
 	const timepoint_ns trusted_ns = ctrl.last_tracked_pose_ts;
 	const auto tilt_updates = ctrl.optical_tilt_updates;
@@ -1912,7 +1955,7 @@ TEST_CASE("Rift S unconfirmed position candidates cannot rotate or replace the a
 	math_quat_from_angle_vector(DEG_TO_RAD(30.f), &yaw_axis, &obs.pose.orientation);
 	for (int i = 1; i <= 2; ++i) {
 		obs.capture_ns = trusted_ns + i * 10000000LL;
-		rift_s_controller_push_pose_observation(&ctrl.base, &obs);
+		CHECK_FALSE(rift_s_controller_push_pose_observation(&ctrl.base, &obs));
 		CHECK(ctrl.last_tracked_pose_ts == trusted_ns);
 		CHECK(ctrl.last_tracked_pose.position.x == trusted.position.x);
 		CHECK(ctrl.fusion.rot.y == Catch::Approx(0).margin(1e-6));
@@ -1922,7 +1965,7 @@ TEST_CASE("Rift S unconfirmed position candidates cannot rotate or replace the a
 	// A continuous fit at the trusted position clears the rejected candidate.
 	obs.capture_ns += 10000000LL;
 	obs.pose = trusted;
-	rift_s_controller_push_pose_observation(&ctrl.base, &obs);
+	CHECK(rift_s_controller_push_pose_observation(&ctrl.base, &obs));
 	CHECK(ctrl.position_filter.candidate_count == 0);
 	CHECK(ctrl.last_tracked_pose_ts == obs.capture_ns);
 	CHECK(ctrl.diagnostics.optical_accepted == 2);
@@ -1931,6 +1974,53 @@ TEST_CASE("Rift S unconfirmed position candidates cannot rotate or replace the a
 	os_mutex_destroy(&ctrl.mutex);
 }
 
+TEST_CASE("Rift S confirmed optical reacquisition seeds an age-lost controller without a second search")
+{
+	rift_s_controller ctrl{};
+	ctrl.gravity_initialized = true;
+	ctrl.device_type = RIFT_S_DEVICE_RIGHT_CONTROLLER;
+	ctrl.fusion.rot = ctrl.pose.orientation = XRT_QUAT_IDENTITY;
+	os_mutex_init(&ctrl.mutex);
+	rift_s_position_filter_reset(&ctrl.position_filter);
+	ctrl.position_filter.accel_noise_density = 0.31;
+	ctrl.position_filter.bias_random_walk = 0.002;
+	for (int i = 0; i < 100; ++i)
+		REQUIRE(rift_s_controller_record_attitude(&ctrl, 1000000000LL + i * 2000000LL,
+		                                         1000000000LL + i * 2000000LL));
+	t_constellation_pose_observation obs{};
+	obs.capture_ns = 1100000000LL;
+	obs.pose = XRT_POSE_IDENTITY;
+	obs.pose.position = {0.1f, 1.1f, -0.4f};
+	obs.position_covariance[0] = obs.position_covariance[4] = obs.position_covariance[8] = 1e-6f;
+	obs.hypothesis = "confirmed_constellation";
+	REQUIRE(rift_s_controller_push_pose_observation(&ctrl.base, &obs));
+	const xrt_vec3 zero{};
+	for (int i = 100; i <= 420; ++i) {
+		const timepoint_ns ns = 1000000000LL + i * 2000000LL;
+		rift_s_position_filter_imu(&ctrl.position_filter, ns, &zero);
+		REQUIRE(rift_s_controller_record_attitude(&ctrl, ns, ns));
+	}
+	obs.capture_ns = 1800000000LL;
+	obs.pose.position.x += 1;
+	SECTION("Age-based optical loss accepts the already confirmed new track") {
+		REQUIRE(rift_s_controller_push_pose_observation(&ctrl.base, &obs));
+		CHECK(ctrl.position_filter.track_updates == 1);
+		CHECK(ctrl.position_filter.history_count > 0);
+		CHECK(ctrl.position_filter.state.t == 1840000000LL);
+		CHECK(ctrl.position_filter.state.x[0][0] == Catch::Approx(obs.pose.position.x));
+		CHECK(ctrl.position_filter.accel_noise_density == 0.31);
+		CHECK(ctrl.position_filter.bias_random_walk == 0.002);
+		CHECK(ctrl.diagnostics.optical_accepted == 2);
+	}
+	SECTION("A clock discontinuity retains the filter's confirmation gate") {
+		rift_s_position_filter_suspend(&ctrl.position_filter);
+		CHECK_FALSE(rift_s_controller_push_pose_observation(&ctrl.base, &obs));
+		CHECK(ctrl.position_filter.suspended);
+		CHECK(ctrl.diagnostics.optical_accepted == 1);
+	}
+	os_mutex_destroy(&ctrl.mutex);
+}
+
 #ifdef TEST_RIFT_S_STEAMVR_POSE
 TEST_CASE("Rift S complete grip to SteamVR model chain preserves metric position in a rotated world")
 {
@@ -2001,7 +2091,7 @@ TEST_CASE("Rift S controller does not integrate missing radio motion")
 	CHECK(ctrl.fusion.rot.w == Catch::Approx(before.w));
 	CHECK(ctrl.attitude_history_count == 0);
 	CHECK_FALSE(ctrl.optical_heading_valid);
-	CHECK(ctrl.position_filter.frozen);
+	CHECK(ctrl.position_filter.suspended);
 	REQUIRE(rift_s_controller_record_attitude(&ctrl, 3000000000LL, 3000000000LL));
 	rift_s_attitude_sample sample{};
 	CHECK_FALSE(rift_s_controller_get_attitude(&ctrl, 2000000000LL, &sample));

tests/tests_rift_s_world_anchor.cpp

@@ -3,6 +3,7 @@
 #include "catch_amalgamated.hpp"
 #include "rift_s_world_anchor.h"
 #include "rift_s_slam_guard.h"
+#include "../src/xrt/state_trackers/steamvr_drv/ovrd_world_universe.hpp"
 #include <opencv2/calib3d.hpp>
 #include <opencv2/imgproc.hpp>
 #include <algorithm>
@@ -505,7 +506,8 @@ TEST_CASE("Cross-session camera map replay", "[.camera-replay]")
 	};
 	unsigned keyframes = 0, matched = 0, attempted = 0, confirmations = 0;
 	int64_t start_ns = 0, confirmed_ns = 0, previous_ns = 0;
-	xrt_pose previous = XRT_POSE_IDENTITY, confirmed = XRT_POSE_IDENTITY, confirmed_head = XRT_POSE_IDENTITY;
+	xrt_pose previous = XRT_POSE_IDENTITY, confirmed = XRT_POSE_IDENTITY, confirmed_head = XRT_POSE_IDENTITY,
+	         confirmed_raw = XRT_POSE_IDENTITY;
 	double max_error = 0, max_vote_translation = 0, max_vote_angle = 0;
 	for (const auto &[dir, record] :
 	     {std::pair{std::string(session_a), true}, std::pair{std::string(session_b), false}}) {
@@ -618,6 +620,7 @@ TEST_CASE("Cross-session camera map replay", "[.camera-replay]")
 				confirmed_ns = ts;
 				confirmed = candidate;
 				confirmed_head = result.world_from_imu;
+				confirmed_raw = raw;
 			}
 			std::printf(
 			    "Replay time_s=%.3f inliers=%u/%u error_px=%.3f votes=%u transform=%.4f,%.4f,%.4f\n",
@@ -637,6 +640,29 @@ TEST_CASE("Cross-session camera map replay", "[.camera-replay]")
 	if (!load_only)
 		REQUIRE(keyframes > 0);
 	REQUIRE(confirmed_ns > 0);
+	if (!load_only && std::strcmp(session_a, session_b) == 0) {
+		// Replaying the same exposures must recover their original gauge, not only
+		// an internally consistent but displaced room pose.
+		CHECK(cv::norm(translation(confirmed)) < 0.025);
+		cv::Vec3d delta;
+		cv::Rodrigues(rotation(confirmed.orientation), delta);
+		CHECK(cv::norm(delta) < M_PI / 180);
+	}
+	rift_s_slam_guard guard{};
+	rift_s_slam_guard_init(&guard);
+	guard.require_relocalization = c.require_boundary;
+	CHECK(rift_s_world_universe("relocalizing", confirmed_ns, confirmed_ns) == 3);
+	rift_s_slam_guard_set_world(&guard, &confirmed);
+	REQUIRE(guard.world_valid);
+	REQUIRE(guard.boundary_confirmed);
+	CHECK_FALSE(guard.world_transition);
+	CHECK(rift_s_world_universe("localized", confirmed_ns, confirmed_ns) == 2);
+	xrt_pose published;
+	math_pose_transform(&guard.align, &confirmed_raw, &published);
+	CHECK(cv::norm(translation(published) - translation(confirmed_head)) < 0.0001);
+	std::printf("Replay publication helper universe=2 saved_world_error_m=%.6f\n",
+	            cv::norm(translation(published) - translation(confirmed_head)));
+
 }
 
 TEST_CASE("Native fisheye recovery tolerates subpixel peripheral feature noise")
@@ -659,6 +685,27 @@ TEST_CASE("Native fisheye recovery tolerates subpixel peripheral feature noise")
 	CHECK(result.inliers[1] >= 60);
 }
 
+TEST_CASE("Fisheye PnP seed retains native-pixel support at peripheral bearings")
+{
+	auto c = calibration();
+	xrt_pose expected{{0, 0.24740396f, 0, 0.96891242f}, {0.2f, 0.1f, -0.45f}};
+	auto peripheral = observations(c, expected);
+	for (auto &eye : peripheral) {
+		eye.erase(std::remove_if(eye.begin(), eye.end(), [](const auto &o) {
+			return o.pixel.x > 200 && o.pixel.x < 450;
+		}), eye.end());
+		for (unsigned i = 0; i < eye.size(); ++i) {
+			eye[i].pixel.x += float(1.4 * std::sin(i * 1.7));
+			eye[i].pixel.y += float(1.4 * std::cos(i * 2.3));
+		}
+	}
+	rift_s_world_anchor_result result{};
+	REQUIRE(solve(c, peripheral, result));
+	CHECK(result.supporting_cameras == 2);
+	CHECK(result.max_reprojection_px <= 2.5f);
+	CHECK(cv::norm(translation(result.world_from_imu) - translation(expected)) < 0.025);
+}
+
 TEST_CASE("Native reprojection drops a few wrong correspondences without losing the room")
 {
 	auto c = calibration();

tests/tests_steamvr_roomsetup_guard.cpp

@@ -152,6 +152,10 @@ TEST_CASE("Saved floor sends a numeric detection and only seeds manual entry onc
 		}
 	} paths;
 	roomsetup::SavedFloor floor{&messages, &paths};
+	// An unlocalized room must not inject either the old plane or a manual-floor reset.
+	CHECK_FALSE(floor.publish(true, false));
+	CHECK_FALSE(floor.publish(false, false));
+	CHECK(messages.sent.empty());
 	REQUIRE(floor.publish());
 	REQUIRE(messages.sent.size() == 1);
 	auto *result = cJSON_Parse(messages.sent[0].c_str());
@@ -166,6 +170,8 @@ TEST_CASE("Saved floor sends a numeric detection and only seeds manual entry onc
 	REQUIRE(reset);
 	CHECK(cJSON_GetNumberValue(cJSON_GetObjectItem(reset, "value_source")) == 2);
 	cJSON_Delete(reset);
+	CHECK_FALSE(floor.publish(true, false));
+	CHECK(messages.sent.size() == 3);
 }
 
 TEST_CASE("Boundary warning retries failed delivery and removal")