Manifold states: difference and combine for the state space, not just the measurement #1

New Issue

2026-10-03T18:59:54-04:00

connor commented

2026-10-03 18:59:54 -04:00

MeasurementModel has difference and combine so that an angular measurement
wraps correctly. There is no equivalent for the state, and the state side has
exactly the same problem.

Where it breaks today:

sigma::weighted_mean over state points is a plain weighted sum, so the mean of
sigma points either side of a wrap is wrong.
Gaussian::mean + correction in Ukf::update assumes a vector space, so the
corrected state can leave the manifold.
ScaledUnscented::generate offsets the mean by Cholesky columns additively,
which is the same assumption again.

A filter with a heading, an attitude quaternion or an MRP in its state is
therefore silently wrong across the wrap, and nothing warns about it. The library
does not currently say this anywhere, which is the immediate fix.

The standard treatment is a manifold (boxplus/boxminus) UKF: the state carries
oplus(x, delta) and ominus(a, b) operations between the manifold and a local
tangent vector, the sigma points are generated in the tangent space, the mean is
found by an iterative Karcher-mean loop rather than a weighted sum, and the
covariance lives in the tangent space throughout.

Shape of the work:

Document the limitation now, in ProcessModel and in the crate docs. Cheap and
it stops someone trusting it.
A StateSpace<N> trait (or two provided methods on ProcessModel, mirroring
the measurement side) with oplus, ominus and a combine. A Euclidean
default impl keeps every existing caller unchanged.
sigma, Ukf::update and Imm's mixing all route their state arithmetic
through it. ConsiderUkf too, which is more awkward because its joint state
may be part manifold and part not.
A test on a 2D pose (position plus heading) crossing the wrap, built the same
way as angle_wrapping_is_what_makes_a_bearing_filter_work in
tests/nonlinear.rs — which is worth reading first, because the first version
of that test crossed nothing and passed anyway.

Step 2 is where the design risk is: an iterative mean inside the sigma-point
reconstruction changes the cost of every step, so it wants a measurement before
it is made unconditional.

`MeasurementModel` has `difference` and `combine` so that an angular *measurement* wraps correctly. There is no equivalent for the *state*, and the state side has exactly the same problem. Where it breaks today: - `sigma::weighted_mean` over state points is a plain weighted sum, so the mean of sigma points either side of a wrap is wrong. - `Gaussian::mean + correction` in `Ukf::update` assumes a vector space, so the corrected state can leave the manifold. - `ScaledUnscented::generate` offsets the mean by Cholesky columns additively, which is the same assumption again. A filter with a heading, an attitude quaternion or an MRP in its state is therefore silently wrong across the wrap, and nothing warns about it. The library does not currently say this anywhere, which is the immediate fix. The standard treatment is a manifold (boxplus/boxminus) UKF: the state carries `oplus(x, delta)` and `ominus(a, b)` operations between the manifold and a local tangent vector, the sigma points are generated in the tangent space, the mean is found by an iterative Karcher-mean loop rather than a weighted sum, and the covariance lives in the tangent space throughout. Shape of the work: 1. Document the limitation now, in `ProcessModel` and in the crate docs. Cheap and it stops someone trusting it. 2. A `StateSpace<N>` trait (or two provided methods on `ProcessModel`, mirroring the measurement side) with `oplus`, `ominus` and a `combine`. A Euclidean default impl keeps every existing caller unchanged. 3. `sigma`, `Ukf::update` and `Imm`'s mixing all route their state arithmetic through it. `ConsiderUkf` too, which is more awkward because its joint state may be part manifold and part not. 4. A test on a 2D pose (position plus heading) crossing the wrap, built the same way as `angle_wrapping_is_what_makes_a_bearing_filter_work` in `tests/nonlinear.rs` — which is worth reading first, because the first version of that test crossed nothing and passed anyway. Step 2 is where the design risk is: an iterative mean inside the sigma-point reconstruction changes the cost of every step, so it wants a measurement before it is made unconditional.

connor added the size/L label 2026-10-03 18:59:55 -04:00

Sign in to join this conversation.

1 Participants

Notifications

Due Date

No due date set.

Dependencies

No dependencies set.

Reference: connor/nonlinear-filters#1