Skip to content

Latest commit

 

History

History
577 lines (490 loc) · 29.3 KB

estimation.md

File metadata and controls

577 lines (490 loc) · 29.3 KB

Estimation

State estimation from noisy measurements. KalmanFilter is the linear filter: predict rolls the state forward through a matrix model and grows the covariance by the process noise; update folds in a measurement and shrinks it. Fixed-size, no allocation, and generic over the Numeric scalar, so a Dual state differentiates the whole filter.

  • KalmanFilter<STATE_DIMENSION, MEASUREMENT_DIMENSION, T>: built from an initial estimate and a KalmanModel.
  • KalmanModel<STATE_DIMENSION, MEASUREMENT_DIMENSION, T>: the four matrices that describe what the filter is tracking — transition, measurement model, process noise, measurement noise. Three of them are the same shape, so naming each field is what stops a swapped pair compiling.
  • predict / predict_with_control: the time step, undriven or with a control_model · control_input term. CONTROL_DIMENSION lives on the method, so undriven users never meet it.
  • update: the measurement step. The only fallible operation in the module.
  • CovarianceUpdate: Joseph (the default) or Naive.
  • innovation / innovation_covariance / normalized_innovation_squared: for measurement gating.
  • The setters (set_state_transition, set_process_noise, …) cover the time-varying case, where a changing timestep changes the model between steps.
use multicalc::{KalmanFilter, KalmanModel};
use multicalc::{Matrix, Vector};

// Constant velocity: position integrates velocity over a 1 s step; position is measured.
let initial_state = Vector::new([0.0, 0.0]);   // [position, velocity]
let initial_covariance = Matrix::new([[1.0, 0.0], [0.0, 1.0]]);
let model = KalmanModel {
    state_transition: Matrix::new([[1.0, 1.0], [0.0, 1.0]]),
    measurement_model: Matrix::new([[1.0, 0.0]]),   // position only
    process_noise: Matrix::new([[0.01, 0.0], [0.0, 0.01]]),
    measurement_noise: Matrix::new([[0.1]]),
};

let mut filter = KalmanFilter::new(initial_state, initial_covariance, model);

filter.predict();
let measurement = Vector::new([1.0]);
filter.update(measurement).unwrap();
let position = filter.state()[0];

// Gate an outlier before folding it in.
filter.predict();
let outlier = Vector::new([1.9]);
filter.update(outlier).unwrap();
let gate = filter.normalized_innovation_squared().unwrap();

The covariance update uses Joseph form by default — (I − K·H)·P·(I − K·H)ᵀ + K·R·Kᵀ — which stays symmetric and positive definite by construction, while the naive (I − K·H)·P loses symmetry as rounding builds up. Joseph form is not a guarantee at every scale: over roughly 10⁷ single-precision updates it drifts too, and the fix there is to symmetrize and clamp the covariance.

update returns EstimationError::NonFinite for a non-finite measurement or innovation covariance, and EstimationError::NotPositiveDefinite when the innovation covariance cannot be factorized — the gain is undefined. predict is a cheap element-wise path and propagates non-finite values silently.

ExtendedKalmanFilter<STATE_DIMENSION, MEASUREMENT_DIMENSION, T> takes the process and measurement models as functions rather than matrices — any VectorFn — and re-linearizes them at the current estimate on every step. The Jacobians come from automatic differentiation: write the model once and its partial derivatives are exact, with no hand-derived Jacobians anywhere — the classic source of silent estimator bugs.

  • new / from_derivator: the autodiff default, or an explicit differentiation backend (e.g. FiniteDifferenceMulti).
  • predict(&process_model) / update(&measurement_model, measurement): the models are passed per step, not stored, so the type stays ExtendedKalmanFilter<3, 2>. A control input or a changing timestep lives in the model as a field the caller sets between steps — there is no predict_with_control. Unlike the linear filter, predict here evaluates and differentiates a model, so it returns a Result.
  • update_with_residual(&measurement_model, residual): update with a caller-formed residual, for when a measurement component is an angle — plain subtraction is wrong across the ±π wrap, and only the caller knows which components are angular.
  • CovarianceUpdate, the accessors, and normalized_innovation_squared are shared with the linear filter. predict and update also return EstimationError::Diff if a Jacobian step fails — reachable only with a finite-difference backend, as the autodiff default cannot.
