diff --git a/README.md b/README.md
index 84798b2..0cce6a6 100644
--- a/README.md
+++ b/README.md
@@ -16,12 +16,15 @@ the per-platform SDK packaging live in
   set to this machine's default.
 
 ```rust
-use dim_slam::{FusionCore, OdometryFusionConfig, SourceConfig};
+use dim_slam::{FusionCore, OdometryFusionConfig, SourceConfig, SourceKind};
 
+// Sources are `pose` (a non-drifting world frame), `twist` (body velocity) or `pose_delta`
+// (a drifting pose, fused by its motion only).
 let mut fusion = FusionCore::new(OdometryFusionConfig {
     odom_sources: vec![SourceConfig {
         parent_frame_id: "visual_odom".into(),
         child_frame_id: "base_link".into(),
+        kind: SourceKind::PoseDelta,
         pose_variances: [1e-4; 6],
         ..Default::default()
     }],
diff --git a/python/Cargo.lock b/python/Cargo.lock
index ca17087..e369f7b 100644
--- a/python/Cargo.lock
+++ b/python/Cargo.lock
@@ -61,7 +61,7 @@ dependencies = [
 
 [[package]]
 name = "dim_odom"
-version = "0.4.2"
+version = "0.5.0-beta.3"
 dependencies = [
  "dim_slam",
  "pyo3",
diff --git a/python/Cargo.toml b/python/Cargo.toml
index e5bb1de..ad94145 100644
--- a/python/Cargo.toml
+++ b/python/Cargo.toml
@@ -5,7 +5,7 @@
 name = "dim_odom"
 # Tracks the repository's release tag, so a consumer pinning the rust crate at a
 # tag and the wheel at a version can see at a glance that they agree.
-version = "0.4.2"
+version = "0.5.0-beta.3"
 edition = "2021"
 license = "Apache-2.0"
 description = "Python bindings for dimSLAM: cuVSLAM visual odometry plus error-state Kalman fusion"
diff --git a/python/README.md b/python/README.md
index 61b61f8..f1a6e5a 100644
--- a/python/README.md
+++ b/python/README.md
@@ -2,8 +2,8 @@
 
 Python bindings for [dimSLAM](https://github.com/dimensionalOS/dimSLAM):
 `CuvslamOdometry` wraps the cuVSLAM visual(-inertial) odometry front end,
-`OdometryFusion` the error-state Kalman filter that fuses odometry sources with an
-IMU. Everything runs in-process and on the caller's data stamps, so replaying a
+`OdometryFusion` the error-state Kalman filter that fuses odometry sources, with or
+without an IMU. Everything runs in-process and on the caller's data stamps, so replaying a
 recording is deterministic.
 
 ```python
