Skip to main content

rust_robotics_slam/
lidar_mcl.rs

1//! Monte Carlo localization of a 2D LiDAR in a known occupancy grid.
2//!
3//! - **Motion:** every particle applies the odometry step with noise that
4//!   grows with the distance and rotation travelled (odometry motion model).
5//! - **Measurement:** the likelihood-field model scores a subsampled scan by
6//!   the distance from each beam end point to the nearest occupied cell,
7//!   mixed with a uniform term for unexplained returns.
8//! - **Global initialization:** the first scan after
9//!   [`LidarMcl::initialize_global`] scores `global_oversampling` times as
10//!   many uniform poses as there are particles and keeps the best by weight.
11//! - **Recovery:** augmented MCL tracks short- and long-term averages of the
12//!   measurement likelihood and injects new particles when the short-term
13//!   average drops; sensor resetting also injects while the scan fit at the
14//!   particles stays poor. Each injected particle is the best-scoring of a few
15//!   uniform free-space draws, so the filter recovers from a kidnapping
16//!   without needing a lucky sample. A small fraction of scored particles is
17//!   injected on every update, so a filter that converged on a look-alike
18//!   place (long uniform corridors) keeps probing for the true one.
19//! - **Resampling:** weights accumulate across updates and the particles are
20//!   resampled (low variance) only when the effective sample size halves.
21
22use nalgebra::Vector2;
23use rand::{rngs::StdRng, Rng, SeedableRng};
24use rand_distr::{Distribution, Normal};
25use rust_robotics_core::Pose2D;
26
27use crate::lidar_occupancy::{CellState, OccupancyGrid};
28use crate::scan_to_map::compose_pose;
29
30/// Tuning parameters for [`LidarMcl`].
31#[derive(Debug, Clone, Copy, PartialEq)]
32pub struct LidarMclConfig {
33    /// Number of particles.
34    pub particles: usize,
35    /// Translation noise per meter travelled \[m/m\].
36    pub translation_noise: f64,
37    /// Rotation noise per radian turned \[rad/rad\].
38    pub rotation_noise: f64,
39    /// Rotation noise per meter travelled \[rad/m\].
40    pub rotation_noise_per_meter: f64,
41    /// Standard deviation of a beam end point around the nearest wall \[m\].
42    pub hit_sigma: f64,
43    /// Weight of the uniform "unexplained return" term per beam.
44    pub random_weight: f64,
45    /// Use every `beam_stride`-th beam of a scan.
46    pub beam_stride: usize,
47    /// Distances beyond this are treated as equally unlikely \[m\].
48    pub max_field_distance: f64,
49    /// Effective number of independent beams: the per-beam geometric-mean
50    /// likelihood is raised to this power. Beams are correlated, so a value
51    /// below the beam count keeps the filter from collapsing too early.
52    pub independent_beams: f64,
53    /// A per-beam likelihood below this at the weighted particle mean counts
54    /// as a poor fit (sensor-resetting localization).
55    pub poor_fit: f64,
56    /// Fraction of particles re-drawn per update during a poor fit.
57    pub reset_fraction: f64,
58    /// A re-drawn particle is the best-scoring of this many uniform free
59    /// poses, so recovery does not need a lucky draw.
60    pub reset_candidates: usize,
61    /// Fraction of particles replaced by scored free poses on every update,
62    /// so a filter confidently locked onto a look-alike place keeps probing
63    /// for the true one.
64    pub min_injection: f64,
65    /// Global (re)initialization scores this many times `particles` uniform
66    /// poses against the first scan and keeps `particles` of them.
67    pub global_oversampling: usize,
68    /// Correlative scan matching proposes this many distinct candidate poses
69    /// (0 disables it); global seeding and injected particles draw from
70    /// them, so every place the scan fits keeps particles until motion
71    /// tells the look-alikes apart.
72    pub scan_match_candidates: usize,
73    /// Recompute the candidates every this many updates.
74    pub scan_match_interval: usize,
75    /// Fraction of injected particles drawn around a candidate (the rest are
76    /// scored uniform draws).
77    pub scan_match_share: f64,
78    /// Smoothing factors of the long- and short-term likelihood averages
79    /// (augmented MCL, `alpha_slow < alpha_fast`).
80    pub alpha_slow: f64,
81    pub alpha_fast: f64,
82    /// RNG seed.
83    pub seed: u64,
84}
85
86impl Default for LidarMclConfig {
87    fn default() -> Self {
88        Self {
89            particles: 600,
90            translation_noise: 0.1,
91            rotation_noise: 0.1,
92            rotation_noise_per_meter: 0.05,
93            hit_sigma: 0.12,
94            random_weight: 0.05,
95            beam_stride: 6,
96            max_field_distance: 1.5,
97            independent_beams: 8.0,
98            poor_fit: 0.6,
99            reset_fraction: 0.1,
100            reset_candidates: 20,
101            min_injection: 0.01,
102            global_oversampling: 10,
103            scan_match_candidates: 8,
104            scan_match_interval: 20,
105            scan_match_share: 0.5,
106            alpha_slow: 0.01,
107            alpha_fast: 0.3,
108            seed: 3,
109        }
110    }
111}
112
113/// A weighted pose hypothesis.
114#[derive(Debug, Clone, Copy, PartialEq)]
115pub struct Particle {
116    pub pose: Pose2D,
117    pub weight: f64,
118}
119
120/// Particle-filter localization against an [`OccupancyGrid`].
121#[derive(Debug, Clone)]
122pub struct LidarMcl {
123    config: LidarMclConfig,
124    grid: OccupancyGrid,
125    free_cells: Vec<(usize, usize)>,
126    particles: Vec<Particle>,
127    rng: StdRng,
128    w_slow: f64,
129    w_fast: f64,
130    fit: f64,
131    /// Particles are uniform and the next scan should seed them.
132    global_pending: bool,
133    /// Per-cell beam log-likelihood with the configured hit sigma, and with
134    /// a wide sigma for the coarse correlative search.
135    log_fine: Vec<f32>,
136    log_coarse: Vec<f32>,
137    candidates: Vec<(Pose2D, f64)>,
138    updates_since_candidates: usize,
139    /// A candidate refresh in progress.
140    match_job: Option<MatchJob>,
141    /// A candidate away from the current estimate explains the scan clearly
142    /// better than the (equally refined) estimate does.
143    alternative_better: bool,
144}
145
146/// A candidate must beat the refined estimate by this per-beam fit to
147/// count as a better explanation of the scan.
148const BETTER_FIT_MARGIN: f64 = 0.03;
149
150/// Relative prior weight of a continuously injected probe particle.
151const PROBE_PRIOR: f64 = 0.05;
152
153/// Lattice cells scored per update while candidates refresh in the
154/// background.
155const MATCH_CELLS_PER_UPDATE: usize = 250;
156
157/// A correlative search in progress, spread over updates.
158#[derive(Debug, Clone)]
159struct MatchJob {
160    fine: Vec<Vector2<f64>>,
161    coarse: Vec<Vector2<f64>>,
162    cells: Vec<(usize, usize)>,
163    cursor: usize,
164    scored: Vec<(Pose2D, f64)>,
165    count: usize,
166    /// Odometry since the scan the search uses.
167    motion: Pose2D,
168}
169
170/// Coarse lattice spacing of the correlative search \[m\].
171const MATCH_STEP: f64 = 0.3;
172/// Headings of the correlative search.
173const MATCH_HEADINGS: usize = 48;
174/// Hit sigma of the coarse search \[m\]: wide enough that the true pose on
175/// the lattice still scores well.
176const MATCH_COARSE_SIGMA: f64 = 0.35;
177/// Candidates closer than this (and `MATCH_DISTINCT_YAW`) are one candidate.
178const MATCH_DISTINCT_XY: f64 = 0.6;
179const MATCH_DISTINCT_YAW: f64 = 0.35;
180
181impl LidarMcl {
182    /// Creates a filter on `grid` with particles spread uniformly over the
183    /// free cells (global localization).
184    pub fn new(grid: OccupancyGrid, config: LidarMclConfig) -> Self {
185        let field = grid.distance_field(config.max_field_distance);
186        let log_table = |sigma: f64| -> Vec<f32> {
187            field
188                .iter()
189                .map(|&distance| {
190                    let distance = f64::from(distance);
191                    let hit = (-distance * distance / (2.0 * sigma * sigma)).exp();
192                    (hit + config.random_weight).ln() as f32
193                })
194                .collect()
195        };
196        let log_fine = log_table(config.hit_sigma);
197        let log_coarse = log_table(MATCH_COARSE_SIGMA);
198        let (width, height) = grid.size();
199        let free_cells = (0..height)
200            .flat_map(|y| (0..width).map(move |x| (x, y)))
201            .filter(|&(x, y)| grid.state(x, y) == CellState::Free)
202            .collect();
203        let mut mcl = Self {
204            config,
205            grid,
206            free_cells,
207            particles: Vec::new(),
208            rng: StdRng::seed_from_u64(config.seed),
209            w_slow: 0.0,
210            w_fast: 0.0,
211            fit: 0.0,
212            global_pending: false,
213            log_fine,
214            log_coarse,
215            candidates: Vec::new(),
216            updates_since_candidates: 0,
217            match_job: None,
218            alternative_better: false,
219        };
220        mcl.initialize_global();
221        mcl
222    }
223
224    /// The map being localized in.
225    pub fn grid(&self) -> &OccupancyGrid {
226        &self.grid
227    }
228
229    /// Current particles.
230    pub fn particles(&self) -> &[Particle] {
231        &self.particles
232    }
233
234    fn random_free_pose(&mut self) -> Pose2D {
235        if self.free_cells.is_empty() {
236            return Pose2D::origin();
237        }
238        let (x, y) = self.free_cells[self.rng.random_range(0..self.free_cells.len())];
239        let center = self.grid.cell_center(x, y);
240        let half = self.grid.resolution() / 2.0;
241        Pose2D::new(
242            center.x + self.rng.random_range(-half..half),
243            center.y + self.rng.random_range(-half..half),
244            self.rng
245                .random_range(-std::f64::consts::PI..std::f64::consts::PI),
246        )
247    }
248
249    /// Spreads the particles uniformly over the free space.
250    pub fn initialize_global(&mut self) {
251        // A verdict about where the old particles were no longer applies.
252        self.alternative_better = false;
253        let count = self.config.particles.max(1);
254        let weight = 1.0 / count as f64;
255        self.particles = (0..count)
256            .map(|_| Particle {
257                pose: self.random_free_pose(),
258                weight,
259            })
260            .collect();
261        self.w_slow = 0.0;
262        self.w_fast = 0.0;
263        self.global_pending = true;
264    }
265
266    /// Spreads the particles around `pose` (position σ `xy`, heading σ `yaw`).
267    pub fn initialize_at(&mut self, pose: Pose2D, xy: f64, yaw: f64) {
268        // A verdict about where the old particles were no longer applies.
269        self.alternative_better = false;
270        let count = self.config.particles.max(1);
271        let position = Normal::new(0.0, xy.max(1.0e-9)).expect("finite sigma");
272        let heading = Normal::new(0.0, yaw.max(1.0e-9)).expect("finite sigma");
273        self.particles = (0..count)
274            .map(|_| Particle {
275                pose: Pose2D::new(
276                    pose.x + position.sample(&mut self.rng),
277                    pose.y + position.sample(&mut self.rng),
278                    pose.yaw + heading.sample(&mut self.rng),
279                ),
280                weight: 1.0 / count as f64,
281            })
282            .collect();
283        self.global_pending = false;
284    }
285
286    /// Motion update with a body-frame odometry step.
287    pub fn predict(&mut self, odom_delta: Pose2D) {
288        let distance = odom_delta.x.hypot(odom_delta.y);
289        let turn = odom_delta.yaw.abs();
290        let sigma_xy = self.config.translation_noise * distance;
291        let sigma_yaw =
292            self.config.rotation_noise * turn + self.config.rotation_noise_per_meter * distance;
293        let xy = Normal::new(0.0, sigma_xy.max(1.0e-9)).expect("finite sigma");
294        let yaw = Normal::new(0.0, sigma_yaw.max(1.0e-9)).expect("finite sigma");
295        for particle in &mut self.particles {
296            let noisy = Pose2D::new(
297                odom_delta.x + xy.sample(&mut self.rng),
298                odom_delta.y + xy.sample(&mut self.rng),
299                odom_delta.yaw + yaw.sample(&mut self.rng),
300            );
301            particle.pose = compose_pose(particle.pose, noisy);
302        }
303        if let Some(job) = &mut self.match_job {
304            job.motion = compose_pose(job.motion, odom_delta);
305        }
306    }
307
308    /// Mean per-beam log-likelihood of `beams` from `pose` under `table`.
309    fn mean_log(&self, table: &[f32], pose: Pose2D, beams: &[Vector2<f64>]) -> f64 {
310        let (width, _) = self.grid.size();
311        let (sin, cos) = pose.yaw.sin_cos();
312        let outside = {
313            let distance = self.config.max_field_distance;
314            let sigma = self.config.hit_sigma;
315            let hit = (-distance * distance / (2.0 * sigma * sigma)).exp();
316            (hit + self.config.random_weight).ln()
317        };
318        let sum: f64 = beams
319            .iter()
320            .map(|beam| {
321                let world = Vector2::new(
322                    pose.x + cos * beam.x - sin * beam.y,
323                    pose.y + sin * beam.x + cos * beam.y,
324                );
325                self.grid
326                    .cell_of(world)
327                    .map_or(outside, |(x, y)| f64::from(table[y * width + x]))
328            })
329            .sum();
330        sum / beams.len().max(1) as f64
331    }
332
333    /// Per-beam likelihood of the scan from `pose` (geometric mean, in (0, 1]).
334    fn beam_likelihood(&self, pose: Pose2D, beams: &[Vector2<f64>]) -> f64 {
335        if beams.is_empty() {
336            return 1.0;
337        }
338        self.mean_log(&self.log_fine, pose, beams).exp() / (1.0 + self.config.random_weight)
339    }
340
341    /// Correlative scan matching (Olson, 2009) over the whole map: scores a
342    /// coarse lattice of free positions × headings against a wide likelihood
343    /// field, refines the best by local search on the fine field, and returns
344    /// up to `count` distinct poses with their per-beam likelihood, best
345    /// first. Look-alike places come back as separate candidates.
346    pub fn scan_match_candidates(
347        &self,
348        scan_body: &[Vector2<f64>],
349        count: usize,
350    ) -> Vec<(Pose2D, f64)> {
351        let Some(mut job) = self.start_match(scan_body, count) else {
352            return Vec::new();
353        };
354        while !self.advance_match(&mut job, usize::MAX) {}
355        self.finish_match(job)
356    }
357
358    /// Sets up a correlative search; `None` without beams or candidates.
359    fn start_match(&self, scan_body: &[Vector2<f64>], count: usize) -> Option<MatchJob> {
360        let stride = self.config.beam_stride.max(1);
361        let fine: Vec<Vector2<f64>> = scan_body.iter().step_by(stride).copied().collect();
362        let coarse: Vec<Vector2<f64>> = scan_body.iter().step_by(stride * 2).copied().collect();
363        if fine.is_empty() || count == 0 {
364            return None;
365        }
366        let lattice = ((MATCH_STEP / self.grid.resolution()).round() as usize).max(1);
367        let cells = self
368            .free_cells
369            .iter()
370            .filter(|(x, y)| x % lattice == 0 && y % lattice == 0)
371            .copied()
372            .collect();
373        Some(MatchJob {
374            fine,
375            coarse,
376            cells,
377            cursor: 0,
378            scored: Vec::new(),
379            count,
380            motion: Pose2D::origin(),
381        })
382    }
383
384    /// Scores up to `max_cells` more lattice cells (all headings); returns
385    /// whether the coarse search is complete.
386    fn advance_match(&self, job: &mut MatchJob, max_cells: usize) -> bool {
387        let end = job.cursor.saturating_add(max_cells).min(job.cells.len());
388        for &(x, y) in &job.cells[job.cursor..end] {
389            let center = self.grid.cell_center(x, y);
390            for i in 0..MATCH_HEADINGS {
391                let yaw = -std::f64::consts::PI
392                    + std::f64::consts::TAU * i as f64 / MATCH_HEADINGS as f64;
393                let pose = Pose2D::new(center.x, center.y, yaw);
394                job.scored
395                    .push((pose, self.mean_log(&self.log_coarse, pose, &job.coarse)));
396            }
397        }
398        job.cursor = end;
399        // Keep only the best few between chunks.
400        let keep = (job.count * 6).min(job.scored.len());
401        if keep > 0 && job.scored.len() > keep {
402            job.scored
403                .select_nth_unstable_by(keep - 1, |a, b| b.1.total_cmp(&a.1));
404            job.scored.truncate(keep);
405        }
406        job.cursor == job.cells.len()
407    }
408
409    /// Refines the coarse winners and keeps distinct ones, moved by the
410    /// odometry since the search started.
411    fn finish_match(&self, job: MatchJob) -> Vec<(Pose2D, f64)> {
412        let MatchJob {
413            fine,
414            scored,
415            count,
416            motion,
417            ..
418        } = job;
419        let mut refined: Vec<(Pose2D, f64)> = scored
420            .into_iter()
421            .map(|(pose, _)| self.refine(pose, &fine))
422            .map(|(pose, score)| (compose_pose(pose, motion), score))
423            .collect();
424        refined.sort_by(|a, b| b.1.total_cmp(&a.1));
425        let mut distinct: Vec<(Pose2D, f64)> = Vec::with_capacity(count);
426        for (pose, score) in refined {
427            let duplicate = distinct.iter().any(|(kept, _)| {
428                let yaw = (kept.yaw - pose.yaw)
429                    .sin()
430                    .atan2((kept.yaw - pose.yaw).cos());
431                (kept.x - pose.x).hypot(kept.y - pose.y) < MATCH_DISTINCT_XY
432                    && yaw.abs() < MATCH_DISTINCT_YAW
433            });
434            if !duplicate {
435                distinct.push((pose, score));
436                if distinct.len() == count {
437                    break;
438                }
439            }
440        }
441        distinct
442            .into_iter()
443            .map(|(pose, score)| (pose, score.exp() / (1.0 + self.config.random_weight)))
444            .collect()
445    }
446
447    /// Hill-climbs `pose` on the fine field with shrinking steps.
448    fn refine(&self, mut pose: Pose2D, beams: &[Vector2<f64>]) -> (Pose2D, f64) {
449        let mut score = self.mean_log(&self.log_fine, pose, beams);
450        let (mut step_xy, mut step_yaw) = (
451            MATCH_STEP / 2.0,
452            std::f64::consts::PI / MATCH_HEADINGS as f64,
453        );
454        for _ in 0..4 {
455            loop {
456                let mut improved = false;
457                for (dx, dy, dyaw) in [
458                    (step_xy, 0.0, 0.0),
459                    (-step_xy, 0.0, 0.0),
460                    (0.0, step_xy, 0.0),
461                    (0.0, -step_xy, 0.0),
462                    (0.0, 0.0, step_yaw),
463                    (0.0, 0.0, -step_yaw),
464                ] {
465                    let candidate = Pose2D::new(pose.x + dx, pose.y + dy, pose.yaw + dyaw);
466                    let candidate_score = self.mean_log(&self.log_fine, candidate, beams);
467                    if candidate_score > score + 1.0e-9 {
468                        pose = candidate;
469                        score = candidate_score;
470                        improved = true;
471                    }
472                }
473                if !improved {
474                    break;
475                }
476            }
477            step_xy /= 2.0;
478            step_yaw /= 2.0;
479        }
480        (pose, score)
481    }
482
483    /// The current scan-matching candidates (refreshed during updates).
484    pub fn candidates(&self) -> &[(Pose2D, f64)] {
485        &self.candidates
486    }
487
488    /// A pose drawn around a uniformly chosen candidate, if there are any.
489    fn candidate_pose(&mut self) -> Option<Pose2D> {
490        if self.candidates.is_empty() {
491            return None;
492        }
493        let (pose, _) = self.candidates[self.rng.random_range(0..self.candidates.len())];
494        let xy = Normal::new(0.0, 0.08).expect("finite sigma");
495        let yaw = Normal::new(0.0, 0.04).expect("finite sigma");
496        Some(Pose2D::new(
497            pose.x + xy.sample(&mut self.rng),
498            pose.y + xy.sample(&mut self.rng),
499            pose.yaw + yaw.sample(&mut self.rng),
500        ))
501    }
502
503    /// Whether a candidate away from the current estimate fits the scan
504    /// clearly better than the estimate itself, refined the same way. A
505    /// look-alike place only ties, so it does not count.
506    fn alternative_is_better(&self, beams: &[Vector2<f64>]) -> bool {
507        let (estimate, _) = self.estimate();
508        let current = self.refine(estimate, beams).1.exp() / (1.0 + self.config.random_weight);
509        self.candidates
510            .iter()
511            .filter(|(pose, _)| {
512                let yaw = (pose.yaw - estimate.yaw)
513                    .sin()
514                    .atan2((pose.yaw - estimate.yaw).cos());
515                (pose.x - estimate.x).hypot(pose.y - estimate.y) > MATCH_DISTINCT_XY
516                    || yaw.abs() > MATCH_DISTINCT_YAW
517            })
518            .any(|(_, fit)| *fit > current + BETTER_FIT_MARGIN)
519    }
520
521    /// An injected particle: around a scan-matching candidate with
522    /// probability `scan_match_share` while some other place explains the
523    /// scan clearly better than the estimate, else the best of a few uniform
524    /// draws. (Otherwise candidates at look-alike places would only compete
525    /// with the true pose on alignment, not on evidence.)
526    fn injected_pose(&mut self, beams: &[Vector2<f64>]) -> Pose2D {
527        if self.alternative_better
528            && self
529                .rng
530                .random_bool(self.config.scan_match_share.clamp(0.0, 1.0))
531        {
532            if let Some(pose) = self.candidate_pose() {
533                return pose;
534            }
535        }
536        self.scored_free_pose(beams)
537    }
538
539    /// Measurement update and resampling with a beam-ordered body-frame scan.
540    pub fn update(&mut self, scan_body: &[Vector2<f64>]) {
541        let beams: Vec<Vector2<f64>> = scan_body
542            .iter()
543            .step_by(self.config.beam_stride.max(1))
544            .copied()
545            .collect();
546        if beams.is_empty() || self.particles.is_empty() {
547            return;
548        }
549        self.updates_since_candidates += 1;
550        if self.config.scan_match_candidates > 0 {
551            if self.global_pending {
552                // Seeding needs the candidates now.
553                self.match_job = None;
554                self.candidates =
555                    self.scan_match_candidates(scan_body, self.config.scan_match_candidates);
556                self.updates_since_candidates = 0;
557            } else if let Some(mut job) = self.match_job.take() {
558                // Refresh in the background, a slice of the map per update.
559                if self.advance_match(&mut job, MATCH_CELLS_PER_UPDATE) {
560                    self.candidates = self.finish_match(job);
561                    self.alternative_better = self.alternative_is_better(&beams);
562                } else {
563                    self.match_job = Some(job);
564                }
565            } else if self.updates_since_candidates >= self.config.scan_match_interval.max(1) {
566                self.match_job = self.start_match(scan_body, self.config.scan_match_candidates);
567                self.updates_since_candidates = 0;
568            }
569        }
570        if self.global_pending {
571            self.seed_from_scan(&beams);
572            return;
573        }
574        let probes =
575            (self.particles.len() as f64 * self.config.min_injection.clamp(0.0, 1.0)) as usize;
576        for _ in 0..probes {
577            let slot = self.rng.random_range(0..self.particles.len());
578            let pose = self.injected_pose(&beams);
579            // A probe starts from a small prior weight (a teleport is
580            // unlikely), not from whatever weight the replaced slot had
581            // earned: it takes over only by explaining the scans clearly
582            // better, not by surviving a stretch where places look alike.
583            self.particles[slot] = Particle {
584                pose,
585                weight: PROBE_PRIOR / self.particles.len() as f64,
586            };
587        }
588        // Sharpen the per-beam geometric mean back to a scan likelihood. A
589        // particle inside a wall is implausible (unknown cells are not: the
590        // map may simply have a hole there).
591        let exponent = self.config.independent_beams.max(1.0);
592        let likelihoods: Vec<f64> = self
593            .particles
594            .iter()
595            .map(|particle| {
596                let position = Vector2::new(particle.pose.x, particle.pose.y);
597                let likelihood = self.beam_likelihood(particle.pose, &beams);
598                if self.grid.state_at(position) == CellState::Occupied {
599                    likelihood * 0.1
600                } else {
601                    likelihood
602                }
603            })
604            .collect();
605        let average = likelihoods.iter().sum::<f64>() / likelihoods.len() as f64;
606        let best = likelihoods
607            .iter()
608            .copied()
609            .fold(0.0, f64::max)
610            .max(1.0e-300);
611        // Weights accumulate between resamplings.
612        for (particle, likelihood) in self.particles.iter_mut().zip(&likelihoods) {
613            particle.weight *= (likelihood / best).powf(exponent);
614        }
615        let total: f64 = self.particles.iter().map(|p| p.weight).sum();
616        if total > 0.0 && total.is_finite() {
617            for particle in &mut self.particles {
618                particle.weight /= total;
619            }
620        } else {
621            let uniform = 1.0 / self.particles.len() as f64;
622            for particle in &mut self.particles {
623                particle.weight = uniform;
624            }
625        }
626
627        if self.w_slow == 0.0 {
628            self.w_slow = average;
629            self.w_fast = average;
630        } else {
631            self.w_slow += self.config.alpha_slow * (average - self.w_slow);
632            self.w_fast += self.config.alpha_fast * (average - self.w_fast);
633        }
634        // Posterior-weighted per-beam likelihood: how well the filter explains the scan.
635        let fit: f64 = self
636            .particles
637            .iter()
638            .zip(&likelihoods)
639            .map(|(particle, likelihood)| particle.weight * likelihood)
640            .sum();
641        self.fit = fit;
642        let reset = if fit < self.config.poor_fit {
643            self.config.reset_fraction
644        } else {
645            0.0
646        };
647        let mut inject = (1.0 - self.w_fast / self.w_slow).max(reset);
648        // With scan matching on, a poor fit alone is not evidence of a
649        // kidnapping: the robot may simply see something the map lacks.
650        // Re-seed only when some other place explains the scan clearly
651        // better than where the particles are.
652        if self.config.scan_match_candidates > 0 && !self.alternative_better {
653            inject = 0.0;
654        }
655        let effective = 1.0
656            / self
657                .particles
658                .iter()
659                .map(|p| p.weight * p.weight)
660                .sum::<f64>();
661        if inject > 0.0 || effective < 0.5 * self.particles.len() as f64 {
662            self.resample(inject, &beams);
663        }
664    }
665
666    /// Global initialization from a scan: scores `global_oversampling ×
667    /// particles` uniform free poses and resamples `particles` of them.
668    fn seed_from_scan(&mut self, beams: &[Vector2<f64>]) {
669        let count = self.config.particles.max(1);
670        let exponent = self.config.independent_beams.max(1.0);
671        let candidates: Vec<(Pose2D, f64)> = (0..count * self.config.global_oversampling.max(1))
672            .map(|_| {
673                let pose = self.random_free_pose();
674                (pose, self.beam_likelihood(pose, beams))
675            })
676            .collect();
677        let best = candidates
678            .iter()
679            .map(|(_, likelihood)| *likelihood)
680            .fold(1.0e-300, f64::max);
681        let total: f64 = candidates
682            .iter()
683            .map(|(_, likelihood)| (likelihood / best).powf(exponent))
684            .sum();
685        self.particles = candidates
686            .iter()
687            .map(|(pose, likelihood)| Particle {
688                pose: *pose,
689                weight: (likelihood / best).powf(exponent) / total,
690            })
691            .collect();
692        self.resample_to(count, 0.0, beams);
693        // Every candidate place gets an equal share of particles, so a
694        // look-alike that happened to score higher cannot crowd out the rest.
695        let share = (count as f64 * self.config.scan_match_share.clamp(0.0, 1.0) / 2.0) as usize;
696        for slot in 0..share.min(count) {
697            if let Some(pose) = self.candidate_pose() {
698                self.particles[slot].pose = pose;
699            }
700        }
701        let average = candidates.iter().map(|(_, l)| l).sum::<f64>() / candidates.len() as f64;
702        self.w_slow = average;
703        self.w_fast = average;
704        self.fit = best;
705        self.global_pending = false;
706    }
707
708    /// The best-scoring of `reset_candidates` uniform free poses.
709    fn scored_free_pose(&mut self, beams: &[Vector2<f64>]) -> Pose2D {
710        let mut best = (f64::NEG_INFINITY, Pose2D::origin());
711        for _ in 0..self.config.reset_candidates.max(1) {
712            let pose = self.random_free_pose();
713            let score = self.beam_likelihood(pose, beams);
714            if score > best.0 {
715                best = (score, pose);
716            }
717        }
718        best.1
719    }
720
721    /// Low-variance resampling; each slot is replaced by a scored free pose
722    /// with probability `inject`.
723    fn resample(&mut self, inject: f64, beams: &[Vector2<f64>]) {
724        self.resample_to(self.particles.len(), inject, beams);
725    }
726
727    /// Low-variance resampling of the current weighted particles into
728    /// `count` equally weighted ones.
729    fn resample_to(&mut self, count: usize, inject: f64, beams: &[Vector2<f64>]) {
730        let step = 1.0 / count as f64;
731        let mut target = self.rng.random_range(0.0..step);
732        let mut cumulative = self.particles[0].weight;
733        let mut index = 0;
734        let mut next = Vec::with_capacity(count);
735        for _ in 0..count {
736            while target > cumulative && index + 1 < self.particles.len() {
737                index += 1;
738                cumulative += self.particles[index].weight;
739            }
740            let pose = if self.rng.random_bool(inject.clamp(0.0, 1.0)) {
741                self.injected_pose(beams)
742            } else {
743                self.particles[index].pose
744            };
745            next.push(Particle { pose, weight: step });
746            target += step;
747        }
748        self.particles = next;
749    }
750
751    /// Posterior-weighted per-beam likelihood of the last scan, in (0, 1]:
752    /// how well the particles explain what the LiDAR sees.
753    pub fn fit(&self) -> f64 {
754        self.fit
755    }
756
757    /// Pose of the heaviest particle mode and the RMS distance of all
758    /// particles from it \[m\].
759    pub fn estimate(&self) -> (Pose2D, f64) {
760        // The weighted mean of a multimodal cloud lies between the modes:
761        // average only the heaviest mode (particles near the heaviest 1 m ×
762        // 45° bin), and measure the spread of all particles around it, so a
763        // filter still torn between look-alike places reads as unconverged.
764        const BIN_XY: f64 = 1.0;
765        const BIN_YAW: f64 = std::f64::consts::FRAC_PI_4;
766        let bin = |pose: &Pose2D| {
767            (
768                (pose.x / BIN_XY).floor() as i64,
769                (pose.y / BIN_XY).floor() as i64,
770                (pose.yaw.rem_euclid(std::f64::consts::TAU) / BIN_YAW).floor() as i64,
771            )
772        };
773        let mut bins: std::collections::HashMap<(i64, i64, i64), f64> =
774            std::collections::HashMap::new();
775        for particle in &self.particles {
776            *bins.entry(bin(&particle.pose)).or_insert(0.0) += particle.weight;
777        }
778        let Some(&heaviest) = bins
779            .iter()
780            .max_by(|a, b| a.1.total_cmp(b.1))
781            .map(|(key, _)| key)
782        else {
783            return (Pose2D::origin(), f64::INFINITY);
784        };
785        let seed = self
786            .particles
787            .iter()
788            .filter(|particle| bin(&particle.pose) == heaviest)
789            .fold((0.0, 0.0, 0.0, 0.0, 0.0), |(x, y, s, c, w), p| {
790                (
791                    x + p.weight * p.pose.x,
792                    y + p.weight * p.pose.y,
793                    s + p.weight * p.pose.yaw.sin(),
794                    c + p.weight * p.pose.yaw.cos(),
795                    w + p.weight,
796                )
797            });
798        let seed_weight = seed.4.max(1.0e-300);
799        let center = Pose2D::new(
800            seed.0 / seed_weight,
801            seed.1 / seed_weight,
802            seed.2.atan2(seed.3),
803        );
804        let (mut x, mut y, mut sin, mut cos, mut total) = (0.0, 0.0, 0.0, 0.0, 0.0);
805        for particle in &self.particles {
806            let yaw = (particle.pose.yaw - center.yaw)
807                .sin()
808                .atan2((particle.pose.yaw - center.yaw).cos());
809            let near = (particle.pose.x - center.x).hypot(particle.pose.y - center.y) < BIN_XY
810                && yaw.abs() < BIN_YAW;
811            if near {
812                let w = particle.weight;
813                x += w * particle.pose.x;
814                y += w * particle.pose.y;
815                sin += w * particle.pose.yaw.sin();
816                cos += w * particle.pose.yaw.cos();
817                total += w;
818            }
819        }
820        let total = total.max(1.0e-300);
821        let mean = Pose2D::new(x / total, y / total, sin.atan2(cos));
822        let all: f64 = self
823            .particles
824            .iter()
825            .map(|p| p.weight)
826            .sum::<f64>()
827            .max(1.0e-300);
828        let spread = (self
829            .particles
830            .iter()
831            .map(|p| p.weight * ((p.pose.x - mean.x).powi(2) + (p.pose.y - mean.y).powi(2)))
832            .sum::<f64>()
833            / all)
834            .sqrt();
835        (mean, spread)
836    }
837}
838
839#[cfg(test)]
840mod tests {
841    use super::*;
842    use crate::lidar_occupancy::OccupancyConfig;
843    use crate::scan_to_map::{ranges_to_points, ray_cast_ranges, relative_pose, LineSegment};
844
845    fn polygon(corners: &[(f64, f64)]) -> Vec<LineSegment> {
846        (0..corners.len())
847            .map(|i| {
848                let (ax, ay) = corners[i];
849                let (bx, by) = corners[(i + 1) % corners.len()];
850                LineSegment::new(Vector2::new(ax, ay), Vector2::new(bx, by))
851            })
852            .collect()
853    }
854
855    /// An asymmetric room, so the pose is globally identifiable.
856    fn world() -> Vec<LineSegment> {
857        let mut walls = polygon(&[(-6.0, -4.0), (6.0, -4.0), (6.0, 4.0), (-6.0, 4.0)]);
858        walls.extend(polygon(&[(1.0, 1.0), (2.0, 1.4), (1.6, 2.4), (0.6, 2.0)]));
859        walls.extend(polygon(&[(-4.5, -3.0), (-3.7, -3.0), (-3.7, -2.2)]));
860        walls.push(LineSegment::new(
861            Vector2::new(3.5, -4.0),
862            Vector2::new(3.5, -1.5),
863        ));
864        walls
865    }
866
867    fn scan(pose: Pose2D) -> Vec<Vector2<f64>> {
868        ranges_to_points(&ray_cast_ranges(pose, &world(), 180, 12.0))
869    }
870
871    fn map() -> OccupancyGrid {
872        let poses = [
873            Pose2D::new(-3.0, 0.0, 0.0),
874            Pose2D::new(0.0, -2.0, 1.0),
875            Pose2D::new(3.0, 2.0, -2.0),
876            Pose2D::new(-1.0, 3.0, 0.5),
877        ];
878        let scans: Vec<_> = poses.iter().map(|pose| scan(*pose)).collect();
879        OccupancyGrid::from_scans(
880            poses.iter().copied().zip(scans.iter().map(Vec::as_slice)),
881            0.5,
882            OccupancyConfig::default(),
883        )
884    }
885
886    /// Drives a slow circle from `start`, returning the final truth.
887    #[test]
888    fn reinitializing_drops_the_old_verdict_on_alternatives() {
889        let mut mcl = LidarMcl::new(map(), LidarMclConfig::default());
890        mcl.alternative_better = true;
891        mcl.initialize_at(Pose2D::new(1.0, 0.0, 0.0), 0.1, 0.05);
892        assert!(!mcl.alternative_better);
893        mcl.alternative_better = true;
894        mcl.initialize_global();
895        assert!(!mcl.alternative_better);
896    }
897
898    fn drive(mcl: &mut LidarMcl, start: Pose2D, steps: usize) -> Pose2D {
899        let mut truth = start;
900        let step = Pose2D::new(0.1, 0.0, 0.04);
901        for _ in 0..steps {
902            truth = compose_pose(truth, step);
903            mcl.predict(step);
904            mcl.update(&scan(truth));
905        }
906        truth
907    }
908
909    #[test]
910    fn scan_matching_finds_the_pose_and_its_look_alikes() {
911        let mcl = LidarMcl::new(map(), LidarMclConfig::default());
912        let truth = Pose2D::new(-2.0, -1.0, 0.3);
913        let candidates = mcl.scan_match_candidates(&scan(truth), 4);
914        let (best, fit) = candidates[0];
915        let error = relative_pose(truth, best);
916        assert!(error.x.hypot(error.y) < 0.15, "best {best:?}");
917        assert!(error.yaw.abs() < 0.05, "best {best:?}");
918        assert!(fit > 0.8, "fit {fit}");
919
920        // A bare rectangle looks the same rotated by 180°: both come back.
921        let room = polygon(&[(-5.0, -3.0), (5.0, -3.0), (5.0, 3.0), (-5.0, 3.0)]);
922        let pose = Pose2D::new(2.0, 1.0, 0.4);
923        let scan_of = |pose: Pose2D| ranges_to_points(&ray_cast_ranges(pose, &room, 180, 12.0));
924        let grid = OccupancyGrid::from_scans(
925            [(pose, scan_of(pose).as_slice())],
926            0.5,
927            OccupancyConfig::default(),
928        );
929        let mcl = LidarMcl::new(grid, LidarMclConfig::default());
930        let candidates = mcl.scan_match_candidates(&scan_of(pose), 4);
931        let mirror = Pose2D::new(-2.0, -1.0, 0.4 + std::f64::consts::PI);
932        for expected in [pose, mirror] {
933            assert!(
934                candidates.iter().any(|(candidate, _)| {
935                    let error = relative_pose(expected, *candidate);
936                    error.x.hypot(error.y) < 0.2 && error.yaw.abs() < 0.1
937                }),
938                "{expected:?} missing from {candidates:?}"
939            );
940        }
941    }
942
943    #[test]
944    fn global_localization_converges_from_uniform_particles() {
945        let mut mcl = LidarMcl::new(map(), LidarMclConfig::default());
946        let truth = drive(&mut mcl, Pose2D::new(-2.0, -1.0, 0.3), 40);
947        let (estimate, spread) = mcl.estimate();
948        let error = relative_pose(truth, estimate);
949        assert!(error.x.hypot(error.y) < 0.2, "error {error:?}");
950        assert!(error.yaw.abs() < 0.1, "yaw error {}", error.yaw);
951        assert!(spread < 0.3, "spread {spread}");
952    }
953
954    #[test]
955    fn recovers_from_a_kidnapping() {
956        let mut mcl = LidarMcl::new(map(), LidarMclConfig::default());
957        let start = Pose2D::new(-2.0, -1.0, 0.3);
958        mcl.initialize_at(start, 0.05, 0.02);
959        let _ = drive(&mut mcl, start, 10);
960        // Teleport the robot; the particles still believe the old pose.
961        let truth = drive(&mut mcl, Pose2D::new(3.0, 1.0, 2.5), 80);
962        let error = relative_pose(truth, mcl.estimate().0);
963        assert!(error.x.hypot(error.y) < 0.3, "error {error:?}");
964    }
965}