use multicalc::ExtendedKalmanFilter;
use multicalc::{Matrix, Vector};
use multicalc::{Numeric, VectorFn};

// Range to a landmark at (3, 4): nonlinear in the pose, so the linear filter cannot take it.
struct RangeToLandmark;
impl VectorFn<2, 1> for RangeToLandmark {
    fn eval<S: Numeric>(&self, state: &[S; 2]) -> [S; 1] {
        let to_landmark_x = S::from_f64(3.0) - state[0];
        let to_landmark_y = S::from_f64(4.0) - state[1];
        [(to_landmark_x * to_landmark_x + to_landmark_y * to_landmark_y).sqrt()]
    }
}

// A stationary target: the pose carries over unchanged.
struct Stationary;
impl VectorFn<2, 2> for Stationary {
    fn eval<S: Numeric>(&self, state: &[S; 2]) -> [S; 2] {
        [state[0], state[1]]
    }
}

let mut filter = ExtendedKalmanFilter::<2, 1>::new(
    Vector::new([0.0, 0.0]),                  // initial pose, 5.0 from the landmark
    Matrix::new([[1.0, 0.0], [0.0, 1.0]]),    // initial covariance
    Matrix::new([[0.01, 0.0], [0.0, 0.01]]),  // process noise
    Matrix::new([[0.1]]),                     // measurement noise
);
filter.predict(&Stationary).unwrap();
filter.update(&RangeToLandmark, Vector::new([5.5])).unwrap();

Unscented filter

UnscentedKalmanFilter<STATE_DIMENSION, MEASUREMENT_DIMENSION, T> takes the same VectorFn models the extended filter does, and handles their curvature a different way. Rather than flattening the model to a straight line at the current estimate, it picks 2·STATE_DIMENSION + 1 points spread around it, pushes each one through the model untouched, and rebuilds the estimate from where they land. The model is never differentiated, so it does not have to be smooth — a lookup table, a saturating actuator, or a branch on a threshold works here and does not work in a filter that needs a derivative. On a strongly curved model the answer is usually closer than one straight-line fit gets.

  • new: the same four matrices as the extended filter — initial estimate, initial covariance, process noise, measurement noise.
  • with_scaling(alpha, beta, kappa): how far the points spread and how the middle one is weighted. alpha = 1e-3, beta = 2, kappa = 0 by default. It returns a Result rather than chaining, because a spread that works out to zero or less has no points to place and is worth catching where it is written rather than a step later.
  • with_regularization(epsilon): adds epsilon to the diagonal before the covariance is factorized. Off by default and never applied on its own — a covariance that cannot be factorized returns EstimationError::NotPositiveDefinite, and quietly nudging it would hide a filter that has gone wrong.
  • predict(&process_model) / update(&measurement_model, measurement) / update_with_residual(&measurement_model, residual): as on the extended filter. update works from the points predict left behind, so predict first — with no prediction the gain is zero and the estimate does not move.
  • The accessors and normalized_innovation_squared are shared with the other two filters. There is no CovarianceUpdate here: Joseph and naive are two ways of writing one step this filter does not have. Its covariance is made exactly symmetric every time it is formed instead.
use multicalc::UnscentedKalmanFilter;
use multicalc::{Matrix, Vector};
use multicalc::{Numeric, VectorFn};

// Range to a landmark at (3, 4), measured by a sensor that saturates at 6 — a model with a corner
// in it, which no derivative describes and this filter does not need one for.
struct SaturatingRange;
impl VectorFn<2, 1> for SaturatingRange {
    fn eval<S: Numeric>(&self, state: &[S; 2]) -> [S; 1] {
        let to_landmark_x = S::from_f64(3.0) - state[0];
        let to_landmark_y = S::from_f64(4.0) - state[1];
        let range = (to_landmark_x * to_landmark_x + to_landmark_y * to_landmark_y).sqrt();
        let ceiling = S::from_f64(6.0);
        [if range > ceiling { ceiling } else { range }]
    }
}