@@ -16,7 +16,9 @@ tracker = dim_odom.CuvslamOdometry(
 tracker.handle_camera_info(dim_odom.CameraModel(...))
 estimate = tracker.handle_image(dim_odom.ImageFrame(...))
 
-fusion = dim_odom.OdometryFusion({"odom_sources": [{"parent_frame_id": "odom", "child_frame_id": "base_link"}]})
+fusion = dim_odom.OdometryFusion({"odom_sources": [
+    {"parent_frame_id": "odom", "child_frame_id": "base_link", "kind": "pose_delta", "pose_variances": [1e-4] * 6},
+]})
 if estimate is not None:
     fusion.handle_source(estimate)
 fused = fusion.maybe_publish()
diff --git a/python/build_fallback_wheel.sh b/python/build_fallback_wheel.sh
index 2f8857a..eff286f 100755
--- a/python/build_fallback_wheel.sh
+++ b/python/build_fallback_wheel.sh
@@ -52,5 +52,6 @@ mkdir -p "$output"
 python3 -m pip wheel --no-deps --wheel-dir "$output" "$build"
 
 # A setuptools older than 61 ignores the [project] table and silently emits a
-# UNKNOWN-0.0.0 wheel, which uploads to PyPI just as happily as a real one.
-test -f "$output/dim_odom-$version-py3-none-any.whl"
+# UNKNOWN-0.0.0 wheel, which uploads to PyPI just as happily as a real one. The
+# wheel spells the version normalized (0.5.0-beta.1 -> 0.5.0b1), hence the glob.
+compgen -G "$output/dim_odom-[0-9]*-py3-none-any.whl" >/dev/null
diff --git a/python/src/lib.rs b/python/src/lib.rs
index 5ddc248..c48f774 100644
--- a/python/src/lib.rs
+++ b/python/src/lib.rs
@@ -448,7 +448,8 @@ impl CuvslamOdometry {
 }
 
 /// The error-state Kalman fuser. `config` mirrors the Rust `OdometryFusionConfig`
-/// as a dict; `odom_sources` entries name their parent_frame_id and child_frame_id.
+/// as a dict; `odom_sources` entries name their parent_frame_id, child_frame_id and
+/// kind ("pose", "twist" or "pose_delta"). Unknown keys are refused.
 #[pyclass]
 struct OdometryFusion {
     core: FusionCore,
@@ -474,12 +475,16 @@ impl OdometryFusion {
         self.core.handle_source(&estimate.inner);
     }
 
-    /// The fused estimate once the publish period has elapsed on the data clock,
-    /// else None. Call after each input.
-    fn maybe_publish(&mut self) -> Option<OdometryEstimate> {
-        self.core
-            .maybe_publish()
-            .map(|inner| OdometryEstimate { inner })
+    /// The fused estimate, else None. Without `now_ns`: at the newest input's stamp once
+    /// the publish period has elapsed (call after each input). With it: predicted to that
+    /// time, whenever it is past the last output (call on a timer).
+    #[pyo3(signature = (now_ns=None))]
+    fn maybe_publish(&mut self, now_ns: Option<i64>) -> Option<OdometryEstimate> {
+        match now_ns {
+            Some(now_ns) => self.core.maybe_publish_at(now_ns),
+            None => self.core.maybe_publish(),
+        }
+        .map(|inner| OdometryEstimate { inner })
     }
 
     fn report(&self) {
diff --git a/rust/src/dim_slam_core.rs b/rust/src/dim_slam_core.rs
index ef1ab7a..19b2760 100644
--- a/rust/src/dim_slam_core.rs
+++ b/rust/src/dim_slam_core.rs
@@ -9,7 +9,8 @@ use serde::{Deserialize, Serialize};
 
 use crate::cuvslam_odometry::{CameraConfig, CuvslamCore, CuvslamOdometryConfig};
 use crate::odometry_fusion::{
-    FusionCore, ImuConfig, InitialStds, OdometryFusionConfig, SourceConfig,
+    FusionCore, ImuConfig, InitialStds, OdometryFusionConfig, ProcessNoise, SourceConfig,
+    SourceKind,
 };
 use crate::types::{CameraModel, ImageFrame, ImuSample, OdometryEstimate, PointCloud, TfLookup};
 use crate::warn_throttled;
@@ -41,9 +42,10 @@ pub struct DimSlamCoreConfig {
     pub speed_gate_max_angular: f64,
     /// Stamp spread one frame set may span, milliseconds; 0 keeps cuVSLAM's 1 ms contract.
     pub max_skew_ms: f64,
-    /// Trust in the visual odometry, [x y z roll pitch yaw]. Negative takes the message
-    /// covariance, zero drops the dimension, positive is a fixed variance. It drifts, so its
-    /// covariance describes accumulated drift rather than the delta: prefer a fixed value.
+    /// Noise of the tracker's frame-to-frame motion, [x y z roll pitch yaw]: it drifts, so
+    /// it is fused as a `pose_delta` source. Negative takes the message covariance, zero drops
+    /// the dimension, positive is a fixed variance. Its covariance describes accumulated
+    /// drift rather than one frame's motion: prefer a fixed value.
     pub visual_odom_pose_variances: [f64; 6],
 
     /// Frame the fused output is stamped in.
@@ -51,25 +53,28 @@ pub struct DimSlamCoreConfig {
     /// The body frame everything is resolved into: the tracker's output and the frame the
     /// filter estimates.
     pub output_frame_id: String,
-    /// Output cadence; the filter itself runs at the IMU rate.
+    /// Output cadence; each output is the filter predicted to its own stamp.
     pub publish_rate: f64,
     /// How far back a late measurement can reach before it is dropped.
     pub replay_buffer_seconds: f64,
-    /// Outlier gate in variance units; smaller is more aggressive, 0 is no gate.
+    /// Outlier gate: squared Mahalanobis distance per measurement dimension; 0 is no gate.
     pub outlier_rejection_allowed_variance: f64,
     /// Gate on the filter's own state, which can run away with no bad measurement; 0 disables.
     pub max_position_m: f64,
-    /// Empty disables inertial propagation; each entry keeps its own noise figures and biases.
+    /// Empty predicts by constant velocity instead; each entry keeps its own noise figures
+    /// and biases.
     pub imus: Vec<ImuConfig>,
     /// m/s^2, seeding the filter's gravity estimate.
     pub initial_gravity_estimate: f64,
     pub initial_stds: InitialStds,
-    /// External sources, each identified by the transform its estimates carry. The visual
-    /// odometry is prepended to this list, so a no-IMU blend prefers it.
+    /// The constant-velocity model's noise; only used without IMUs.
+    pub process_noise: ProcessNoise,
+    /// External sources, each identified by the transform its estimates carry.
     pub odom_sources: Vec<SourceConfig>,
-    /// Virtual zero-twist [vx vy vz wx wy wz] fused after each source update (needs an IMU):
-    /// a positive entry pins that dimension to zero with this variance, 0 frees it.
-    pub per_dimension_error_variance: [f64; 6],
+    /// A ground body: z, roll, pitch and their rates held at zero.
+    pub planar: bool,
+    /// See [`OdometryFusionConfig::zero_velocity_variances`].
+    pub zero_velocity_variances: [f64; 6],
 }
 
 /// Defers to the two cores so a field's default cannot drift from the core that reads it.
@@ -97,13 +102,15 @@ impl Default for DimSlamCoreConfig {
             imus: fusion.imus,
             initial_gravity_estimate: fusion.initial_gravity_estimate,
             initial_stds: fusion.initial_stds,
+            process_noise: fusion.process_noise,
             odom_sources: fusion.odom_sources,
-            per_dimension_error_variance: fusion.per_dimension_error_variance,
+            planar: fusion.planar,
+            zero_velocity_variances: fusion.zero_velocity_variances,
         }
     }
 }
 
-/// Visual-inertial odometry: cuVSLAM tracking blended with any other odometry source.
+/// Visual-inertial odometry: cuVSLAM tracking fused with any other odometry source.
 ///
 /// Feed it frames, IMU samples and other sources' estimates, then ask for output. The tracker's
 /// pose is fused in-process and never leaves the struct.
@@ -134,8 +141,9 @@ impl DimSlamCore {
         let mut odom_sources = vec![SourceConfig {
             parent_frame_id: VISUAL_ODOM_FRAME_ID.to_string(),
             child_frame_id: config.output_frame_id.clone(),
+            kind: SourceKind::PoseDelta,
             pose_variances: config.visual_odom_pose_variances,
-            twist_variances: [0.0; 6],
+            ..Default::default()
         }];
         odom_sources.extend(config.odom_sources);
         let fusion_config = OdometryFusionConfig {
@@ -148,8 +156,10 @@ impl DimSlamCore {
             imus: config.imus,
             initial_gravity_estimate: config.initial_gravity_estimate,
             initial_stds: config.initial_stds,
+            process_noise: config.process_noise,
             odom_sources,
-            per_dimension_error_variance: config.per_dimension_error_variance,
+            planar: config.planar,
+            zero_velocity_variances: config.zero_velocity_variances,
         };
         Ok(Self {
             visual_odometry,
@@ -206,6 +216,11 @@ impl DimSlamCore {
         self.fusion.maybe_publish()
     }
 
+    /// The fused pose predicted to `now_ns`, for a caller publishing on its own timer.
+    pub fn maybe_publish_at(&mut self, now_ns: i64) -> Option<OdometryEstimate> {
+        self.fusion.maybe_publish_at(now_ns)
+    }
+
     pub fn report(&self) {
         self.visual_odometry.report();
         self.fusion.report();
diff --git a/rust/src/lib.rs b/rust/src/lib.rs
index 10095c4..a38f7de 100644
--- a/rust/src/lib.rs
+++ b/rust/src/lib.rs
@@ -7,39 +7,43 @@
 //! source, then ask for the fused pose. It owns the tracker's wiring into the filter.
 //!
 //! The two halves are also public on their own. [`CuvslamCore`] turns camera, depth and IMU
-//! frames into a pose; [`FusionCore`] blends a pose with any other odometry source against IMU
-//! propagation. Reach for them only when the wiring itself has to differ.
+//! frames into a pose; [`FusionCore`] is one error-state Kalman filter over any odometry
+//! sources, predicting by IMU or, without one, by constant velocity. Reach for them only when
+//! the wiring itself has to differ.
 //!
 //! All are synchronous and own no threads or transport.
 //!
 //! ```
 //! use dim_slam::nalgebra::Isometry3;
-//! use dim_slam::{DimSlamCore, DimSlamCoreConfig, OdometryEstimate, SourceConfig};
+//! use dim_slam::nalgebra::Vector3;
+//! use dim_slam::{DimSlamCore, DimSlamCoreConfig, OdometryEstimate, SourceConfig, SourceKind, Twist};
 //!
 //! // An odometry source is identified by the transform its estimates carry:
-//! // the message's frame_id -> child_frame_id.
+//! // the message's frame_id -> child_frame_id. Wheels measure the body's velocity.
 //! let mut slam = DimSlamCore::new(DimSlamCoreConfig {
 //!     visual_odom_pose_variances: [1e-4; 6],
 //!     odom_sources: vec![SourceConfig {
 //!         parent_frame_id: "wheel_odom".into(),
 //!         child_frame_id: "base_link".into(),
-//!         pose_variances: [1e-4; 6],
-//!         twist_variances: [0.0; 6],
+//!         kind: SourceKind::Twist,
+//!         twist_variances: [1e-4, 1e-4, 0.0, 0.0, 0.0, 1e-4],
+//!         ..Default::default()
 //!     }],
+//!     planar: true,
 //!     ..Default::default()
 //! })
 //! .expect("a usable config");
-//! for (step, timestamp_ns) in [0, 20_000_000, 40_000_000].into_iter().enumerate() {
+//! for step in 0..=50 {
 //!     slam.handle_source(&OdometryEstimate {
-//!         timestamp_ns,
+//!         timestamp_ns: step * 20_000_000,
 //!         frame_id: "wheel_odom".into(),
 //!         child_frame_id: "base_link".into(),
-//!         pose: Isometry3::translation(step as f64 * 0.1, 0.0, 0.0),
+//!         twist: Twist { linear: Vector3::new(0.5, 0.0, 0.0), angular: Vector3::zeros() },
 //!         ..Default::default()
 //!     });
 //! }
 //! let estimate = slam.maybe_publish().expect("a publish period has elapsed");
-//! assert!((estimate.pose.translation.x - 0.2).abs() < 1e-9);
+//! assert!((estimate.pose.translation.x - 0.5).abs() < 0.05);
 //! ```
 
 pub mod cuvslam_odometry;
@@ -51,7 +55,10 @@ pub mod types;
 
 pub use cuvslam_odometry::{CameraConfig, CuvslamCore, CuvslamOdometryConfig};
 pub use dim_slam_core::{DimSlamCore, DimSlamCoreConfig};
-pub use odometry_fusion::{FusionCore, ImuConfig, InitialStds, OdometryFusionConfig, SourceConfig};
+pub use odometry_fusion::{
+    FusionCore, ImuConfig, InitialStds, OdometryFusionConfig, ProcessNoise, SourceConfig,
+    SourceKind,
+};
 pub use types::{
     CameraModel, ImageFrame, ImuNoiseModel, ImuSample, OdometryEstimate, PointCloud, TfLookup,
     Twist,
diff --git a/rust/src/odometry_fusion.rs b/rust/src/odometry_fusion.rs
index 48ff3dd..af0fe1e 100644
--- a/rust/src/odometry_fusion.rs
+++ b/rust/src/odometry_fusion.rs
@@ -1,20 +1,21 @@
 // Copyright 2026 Dimensional Inc.
 // SPDX-License-Identifier: Apache-2.0
 //
-// Error-state Kalman filter fusing IMU propagation with odometry sources told apart by their
-// frame_id. Without an IMU there is no process model: see `blend`.
+// One error-state Kalman filter fusing odometry sources told apart by their frame_id. With an
+// IMU it propagates by inertial integration, without one by a constant-velocity model; every
+// source is a measurement of that one state, so the two modes differ only in prediction.
 
 mod eskf;
 
-use std::collections::{HashMap, VecDeque};
+use std::collections::VecDeque;
 
-use nalgebra::{DVector, Isometry3, Matrix6, Translation3, UnitQuaternion, Vector3};
+use nalgebra::{DVector, Isometry3, Matrix3, Matrix6, Translation3, UnitQuaternion, Vector3};
 use serde::{Deserialize, Serialize};
-use tracing::info;
+use tracing::{info, warn};
 
 use crate::types::{ImuSample, OdometryEstimate, Twist};
 use crate::warn_throttled;
-use eskf::{gyro_bias_col, Cov, Filter, ImuBias, Jacobian, State};
+use eskf::{yaw_only, Cov, Filter, ImuBias, ImuInput, Jacobian, State};
 
 const NS_PER_SEC: i64 = 1_000_000_000;
 
@@ -36,24 +37,71 @@ fn resolve_variance(configured: f64, from_message: f64) -> f64 {
     }
 }
 
+/// The same pose with z, roll and pitch dropped.
+fn planar_pose(pose: &Isometry3<f64>) -> Isometry3<f64> {
+    let translation = pose.translation.vector;
+    Isometry3::from_parts(
+        Translation3::new(translation.x, translation.y, 0.0),
+        yaw_only(&pose.rotation),
+    )
+}
+
+/// What a source's pose means to the filter. Its twist, wherever twist_variances are set,
+/// is a body-frame velocity measurement whatever the kind.
+#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Serialize, Deserialize)]
+#[serde(rename_all = "snake_case")]
+pub enum SourceKind {
+    /// The pose in a world frame of the source's own that does not drift (e.g. lidar-inertial
+    /// odometry). Its first message aligns that frame to the fused one; later ones are poses.
+    Pose,
+    /// The pose is not used, only the twist (e.g. wheel odometry).
+    Twist,
+    /// A pose that drifts (e.g. a visual tracker): only the motion between consecutive messages
+    /// is fused, as a relative measurement against the filter's own pose at the earlier one.
+    #[default]
+    PoseDelta,
+}
+
 #[derive(Clone, Debug, Default, Serialize, Deserialize)]
-#[serde(default)]
+#[serde(default, deny_unknown_fields)]
 pub struct SourceConfig {
     /// The transform this source's estimates carry. Both halves are needed: two sources can
-    /// share a parent.
+    /// share a parent. The child is the output frame: twist is read as the body's own.
     pub parent_frame_id: String,
     pub child_frame_id: String,
-    /// [x y z roll pitch yaw]: <0 message covariance, 0 drop, >0 fixed.
-    /// A drifting source's covariance describes accumulated drift, not the delta: prefer a fixed value.
+    pub kind: SourceKind,
+    /// Measurement noise, [x y z roll pitch yaw]: <0 message covariance, 0 drop, >0 fixed.
+    /// For `pose` the noise of the pose itself, position along the world axes; for `pose_delta`
+    /// the noise of one message-to-message motion, in the body frame at the earlier message.
     pub pose_variances: [f64; 6],
     /// [vx vy vz wx wy wz], same convention, body frame.
     pub twist_variances: [f64; 6],
+    /// `pose` only: after rejecting this source this long without a break, the source's frame
+    /// is re-aligned to the fused pose rather than kept out for good. 0 never re-aligns.
+    pub realign_after_seconds: f64,
+    /// Zero-velocity update: a twist at or under `still_speed` (m/s) and `still_angular_speed`
+    /// (rad/s) says the body is still: it is fused as zero velocity with `still_variance` in
+    /// every axis and, without an IMU, the motion model adds no noise until the source reports
+    /// motion, so the pose settles instead of wandering under the other sources' noise. 0 disables.
+    pub still_variance: f64,
+    pub still_speed: f64,
+    pub still_angular_speed: f64,
+}
+
+impl SourceConfig {
+    fn uses_pose(&self) -> bool {
+        self.kind != SourceKind::Twist && self.pose_variances.iter().any(|v| *v != 0.0)
+    }
+
+    fn uses_twist(&self) -> bool {
+        self.twist_variances.iter().any(|v| *v != 0.0)
+    }
 }
 
 /// These all describe the physical IMU rather than the filter itself, so they live together
 /// in one object.
 #[derive(Clone, Debug, Serialize, Deserialize)]
-#[serde(default)]
+#[serde(default, deny_unknown_fields)]
 pub struct ImuConfig {
     /// The frame_id the IMU's samples carry; samples from any other frame are dropped.
     pub frame_id: String,
@@ -84,46 +132,75 @@ impl Default for ImuConfig {
     }
 }
 
+/// How fast the body's velocity can change unannounced, as white-noise acceleration
+/// densities: the constant-velocity model's only tuning. Unused with an IMU, whose own noise
+/// figures take its place.
+#[derive(Clone, Copy, Debug, Serialize, Deserialize)]
+#[serde(default, deny_unknown_fields)]
+pub struct ProcessNoise {
+    /// m/s^2/sqrt(Hz).
+    pub linear_acceleration: f64,
+    /// rad/s^2/sqrt(Hz).
+    pub angular_acceleration: f64,
+}
+
+impl Default for ProcessNoise {
+    fn default() -> Self {
+        Self {
+            linear_acceleration: 1.0,
+            angular_acceleration: 1.0,
+        }
+    }
+}
+
 #[derive(Clone, Debug, Serialize, Deserialize)]
-#[serde(default)]
+#[serde(default, deny_unknown_fields)]
 pub struct OdometryFusionConfig {
     pub odom_frame_id: String,
     pub output_frame_id: String,
-    /// Output cadence; the filter itself runs at the IMU rate.
+    /// Output cadence. Each output is the filter predicted to its own stamp, so it is smooth
+    /// whatever the sources' rates.
     pub publish_rate: f64,
-    /// How far back a late measurement can reach before it is dropped.
+    /// How far back a late measurement can reach before it is dropped, and how far past the
+    /// newest input an output may be predicted.
     pub replay_buffer_seconds: f64,
-    /// Outlier gate in variance units: reject a measurement whose squared Mahalanobis distance
-    /// per measurement dimension exceeds this multiple of the expected noise variance.
-    /// Smaller is more aggressive, 0 is no gate.
+    /// Outlier gate: reject a measurement whose squared Mahalanobis distance, under the full
+    /// innovation covariance, exceeds this per measurement dimension. 0 is no gate.
     pub outlier_rejection_allowed_variance: f64,
     /// Gate on the filter's own state, which can run away with no bad measurement; 0 disables.
     pub max_position_m: f64,
     /// One entry per IMU, told apart by the frame_id its samples carry. Each keeps its own
-    /// bias states in the filter; interleaved samples each integrate the slice of time since
-    /// the previous sample of any IMU. Empty disables the Kalman machinery entirely: the
-    /// filter seeds level from the first source message and holds pose between them (see
-    /// `blend`).
+    /// bias states in the filter, and the newest sample of any IMU drives prediction. Empty
+    /// predicts by constant velocity instead (see `process_noise`), seeded level at the
+    /// origin from the first source message.
     pub imus: Vec<ImuConfig>,
     /// m/s^2. Seeds `filter.gravity` at init; a future ZUPT is meant to refine it from there.
     pub initial_gravity_estimate: f64,
     pub initial_stds: InitialStds,
+    /// The constant-velocity model's noise; only used without IMUs.
+    pub process_noise: ProcessNoise,
     /// One entry per external odometry stream, told apart by the transform its estimates carry.
-    /// The no-IMU blend sums in list order, so the order is part of the config.
     pub odom_sources: Vec<SourceConfig>,
-    /// Virtual zero-twist [vx vy vz wx wy wz] fused after each source update (needs an IMU):
-    /// >0 pins that dimension to zero with this variance (e.g. non-holonomic vy, vz), 0 frees it.
-    pub per_dimension_error_variance: [f64; 6],
+    /// A ground body: z, roll, pitch and their rates are held at zero in the state itself,
+    /// and sources are fused in x, y and yaw only.
+    pub planar: bool,
+    /// Body [vx vy vz wx wy wz]: >0 is a soft zero-velocity measurement of that axis with this
+    /// variance, fused with every source message, e.g. vy for a base that cannot slide sideways;
+    /// 0 leaves the axis free. Ungated: it is the model's own assumption, not a sensor. With an
+    /// IMU the angular entries are ignored: the gyro measures rate, so they could only drag its bias.
+    pub zero_velocity_variances: [f64; 6],
 }
 
 /// 1-sigma uncertainty the filter starts with, per state block. The bias std applies to
 /// every IMU's gyro and accel bias blocks.
 #[derive(Clone, Debug, Serialize, Deserialize)]
-#[serde(default)]
+#[serde(default, deny_unknown_fields)]
 pub struct InitialStds {
     pub position: f64,
     pub velocity: f64,
     pub rotation: f64,
+    /// Without an IMU only.
+    pub angular_velocity: f64,
     pub bias: f64,
 }
 
@@ -133,6 +210,7 @@ impl Default for InitialStds {
             position: 0.1,
             velocity: 0.1,
             rotation: 0.05,
+            angular_velocity: 0.1,
             bias: 0.01,
         }
     }
@@ -157,8 +235,10 @@ impl Default for OdometryFusionConfig {
             imus: Vec::new(),
             initial_gravity_estimate: 9.8,
             initial_stds: InitialStds::default(),
+            process_noise: ProcessNoise::default(),
             odom_sources: Vec::new(),
-            per_dimension_error_variance: [0.0; 6],
+            planar: false,
+            zero_velocity_variances: [0.0; 6],
         }
     }
 }
@@ -166,32 +246,30 @@ impl Default for OdometryFusionConfig {
 #[derive(Clone, Debug)]
 struct Measurement {
     source: usize,
+    /// Planar-projected in planar mode. For `pose_delta`, marks that this message re-clones.
+    pose: Option<Isometry3<f64>>,
+    /// `pose_delta`: the motion since the source's previous message.
     delta: Option<Isometry3<f64>>,
     pose_variance: [f64; 6], // 0 means dropped
+    twist: Option<Twist>,
     twist_variance: [f64; 6],
-    linear: Vector3<f64>,
-    angular: Vector3<f64>,
 }
 
-/// A source keeps its share of the no-IMU blend until it has been silent for the timeout.
+/// Per-source state that lives in the replay history next to the filter's.
 #[derive(Clone, Debug, Default)]
-struct SourceActivity {
-    last_ns: i64,
-    inverse_variance: [f64; 6],
+struct SourceRuntime {
+    /// `pose`: fused_from_source_world, set from its first message and by a re-align.
+    alignment: Option<Isometry3<f64>>,
+    /// `pose`: when its current unbroken run of gate rejections began.
+    rejected_since_ns: Option<i64>,
 }
 
-const SOURCE_ACTIVITY_TIMEOUT_SECONDS: f64 = 0.5;
-
 // The snapshot alongside the kind dwarfs any variant, so boxing buys nothing.
 #[allow(clippy::large_enum_variant)]
 #[derive(Clone, Debug)]
 enum EventKind {
     Seed,
-    Imu {
-        imu: usize,
-        gyro: Vector3<f64>,
-        accel: Vector3<f64>,
-    },
+    Imu(ImuInput),
     Measurement(Measurement),
 }
 
@@ -202,12 +280,7 @@ struct Event {
     kind: EventKind,
     state: State,
     covariance: Cov,
-    anchors: Vec<Option<Isometry3<f64>>>,
-    activity: Vec<SourceActivity>,
-    last_imu_ns: i64,
-    last_gyro: Vector3<f64>,
-    /// Which IMU `last_gyro` came from, so its bias is the one subtracted.
-    last_gyro_imu: usize,
+    sources: Vec<SourceRuntime>,
 }
 
 impl Event {
@@ -216,12 +289,8 @@ impl Event {
             ts_ns,
             kind,
             state: State::default(),
-            covariance: Cov::identity(9, 9),
-            anchors: Vec::new(),
-            activity: Vec::new(),
-            last_imu_ns: 0,
-            last_gyro: Vector3::zeros(),
-            last_gyro_imu: 0,
+            covariance: Cov::zeros(0, 0),
+            sources: Vec::new(),
         }
     }
 }
@@ -241,20 +310,20 @@ struct ImuRuntime {
 pub struct FusionCore {
     config: OdometryFusionConfig,
     filter: Filter,
+    sources: Vec<SourceRuntime>,
+    /// Per source, the filter's pose-clone slot: `pose_delta` sources that fuse a pose.
+    clone_slots: Vec<Option<usize>>,
     initialized: bool,
     imu_runtime: Vec<ImuRuntime>,
     events: VecDeque<Event>,
-    anchors: Vec<Option<Isometry3<f64>>>,
-    activity: Vec<SourceActivity>,
-    last_imu_ns: i64,
-    last_gyro: Vector3<f64>,
-    last_gyro_imu: usize,
-    /// Per-source last raw pose. A delta belongs to the message stream, so replay never redoes it.
-    previous_source_pose: HashMap<usize, Isometry3<f64>>,
+    /// Per-source last raw pose and stamp. A delta belongs to the message stream, so replay
+    /// never redoes it.
+    previous_source_pose: Vec<Option<(i64, Isometry3<f64>)>>,
     last_publish_ns: i64,
     imu_samples: u64,
     measurements: u64,
     gated: u64,
+    realigned: u64,
     replayed: u64,
     too_late: u64,
 }
@@ -262,22 +331,32 @@ pub struct FusionCore {
 impl FusionCore {
     pub fn new(config: OdometryFusionConfig) -> Self {
         let imu_runtime = vec![ImuRuntime::default(); config.imus.len()];
+        let mut clones = 0;
+        let clone_slots = config
+            .odom_sources
+            .iter()
+            .map(|source| {
+                (source.kind == SourceKind::PoseDelta && source.uses_pose()).then(|| {
+                    clones += 1;
+                    clones - 1
+                })
+            })
+            .collect();
+        let source_count = config.odom_sources.len();
         Self {
             config,
             filter: Filter::default(),
+            sources: vec![SourceRuntime::default(); source_count],
+            clone_slots,
             initialized: false,
             imu_runtime,
             events: VecDeque::new(),
-            anchors: Vec::new(),
-            activity: Vec::new(),
-            last_imu_ns: 0,
-            last_gyro: Vector3::zeros(),
-            last_gyro_imu: 0,
-            previous_source_pose: HashMap::new(),
+            previous_source_pose: vec![None; source_count],
             last_publish_ns: 0,
             imu_samples: 0,
             measurements: 0,
             gated: 0,
+            realigned: 0,
             replayed: 0,
             too_late: 0,
         }
@@ -298,6 +377,16 @@ impl FusionCore {
             );
             return;
         };
+        let finite = imu.angular_velocity.iter().all(|v| v.is_finite())
+            && imu.linear_acceleration.iter().all(|v| v.is_finite());
+        if !finite {
+            warn_throttled!(
+                std::time::Duration::from_secs(5),
+                frame_id = %imu.frame_id,
+                "imu sample with a non-finite reading dropped",
+            );
+            return;
+        }
         let lever = base_from_imu.translation.vector;
         let base_from_imu = base_from_imu.rotation;
         let gyro = base_from_imu * imu.angular_velocity;
@@ -318,6 +407,11 @@ impl FusionCore {
             // The lever arm adds centripetal and tangential terms: mount kinematics, not base motion.
             accel -= gyro.cross(&gyro.cross(&lever)) + runtime.lever_alpha.cross(&lever);
         }
+        let input = ImuInput {
+            imu: index,
+            gyro,
+            accel,
+        };
         if !self.initialized {
             self.check_config();
             if gyro.norm() > self.config.imus[index].init_gyro_limit {
@@ -358,8 +452,12 @@ impl FusionCore {
                 // Averaging over IMUs halves what one sensor's bias does to the leveling.
                 let combined: Vector3<f64> =
                     mean_accels.iter().sum::<Vector3<f64>>() / mean_accels.len() as f64;
-                let world_from_body =
+                let mut world_from_body =
                     UnitQuaternion::rotation_between(&combined, &Vector3::z()).unwrap_or_default();
+                // A planar body is level by definition, so any tilt the IMU reads is its mount's.
+                if self.config.planar {
+                    world_from_body = yaw_only(&world_from_body);
+                }
                 // Any stationary reading beyond g is that IMU's accel bias; left at zero it
                 // dead-reckons z away.
                 let expected = world_from_body.inverse_transform_vector(&Vector3::new(
@@ -376,18 +474,11 @@ impl FusionCore {
                         accel: mean_accel - expected,
                     })
                     .collect();
-                self.initialize(ts_ns, world_from_body, biases);
+                self.initialize(ts_ns, world_from_body, biases, Some(input));
             }
             return;
         }
-        self.insert(Event::empty(
-            ts_ns,
-            EventKind::Imu {
-                imu: index,
-                gyro,
-                accel,
-            },
-        ));
+        self.insert(Event::empty(ts_ns, EventKind::Imu(input)));
         self.imu_samples += 1;
     }
 
@@ -397,7 +488,12 @@ impl FusionCore {
                 return;
             }
             self.check_config();
-            self.initialize(msg.timestamp_ns, UnitQuaternion::identity(), Vec::new());
+            self.initialize(
+                msg.timestamp_ns,
+                UnitQuaternion::identity(),
+                Vec::new(),
+                None,
+            );
         }
         let Some((source, source_config)) =
             self.config
@@ -432,41 +528,85 @@ impl FusionCore {
             );
         }
 
-        let pose_wanted = pose_variance.iter().any(|v| *v > 0.0);
-
-        let mut measurement = Measurement {
-            source,
-            delta: None,
-            pose_variance,
-            twist_variance,
-            linear: msg.twist.linear,
-            angular: msg.twist.angular,
-        };
         // A caller building a UnitQuaternion from a zeroed message gets NaN, not an error, and
         // one NaN reaching the state wedges the filter for good: nothing downstream recovers.
         let pose_finite = msg.pose.translation.vector.iter().all(|v| v.is_finite())
             && msg.pose.rotation.coords.iter().all(|v| v.is_finite());
-        if pose_wanted && pose_finite {
-            if let Some(previous) = self.previous_source_pose.get(&source) {
-                measurement.delta = Some(previous.inverse() * msg.pose);
+        let twist_finite = msg.twist.linear.iter().all(|v| v.is_finite())
+            && msg.twist.angular.iter().all(|v| v.is_finite());
+        let pose = (source_config.uses_pose() && pose_finite).then(|| {
+            if self.config.planar {
+                planar_pose(&msg.pose)
+            } else {
+                msg.pose
+            }
+        });
+        let mut delta = None;
+        if source_config.kind == SourceKind::PoseDelta {
+            if let (Some(pose), Some((previous_ns, previous))) =
+                (pose, self.previous_source_pose[source])
+            {
+                if msg.timestamp_ns <= previous_ns {
+                    warn_throttled!(
+                        std::time::Duration::from_secs(5),
+                        frame_id = %msg.frame_id,
+                        "pose_delta message not newer than its predecessor dropped",
+                    );
+                    return;
+                }
+                delta = Some(previous.inverse() * pose);
             }
-            self.previous_source_pose.insert(source, msg.pose);
         }
 
-        self.insert(Event::empty(
+        let kind = source_config.kind;
+        let twist = (source_config.uses_twist() && twist_finite).then_some(msg.twist);
+        let inserted = self.insert(Event::empty(
             msg.timestamp_ns,
-            EventKind::Measurement(measurement),
+            EventKind::Measurement(Measurement {
+                source,
+                pose,
+                delta,
+                pose_variance,
+                twist,
+                twist_variance,
+            }),
         ));
+        if inserted && kind == SourceKind::PoseDelta {
+            if let Some(pose) = pose {
+                self.previous_source_pose[source] = Some((msg.timestamp_ns, pose));
+            }
+        }
         self.measurements += 1;
     }
 
     fn check_config(&self) {
         for source in &self.config.odom_sources {
+            let name = &source.parent_frame_id;
             assert!(
                 !source.parent_frame_id.trim().is_empty()
                     && !source.child_frame_id.trim().is_empty(),
                 "an `odom_sources` entry needs both parent_frame_id and child_frame_id set"
             );
+            assert!(
+                source.kind != SourceKind::Twist || source.pose_variances == [0.0; 6],
+                "source {name:?} is kind twist, which fuses no pose: zero its pose_variances"
+            );
+            assert!(
+                source.uses_pose() || source.uses_twist(),
+                "source {name:?} fuses nothing: set pose_variances or twist_variances"
+            );
+            assert!(
+                source.realign_after_seconds >= 0.0,
+                "source {name:?} needs realign_after_seconds at or above zero"
+            );
+        }
+        if !self.config.use_imu() {
+            let noise = &self.config.process_noise;
+            assert!(
+                noise.linear_acceleration > 0.0 && noise.angular_acceleration > 0.0,
+                "without an imu, process_noise needs both densities above zero, or the filter \
+                 grows overconfident and gates every source out"
+            );
         }
         for (index, imu) in self.config.imus.iter().enumerate() {
             let frame_id = &imu.frame_id;
@@ -499,6 +639,7 @@ impl FusionCore {
         timestamp_ns: i64,
         world_from_body: UnitQuaternion<f64>,
         biases: Vec<ImuBias>,
+        input: Option<ImuInput>,
     ) {
         self.filter.noise = self
             .config
@@ -511,18 +652,18 @@ impl FusionCore {
                 accel_random_walk: imu.accel_random_walk,
             })
             .collect();
+        self.filter.process_noise = self.config.process_noise;
         self.filter.gravity = self.config.initial_gravity_estimate;
+        self.filter.planar = self.config.planar;
+        self.filter.layout.clones = self.clone_slots.iter().flatten().count();
         let gyro_bias_norms: Vec<f64> = biases.iter().map(|bias| bias.gyro.norm()).collect();
         self.filter.init(
+            timestamp_ns,
             world_from_body,
             biases,
-            self.config.initial_stds.position,
-            self.config.initial_stds.velocity,
-            self.config.initial_stds.rotation,
-            self.config.initial_stds.bias,
+            input,
+            &self.config.initial_stds,
         );
-        self.anchors = vec![None; self.config.odom_sources.len()];
-        self.activity = vec![SourceActivity::default(); self.config.odom_sources.len()];
         self.initialized = true;
         self.last_publish_ns = timestamp_ns;
         let mut seed = Event::empty(timestamp_ns, EventKind::Seed);
@@ -531,11 +672,13 @@ impl FusionCore {
         info!(
             gyro_biases = ?gyro_bias_norms,
             use_imu = self.config.use_imu(),
+            planar = self.config.planar,
             "odometry_fusion initialized"
         );
     }
 
-    fn insert(&mut self, event: Event) {
+    /// False when the event is older than the replay buffer and was dropped.
+    fn insert(&mut self, event: Event) -> bool {
         let newest = self.events.back().expect("seeded at init").ts_ns;
         if event.ts_ns >= newest {
             self.events.push_back(event);
@@ -550,7 +693,7 @@ impl FusionCore {
                 "measurement older than the replay buffer dropped. Raise \
                  replay_buffer_seconds, or cut the source's latency.",
             );
-            return;
+            return false;
         } else {
             let slot = self.events.partition_point(|e| e.ts_ns <= event.ts_ns);
             self.events.insert(slot, event);
@@ -564,6 +707,7 @@ impl FusionCore {
         while self.events.len() > 1 && self.events.front().expect("nonempty").ts_ns < horizon {
             self.events.pop_front();
         }
+        true
     }
 
     fn process_at(&mut self, index: usize) {
@@ -571,32 +715,23 @@ impl FusionCore {
             let previous = &self.events[index - 1];
             self.filter.x = previous.state.clone();
             self.filter.p_cov = previous.covariance.clone();
-            self.anchors = previous.anchors.clone();
-            self.activity = previous.activity.clone();
-            self.last_imu_ns = previous.last_imu_ns;
-            self.last_gyro = previous.last_gyro;
-            self.last_gyro_imu = previous.last_gyro_imu;
+            self.sources = previous.sources.clone();
         }
+        // Logs once per event, not again each time a late arrival replays it.
+        let replaying = index + 1 < self.events.len();
         let ts_ns = self.events[index].ts_ns;
+        if !self.filter.predict_to(ts_ns) && !replaying {
+            warn_throttled!(
+                std::time::Duration::from_secs(10),
+                "imu gap over a second left unbridged: the pose held through it. Check the \
+                 IMU stream.",
+            );
+        }
         let kind = self.events[index].kind.clone();
         match kind {
             EventKind::Seed => {}
-            EventKind::Imu { imu, gyro, accel } => {
-                let dt = (ts_ns - self.last_imu_ns) as f64 / 1.0e9;
-                if dt > 0.0 && dt < 1.0 {
-                    self.filter.propagate(dt, imu, &gyro, &accel);
-                }
-                self.last_imu_ns = ts_ns;
-                self.last_gyro = gyro;
-                self.last_gyro_imu = imu;
-            }
-            EventKind::Measurement(measurement) => {
-                if self.config.use_imu() {
-                    self.apply(&measurement);
-                } else {
-                    self.blend(&measurement, ts_ns);
-                }
-            }
+            EventKind::Imu(input) => self.filter.x.input = Some(input),
+            EventKind::Measurement(measurement) => self.apply(&measurement, ts_ns, replaying),
         }
         let mut event = std::mem::replace(
             &mut self.events[index],
@@ -606,195 +741,224 @@ impl FusionCore {
         self.events[index] = event;
     }
 
-    fn apply(&mut self, measurement: &Measurement) {
-        let mut rows: Vec<DVector<f64>> = Vec::new();
-        let mut residuals: Vec<f64> = Vec::new();
-        let mut variances: Vec<f64> = Vec::new();
-
-        let target = if let Some(delta) = measurement.delta {
-            match self.anchors[measurement.source] {
-                Some(anchor) => Some(anchor * delta),
-                None => {
-                    // First delta from this source: adopt the filter pose, fuse nothing.
-                    self.anchor(measurement.source);
-                    return;
+    fn apply(&mut self, measurement: &Measurement, ts_ns: i64, replaying: bool) {
+        let source = measurement.source;
+        match self.config.odom_sources[source].kind {
+            SourceKind::Pose => {
+                if let Some(pose) = &measurement.pose {
+                    self.fuse_pose(source, pose, &measurement.pose_variance, ts_ns, replaying);
                 }
             }
-        } else {
-            None
-        };
-        if let Some(target) = target {
-            let position_residual = target.translation.vector - self.filter.x.position;
-            let rotation_residual = (self.filter.x.q.inverse() * target.rotation).scaled_axis();
-            for dim in 0..3 {
-                if measurement.pose_variance[dim] > 0.0 {
-                    let mut row = DVector::zeros(self.filter.dim());
-                    row[dim] = 1.0;
-                    rows.push(row);
-                    residuals.push(position_residual[dim]);
-                    variances.push(measurement.pose_variance[dim]);
+            SourceKind::PoseDelta => {
+                if let Some(slot) = self.clone_slots[source] {
+                    if let Some(delta) = &measurement.delta {
+                        self.fuse_pose_delta(slot, delta, &measurement.pose_variance);
+                    }
+                    if measurement.pose.is_some() {
+                        self.filter.clone_pose(slot);
+                    }
                 }
             }
-            for dim in 0..3 {
-                if measurement.pose_variance[3 + dim] > 0.0 {
-                    let mut row = DVector::zeros(self.filter.dim());
-                    row[6 + dim] = 1.0;
-                    rows.push(row);
-                    residuals.push(rotation_residual[dim]);
-                    variances.push(measurement.pose_variance[3 + dim]);
+            SourceKind::Twist => {}
+        }
+        if let Some(twist) = &measurement.twist {
+            let config = &self.config.odom_sources[source];
+            let still = config.still_variance > 0.0
+                && twist.linear.norm() <= config.still_speed
+                && twist.angular.norm() <= config.still_angular_speed;
+            self.filter.x.still = still;
+            if still {
+                let mut variance = [config.still_variance; 6];
+                if self.config.use_imu() {
+                    // As below: against the gyro, a zero rate only drags its bias.
+                    variance[3..].fill(0.0);
                 }
+                self.fuse_twist(&Twist::default(), &variance);
+            } else {
+                self.fuse_twist(twist, &measurement.twist_variance);
             }
         }
-
-        self.add_twist_rows(
-            &measurement.linear,
-            &measurement.angular,
-            &measurement.twist_variance,
-            &mut rows,
-            &mut residuals,
-            &mut variances,
-        );
-        let accepted = self.update(&rows, &residuals, &variances);
-        if accepted && target.is_some() {
-            // Anchoring to the corrected pose, not each source's own chain, keeps drifters bounded.
-            self.anchor(measurement.source);
+        let mut constraint = self.config.zero_velocity_variances;
+        if self.config.use_imu() {
+            // A zero-rate row against the gyro moves only its bias, so real rocking would tilt the filter.
+            constraint[3..].fill(0.0);
         }
+        if constraint.iter().any(|variance| *variance > 0.0) {
+            let rows = self.twist_rows(&Twist::default(), &constraint);
+            if !rows.is_empty() {
+                let (residual, jacobian, variance) = rows.matrices();
+                self.filter.update(&residual, &jacobian, &variance, 0.0);
+            }
+        }
+    }
 
-        let constraint_variance = self.config.per_dimension_error_variance;
-        let mut constraint_rows = Vec::new();
-        let mut constraint_residuals = Vec::new();
-        let mut constraint_variances = Vec::new();
-        self.add_twist_rows(
-            &Vector3::zeros(),
-            &Vector3::zeros(),
-            &constraint_variance,
-            &mut constraint_rows,
-            &mut constraint_residuals,
-            &mut constraint_variances,
-        );
-        self.update(
-            &constraint_rows,
-            &constraint_residuals,
-            &constraint_variances,
-        );
+    /// Planar mode fuses nothing in z, roll or pitch (and so vz, wx, wy: the same indices).
+    fn pinned(&self, dim: usize) -> bool {
+        self.config.planar && (2..5).contains(&dim)
+    }
+
+    fn current_pose(&self) -> Isometry3<f64> {
+        Isometry3::from_parts(Translation3::from(self.filter.x.position), self.filter.x.q)
     }
 
-    /// Not a Kalman update: inverse-variance shares sum to one, so a lone source passes through exactly.
-    fn blend(&mut self, measurement: &Measurement, ts_ns: i64) {
-        let horizon = ts_ns - (SOURCE_ACTIVITY_TIMEOUT_SECONDS * 1.0e9) as i64;
-        let activity = &mut self.activity[measurement.source];
-        let returning = activity.last_ns < horizon;
-        activity.last_ns = ts_ns;
+    fn fuse_pose(
+        &mut self,
+        source: usize,
+        pose: &Isometry3<f64>,
+        variance: &[f64; 6],
+        ts_ns: i64,
+        replaying: bool,
+    ) {
+        let Some(alignment) = self.sources[source].alignment else {
+            self.sources[source].alignment = Some(self.current_pose() * pose.inverse());
+            return;
+        };
+        let target = alignment * pose;
+        let position_residual = target.translation.vector - self.filter.x.position;
+        let rotation_residual = (self.filter.x.q.inverse() * target.rotation).scaled_axis();
+        let mut rows = Rows::new(self.filter.dim());
+        let mut rotation_rows = Rows::new(self.filter.dim());
         for dim in 0..6 {
-            activity.inverse_variance[dim] = if measurement.pose_variance[dim] > 0.0 {
-                1.0 / measurement.pose_variance[dim]
+            if variance[dim] <= 0.0 || self.pinned(dim) {
+                continue;
+            }
+            if dim < 3 {
+                rows.push(position_residual[dim], variance[dim])[dim] = 1.0;
             } else {
-                0.0
+                let axis = dim - 3;
+                rotation_rows.push(rotation_residual[axis], variance[dim])[6 + axis] = 1.0;
+            }
+        }
+        let rotation_agrees = rotation_rows.is_empty() || self.within_gate(&rotation_rows);
+        rows.append(rotation_rows);
+        if rows.is_empty() {
+            return;
+        }
+        if self.update(&rows) {
+            self.sources[source].rejected_since_ns = None;
+            return;
+        }
+        let realign_after = self.config.odom_sources[source].realign_after_seconds;
+        let since = *self.sources[source].rejected_since_ns.get_or_insert(ts_ns);
+        if realign_after > 0.0 && (ts_ns - since) as f64 >= realign_after * 1.0e9 {
+            // The fused pose stays put and the source's frame moves: odometry must not jump. Only
+            // what disagrees moves, or the heading error of the moment would be locked in.
+            let rotation = if rotation_agrees {
+                alignment.rotation
+            } else {
+                self.filter.x.q * pose.rotation.inverse()
             };
+            let translation = self.filter.x.position - rotation * pose.translation.vector;
+            self.sources[source].alignment =
+                Some(Isometry3::from_parts(translation.into(), rotation));
+            self.sources[source].rejected_since_ns = None;
+            self.realigned += 1;
+            if !replaying {
+                warn!(
+                    source = %self.config.odom_sources[source].parent_frame_id,
+                    jump_m = position_residual.norm(),
+                    jump_rad = rotation_residual.norm(),
+                    rotation_realigned = !rotation_agrees,
+                    realign_after_seconds = realign_after,
+                    "pose source rejected for too long: re-aligned its frame to the fused pose",
+                );
+            }
         }
+    }
 
-        let Some(increment) = measurement.delta else {
-            self.anchor(measurement.source);
+    /// Stochastic cloning: the measured motion against the filter's own, from the clone taken
+    /// at the source's previous message to now, both still correctable.
+    fn fuse_pose_delta(&mut self, slot: usize, delta: &Isometry3<f64>, variance: &[f64; 6]) {
+        let Some(clone) = self.filter.x.clones[slot].clone() else {
             return;
         };
-        let current =
-            Isometry3::from_parts(Translation3::from(self.filter.x.position), self.filter.x.q);
-        // Back from a silence the others carried, its delta spans the gap: apply it from where
-        // the fused pose stood at its last message, not on top of what the others added since.
-        let start = match (returning, self.anchors[measurement.source]) {
-            (false, _) => current,
-            (true, Some(anchor)) => anchor,
-            (true, None) => {
-                self.anchor(measurement.source);
-                return;
+        let layout = self.filter.layout;
+        let clone_from_world = clone.q.inverse().to_rotation_matrix();
+        let predicted_translation = clone_from_world * (self.filter.x.position - clone.position);
+        let predicted_rotation = clone.q.inverse() * self.filter.x.q;
+        let translation_residual = delta.translation.vector - predicted_translation;
+        let rotation_residual = (predicted_rotation.inverse() * delta.rotation).scaled_axis();
+        let translation_skew = eskf::skew(&predicted_translation);
+        let predicted_rotation_transposed = predicted_rotation.inverse().to_rotation_matrix();
+        let clone_position = layout.clone_position_col(slot);
+        let clone_rotation = layout.clone_rotation_col(slot);
+        let mut rows = Rows::new(self.filter.dim());
+        for dim in 0..6 {
+            if variance[dim] <= 0.0 || self.pinned(dim) {
+                continue;
             }
-        };
-        let step = current.inverse() * (start * increment);
-
-        let mut share = [0.0; 6];
-        for (dim, share_dim) in share.iter_mut().enumerate() {
-            let total: f64 = self
-                .activity
-                .iter()
-                .filter(|a| a.last_ns >= horizon)
-                .map(|a| a.inverse_variance[dim])
-                .sum();
-            if total > 0.0 {
-                *share_dim = self.activity[measurement.source].inverse_variance[dim] / total;
+            if dim < 3 {
+                let row = rows.push(translation_residual[dim], variance[dim]);
+                for col in 0..3 {
+                    row[col] = clone_from_world.matrix()[(dim, col)];
+                    row[clone_position + col] = -clone_from_world.matrix()[(dim, col)];
+                    row[clone_rotation + col] = translation_skew[(dim, col)];
+                }
+            } else {
+                let axis = dim - 3;
+                let row = rows.push(rotation_residual[axis], variance[dim]);
+                row[6 + axis] = 1.0;
+                for col in 0..3 {
+                    row[clone_rotation + col] =
+                        -predicted_rotation_transposed.matrix()[(axis, col)];
+                }
             }
         }
+        if !rows.is_empty() {
+            self.update(&rows);
+        }
+    }
 
-        let translation = step.translation.vector;
-        let rotation = step.rotation.scaled_axis();
-        self.filter.x.position += self.filter.x.q
-            * Vector3::new(
-                share[0] * translation.x,
-                share[1] * translation.y,
-                share[2] * translation.z,
-            );
-        self.filter.x.q *= UnitQuaternion::from_scaled_axis(Vector3::new(
-            share[3] * rotation.x,
-            share[4] * rotation.y,
-            share[5] * rotation.z,
-        ));
-        self.anchor(measurement.source);
+    fn fuse_twist(&mut self, twist: &Twist, variance: &[f64; 6]) {
+        let rows = self.twist_rows(twist, variance);
+        if !rows.is_empty() {
+            self.update(&rows);
+        }
     }
 
-    /// Body-frame twist rows. Angular rate is not a state: it is predicted from the last gyro
-    /// sample, so the gyro-bias row belongs to the IMU that sample came from.
-    fn add_twist_rows(
-        &self,
-        linear: &Vector3<f64>,
-        angular: &Vector3<f64>,
-        variance: &[f64; 6],
-        rows: &mut Vec<DVector<f64>>,
-        residuals: &mut Vec<f64>,
-        variances: &mut Vec<f64>,
-    ) {
+    /// Body-frame twist rows, against the rate state or the held gyro sample.
+    fn twist_rows(&self, twist: &Twist, variance: &[f64; 6]) -> Rows {
         let body_from_world = self.filter.x.q.inverse().to_rotation_matrix();
         let body_velocity = body_from_world * self.filter.x.velocity;
         let velocity_skew = eskf::skew(&body_velocity);
-        for dim in 0..3 {
-            if variance[dim] > 0.0 {
-                let mut row = DVector::zeros(self.filter.dim());
+        let (predicted_angular, angular_correction) = self.filter.angular_velocity();
+        let mut rows = Rows::new(self.filter.dim());
+        for dim in 0..6 {
+            if variance[dim] <= 0.0 || self.pinned(dim) {
+                continue;
+            }
+            if dim < 3 {
+                let row = rows.push(twist.linear[dim] - body_velocity[dim], variance[dim]);
                 for col in 0..3 {
                     row[3 + col] = body_from_world.matrix()[(dim, col)];
                     row[6 + col] = velocity_skew[(dim, col)];
                 }
-                rows.push(row);
-                residuals.push(linear[dim] - body_velocity[dim]);
-                variances.push(variance[dim]);
-            }
-        }
-        let gyro_bias = self.filter.x.biases[self.last_gyro_imu].gyro;
-        let predicted_angular = self.last_gyro - gyro_bias;
-        for dim in 0..3 {
-            if variance[3 + dim] > 0.0 {
-                let mut row = DVector::zeros(self.filter.dim());
-                row[gyro_bias_col(self.last_gyro_imu) + dim] = -1.0;
-                rows.push(row);
-                residuals.push(angular[dim] - predicted_angular[dim]);
-                variances.push(variance[3 + dim]);
+            } else if let Some((col, sign)) = angular_correction {
+                let axis = dim - 3;
+                let row = rows.push(twist.angular[axis] - predicted_angular[axis], variance[dim]);
+                row[col + axis] = sign;
             }
         }
+        rows
     }
 
-    fn update(&mut self, rows: &[DVector<f64>], residuals: &[f64], variances: &[f64]) -> bool {
-        if rows.is_empty() {
-            return false;
-        }
-        let residual = DVector::from_row_slice(residuals);
-        let variance = DVector::from_row_slice(variances);
-        let mut jacobian = Jacobian::zeros(rows.len(), self.filter.dim());
-        for (index, row) in rows.iter().enumerate() {
-            jacobian.row_mut(index).copy_from(&row.transpose());
-        }
+    fn within_gate(&self, rows: &Rows) -> bool {
+        let (residual, jacobian, variance) = rows.matrices();
+        self.filter.within_gate(
+            &residual,
+            &jacobian,
+            &variance,
+            self.config.outlier_rejection_allowed_variance,
+        )
+    }
+
+    fn update(&mut self, rows: &Rows) -> bool {
+        let (residual, jacobian, variance) = rows.matrices();
         let accepted = self.filter.update(
             &residual,
             &jacobian,
             &variance,
-            // The filter gates in standard deviations; the config speaks variance.
-            self.config.outlier_rejection_allowed_variance.sqrt(),
+            self.config.outlier_rejection_allowed_variance,
         );
         if !accepted {
             self.gated += 1;
@@ -804,28 +968,16 @@ impl FusionCore {
                 gated = self.gated,
                 outlier_rejection_allowed_variance = self.config.outlier_rejection_allowed_variance,
                 "measurement rejected by the outlier gate. Raise outlier_rejection_allowed_variance, \
-                 or \
-                 correct the source's pose_variances / twist_variances.",
+                 or correct the source's pose_variances / twist_variances.",
             );
         }
         accepted
     }
 
-    fn anchor(&mut self, source: usize) {
-        self.anchors[source] = Some(Isometry3::from_parts(
-            Translation3::from(self.filter.x.position),
-            self.filter.x.q,
-        ));
-    }
-
     fn snapshot(&self, event: &mut Event) {
         event.state = self.filter.x.clone();
         event.covariance = self.filter.p_cov.clone();
-        event.anchors = self.anchors.clone();
-        event.activity = self.activity.clone();
-        event.last_imu_ns = self.last_imu_ns;
-        event.last_gyro = self.last_gyro;
-        event.last_gyro_imu = self.last_gyro_imu;
+        event.sources = self.sources.clone();
     }
 
     fn state_is_publishable(&self, state: &State) -> bool {
@@ -847,39 +999,54 @@ impl FusionCore {
         finite && capped
     }
 
+    /// The fused estimate at the newest input's stamp, once a publish period has elapsed on
+    /// the data clock.
     pub fn maybe_publish(&mut self) -> Option<OdometryEstimate> {
-        let latest = self.events.back()?;
+        let latest_ns = self.events.back()?.ts_ns;
         let period = (1.0e9 / self.config.publish_rate.max(1.0)) as i64;
-        if latest.ts_ns - self.last_publish_ns < period {
+        if latest_ns - self.last_publish_ns < period {
+            return None;
+        }
+        self.publish(latest_ns)
+    }
+
+    /// The fused estimate predicted to `now_ns`, for a caller on a timer: the timer sets the
+    /// cadence, so output is steady however sparse the sources. None if `now_ns` is not past
+    /// the last output, or past the replay buffer, where every source has gone quiet for
+    /// longer than it may lag.
+    pub fn maybe_publish_at(&mut self, now_ns: i64) -> Option<OdometryEstimate> {
+        let latest_ns = self.events.back()?.ts_ns;
+        let stale = (now_ns - latest_ns) as f64 > self.config.replay_buffer_seconds * 1.0e9;
+        if now_ns <= self.last_publish_ns || stale {
             return None;
         }
-        self.last_publish_ns = latest.ts_ns;
-        let state = latest.state.clone();
-        if !self.state_is_publishable(&state) {
+        self.publish(now_ns.max(latest_ns))
+    }
+
+    fn publish(&mut self, ts_ns: i64) -> Option<OdometryEstimate> {
+        let latest = self.events.back()?;
+        self.last_publish_ns = ts_ns;
+        let mut filter = self.filter.clone();
+        filter.x = latest.state.clone();
+        filter.p_cov = latest.covariance.clone();
+        filter.predict_to(ts_ns);
+        if !self.state_is_publishable(&filter.x) {
             return None;
         }
-        let covariance = latest.covariance.clone();
-        let last_gyro = latest.last_gyro;
-        let last_gyro_imu = latest.last_gyro_imu;
-        let ts_ns = latest.ts_ns;
+        let state = &filter.x;
+        let covariance = &filter.p_cov;
 
         let body_from_world = state.q.inverse().to_rotation_matrix();
         let body_velocity = body_from_world * state.velocity;
-        // The predicted rate is the last gyro sample minus that IMU's own bias; without an
-        // IMU there is no rate estimate at all.
-        let gyro_bias = state
-            .biases
-            .get(last_gyro_imu)
-            .map(|bias| bias.gyro)
-            .unwrap_or_default();
-        let body_angular = last_gyro - gyro_bias;
-
+        let (body_angular, angular_correction) = filter.angular_velocity();
         let velocity_world = covariance.view((3, 3), (3, 3)).into_owned();
         let velocity_body =
             body_from_world.matrix() * velocity_world * body_from_world.matrix().transpose();
+        let angular_body: Matrix3<f64> = angular_correction
+            .map(|(col, _)| covariance.fixed_view::<3, 3>(col, col).into_owned())
+            .unwrap_or_default();
         let mut pose_covariance = Matrix6::zeros();
         let mut twist_covariance = Matrix6::zeros();
-        let angular_block = (!state.biases.is_empty()).then(|| gyro_bias_col(last_gyro_imu));
         for row in 0..3 {
             for col in 0..3 {
                 pose_covariance[(row, col)] = covariance[(row, col)];
@@ -887,9 +1054,7 @@ impl FusionCore {
                 pose_covariance[(row + 3, col)] = covariance[(row + 6, col)];
                 pose_covariance[(row + 3, col + 3)] = covariance[(row + 6, col + 6)];
                 twist_covariance[(row, col)] = velocity_body[(row, col)];
-                if let Some(block) = angular_block {
-                    twist_covariance[(row + 3, col + 3)] = covariance[(block + row, block + col)];
-                }
+                twist_covariance[(row + 3, col + 3)] = angular_body[(row, col)];
             }
         }
 
@@ -912,6 +1077,7 @@ impl FusionCore {
             imu_samples = self.imu_samples,
             measurements = self.measurements,
             gated = self.gated,
+            realigned = self.realigned,
             replayed = self.replayed,
             too_late = self.too_late,
             "odometry fusion counters"
@@ -919,12 +1085,61 @@ impl FusionCore {
     }
 }
 
+/// One measurement's rows, built up dimension by dimension.
+struct Rows {
+    dim: usize,
+    rows: Vec<DVector<f64>>,
+    residuals: Vec<f64>,
+    variances: Vec<f64>,
+}
+
+impl Rows {
+    fn new(dim: usize) -> Self {
+        Self {
+            dim,
+            rows: Vec::new(),
+            residuals: Vec::new(),
+            variances: Vec::new(),
+        }
+    }
+
+    fn push(&mut self, residual: f64, variance: f64) -> &mut DVector<f64> {
+        self.residuals.push(residual);
+        self.variances.push(variance);
+        self.rows.push(DVector::zeros(self.dim));
+        self.rows.last_mut().expect("just pushed")
+    }
+
+    fn append(&mut self, mut other: Rows) {
+        self.rows.append(&mut other.rows);
+        self.residuals.append(&mut other.residuals);
+        self.variances.append(&mut other.variances);
+    }
+
+    fn is_empty(&self) -> bool {
+        self.rows.is_empty()
+    }
+
+    fn matrices(&self) -> (DVector<f64>, Jacobian, DVector<f64>) {
+        let mut jacobian = Jacobian::zeros(self.rows.len(), self.dim);
+        for (index, row) in self.rows.iter().enumerate() {
+            jacobian.row_mut(index).copy_from(&row.transpose());
+        }
+        (
+            DVector::from_column_slice(&self.residuals),
+            jacobian,
+            DVector::from_column_slice(&self.variances),
+        )
+    }
+}
+
 #[cfg(test)]
 mod tests {
     use super::*;
     use nalgebra::Quaternion;
 
     const GRAVITY: f64 = 9.80665;
+    const MS: i64 = 1_000_000;
 
     fn base_config(use_imu: bool) -> OdometryFusionConfig {
         OdometryFusionConfig {
@@ -941,8 +1156,10 @@ mod tests {
             },
             initial_gravity_estimate: GRAVITY,
             initial_stds: InitialStds::default(),
-            odom_sources: one_source("odom"),
-            per_dimension_error_variance: [0.0; 6],
+            process_noise: ProcessNoise::default(),
+            odom_sources: vec![source("odom", SourceKind::Pose)],
+            planar: false,
+            zero_velocity_variances: [0.0; 6],
         }
     }
 
@@ -958,16 +1175,23 @@ mod tests {
         }
     }
 
-    fn one_source(parent_frame_id: &str) -> Vec<SourceConfig> {
-        vec![SourceConfig {
+    fn source(parent_frame_id: &str, kind: SourceKind) -> SourceConfig {
+        let (pose_variances, twist_variances) = match kind {
+            SourceKind::Twist => ([0.0; 6], [1e-4; 6]),
+            _ => ([1e-4; 6], [0.0; 6]),
+        };
+        SourceConfig {
             parent_frame_id: parent_frame_id.to_string(),
             child_frame_id: "base".to_string(),
-            pose_variances: [1e-4; 6],
-            twist_variances: [0.0; 6],
-        }]
+            kind,
+            pose_variances,
+            twist_variances,
+            realign_after_seconds: 1.0,
+            ..Default::default()
+        }
     }
 
-    fn source_message(parent_frame_id: &str, ts_ns: i64, x: f64) -> OdometryEstimate {
+    fn pose_message(parent_frame_id: &str, ts_ns: i64, x: f64) -> OdometryEstimate {
         OdometryEstimate {
             timestamp_ns: ts_ns,
             frame_id: parent_frame_id.to_string(),
@@ -977,6 +1201,16 @@ mod tests {
         }
     }
 
+    fn twist_message(parent_frame_id: &str, ts_ns: i64, vx: f64, wz: f64) -> OdometryEstimate {
+        OdometryEstimate {
+            twist: Twist {
+                linear: Vector3::new(vx, 0.0, 0.0),
+                angular: Vector3::new(0.0, 0.0, wz),
+            },
+            ..pose_message(parent_frame_id, ts_ns, 0.0)
+        }
+    }
+
     fn imu_message(ts_ns: i64, gyro_z: f64, accel_x: f64) -> ImuSample {
         ImuSample {
             timestamp_ns: ts_ns,
@@ -986,8 +1220,14 @@ mod tests {
         }
     }
 
-    fn identity_mount() -> Isometry3<f64> {
-        Isometry3::identity()
+    fn init_imu(core: &mut FusionCore) {
+        for sample in 0..5 {
+            core.handle_imu(
+                &imu_message(sample * 10 * MS, 0.0, 0.0),
+                &Isometry3::identity(),
+            );
+        }
+        assert!(core.initialized);
     }
 
     fn published_position(core: &mut FusionCore) -> Option<Vector3<f64>> {
@@ -995,77 +1235,354 @@ mod tests {
     }
 
     #[test]
-    fn a_single_source_without_an_imu_drives_the_fused_pose() {
+    fn a_pose_source_aligns_on_its_first_message_and_is_followed_after() {
         let mut core = FusionCore::new(base_config(false));
-        core.handle_source(&source_message("odom", 0, 0.0));
-        core.handle_source(&source_message("odom", NS_PER_SEC / 10, 1.0));
+        core.handle_source(&pose_message("odom", 0, 5.0));
+        for step in 1..=20 {
+            core.handle_source(&pose_message(
+                "odom",
+                step * 50 * MS,
+                5.0 + step as f64 * 0.05,
+            ));
+        }
         let position = published_position(&mut core).expect("a period has elapsed");
-        assert!((position.x - 1.0).abs() < 1e-12);
+        assert!((position.x - 1.0).abs() < 0.01, "{position}");
     }
 
     #[test]
-    fn a_drifting_source_contributes_only_its_delta() {
+    fn a_pose_delta_source_contributes_only_its_motion() {
         let mut config = base_config(false);
-        config.odom_sources = one_source("visual_odom");
+        config.odom_sources = vec![source("visual_odom", SourceKind::PoseDelta)];
         let mut core = FusionCore::new(config);
-        core.handle_source(&source_message("visual_odom", 0, 10.0));
-        core.handle_source(&source_message("visual_odom", NS_PER_SEC / 10, 10.5));
+        let mut halfway = 0.0;
+        for step in 0..=20 {
+            let x = 10.0 + step as f64 * 0.05;
+            core.handle_source(&pose_message("visual_odom", step * 50 * MS, x));
+            if step == 10 {
+                halfway = core.filter.x.position.x;
+            }
+        }
         let position = published_position(&mut core).expect("a period has elapsed");
-        assert!((position.x - 0.5).abs() < 1e-12);
+        // The first steps fight the prior's zero velocity; once it has converged the motion is the source's.
+        assert!(position.x < 1.0, "{position}");
+        assert!(
+            (position.x - halfway - 0.5).abs() < 0.005,
+            "{position} {halfway}"
+        );
+    }
+
+    #[test]
+    fn a_twist_source_is_integrated_through_the_velocity_state() {
+        let mut config = base_config(false);
+        config.odom_sources = vec![source("wheel", SourceKind::Twist)];
+        let mut core = FusionCore::new(config);
+        for step in 0..=50 {
+            core.handle_source(&twist_message("wheel", step * 20 * MS, 1.0, 0.5));
+        }
+        let msg = core.maybe_publish().expect("a period has elapsed");
+        // The velocity state has to converge first, so the start lags a little.
+        assert!(
+            (msg.pose.translation.x - 0.95).abs() < 0.1,
+            "{}",
+            msg.pose.translation.x
+        );
+        assert!((msg.pose.rotation.euler_angles().2 - 0.5).abs() < 0.05);
+        assert!((msg.twist.linear.x - 1.0).abs() < 0.01);
+        assert!((msg.twist.angular.z - 0.5).abs() < 0.01);
+    }
+
+    #[test]
+    fn a_zero_velocity_variance_keeps_a_base_from_sliding_sideways() {
+        let lateral_after_one_second = |constraint: f64| {
+            let mut config = base_config(false);
+            let mut wheel = source("wheel", SourceKind::Twist);
+            wheel.twist_variances = [1e-2, 1.0, 0.0, 0.0, 0.0, 1e-2];
+            config.odom_sources = vec![wheel];
+            config.zero_velocity_variances = [0.0, constraint, 0.0, 0.0, 0.0, 0.0];
+            let mut core = FusionCore::new(config);
+            for step in 0..=50 {
+                let mut msg = twist_message("wheel", step * 20 * MS, 1.0, 0.0);
+                msg.twist.linear.y = 0.5;
+                core.handle_source(&msg);
+            }
+            core.maybe_publish()
+                .expect("a period has elapsed")
+                .pose
+                .translation
+                .y
+        };
+        let free = lateral_after_one_second(0.0);
+        let constrained = lateral_after_one_second(1e-4);
+        assert!(free > 0.3, "{free}");
+        assert!(constrained.abs() < 0.01, "{constrained}");
+    }
+
+    #[test]
+    fn with_an_imu_a_zero_rate_constraint_leaves_the_gyro_bias_alone() {
+        let mut config = base_config(true);
+        config.odom_sources = vec![source("wheel", SourceKind::Twist)];
+        config.zero_velocity_variances = [0.0, 0.0, 0.0, 1e-6, 1e-6, 0.0];
+        let mut core = FusionCore::new(config);
+        init_imu(&mut core);
+        for step in 1..=100 {
+            let ts_ns = 40 * MS + step * 10 * MS;
+            let mut rocking = imu_message(ts_ns, 0.0, 0.0);
+            rocking.angular_velocity.x = 0.2 * if step % 20 < 10 { 1.0 } else { -1.0 };
+            core.handle_imu(&rocking, &Isometry3::identity());
+            core.handle_source(&twist_message("wheel", ts_ns, 0.0, 0.0));
+        }
+        let gyro_bias = core.filter.x.biases[0].gyro;
+        assert!(gyro_bias.x.abs() < 1e-3, "{gyro_bias:?}");
+    }
+
+    #[test]
+    fn a_still_twist_source_holds_the_pose_against_a_noisy_pose_source() {
+        let spread = |still_variance: f64| {
+            let mut config = base_config(false);
+            config.publish_rate = 50.0;
+            let mut wheel = source("wheel", SourceKind::Twist);
+            wheel.still_variance = still_variance;
+            wheel.still_speed = 1e-3;
+            wheel.still_angular_speed = 1e-3;
+            config.odom_sources = vec![source("odom", SourceKind::Pose), wheel];
+            let mut core = FusionCore::new(config);
+            let mut xs = Vec::new();
+            for step in 0..500 {
+                let ts_ns = step * 20 * MS;
+                core.handle_twist_source_still(ts_ns);
+                if step % 5 == 0 {
+                    // Point-LIO-like noise: +-2 mm, alternating.
+                    let noise = if step % 10 == 0 { 0.002 } else { -0.002 };
+                    core.handle_source(&pose_message("odom", ts_ns, noise));
+                }
+                if step >= 250 {
+                    xs.push(core.maybe_publish_at(ts_ns + MS).map(|m| m.pose.translation.x));
+                }
+            }
+            let xs: Vec<f64> = xs.into_iter().flatten().collect();
+            xs.iter().cloned().fold(f64::MIN, f64::max) - xs.iter().cloned().fold(f64::MAX, f64::min)
+        };
+        let free = spread(0.0);
+        let held = spread(1e-6);
+        assert!(held < free / 5.0, "held {held} free {free}");
+    }
+
+    impl FusionCore {
+        fn handle_twist_source_still(&mut self, ts_ns: i64) {
+            self.handle_source(&twist_message("wheel", ts_ns, 0.0, 0.0));
+        }
     }
 
     #[test]
-    fn a_source_back_from_a_silence_does_not_add_the_motion_the_others_carried() {
+    fn output_between_sparse_sources_is_predicted_to_the_publish_time() {
         let mut config = base_config(false);
-        let mut wheel = one_source("wheel");
-        wheel[0].pose_variances = [1e-2; 6];
-        config.odom_sources = [one_source("lio"), wheel].concat();
+        config.publish_rate = 100.0;
+        config.odom_sources = vec![source("wheel", SourceKind::Twist)];
         let mut core = FusionCore::new(config);
-        for step in 0..=10 {
-            let ts_ns = step * NS_PER_SEC / 10;
-            let x = step as f64 / 10.0;
-            core.handle_source(&source_message("wheel", ts_ns, x));
-            // Silent from 0.2 s to 1.0 s, past the activity timeout.
-            if step <= 1 || step == 10 {
-                core.handle_source(&source_message("lio", ts_ns, x));
+        for step in 0..=20 {
+            core.handle_source(&twist_message("wheel", step * 100 * MS, 1.0, 0.0));
+        }
+        let at_source = core.maybe_publish().expect("a period has elapsed");
+        let between = core
+            .maybe_publish_at(2_050 * MS)
+            .expect("a period has elapsed");
+        let step = between.pose.translation.x - at_source.pose.translation.x;
+        assert!((step - 0.05).abs() < 0.005, "{step}");
+        assert_eq!(between.timestamp_ns, 2_050 * MS);
+        assert!(
+            core.maybe_publish_at(5_000 * MS).is_none(),
+            "every source has been silent past the replay buffer"
+        );
+    }
+
+    #[test]
+    fn planar_keeps_z_roll_and_pitch_at_zero_without_and_with_an_imu() {
+        for use_imu in [false, true] {
+            let mut config = base_config(use_imu);
+            config.planar = true;
+            config.odom_sources = vec![source("visual_odom", SourceKind::PoseDelta)];
+            let mut core = FusionCore::new(config);
+            if use_imu {
+                init_imu(&mut core);
             }
+            let tilt = UnitQuaternion::from_euler_angles(0.1, -0.05, 0.0);
+            for step in 0..=20 {
+                let ts_ns = 50 * MS + step * 50 * MS;
+                if use_imu {
+                    core.handle_imu(&imu_message(ts_ns, 0.0, 0.0), &Isometry3::identity());
+                }
+                let mut msg = pose_message("visual_odom", ts_ns, step as f64 * 0.05);
+                msg.pose.translation.z = step as f64 * 0.1;
+                msg.pose.rotation = tilt.powf(step as f64);
+                core.handle_source(&msg);
+            }
+            let msg = core.maybe_publish().expect("a period has elapsed");
+            let (roll, pitch, _) = msg.pose.rotation.euler_angles();
+            assert_eq!(msg.pose.translation.z, 0.0, "use_imu {use_imu}");
+            assert!(
+                roll.abs() < 1e-12 && pitch.abs() < 1e-12,
+                "use_imu {use_imu}"
+            );
+            assert!(msg.pose.translation.x > 0.5, "use_imu {use_imu}");
         }
-        // Not 1.9: the 0.8 s the wheels carried is not counted twice.
-        let x = core.filter.x.position.x;
-        assert!((x - 1.0).abs() < 0.02, "{x}");
+    }
+
+    /// Wheels at 50 Hz carry the pose through a silence of the pose source.
+    fn wheels_and_pose(pose_at: impl Fn(i64) -> Option<Isometry3<f64>>) -> FusionCore {
+        let mut config = base_config(false);
+        let mut pose = source("lio", SourceKind::Pose);
+        pose.pose_variances = [1e-3; 6];
+        config.odom_sources = vec![pose, source("wheel", SourceKind::Twist)];
+        let mut core = FusionCore::new(config);
+        for step in 0..=300_i64 {
+            let ts_ns = step * 20 * MS;
+            core.handle_source(&twist_message("wheel", ts_ns, 0.5, 0.0));
+            if step % 5 == 0 {
+                if let Some(pose) = pose_at(ts_ns) {
+                    core.handle_source(&OdometryEstimate {
+                        pose,
+                        ..pose_message("lio", ts_ns, 0.0)
+                    });
+                }
+            }
+        }
+        core
+    }
+
+    #[test]
+    fn the_twist_carries_a_dropout_of_the_pose_source_which_is_taken_back_after() {
+        let truth = |ts_ns: i64| 0.5 * ts_ns as f64 / 1.0e9;
+        let silent = |ts_ns: i64| (2_000 * MS..4_000 * MS).contains(&ts_ns);
+        let core = wheels_and_pose(|ts_ns| {
+            (!silent(ts_ns)).then(|| Isometry3::translation(truth(ts_ns), 0.0, 0.0))
+        });
+        assert_eq!(core.realigned, 0);
+        assert_eq!(core.gated, 0);
+        assert!((core.filter.x.position.x - truth(6_000 * MS)).abs() < 0.01);
+    }
+
+    #[test]
+    fn a_pose_source_that_jumps_is_gated_then_realigned_without_moving_the_output() {
+        // A 5 m jump in position at 3 s, then one of half a radian in heading at 4.5 s.
+        let jumped = |ts_ns: i64| {
+            let x = 0.5 * ts_ns as f64 / 1.0e9;
+            let shift = if ts_ns >= 3_000 * MS { 5.0 } else { 0.0 };
+            let turn = if ts_ns >= 4_500 * MS { 0.5 } else { 0.0 };
+            Isometry3::from_parts(
+                Translation3::new(x + shift, 0.0, 0.0),
+                UnitQuaternion::from_euler_angles(0.0, 0.0, turn),
+            )
+        };
+        let core = wheels_and_pose(|ts_ns| Some(jumped(ts_ns)));
+        assert_eq!(core.realigned, 2);
+        // The wheels agree with the motion before and after, so the output never jumps.
+        assert!(
+            (core.filter.x.position.x - 3.0).abs() < 0.01,
+            "{}",
+            core.filter.x.position.x
+        );
+        // Not the half radian: only the wheels' own heading drift through the rejected second.
+        assert!(
+            core.filter.x.q.angle() < 0.02,
+            "{}",
+            core.filter.x.q.angle()
+        );
+        let reaccepted = core.sources[0].rejected_since_ns.is_none();
+        assert!(reaccepted, "the source is fused again after the re-align");
+    }
+
+    #[test]
+    fn a_late_message_replays_to_the_same_state_as_one_in_order() {
+        let run = |late: bool| {
+            let mut config = base_config(false);
+            config.odom_sources = vec![
+                source("lio", SourceKind::Pose),
+                source("wheel", SourceKind::Twist),
+            ];
+            let mut core = FusionCore::new(config);
+            let mut held = None;
+            for step in 0..=50_i64 {
+                let ts_ns = step * 20 * MS;
+                if step % 5 == 0 {
+                    let lio = pose_message("lio", ts_ns, 0.3 * ts_ns as f64 / 1.0e9);
+                    if late && step == 25 {
+                        held = Some(lio);
+                    } else {
+                        core.handle_source(&lio);
+                    }
+                }
+                core.handle_source(&twist_message("wheel", ts_ns, 0.3, 0.0));
+                if step == 30 {
+                    if let Some(lio) = held.take() {
+                        core.handle_source(&lio);
+                    }
+                }
+            }
+            (
+                core.filter.x.position,
+                core.filter.x.velocity,
+                core.replayed,
+            )
+        };
+        let (in_order, in_order_velocity, _) = run(false);
+        let (late, late_velocity, replayed) = run(true);
+        assert_eq!(replayed, 1);
+        assert_eq!(in_order, late);
+        assert_eq!(in_order_velocity, late_velocity);
     }
 
     #[test]
     fn a_source_from_an_unconfigured_frame_is_ignored() {
         let mut core = FusionCore::new(base_config(false));
-        core.handle_source(&source_message("odom", 0, 0.0));
-        core.handle_source(&source_message("odom", NS_PER_SEC / 10, 1.0));
-        core.handle_source(&source_message("stray", NS_PER_SEC / 5, 99.0));
+        core.handle_source(&pose_message("odom", 0, 0.0));
+        core.handle_source(&pose_message("odom", 100 * MS, 0.0));
+        core.handle_source(&pose_message("stray", 150 * MS, 99.0));
         let position = published_position(&mut core).expect("a period has elapsed");
-        assert!((position.x - 1.0).abs() < 1e-12);
+        assert!(position.x.abs() < 1e-12);
     }
 
     #[test]
     fn a_matching_parent_with_the_wrong_child_is_ignored() {
         let mut core = FusionCore::new(base_config(false));
-        core.handle_source(&source_message("odom", 0, 0.0));
-        core.handle_source(&source_message("odom", NS_PER_SEC / 10, 1.0));
-        let mut wrong_child = source_message("odom", NS_PER_SEC / 5, 99.0);
+        core.handle_source(&pose_message("odom", 0, 0.0));
+        core.handle_source(&pose_message("odom", 100 * MS, 0.0));
+        let mut wrong_child = pose_message("odom", 150 * MS, 99.0);
         wrong_child.child_frame_id = "some_other_link".to_string();
         core.handle_source(&wrong_child);
         let position = published_position(&mut core).expect("a period has elapsed");
-        assert!((position.x - 1.0).abs() < 1e-12);
+        assert!(position.x.abs() < 1e-12);
     }
 
     #[test]
-    fn an_odom_sources_entry_deserializes_with_defaulted_variances() {
+    fn an_odom_sources_entry_deserializes_with_defaulted_kind_and_variances() {
         let parsed: Vec<SourceConfig> = serde_json::from_str(
-            r#"[{"parent_frame_id": "visual_odom", "child_frame_id": "base"}]"#,
+            r#"[{"parent_frame_id": "visual_odom", "child_frame_id": "base"},
+                {"parent_frame_id": "wheel", "child_frame_id": "base", "kind": "twist"}]"#,
         )
         .expect("deserializable");
-        assert_eq!(parsed[0].parent_frame_id, "visual_odom");
-        assert_eq!(parsed[0].child_frame_id, "base");
+        assert_eq!(parsed[0].kind, SourceKind::PoseDelta);
         assert_eq!(parsed[0].pose_variances, [0.0; 6]);
+        assert_eq!(parsed[1].kind, SourceKind::Twist);
+    }
+
+    #[test]
+    fn a_config_key_this_filter_no_longer_has_is_refused_rather_than_ignored() {
+        let parsed: Result<SourceConfig, _> = serde_json::from_str(
+            r#"{"parent_frame_id": "odom", "child_frame_id": "base", "absolute": true}"#,
+        );
+        assert!(parsed.is_err());
+        let parsed: Result<OdometryFusionConfig, _> =
+            serde_json::from_str(r#"{"blend_weights": [0, 0, 1, 1, 1, 0]}"#);
+        assert!(parsed.is_err());
+    }
+
+    #[test]
+    #[should_panic(expected = "fuses no pose")]
+    fn a_twist_source_with_pose_variances_is_a_config_error() {
+        let mut config = base_config(false);
+        config.odom_sources[0].kind = SourceKind::Twist;
+        config.odom_sources[0].twist_variances = [1e-4; 6];
+        FusionCore::new(config).handle_source(&pose_message("odom", 0, 0.0));
     }
 
     #[test]
@@ -1074,7 +1591,7 @@ mod tests {
         let mut stray = imu_message(0, 0.0, 0.0);
         stray.frame_id = "some_other_imu".to_string();
         for _ in 0..20 {
-            core.handle_imu(&stray, &identity_mount());
+            core.handle_imu(&stray, &Isometry3::identity());
         }
         assert!(!core.initialized);
     }
@@ -1086,8 +1603,8 @@ mod tests {
         let mut core = FusionCore::new(config);
         for sample in 0..20 {
             core.handle_imu(
-                &imu_message(sample * NS_PER_SEC / 100, 0.0, 0.0),
-                &identity_mount(),
+                &imu_message(sample * 10 * MS, 0.0, 0.0),
+                &Isometry3::identity(),
             );
         }
         assert!(!core.initialized, "the second imu has produced nothing yet");
@@ -1099,12 +1616,12 @@ mod tests {
         config.imus.push(test_imu("imu2"));
         let mut core = FusionCore::new(config);
         for sample in 0..5_i64 {
-            let ts_ns = sample * NS_PER_SEC / 100;
-            core.handle_imu(&imu_message(ts_ns, 0.0, 0.0), &identity_mount());
-            let mut second = imu_message(ts_ns + NS_PER_SEC / 200, 0.0, 0.0);
+            let ts_ns = sample * 10 * MS;
+            core.handle_imu(&imu_message(ts_ns, 0.0, 0.0), &Isometry3::identity());
+            let mut second = imu_message(ts_ns + 5 * MS, 0.0, 0.0);
             second.frame_id = "imu2".to_string();
             second.angular_velocity = Vector3::new(0.02, 0.0, 0.0); // this gyro alone is biased
-            core.handle_imu(&second, &identity_mount());
+            core.handle_imu(&second, &Isometry3::identity());
         }
         assert!(core.initialized);
         assert_eq!(core.filter.x.biases.len(), 2);
@@ -1115,19 +1632,25 @@ mod tests {
     #[test]
     fn a_nan_pose_is_dropped_rather_than_wedging_the_filter() {
         let mut config = base_config(false);
-        config.odom_sources = one_source("visual_odom");
+        config.odom_sources = vec![source("visual_odom", SourceKind::PoseDelta)];
         let mut core = FusionCore::new(config);
-        core.handle_source(&source_message("visual_odom", 0, 10.0));
-
-        let mut nan_pose = source_message("visual_odom", NS_PER_SEC / 20, 10.25);
-        nan_pose.pose.rotation =
-            UnitQuaternion::from_quaternion(Quaternion::new(0.0, 0.0, 0.0, 0.0));
-        assert!(nan_pose.pose.rotation.coords.iter().any(|v| v.is_nan()));
-        core.handle_source(&nan_pose);
-
-        core.handle_source(&source_message("visual_odom", NS_PER_SEC / 10, 10.5));
+        let mut before = 0.0;
+        for step in 0..=20 {
+            let mut msg = pose_message("visual_odom", step * 50 * MS, 10.0 + step as f64 * 0.05);
+            if step == 10 {
+                before = core.filter.x.position.x;
+                msg.pose.rotation =
+                    UnitQuaternion::from_quaternion(Quaternion::new(0.0, 0.0, 0.0, 0.0));
+                assert!(msg.pose.rotation.coords.iter().any(|v| v.is_nan()));
+            }
+            core.handle_source(&msg);
+        }
         let position = published_position(&mut core).expect("a period has elapsed");
-        assert!((position.x - 0.5).abs() < 1e-12);
+        // The delta after the NaN spans both steps, so nothing is lost across it.
+        assert!(
+            (position.x - before - 0.55).abs() < 0.005,
+            "{position} {before}"
+        );
     }
 
     #[test]
@@ -1135,76 +1658,78 @@ mod tests {
         let mut config = base_config(false);
         config.publish_rate = 10.0;
         let mut core = FusionCore::new(config);
-        core.handle_source(&source_message("odom", 0, 0.0));
+        core.handle_source(&pose_message("odom", 0, 0.0));
         assert!(core.maybe_publish().is_none());
-        core.handle_source(&source_message("odom", NS_PER_SEC / 20, 0.5));
+        core.handle_source(&pose_message("odom", 50 * MS, 0.0));
         assert!(core.maybe_publish().is_none());
-        core.handle_source(&source_message("odom", NS_PER_SEC / 10, 1.0));
+        core.handle_source(&pose_message("odom", 100 * MS, 0.0));
         let msg = core.maybe_publish().expect("a full period has elapsed");
         assert_eq!(msg.frame_id, "odom");
         assert_eq!(msg.child_frame_id, "base");
+        assert_eq!(msg.timestamp_ns, 100 * MS);
     }
 
     #[test]
     fn the_outlier_gate_rejects_a_wild_pose_and_accepts_a_consistent_one() {
-        let mut core = FusionCore::new(base_config(true));
-        for sample in 0..5 {
-            core.handle_imu(
-                &imu_message(sample * NS_PER_SEC / 100, 0.0, 0.0),
-                &identity_mount(),
-            );
-        }
-        // Two to anchor: the first delta needs a predecessor, the second adopts the filter pose.
-        core.handle_source(&source_message("odom", NS_PER_SEC / 20, 0.0));
-        core.handle_source(&source_message("odom", NS_PER_SEC / 10, 0.0));
-        core.handle_source(&source_message("odom", 3 * NS_PER_SEC / 20, 1000.0));
-        let after_outlier = published_position(&mut core).expect("a period has elapsed");
-        assert!(after_outlier.x.abs() < 1e-9);
-        // From the outlier's own pose, so this delta is the small one.
-        core.handle_source(&source_message("odom", NS_PER_SEC / 5, 1000.01));
-        let after_inlier = published_position(&mut core).expect("a period has elapsed");
-        assert!(after_inlier.x > 0.005);
+        let mut config = base_config(true);
+        config.odom_sources[0].realign_after_seconds = 0.0;
+        let mut core = FusionCore::new(config);
+        init_imu(&mut core);
+        core.handle_source(&pose_message("odom", 50 * MS, 0.0));
+        core.handle_source(&pose_message("odom", 100 * MS, 1000.0));
+        assert_eq!(core.gated, 1);
+        assert!(core.filter.x.position.x.abs() < 1e-6);
+        core.handle_source(&pose_message("odom", 150 * MS, 0.01));
+        assert_eq!(core.gated, 1);
+        assert!(core.filter.x.position.x > 0.005);
     }
 
     #[test]
     fn a_state_past_the_position_cap_is_held_back_unless_the_cap_is_zero() {
         let mut config = base_config(false);
+        config.odom_sources = vec![source("wheel", SourceKind::Twist)];
         config.max_position_m = 0.5;
-        let mut core = FusionCore::new(config.clone());
-        core.handle_source(&source_message("odom", 0, 0.0));
-        core.handle_source(&source_message("odom", NS_PER_SEC / 10, 1.0));
-        assert!(core.maybe_publish().is_none());
-
+        let run = |config: OdometryFusionConfig| {
+            let mut core = FusionCore::new(config);
+            for step in 0..=50 {
+                core.handle_source(&twist_message("wheel", step * 20 * MS, 1.0, 0.0));
+            }
+            core.maybe_publish()
+        };
+        assert!(run(config.clone()).is_none());
         config.max_position_m = 0.0;
-        let mut core = FusionCore::new(config);
-        core.handle_source(&source_message("odom", 0, 0.0));
-        core.handle_source(&source_message("odom", NS_PER_SEC / 10, 1.0));
-        assert!(core.maybe_publish().is_some());
+        assert!(run(config).is_some());
     }
 
     #[test]
     fn the_same_imu_and_source_sequence_replays_bit_identically() {
-        let mount = identity_mount();
         let run = || {
             let mut config = base_config(true);
             // Position only: the source reports no rotation, so it must not argue against the yaw.
             config.odom_sources[0].pose_variances = [1e-4, 1e-4, 1e-4, 0.0, 0.0, 0.0];
+            config.odom_sources.push(source("wheel", SourceKind::Twist));
+            config.odom_sources[1].twist_variances = [1e-2, 1e-2, 0.0, 0.0, 0.0, 0.0];
             let mut core = FusionCore::new(config);
             let mut published = Vec::new();
             for step in 0..60_i64 {
-                let ts_ns = step * NS_PER_SEC / 100;
+                let ts_ns = step * 10 * MS;
                 let moving = step >= 10;
                 let gyro_z = if moving { 0.3 } else { 0.0 };
                 let accel_x = if moving { 0.5 } else { 0.0 };
-                core.handle_imu(&imu_message(ts_ns, gyro_z, accel_x), &mount);
+                core.handle_imu(&imu_message(ts_ns, gyro_z, accel_x), &Isometry3::identity());
                 if step % 10 == 0 {
-                    core.handle_source(&source_message("odom", ts_ns, step as f64 * 0.02));
+                    core.handle_source(&pose_message("odom", ts_ns, step as f64 * 0.02));
+                }
+                if step % 4 == 0 {
+                    // Slightly late, so the run replays as well.
+                    core.handle_source(&twist_message("wheel", ts_ns - 15 * MS, 0.1, 0.0));
                 }
                 if let Some(msg) = core.maybe_publish() {
                     let position = msg.pose.translation.vector;
                     published.push((position.x, position.y, position.z, msg.pose.rotation.w));
                 }
             }
+            assert!(core.replayed > 0);
             published
         };
         let first = run();
diff --git a/rust/src/odometry_fusion/eskf.rs b/rust/src/odometry_fusion/eskf.rs
index 8cc9a30..9f65e39 100644
--- a/rust/src/odometry_fusion/eskf.rs
+++ b/rust/src/odometry_fusion/eskf.rs
@@ -1,12 +1,14 @@
 // Copyright 2026 Dimensional Inc.
 // SPDX-License-Identifier: Apache-2.0
 //
-// Error-state order: position(0:3) velocity(3:6) theta(6:9), then a gyro_bias/accel_bias
-// pair per IMU at (9+6i : 15+6i). theta is a right (body-frame) perturbation:
-// q_true = q_est * Exp(theta).
+// Error-state order: position(0:3) velocity(3:6) theta(6:9), then angular velocity(9:12) when
+// there is no IMU, else a gyro_bias/accel_bias pair per IMU, then a position/theta pair per
+// cloned pose. theta is a right (body-frame) perturbation: q_true = q_est * Exp(theta).
 
 use nalgebra::{DMatrix, DVector, Matrix3, UnitQuaternion, Vector3};
 
+use super::{InitialStds, ProcessNoise};
+
 pub type Cov = DMatrix<f64>;
 pub type Jacobian = DMatrix<f64>;
 
@@ -14,12 +16,66 @@ pub fn skew(v: &Vector3<f64>) -> Matrix3<f64> {
     v.cross_matrix()
 }
 
-pub fn gyro_bias_col(imu: usize) -> usize {
-    9 + 6 * imu
+/// Rotation about z alone: what a planar body's orientation is allowed to be.
+pub fn yaw_only(q: &UnitQuaternion<f64>) -> UnitQuaternion<f64> {
+    let (_, _, yaw) = q.euler_angles();
+    UnitQuaternion::from_axis_angle(&Vector3::z_axis(), yaw)
 }
 
-pub fn accel_bias_col(imu: usize) -> usize {
-    12 + 6 * imu
+/// Where each block lives in the error state; fixed at init.
+#[derive(Clone, Copy, Debug, Default)]
+pub struct Layout {
+    pub imus: usize,
+    pub clones: usize,
+}
+
+impl Layout {
+    /// Without an IMU the angular rate is a state; with one it is the gyro reading.
+    pub fn angular_velocity_col(&self) -> Option<usize> {
+        (self.imus == 0).then_some(9)
+    }
+
+    pub fn gyro_bias_col(&self, imu: usize) -> usize {
+        9 + 6 * imu
+    }
+
+    pub fn accel_bias_col(&self, imu: usize) -> usize {
+        12 + 6 * imu
+    }
+
+    fn clones_start(&self) -> usize {
+        if self.imus == 0 {
+            12
+        } else {
+            9 + 6 * self.imus
+        }
+    }
+
+    pub fn clone_position_col(&self, slot: usize) -> usize {
+        self.clones_start() + 6 * slot
+    }
+
+    pub fn clone_rotation_col(&self, slot: usize) -> usize {
+        self.clone_position_col(slot) + 3
+    }
+
+    pub fn dim(&self) -> usize {
+        self.clones_start() + 6 * self.clones
+    }
+
+    /// The dimensions a planar body cannot move in: z, roll, pitch and their rates.
+    fn pinned_cols(&self) -> Vec<usize> {
+        let mut cols = vec![2, 5, 6, 7];
+        if let Some(col) = self.angular_velocity_col() {
+            cols.extend([col, col + 1]);
+        }
+        for slot in 0..self.clones {
+            let position = self.clone_position_col(slot);
+            let rotation = self.clone_rotation_col(slot);
+            cols.extend([position + 2, rotation, rotation + 1]);
+        }
+        cols
+    }
 }
 
 #[derive(Clone, Copy, Debug, Default)]
@@ -36,83 +92,196 @@ pub struct ImuBias {
     pub accel: Vector3<f64>,
 }
 
+/// The latest IMU sample, base-frame, held as the process input until the next one.
+#[derive(Clone, Debug)]
+pub struct ImuInput {
+    pub imu: usize,
+    pub gyro: Vector3<f64>,
+    pub accel: Vector3<f64>,
+}
+
+/// A copy of the pose at some past time, so a relative measurement can span to the present.
+#[derive(Clone, Debug)]
+pub struct PoseClone {
+    pub position: Vector3<f64>,
+    pub q: UnitQuaternion<f64>,
+}
+
 #[derive(Clone, Debug)]
 pub struct State {
+    /// The instant the rest of the state describes.
+    pub time_ns: i64,
     pub position: Vector3<f64>, // world frame
     pub velocity: Vector3<f64>, // world frame
     pub q: UnitQuaternion<f64>, // world_from_body
+    /// Body frame; a state only without an IMU, otherwise unused.
+    pub angular_velocity: Vector3<f64>,
     pub biases: Vec<ImuBias>,
+    pub input: Option<ImuInput>,
+    pub clones: Vec<Option<PoseClone>>,
+    /// A twist source said the body is still: the constant-velocity model holds it without noise.
+    pub still: bool,
 }
 
 impl Default for State {
     fn default() -> Self {
         Self {
+            time_ns: 0,
             position: Vector3::zeros(),
             velocity: Vector3::zeros(),
             q: UnitQuaternion::identity(),
+            angular_velocity: Vector3::zeros(),
             biases: Vec::new(),
+            input: None,
+            clones: Vec::new(),
+            still: false,
         }
     }
 }
 
-#[derive(Clone, Debug)]
+/// Past this an IMU gap is a dropout, and integrating one held sample across it would invent motion.
+const MAX_INERTIAL_STEP_SECONDS: f64 = 1.0;
+
+#[derive(Clone, Debug, Default)]
 pub struct Filter {
+    pub layout: Layout,
     /// One entry per IMU, parallel to `x.biases`.
     pub noise: Vec<Noise>,
+    pub process_noise: ProcessNoise,
     pub gravity: f64,
+    pub planar: bool,
     pub x: State,
     pub p_cov: Cov,
 }
 
-impl Default for Filter {
-    fn default() -> Self {
-        Self {
-            noise: Vec::new(),
-            gravity: 9.80665,
-            x: State::default(),
-            p_cov: Cov::identity(9, 9),
-        }
-    }
-}
-
 impl Filter {
     pub fn dim(&self) -> usize {
-        9 + 6 * self.x.biases.len()
+        self.layout.dim()
     }
 
     pub fn init(
         &mut self,
+        time_ns: i64,
         world_from_body: UnitQuaternion<f64>,
         biases: Vec<ImuBias>,
-        position_std: f64,
-        velocity_std: f64,
-        rotation_std: f64,
-        bias_std: f64,
+        input: Option<ImuInput>,
+        stds: &InitialStds,
     ) {
+        self.layout.imus = biases.len();
         self.x = State {
+            time_ns,
             q: world_from_body,
             biases,
+            input,
+            clones: vec![None; self.layout.clones],
             ..State::default()
         };
         let dim = self.dim();
         self.p_cov = Cov::zeros(dim, dim);
-        let mut blocks = vec![(0, position_std), (3, velocity_std), (6, rotation_std)];
-        for imu in 0..self.x.biases.len() {
-            blocks.push((gyro_bias_col(imu), bias_std));
-            blocks.push((accel_bias_col(imu), bias_std));
+        let mut blocks = vec![(0, stds.position), (3, stds.velocity), (6, stds.rotation)];
+        if let Some(col) = self.layout.angular_velocity_col() {
+            blocks.push((col, stds.angular_velocity));
+        }
+        for imu in 0..self.layout.imus {
+            blocks.push((self.layout.gyro_bias_col(imu), stds.bias));
+            blocks.push((self.layout.accel_bias_col(imu), stds.bias));
         }
         for (block, std) in blocks {
             self.p_cov
                 .view_mut((block, block), (3, 3))
                 .copy_from(&(Matrix3::identity() * std * std));
         }
+        self.constrain();
+    }
+
+    /// Body-frame angular rate and the error-state column that corrects it, with its sign:
+    /// the rate state itself, or minus the gyro bias of the IMU whose sample is held.
+    pub fn angular_velocity(&self) -> (Vector3<f64>, Option<(usize, f64)>) {
+        if let Some(col) = self.layout.angular_velocity_col() {
+            return (self.x.angular_velocity, Some((col, 1.0)));
+        }
+        match &self.x.input {
+            Some(input) => (
+                input.gyro - self.x.biases[input.imu].gyro,
+                Some((self.layout.gyro_bias_col(input.imu), -1.0)),
+            ),
+            None => (Vector3::zeros(), None),
+        }
+    }
+
+    /// Moves the state to `time_ns` under the process model: the held IMU sample when there
+    /// is an IMU, constant velocity otherwise. Returns false over an IMU gap, left unbridged.
+    pub fn predict_to(&mut self, time_ns: i64) -> bool {
+        let dt = (time_ns - self.x.time_ns) as f64 / 1.0e9;
+        if dt <= 0.0 {
+            return true;
+        }
+        self.x.time_ns = time_ns;
+        let bridged = if self.layout.imus == 0 {
+            self.predict_constant_velocity(dt);
+            true
+        } else if let Some(input) = self
+            .x
+            .input
+            .clone()
+            .filter(|_| dt <= MAX_INERTIAL_STEP_SECONDS)
+        {
+            self.predict_inertial(dt, input.imu, &input.gyro, &input.accel);
+            true
+        } else {
+            false
+        };
+        self.constrain();
+        bridged
+    }
+
+    fn predict_constant_velocity(&mut self, dt: f64) {
+        if self.x.still {
+            return;
+        }
+        let dim = self.dim();
+        let omega_col = self.layout.angular_velocity_col().expect("no imu");
+        let rotation_step = UnitQuaternion::from_scaled_axis(self.x.angular_velocity * dt);
+
+        let mut f = Cov::identity(dim, dim);
+        f.view_mut((0, 3), (3, 3))
+            .copy_from(&(Matrix3::identity() * dt));
+        f.view_mut((6, 6), (3, 3))
+            .copy_from(rotation_step.inverse().to_rotation_matrix().matrix());
+        f.view_mut((6, omega_col), (3, 3))
+            .copy_from(&(Matrix3::identity() * dt));
+
+        self.x.position += self.x.velocity * dt;
+        self.x.q *= rotation_step;
+
+        // Integrated white acceleration: the position-velocity pair is correlated, not independent.
+        let mut q_noise = Cov::zeros(dim, dim);
+        for (value_col, rate_col, density) in [
+            (0, 3, self.process_noise.linear_acceleration),
+            (6, omega_col, self.process_noise.angular_acceleration),
+        ] {
+            let spectral = density * density;
+            let identity = Matrix3::identity();
+            q_noise
+                .view_mut((value_col, value_col), (3, 3))
+                .copy_from(&(identity * spectral * dt.powi(3) / 3.0));
+            q_noise
+                .view_mut((value_col, rate_col), (3, 3))
+                .copy_from(&(identity * spectral * dt.powi(2) / 2.0));
+            q_noise
+                .view_mut((rate_col, value_col), (3, 3))
+                .copy_from(&(identity * spectral * dt.powi(2) / 2.0));
+            q_noise
+                .view_mut((rate_col, rate_col), (3, 3))
+                .copy_from(&(identity * spectral * dt));
+        }
+        self.p_cov = &f * &self.p_cov * f.transpose() + q_noise;
     }
 
     /// One step of dead reckoning from IMU `imu`'s sample, using that IMU's bias and noise.
-    /// With interleaved IMUs each sample integrates the slice of time since the previous
-    /// sample of any IMU.
-    pub fn propagate(&mut self, dt: f64, imu: usize, gyro: &Vector3<f64>, accel: &Vector3<f64>) {
+    fn predict_inertial(&mut self, dt: f64, imu: usize, gyro: &Vector3<f64>, accel: &Vector3<f64>) {
         let dim = self.dim();
+        let layout = self.layout;
         let rotation = self.x.q.to_rotation_matrix();
         let unbiased_gyro = gyro - self.x.biases[imu].gyro;
         let unbiased_accel = accel - self.x.biases[imu].accel;
@@ -124,11 +293,11 @@ impl Filter {
             .copy_from(&(Matrix3::identity() * dt));
         f.view_mut((3, 6), (3, 3))
             .copy_from(&(-(rotation.matrix() * skew(&unbiased_accel)) * dt));
-        f.view_mut((3, accel_bias_col(imu)), (3, 3))
+        f.view_mut((3, layout.accel_bias_col(imu)), (3, 3))
             .copy_from(&(-rotation.matrix() * dt));
         f.view_mut((6, 6), (3, 3))
             .copy_from(&(Matrix3::identity() - skew(&unbiased_gyro) * dt));
-        f.view_mut((6, gyro_bias_col(imu)), (3, 3))
+        f.view_mut((6, layout.gyro_bias_col(imu)), (3, 3))
             .copy_from(&(-Matrix3::identity() * dt));
 
         self.x.position += self.x.velocity * dt + 0.5 * world_accel * dt * dt;
@@ -142,8 +311,8 @@ impl Filter {
         ];
         // Every IMU's bias random-walks through the interval, not just the one sampling.
         for (other, noise) in self.noise.iter().enumerate() {
-            blocks.push((gyro_bias_col(other), noise.gyro_random_walk));
-            blocks.push((accel_bias_col(other), noise.accel_random_walk));
+            blocks.push((layout.gyro_bias_col(other), noise.gyro_random_walk));
+            blocks.push((layout.accel_bias_col(other), noise.accel_random_walk));
         }
         for (block, density) in blocks {
             q_noise
@@ -153,9 +322,84 @@ impl Filter {
         self.p_cov = &f * &self.p_cov * f.transpose() + q_noise;
     }
 
-    /// `gate` is a Mahalanobis threshold in standard deviations per degree of freedom; 0 disables.
+    /// Projects out what a planar body cannot do. A hard constraint rather than pseudo-
+    /// measurements: those leave the state off the plane between updates and P ill-conditioned.
+    fn constrain(&mut self) {
+        if !self.planar {
+            return;
+        }
+        self.x.position.z = 0.0;
+        self.x.velocity.z = 0.0;
+        self.x.q = yaw_only(&self.x.q);
+        if self.layout.angular_velocity_col().is_some() {
+            self.x.angular_velocity.x = 0.0;
+            self.x.angular_velocity.y = 0.0;
+        }
+        for clone in self.x.clones.iter_mut().flatten() {
+            clone.position.z = 0.0;
+            clone.q = yaw_only(&clone.q);
+        }
+        for col in self.layout.pinned_cols() {
+            self.p_cov.row_mut(col).fill(0.0);
+            self.p_cov.column_mut(col).fill(0.0);
+        }
+    }
+
+    /// Copies the current pose, with its correlations, into clone `slot`.
+    pub fn clone_pose(&mut self, slot: usize) {
+        self.x.clones[slot] = Some(PoseClone {
+            position: self.x.position,
+            q: self.x.q,
+        });
+        let dim = self.dim();
+        let position = self.layout.clone_position_col(slot);
+        let rotation = self.layout.clone_rotation_col(slot);
+        let mut selector = Cov::identity(dim, dim);
+        for offset in 0..3 {
+            selector.row_mut(position + offset).fill(0.0);
+            selector.row_mut(rotation + offset).fill(0.0);
+            selector[(position + offset, offset)] = 1.0;
+            selector[(rotation + offset, 6 + offset)] = 1.0;
+        }
+        self.p_cov = &selector * &self.p_cov * selector.transpose();
+    }
+
+    /// The innovation covariance's inverse, or None when `gate` rejects the measurement: its
+    /// squared Mahalanobis distance under the full innovation covariance, per measurement
+    /// dimension, at or past `gate`. 0 disables the gate.
     // The negated comparison is deliberate: a NaN distance must reject.
     #[allow(clippy::neg_cmp_op_on_partial_ord)]
+    fn gated_innovation_inverse(
+        &self,
+        residual: &DVector<f64>,
+        jacobian: &Jacobian,
+        variance: &DVector<f64>,
+        gate: f64,
+    ) -> Option<Cov> {
+        let innovation =
+            jacobian * &self.p_cov * jacobian.transpose() + Cov::from_diagonal(variance);
+        let innovation_inverse = innovation.try_inverse()?;
+        if gate > 0.0 {
+            let mahalanobis_sq = (residual.transpose() * &innovation_inverse * residual)[(0, 0)];
+            if !(mahalanobis_sq < gate * residual.len() as f64) {
+                return None;
+            }
+        }
+        Some(innovation_inverse)
+    }
+
+    pub fn within_gate(
+        &self,
+        residual: &DVector<f64>,
+        jacobian: &Jacobian,
+        variance: &DVector<f64>,
+        gate: f64,
+    ) -> bool {
+        self.gated_innovation_inverse(residual, jacobian, variance, gate)
+            .is_some()
+    }
+
+    /// False, leaving the state untouched, when the gate rejects the measurement.
     pub fn update(
         &mut self,
         residual: &DVector<f64>,
@@ -163,17 +407,12 @@ impl Filter {
         variance: &DVector<f64>,
         gate: f64,
     ) -> bool {
-        let noise = Cov::from_diagonal(variance);
-        let innovation = jacobian * &self.p_cov * jacobian.transpose() + &noise;
-        let Some(innovation_inverse) = innovation.try_inverse() else {
+        let Some(innovation_inverse) =
+            self.gated_innovation_inverse(residual, jacobian, variance, gate)
+        else {
             return false;
         };
-        if gate > 0.0 {
-            let mahalanobis_sq = (residual.transpose() * &innovation_inverse * residual)[(0, 0)];
-            if !(mahalanobis_sq < gate * gate * residual.len() as f64) {
-                return false;
-            }
-        }
+        let noise = Cov::from_diagonal(variance);
         let kalman_gain = &self.p_cov * jacobian.transpose() * &innovation_inverse;
         let dx: DVector<f64> = &kalman_gain * residual;
         let identity_minus_kh = Cov::identity(self.dim(), self.dim()) - &kalman_gain * jacobian;
@@ -184,12 +423,25 @@ impl Filter {
     }
 
     fn inject(&mut self, dx: &DVector<f64>) {
+        let layout = self.layout;
         self.x.position += dx.fixed_rows::<3>(0);
         self.x.velocity += dx.fixed_rows::<3>(3);
         self.x.q *= UnitQuaternion::from_scaled_axis(dx.fixed_rows::<3>(6).into_owned());
+        if let Some(col) = layout.angular_velocity_col() {
+            self.x.angular_velocity += dx.fixed_rows::<3>(col);
+        }
         for (imu, bias) in self.x.biases.iter_mut().enumerate() {
-            bias.gyro += dx.fixed_rows::<3>(gyro_bias_col(imu));
-            bias.accel += dx.fixed_rows::<3>(accel_bias_col(imu));
+            bias.gyro += dx.fixed_rows::<3>(layout.gyro_bias_col(imu));
+            bias.accel += dx.fixed_rows::<3>(layout.accel_bias_col(imu));
+        }
+        for (slot, clone) in self.x.clones.iter_mut().enumerate() {
+            if let Some(clone) = clone {
+                clone.position += dx.fixed_rows::<3>(layout.clone_position_col(slot));
+                clone.q *= UnitQuaternion::from_scaled_axis(
+                    dx.fixed_rows::<3>(layout.clone_rotation_col(slot))
+                        .into_owned(),
+                );
+            }
         }
     }
 }
@@ -199,30 +451,62 @@ mod tests {
     use super::*;
 
     const GRAVITY: f64 = 9.80665;
+    const MS: i64 = 1_000_000;
+
+    fn stds() -> InitialStds {
+        InitialStds {
+            position: 0.1,
+            velocity: 0.1,
+            rotation: 0.05,
+            angular_velocity: 0.1,
+            bias: 0.01,
+        }
+    }
+
+    fn imu_noise() -> Noise {
+        Noise {
+            gyro_noise_density: 0.01,
+            gyro_random_walk: 0.001,
+            accel_noise_density: 0.1,
+            accel_random_walk: 0.01,
+        }
+    }
+
+    fn still_input(imu: usize, gyro: Vector3<f64>) -> Option<ImuInput> {
+        Some(ImuInput {
+            imu,
+            gyro,
+            accel: Vector3::new(0.0, 0.0, GRAVITY),
+        })
+    }
 
     fn level_filter() -> Filter {
         let mut filter = Filter {
-            noise: vec![Noise {
-                gyro_noise_density: 0.01,
-                gyro_random_walk: 0.001,
-                accel_noise_density: 0.1,
-                accel_random_walk: 0.01,
-            }],
+            noise: vec![imu_noise()],
+            gravity: GRAVITY,
             ..Filter::default()
         };
         filter.init(
+            0,
             UnitQuaternion::identity(),
             vec![ImuBias::default()],
-            0.1,
-            0.1,
-            0.05,
-            0.01,
+            still_input(0, Vector3::zeros()),
+            &stds(),
         );
         filter
     }
 
-    fn static_accel() -> Vector3<f64> {
-        Vector3::new(0.0, 0.0, GRAVITY)
+    fn constant_velocity_filter(clones: usize) -> Filter {
+        let mut filter = Filter {
+            layout: Layout { imus: 0, clones },
+            process_noise: ProcessNoise {
+                linear_acceleration: 0.5,
+                angular_acceleration: 0.5,
+            },
+            ..Filter::default()
+        };
+        filter.init(0, UnitQuaternion::identity(), Vec::new(), None, &stds());
+        filter
     }
 
     fn position_jacobian(rows: usize, dim: usize) -> Jacobian {
@@ -231,6 +515,14 @@ mod tests {
         jacobian
     }
 
+    /// Steps of `step_ms`, `steps` of them, from wherever the filter stands.
+    fn advance(filter: &mut Filter, steps: i64, step_ms: i64) {
+        let start = filter.x.time_ns;
+        for step in 1..=steps {
+            filter.predict_to(start + step * step_ms * MS);
+        }
+    }
+
     #[test]
     fn skew_is_antisymmetric_and_reproduces_the_cross_product() {
         let a = Vector3::new(1.0, 2.0, 3.0);
@@ -248,46 +540,84 @@ mod tests {
     }
 
     #[test]
-    fn exp_of_a_tiny_rotation_stays_finite_and_normalized() {
-        let q = UnitQuaternion::from_scaled_axis(Vector3::new(1e-14, 0.0, 0.0));
-        let norm: f64 = q.norm();
-        assert!((norm - 1.0).abs() < 1e-12);
+    fn the_layout_places_the_rate_state_only_without_an_imu() {
+        let without = Layout { imus: 0, clones: 1 };
+        assert_eq!(without.angular_velocity_col(), Some(9));
+        assert_eq!(without.clone_position_col(0), 12);
+        assert_eq!(without.dim(), 18);
+        let with = Layout { imus: 2, clones: 1 };
+        assert_eq!(with.angular_velocity_col(), None);
+        assert_eq!(with.accel_bias_col(1), 18);
+        assert_eq!(with.clone_rotation_col(0), 24);
+        assert_eq!(with.dim(), 27);
     }
 
     #[test]
     fn stationary_propagation_holds_the_state() {
         let mut filter = level_filter();
-        for _ in 0..100 {
-            filter.propagate(0.01, 0, &Vector3::zeros(), &static_accel());
-        }
+        advance(&mut filter, 100, 10);
         assert!(filter.x.position.norm() < 1e-9);
         assert!(filter.x.velocity.norm() < 1e-9);
     }
 
     #[test]
-    fn propagation_integrates_constant_velocity() {
+    fn inertial_propagation_integrates_constant_velocity() {
         let mut filter = level_filter();
         filter.x.velocity = Vector3::new(1.0, 0.0, 0.0);
-        for _ in 0..100 {
-            filter.propagate(0.01, 0, &Vector3::zeros(), &static_accel());
-        }
+        advance(&mut filter, 100, 10);
         assert!((filter.x.position.x - 1.0).abs() < 1e-9);
     }
 
     #[test]
-    fn propagation_grows_uncertainty() {
+    fn an_imu_gap_is_not_bridged_by_one_held_sample() {
         let mut filter = level_filter();
+        filter.x.input.as_mut().expect("held").accel.x = 1.0;
+        assert!(!filter.predict_to(2_000 * MS));
+        assert_eq!(filter.x.position, Vector3::zeros());
+    }
+
+    #[test]
+    fn the_constant_velocity_model_carries_position_and_heading() {
+        let mut filter = constant_velocity_filter(0);
+        filter.x.velocity = Vector3::new(1.0, 0.0, 0.0);
+        filter.x.angular_velocity = Vector3::new(0.0, 0.0, 0.5);
         let before = filter.p_cov.trace();
-        for _ in 0..100 {
-            filter.propagate(0.01, 0, &Vector3::zeros(), &static_accel());
-        }
+        advance(&mut filter, 100, 10);
+        assert!((filter.x.position.x - 1.0).abs() < 1e-9);
+        assert!((filter.x.q.euler_angles().2 - 0.5).abs() < 1e-9);
         assert!(filter.p_cov.trace() > before);
     }
 
+    #[test]
+    fn constant_velocity_uncertainty_grows_with_the_cube_of_time_in_position() {
+        let mut filter = constant_velocity_filter(0);
+        filter.p_cov.fill(0.0);
+        advance(&mut filter, 1, 2_000);
+        // q dt^3 / 3 with q = 0.25, dt = 2.
+        assert!((filter.p_cov[(0, 0)] - 0.25 * 8.0 / 3.0).abs() < 1e-12);
+        assert!((filter.p_cov[(3, 3)] - 0.5).abs() < 1e-12);
+    }
+
+    #[test]
+    fn planar_pins_z_roll_pitch_and_their_rates() {
+        let mut filter = constant_velocity_filter(0);
+        filter.planar = true;
+        filter.x.velocity = Vector3::new(1.0, 0.0, 0.3);
+        filter.x.angular_velocity = Vector3::new(0.2, -0.1, 0.4);
+        advance(&mut filter, 10, 10);
+        assert_eq!(filter.x.position.z, 0.0);
+        let (roll, pitch, yaw) = filter.x.q.euler_angles();
+        assert!(roll.abs() < 1e-12 && pitch.abs() < 1e-12);
+        assert!(yaw > 0.0);
+        for col in [2, 5, 6, 7, 9, 10] {
+            assert_eq!(filter.p_cov.row(col).norm(), 0.0, "col {col}");
+        }
+    }
+
     #[test]
     fn a_position_update_pulls_the_estimate() {
         let mut filter = level_filter();
-        filter.propagate(0.01, 0, &Vector3::zeros(), &static_accel());
+        advance(&mut filter, 1, 10);
         let residual = DVector::from_vec(vec![1.0, 0.0, 0.0]);
         let variance = DVector::from_element(3, 1e-4);
         assert!(filter.update(
@@ -297,7 +627,6 @@ mod tests {
             0.0
         ));
         assert!(filter.x.position.x > 0.9);
-        assert!(filter.p_cov.trace() < 15.0);
     }
 
     #[test]
@@ -305,13 +634,8 @@ mod tests {
         let mut filter = level_filter();
         filter.x.velocity = Vector3::new(1.0, 0.0, 0.0); // true velocity is zero; the filter is wrong
         for _ in 0..50 {
-            filter.propagate(0.01, 0, &Vector3::zeros(), &static_accel());
-            // The truth stays at the origin, so the residual is minus the estimate.
-            let residual = DVector::from_vec(vec![
-                -filter.x.position.x,
-                -filter.x.position.y,
-                -filter.x.position.z,
-            ]);
+            advance(&mut filter, 1, 10);
+            let residual = -DVector::from_column_slice(filter.x.position.as_slice());
             let variance = DVector::from_element(3, 1e-6);
             filter.update(
                 &residual,
@@ -326,17 +650,17 @@ mod tests {
     #[test]
     fn the_mahalanobis_gate_rejects_an_outlier_and_passes_an_inlier() {
         let mut filter = level_filter();
-        filter.propagate(0.01, 0, &Vector3::zeros(), &static_accel());
+        advance(&mut filter, 1, 10);
         let variance = DVector::from_element(3, 1e-4);
         let outlier = DVector::from_vec(vec![100.0, 0.0, 0.0]);
         assert!(!filter.update(
             &outlier,
             &position_jacobian(3, filter.dim()),
             &variance,
-            3.0
+            9.0
         ));
         let inlier = DVector::from_vec(vec![0.01, 0.0, 0.0]);
-        assert!(filter.update(&inlier, &position_jacobian(3, filter.dim()), &variance, 3.0));
+        assert!(filter.update(&inlier, &position_jacobian(3, filter.dim()), &variance, 9.0));
     }
 
     #[test]
@@ -353,30 +677,28 @@ mod tests {
     }
 
     #[test]
-    fn a_gyro_bias_propagates_into_a_tilt_the_accel_residual_can_expose() {
+    fn a_gyro_bias_propagates_into_a_tilt() {
         let mut filter = level_filter();
-        let true_bias = Vector3::new(0.02, 0.0, 0.0);
-        // The gyro reads only its bias while the body is still: the estimate tilts.
-        for _ in 0..100 {
-            filter.propagate(0.01, 0, &true_bias, &static_accel());
-        }
+        filter.x.input = still_input(0, Vector3::new(0.02, 0.0, 0.0));
+        advance(&mut filter, 100, 10);
         let tilt = filter.x.q.scaled_axis();
         assert!((tilt.x - 0.02).abs() < 1e-3);
     }
 
     #[test]
-    fn an_angular_rate_update_observes_the_gyro_bias() {
+    fn an_angular_rate_update_observes_the_held_imus_gyro_bias() {
         let mut filter = level_filter();
-        let gyro_reading = Vector3::new(0.05, 0.0, 0.0); // still body, biased gyro
+        filter.x.input = still_input(0, Vector3::new(0.05, 0.0, 0.0));
         for _ in 0..50 {
-            filter.propagate(0.01, 0, &gyro_reading, &static_accel());
+            advance(&mut filter, 1, 10);
             // A perfect rate sensor says the body is not rotating.
-            let predicted = gyro_reading - filter.x.biases[0].gyro;
-            let residual = DVector::from_vec(vec![-predicted.x, -predicted.y, -predicted.z]);
+            let (predicted, correction) = filter.angular_velocity();
+            let (col, sign) = correction.expect("an imu sample is held");
+            let residual = -DVector::from_column_slice(predicted.as_slice());
             let mut jacobian = Jacobian::zeros(3, filter.dim());
             jacobian
-                .view_mut((0, gyro_bias_col(0)), (3, 3))
-                .copy_from(&(-Matrix3::identity()));
+                .view_mut((0, col), (3, 3))
+                .copy_from(&(Matrix3::identity() * sign));
             let variance = DVector::from_element(3, 1e-6);
             filter.update(&residual, &jacobian, &variance, 0.0);
         }
@@ -390,15 +712,20 @@ mod tests {
             ..Filter::default()
         };
         filter.init(
+            0,
             UnitQuaternion::identity(),
             vec![ImuBias {
                 gyro: Vector3::new(0.1, 0.0, 0.0),
                 accel: Vector3::zeros(),
             }],
-            2.0,
-            3.0,
-            0.5,
-            0.01,
+            None,
+            &InitialStds {
+                position: 2.0,
+                velocity: 3.0,
+                rotation: 0.5,
+                angular_velocity: 7.0,
+                bias: 0.01,
+            },
         );
         assert!((filter.p_cov[(0, 0)] - 4.0).abs() < 1e-12);
         assert!((filter.p_cov[(3, 3)] - 9.0).abs() < 1e-12);
@@ -409,37 +736,36 @@ mod tests {
 
     #[test]
     fn two_imus_keep_separate_bias_states() {
-        let noise = Noise {
-            gyro_noise_density: 0.01,
-            gyro_random_walk: 0.001,
-            accel_noise_density: 0.1,
-            accel_random_walk: 0.01,
-        };
         let mut filter = Filter {
-            noise: vec![noise, noise],
+            noise: vec![imu_noise(), imu_noise()],
+            gravity: GRAVITY,
             ..Filter::default()
         };
         filter.init(
+            0,
             UnitQuaternion::identity(),
             vec![ImuBias::default(), ImuBias::default()],
-            0.1,
-            0.1,
-            0.05,
-            0.01,
+            None,
+            &stds(),
         );
         assert_eq!(filter.dim(), 21);
         // Only the second IMU's gyro is biased; a rate update on its block finds it there.
         let gyro_reading = Vector3::new(0.05, 0.0, 0.0);
         for step in 0..100 {
-            if step % 2 == 0 {
-                filter.propagate(0.005, 0, &Vector3::zeros(), &static_accel());
+            let imu = step % 2;
+            let gyro = if imu == 1 {
+                gyro_reading
             } else {
-                filter.propagate(0.005, 1, &gyro_reading, &static_accel());
+                Vector3::zeros()
+            };
+            filter.x.input = still_input(imu, gyro);
+            advance(&mut filter, 1, 5);
+            if imu == 1 {
                 let predicted = gyro_reading - filter.x.biases[1].gyro;
-                let residual = DVector::from_vec(vec![-predicted.x, -predicted.y, -predicted.z]);
+                let residual = -DVector::from_column_slice(predicted.as_slice());
                 let mut jacobian = Jacobian::zeros(3, filter.dim());
                 jacobian
-                    .view_mut((0, gyro_bias_col(1)), (3, 3))
+                    .view_mut((0, filter.layout.gyro_bias_col(1)), (3, 3))
                     .copy_from(&(-Matrix3::identity()));
                 let variance = DVector::from_element(3, 1e-6);
                 filter.update(&residual, &jacobian, &variance, 0.0);
@@ -448,4 +774,19 @@ mod tests {
         assert!((filter.x.biases[1].gyro.x - 0.05).abs() < 2e-3);
         assert!(filter.x.biases[0].gyro.x.abs() < 2e-2);
     }
+
+    #[test]
+    fn a_clone_carries_the_poses_covariance_and_correlation() {
+        let mut filter = constant_velocity_filter(1);
+        filter.x.velocity = Vector3::new(1.0, 0.0, 0.0);
+        advance(&mut filter, 10, 10);
+        filter.clone_pose(0);
+        let col = filter.layout.clone_position_col(0);
+        assert_eq!(filter.p_cov[(col, col)], filter.p_cov[(0, 0)]);
+        assert_eq!(filter.p_cov[(col, 0)], filter.p_cov[(0, 0)]);
+        advance(&mut filter, 10, 10);
+        // The clone stands still while the pose moves on and loosens.
+        assert!((filter.x.clones[0].as_ref().expect("cloned").position.x - 0.1).abs() < 1e-12);
+        assert!(filter.p_cov[(0, 0)] > filter.p_cov[(col, col)]);
+    }
 }