// A stationary target: the state carries over unchanged.
struct Stationary;
impl VectorFn<2, 2> for Stationary {
    fn eval<S: Numeric>(&self, state: &[S; 2]) -> [S; 2] {
        [state[0], state[1]]
    }
}

let mut filter = UnscentedKalmanFilter::<2, 1>::new(
    Vector::new([0.0, 0.0]),                  // initial state, 5.0 from the landmark
    Matrix::new([[1.0, 0.0], [0.0, 1.0]]),
    Matrix::new([[0.01, 0.0], [0.0, 0.01]]),
    Matrix::new([[0.1]]),
)
.with_scaling(0.3, 2.0, 0.0)
.unwrap();

filter.predict(&Stationary).unwrap();
filter.update(&SaturatingRange, Vector::new([5.5])).unwrap();
let position = filter.state();

One thing this filter asks that the other two do not. It averages the points it gets back, so an angle that the process model wraps into a ±π band is a trap: two points a hair apart end up at +π and −π, and their average is nothing like either. Let an angular state component run past ±π inside the model and wrap it afterwards through set_state. The points themselves sit a fraction of a standard deviation apart, so nothing else this filter averages can straddle the boundary — but the innovation can, which is what update_with_residual is for, exactly as on the extended filter.

Which of the two nonlinear filters to reach for comes down to the model. The extended filter is cheaper when the model is cheap to differentiate and gently curved. This one costs 2·STATE_DIMENSION + 1 evaluations per step instead of a derivative, and earns that back on a sharply curved model, or on any model a derivative does not describe.

Error-state filter

ErrorStateKalmanFilter<MEASUREMENT_DIMENSION, T> fuses an IMU — a turn-rate sensor and a push sensor — with whatever corrections a vehicle can get, and tracks where a body is, how it is moving, which way it faces, and what its own two sensors are getting steadily wrong.

It tracks the correction to a running guess rather than the guess itself. That is what lets the facing live on the rotation group, where it can turn any distance without wrapping or needing renormalization, while the uncertainty stays a plain flat fifteen numbers that ordinary matrix arithmetic can carry forward. After every correction the error is folded back into the guess and reset to zero, so it never grows large enough for the flat treatment to strain.

The fifteen numbers run in this order, three each:

Index range Meaning
0..3 where the estimate has the body, in world axes, metres
3..6 how wrong the estimated speed is, world axes, m/s
6..9 a small turn taking the estimated facing to the true one, radians
9..12 the turn-rate sensor's steady offset, rad/s
12..15 the push sensor's steady offset, m/s²
  • NominalState<T>: the running guess — place, motion, facing, and the two sensor offsets. Built with new, or with at_rest from a facing alone. plus_error folds a correction in and error_from takes one back out; the two are exact inverses, which is what the reset relies on. The starting facing usually comes from SO3::from_two_direction_pairs.
  • ImuNoise<T>: how noisy the IMU is, in the figures a datasheet quotes rather than as a raw noise matrix. Four fields of the same type, so naming each is what stops a swapped pair compiling.
  • NominalStateFn<MEASUREMENT_DIMENSION>: a sensor model, written once against named fields and evaluated at whatever kind of number the filter needs. No derivative is ever coded by hand.
  • new(initial_state, initial_covariance, imu_noise, measurement_noise), with with_gravity and with_covariance_update for the two settings that have defaults.
  • predict(gyroscope_reading, accelerometer_reading, timestep): one IMU step. The transition it uses is written in closed form and reachable as error_state_transition.
  • update / update_with_residual / update_other: one correction. Use the second when a measurement is an angle, because plain subtraction is wrong across the ±π wrap. Use the third for a sensor of a different width from the one the filter is declared with — a three-number position fix and a one-number heading aid cannot both set the type's width.
  • inject_error_and_reset(error): update calls this itself; it is public so the step can be exercised with a known correction.
  • condition_covariance(minimum_eigenvalue): see below.
  • normalized_estimation_error_squared(true_state): how far the estimate is from a known truth, measured against its own claimed spread. Only a test or a simulation has the truth to pass in.

Two generic parameters, where the extended filter has four. The state width is fixed at fifteen by the formulation. There is no pluggable differentiation backend either: a stepped difference over an error state is not meaningful, because the error is identically zero and a finite step would move a point on the rotation group by an amount the sensor model cannot tell from real signal. The Jacobian is always taken exactly.

Evening the spread out across its diagonal happens on every predict and every update, and costs almost nothing. Lifting a direction that rounding has pushed below zero is a different matter: it means working out the spread's directions, which costs far more than a filter step does. So condition_covariance is left to the caller's schedule — once a second, or on a health check, not every tick. Joseph form plus the automatic evening is what you get otherwise, and that is good for hours rather than for a ten-million-update single-precision duty cycle.

Two things will look like bugs and are not. The turn-rate offset about the vertical is only visible through a heading aid, so without one it never settles. The push offset along the vertical is only visible when the push itself varies, because a body pushed a little too hard upward looks exactly like a body tilted a little.

use multicalc::{ErrorStateKalmanFilter, ImuNoise, NominalState, NominalStateFn};
use multicalc::{Matrix, Numeric, SO3, Vector};

// A tracker in the room reports where the drone is, and nothing else.
struct RoomTracker;
impl NominalStateFn<3> for RoomTracker {
    fn eval<S: Numeric>(&self, state: &NominalState<S>) -> [S; 3] {
        *state.position().as_array()
    }
}

let level = SO3::<f64>::identity();
let starting_spread = 0.1;
let imu_noise = ImuNoise {
    gyroscope_noise_density: 0.02,
    accelerometer_noise_density: 0.05,
    gyroscope_bias_random_walk: 1e-4,
    accelerometer_bias_random_walk: 1e-3,
};
let tracker_spread = 0.03;
let mut filter = ErrorStateKalmanFilter::<3>::new(
    NominalState::at_rest(level),
    Matrix::from_diagonal([starting_spread; 15]),
    imu_noise,
    Matrix::from_diagonal([tracker_spread * tracker_spread; 3]),
);

// Sitting still, the push sensor reads a full gravity upward.
let gravity_strength = 9.81;
let gyroscope_reading = Vector::new([0.0, 0.0, 0.0]);
let accelerometer_reading = Vector::new([0.0, 0.0, gravity_strength]);
let timestep = 0.001;
filter.predict(gyroscope_reading, accelerometer_reading, timestep).unwrap();

// The tracker says the drone is a little east of where the filter has it.
let step_east = 0.1;
filter.update(&RoomTracker, Vector::new([step_east, 0.0, 0.0])).unwrap();
assert!(filter.nominal_state().position()[0] > 0.0);

// The sensor offsets start at zero and are learned from corrections like that one.
let learned = filter.nominal_state().accelerometer_bias();
assert!(learned.is_finite());

Full demo: error_state_estimation.rs.

Attitude filters

MahonyFilter<T> and MadgwickFilter<T> work out which way a body is facing, and what its turn-rate sensor reads when the body is not turning, from a turn-rate sensor, a push sensor, and optionally a magnetometer. They carry a facing and a three-number offset and nothing else — no place, no speed, no spread. When a spread is wanted, that is ErrorStateKalmanFilter's job, at roughly a hundred times the arithmetic; these are a handful of cross products and one exponential a tick, the same work whatever the readings are.

A turn-rate sensor alone gives a smooth facing that slowly wanders. A push sensor alone says which way is down but jumps about whenever the body is pushed; a magnetometer alone says which way is north and is easily disturbed. Both filters take the turn rate as the answer and nudge it, every tick, by however far the other two say it is off. They differ only in how they nudge:

  • MahonyFilter nudges harder the more wrong it is — a with_proportional_gain term acting now, and a with_integral_gain term that turns the running total of those nudges into the offset estimate. Set the integral gain to zero and no offset is learned, at the price of a small permanent lean whenever the sensor really does have one.
  • MadgwickFilter always nudges by the same amount and takes only the direction from the readings. That makes with_correction_gain, in radians per second, the whole tuning story: it is how fast the filter is willing to walk toward the sensors, and it does not change whether the facing is a degree out or ninety. with_bias_gain says how much of that walking to blame on a sensor offset; set it to zero for the published filter's behaviour, which learns none.

Shared by both:

  • new(initial_orientation): the only thing without a default. The starting facing usually comes from SO3::from_two_direction_pairs on a still body.
  • with_reference_directions(upward_reference, north_reference): which way is up and which way is north, in world axes. Starts at (0, 0, 1) and (1, 0, 0). Both go in at once so their order cannot matter, and north is squared up against up before it is stored.
  • step(gyroscope_reading, accelerometer_reading, magnetometer_reading, timestep): one tick. The magnetometer is an Option, so a tick without one — or with one that is not to be trusted — is a None rather than a separate call. step_without_magnetometer is the same thing spelled shorter.
  • orientation() and gyroscope_bias(): what it has worked out. set_orientation and set_gyroscope_bias put a value back, for re-seeding from a still-body fix or restoring a saved offset at start-up.

The magnetometer's world direction is worked out afresh each tick rather than taken as a setting: the measured field is turned into world axes, its upward part is kept as it is, and everything left over is laid along north. So the magnetometer only ever moves the heading, and a caller who does not know how steeply the local field dips cannot get a lasting lean out of it.

Three things will look like bugs and are not. Without a magnetometer the heading rides on the turn-rate sensor alone and slowly wanders — only the lean is pinned, because down is the only direction a push sensor can see. A body in free fall has no usable down at all: the push reading goes to nothing, contributes nothing, and the facing coasts on the turn rate until the body is caught. And MadgwickFilter never quite stands still even when it is exactly right, because a fixed-rate walk always takes a step; that step, with_correction_gain times the timestep, is its error floor.

Neither filter can tell a sustained push from gravity. A body accelerating steadily gives a push reading that is confidently wrong about down, and both will believe it — which is the honest reason these are the error-state filter's complement rather than its replacement.

The facing is pulled back onto unit length every tick. The step itself already gives a true rotation; the pull back is there because these filters are the ones expected to run for hours at a kilohertz in single precision with nothing else watching for drift.

use multicalc::{MadgwickFilter, MahonyFilter, SO3, Vector};

// Starting off level by about a tenth of a radian, on a body that is in fact still and level.
let tilt = Vector::new([0.1, -0.05, 0.0]);
let tilted = SO3::exp(tilt);
let mut mahony = MahonyFilter::new(tilted);
let mut madgwick = MadgwickFilter::new(tilted);

// A still body reads one gravity upward, and a field pointing north and 60 degrees down.
let gravity_strength = 9.81;
let not_turning = Vector::new([0.0, 0.0, 0.0]);
let one_gravity_up = Vector::new([0.0, 0.0, gravity_strength]);
let dip: f64 = 60.0_f64.to_radians();
let field = Vector::new([dip.cos(), 0.0, -dip.sin()]);
let timestep = 0.005;
let ticks = 12_000; // a minute at 200 Hz

for _ in 0..ticks {
    mahony.step(not_turning, one_gravity_up, Some(field), timestep).unwrap();
    madgwick.step(not_turning, one_gravity_up, Some(field), timestep).unwrap();
}

// Both have found level, and neither was leaned over by the steeply dipping field.
assert!(mahony.orientation().log().norm() < 1e-3);
assert!(madgwick.orientation().log().norm() < 1e-3);

Full demo: attitude_filter.rs.

Ready-made models

Three pieces save writing the same model out for every project.

  • ConstantTurnAndSpeed rolls a vehicle's state [x, y, heading, speed, turn rate] forward one tick along the arc it is turning through, taking speed and turn rate to hold. Tracking work calls this the coordinated-turn model.
  • DirectMeasurement<STATE_DIMENSION, MEASUREMENT_DIMENSION> is a sensor reading state components straight off, in the order listed — a position fix reading [x, y], wheel encoders reading the speed and turn rate.
  • residual_with_wrapped_angles subtracts a prediction from a reading with the components you name as angles folded back into (-π, π]. Without it, a heading either side of the half turn reads as nearly a whole turn of error and the filter lurches to correct it.
use multicalc::estimation::{
    ConstantTurnAndSpeed, DirectMeasurement, ExtendedKalmanFilter, residual_with_wrapped_angles,
};
use multicalc::linear_algebra::{Matrix, Vector};
use multicalc::scalar::VectorFn;

// A vehicle tracked as [x, y, heading, speed, turn rate], fixed by position alone.
let timestep = 0.1;
let motion = ConstantTurnAndSpeed { timestep };
let position_fix = DirectMeasurement::<5, 2>::try_new([0, 1])?;

let initial_state = Vector::new([0.0, 0.0, 0.0, 1.0, 0.2]);
let initial_covariance = Matrix::from_diagonal([0.5; 5]);
let process_noise = Matrix::from_diagonal([1e-4, 1e-4, 1e-4, 1e-3, 1e-3]);
let measurement_noise = Matrix::from_diagonal([0.05, 0.05]);
let mut filter = ExtendedKalmanFilter::<5, 2>::new(
    initial_state,
    initial_covariance,
    process_noise,
    measurement_noise,
);

let fix = Vector::new([0.1, 0.02]);
filter.predict(&motion)?;
filter.update(&position_fix, fix)?;
assert!(filter.state()[0].is_finite());

// A heading sensor needs the angle-aware residual, since its reading wraps.
let heading_sensor = DirectMeasurement::<5, 2>::try_new([2, 4])?;
let reading = Vector::new([3.10, 0.2]);
let predicted = Vector::new(heading_sensor.eval(filter.state().as_array()));
let residual = residual_with_wrapped_angles(reading, predicted, &[0]);
assert!(residual[0].abs() <= core::f64::consts::PI);
filter.update_with_residual(&heading_sensor, residual)?;
# Ok::<(), multicalc::CalcError>(())

Particle filter

ParticleFilter<STATE_DIMENSION, MEASUREMENT_DIMENSION, T, R> carries a cloud of weighted state samples instead of a single Gaussian, so it can track a belief the Kalman filters cannot represent — strongly nonlinear, non-Gaussian, or with several peaks at once (a robot that could be in one of two corridors). It is the tool to reach for when a single-Gaussian belief is the thing that breaks, and the price is running hundreds to thousands of samples every step.

  • new(particle_count, initial_mean, initial_covariance, process_noise, seed): samples the starting cloud from the given Gaussian, with a seeded built-in Pcg32. from_random takes any RandomSource instead. particle_count must be at least one.
  • predict(&process_model): pushes every sample through the model — any VectorFn — and adds a draw of process noise. update(&measurement_model, &likelihood, measurement): reweights each sample by how well its predicted measurement matches, normalizes, and resamples if the cloud has degenerated.
  • Likelihood scores a sample as a log-weight; GaussianLikelihood::new(measurement_noise) is the batteries-included default for additive Gaussian noise. Write your own for anything else.
  • ResamplingScheme: Systematic (the default), Stratified, Multinomial, or Residual. Set it with with_resampling; tune when it fires with with_resample_threshold, and add post-resample jitter with with_roughening.
  • mean (the usual estimate), maximum_a_posteriori_state (the single heaviest sample, for when the belief has several peaks and the mean falls between them), effective_sample_size, particles, and weights.
# use multicalc::{GaussianLikelihood, ParticleFilter};
# use multicalc::{Matrix, Vector};
# use multicalc::{Numeric, VectorFn};
// A stationary 2-D point, measured directly with a little noise.
struct Stationary;
impl VectorFn<2, 2> for Stationary {
    fn eval<S: Numeric>(&self, state: &[S; 2]) -> [S; 2] {
        [state[0], state[1]]
    }
}

let particle_count = 1000;
let initial_mean = Vector::new([0.0, 0.0]);
let initial_covariance = Matrix::new([[1.0, 0.0], [0.0, 1.0]]);
let process_noise = Matrix::new([[0.01, 0.0], [0.0, 0.01]]);
let seed = 7;

let mut filter = ParticleFilter::<2, 2>::new(
    particle_count,
    initial_mean,
    initial_covariance,
    process_noise,
    seed,
)
.unwrap();

let measurement_noise = Matrix::new([[0.05, 0.0], [0.0, 0.05]]);
let sensor = GaussianLikelihood::new(measurement_noise).unwrap();
let measurement = Vector::new([1.0, 2.0]);

for _ in 0..20 {
    filter.predict(&Stationary).unwrap();
    filter.update(&Stationary, &sensor, measurement).unwrap();
}
assert!((filter.mean()[0] - 1.0).abs() < 0.2);

The particle filter is heap-backed, so it is behind the alloc feature and the bare-metal build does not compile it. Its update returns EstimationError::NonFinite for a non-finite measurement and EstimationError::WeightsDegenerate when no sample can explain the measurement. GaussianLikelihood forms the mismatch by plain subtraction, so a measurement with an angular component needs a custom Likelihood that folds the angle into a ±π band first — the same wrap the extended filter's update_with_residual exists for.


Back to the tutorial index

Monte Carlo Localization

MonteCarloLocalizer answers a different question than the filters above: not "where has the robot moved to", but "where on this map is it in the first place" — the problem a robot has when it is switched on and has no fix at all. It carries a cloud of pose guesses; every guess casts the beams it would see against the map, and the guesses matching the real scan gain weight. alloc only, since the cloud lives on the heap.

Feed it travel and turn with predict, scans with update, and read the answer with estimate, which gives the best pose and how tightly the cloud is holding to it. is_converged says when the answer is worth trusting — usually the moment to hand over to a Kalman filter for the drive itself. The map is anything implementing OccupancyMap, and the beam directions come from the same ScanGeometry the scan was taken with.

use multicalc::estimation::{BeamModel, InitialParticleCloud, MonteCarloLocalizer};
use multicalc::mapping::{DynamicOccupancyGrid, MutableOccupancyMap, OccupancyMap, ScanGeometry};

// A walled room the robot already has a map of.
let cell_size = 0.1_f64;
let mut room = DynamicOccupancyGrid::try_new(40, 30, cell_size, [0.0, 0.0])?;
let walls = [[0.2, 0.2], [3.8, 0.2], [3.8, 2.8], [0.2, 2.8]];
room.occupy_polyline(&walls, true);
room.occupy_circle([2.8, 2.0], 0.25);

const NUM_BEAMS: usize = 16;
let scan: ScanGeometry<NUM_BEAMS> = ScanGeometry::try_new(2.0 * core::f64::consts::PI / 3.0, 6.0)?;

// Where the robot really is, and the rough guess the localizer is given.
let truth = [1.2, 1.0, 0.3];
let hint = [1.45, 0.75, 0.35];
let cloud = InitialParticleCloud {
    particle_count: 1500,
    position_variance: 0.16,
    heading_variance: 0.25,
};
let beam_model = BeamModel { range_deviation: 0.15, ..Default::default() };
let seed = 20260804;
let mut localizer = MonteCarloLocalizer::<NUM_BEAMS>::new(hint, cloud, beam_model, seed)?;

// One scan, taken standing still and fed in a few times as the robot looks around.
let reading: [f64; NUM_BEAMS] = core::array::from_fn(|beam| {
    let offset = scan.beam_angle(beam).unwrap_or(0.0);
    room.cast_ray([truth[0], truth[1]], truth[2] + offset, scan.maximum_range())
        .unwrap_or(f64::INFINITY)
});
for _ in 0..10 {
    localizer.update(&reading, &room, &scan)?;
}

// The cloud has settled onto the robot, and says so.
let (pose, _spread) = localizer.estimate();
assert!((pose[0] - truth[0]).abs() < 0.25);
assert!((pose[1] - truth[1]).abs() < 0.25);
assert!(localizer.is_converged(0.05, 0.2));
# Ok::<(), multicalc::CalcError>(())

Full demo: localized_lap_check.rs.