rust_robotics_localization/
monte_carlo_localization.rs

1//! Adaptive Monte Carlo Localization (MCL) with KLD-sampling.
2//!
3//! This module implements a particle filter variant where the particle count
4//! is adapted online using a KLD bound.
5
6use nalgebra::{Matrix4, Vector2, Vector4};
7use rand::Rng;
8use rand_distr::{Distribution, Normal};
9use std::collections::HashSet;
10use std::f64::consts::PI;
11
12use rust_robotics_core::{RoboticsError, RoboticsResult, State2D, StateEstimator};
13
14/// State representation for MCL (x, y, yaw, velocity).
15pub type MCLState = Vector4<f64>;
16
17/// Control input (velocity, yaw rate).
18pub type MCLControl = Vector2<f64>;
19
20/// Landmark observation (distance, landmark_x, landmark_y).
21pub type MCLMeasurement = Vec<(f64, f64, f64)>;
22
23const X_BIN_SIZE: f64 = 0.5;
24const Y_BIN_SIZE: f64 = 0.5;
25const YAW_BIN_SIZE: f64 = 15.0 * PI / 180.0;
26
27/// Single particle.
28#[derive(Debug, Clone)]
29pub struct Particle {
30    pub x: f64,
31    pub y: f64,
32    pub yaw: f64,
33    pub v: f64,
34    pub w: f64,
35}
36
37impl Particle {
38    fn new(x: f64, y: f64, yaw: f64, v: f64, n_particles: usize) -> Self {
39        Self {
40            x,
41            y,
42            yaw,
43            v,
44            w: 1.0 / n_particles as f64,
45        }
46    }
47}
48
49/// Configuration for adaptive MCL.
50#[derive(Debug, Clone)]
51pub struct MonteCarloLocalizationConfig {
52    /// Minimum number of particles.
53    pub min_particles: usize,
54    /// Maximum number of particles.
55    pub max_particles: usize,
56    /// KLD error bound (epsilon).
57    pub kld_epsilon: f64,
58    /// Standard normal quantile (z) used in KLD bound.
59    pub kld_z: f64,
60    /// Range measurement noise (sigma).
61    pub range_noise: f64,
62    /// Velocity process noise (sigma).
63    pub velocity_noise: f64,
64    /// Yaw-rate process noise (sigma).
65    pub yaw_rate_noise: f64,
66    /// Time step.
67    pub dt: f64,
68}
69
70impl Default for MonteCarloLocalizationConfig {
71    fn default() -> Self {
72        Self {
73            min_particles: 100,
74            max_particles: 5000,
75            kld_epsilon: 0.05,
76            kld_z: 2.326,
77            range_noise: 0.2,
78            velocity_noise: 2.0,
79            yaw_rate_noise: 40.0_f64.to_radians(),
80            dt: 0.1,
81        }
82    }
83}
84
85impl MonteCarloLocalizationConfig {
86    /// Validate configuration.
87    pub fn validate(&self) -> RoboticsResult<()> {
88        if self.min_particles == 0 {
89            return Err(RoboticsError::InvalidParameter(
90                "MCL min_particles must be greater than zero".to_string(),
91            ));
92        }
93        if self.max_particles < self.min_particles {
94            return Err(RoboticsError::InvalidParameter(
95                "MCL max_particles must be greater than or equal to min_particles".to_string(),
96            ));
97        }
98        if !self.kld_epsilon.is_finite() || self.kld_epsilon <= 0.0 {
99            return Err(RoboticsError::InvalidParameter(
100                "MCL kld_epsilon must be positive and finite".to_string(),
101            ));
102        }
103        if !self.kld_z.is_finite() || self.kld_z <= 0.0 {
104            return Err(RoboticsError::InvalidParameter(
105                "MCL kld_z must be positive and finite".to_string(),
106            ));
107        }
108        if !self.range_noise.is_finite() || self.range_noise <= 0.0 {
109            return Err(RoboticsError::InvalidParameter(
110                "MCL range_noise must be positive and finite".to_string(),
111            ));
112        }
113        if !self.velocity_noise.is_finite() || self.velocity_noise < 0.0 {
114            return Err(RoboticsError::InvalidParameter(
115                "MCL velocity_noise must be non-negative and finite".to_string(),
116            ));
117        }
118        if !self.yaw_rate_noise.is_finite() || self.yaw_rate_noise < 0.0 {
119            return Err(RoboticsError::InvalidParameter(
120                "MCL yaw_rate_noise must be non-negative and finite".to_string(),
121            ));
122        }
123        if !self.dt.is_finite() || self.dt <= 0.0 {
124            return Err(RoboticsError::InvalidParameter(
125                "MCL dt must be positive and finite".to_string(),
126            ));
127        }
128
129        Ok(())
130    }
131}
132
133/// Adaptive Monte Carlo localizer.
134pub struct MonteCarloLocalizer {
135    particles: Vec<Particle>,
136    config: MonteCarloLocalizationConfig,
137    state_estimate: MCLState,
138    covariance_dyn: nalgebra::DMatrix<f64>,
139}
140
141impl MonteCarloLocalizer {
142    /// Create a new localizer; panics on invalid config.
143    pub fn new(config: MonteCarloLocalizationConfig) -> Self {
144        Self::try_new(config).expect(
145            "invalid MCL configuration: particle bounds, KLD params, noises, and dt must be valid",
146        )
147    }
148
149    /// Create a new validated localizer.
150    pub fn try_new(config: MonteCarloLocalizationConfig) -> RoboticsResult<Self> {
151        config.validate()?;
152        let n = config.min_particles;
153        let particles = (0..n)
154            .map(|_| Particle::new(0.0, 0.0, 0.0, 0.0, n))
155            .collect();
156        let mut mcl = Self {
157            particles,
158            config,
159            state_estimate: MCLState::zeros(),
160            covariance_dyn: nalgebra::DMatrix::zeros(4, 4),
161        };
162        mcl.refresh_cache();
163        Ok(mcl)
164    }
165
166    /// Create with initial state; panics on invalid inputs.
167    pub fn with_initial_state(
168        initial_state: MCLState,
169        config: MonteCarloLocalizationConfig,
170    ) -> Self {
171        Self::try_with_initial_state(initial_state, config)
172            .expect("invalid MCL initialization: state and configuration must be valid")
173    }
174
175    /// Create with validated initial state.
176    pub fn try_with_initial_state(
177        initial_state: MCLState,
178        config: MonteCarloLocalizationConfig,
179    ) -> RoboticsResult<Self> {
180        config.validate()?;
181        if initial_state.iter().any(|value| !value.is_finite()) {
182            return Err(RoboticsError::InvalidParameter(
183                "MCL initial state must contain only finite values".to_string(),
184            ));
185        }
186
187        let mut rng = rand::rng();
188        let n = config.min_particles;
189        let mut particles = Vec::with_capacity(n);
190        for _ in 0..n {
191            let x = initial_state[0] + rng.random_range(-1.0..1.0);
192            let y = initial_state[1] + rng.random_range(-1.0..1.0);
193            let yaw = initial_state[2] + rng.random_range(-0.25..0.25);
194            let v = initial_state[3] + rng.random_range(-0.5..0.5);
195            particles.push(Particle::new(x, y, yaw, v, n));
196        }
197
198        let mut mcl = Self {
199            particles,
200            config,
201            state_estimate: MCLState::zeros(),
202            covariance_dyn: nalgebra::DMatrix::zeros(4, 4),
203        };
204        mcl.refresh_cache();
205        Ok(mcl)
206    }
207
208    /// Predict particles with the same motion model as `particle_filter.rs`.
209    pub fn try_predict_with_control(&mut self, control: &MCLControl) -> RoboticsResult<()> {
210        if control.iter().any(|value| !value.is_finite()) {
211            return Err(RoboticsError::InvalidParameter(
212                "MCL control input must contain only finite values".to_string(),
213            ));
214        }
215
216        let mut rng = rand::rng();
217        let normal_v = if self.config.velocity_noise > 0.0 {
218            Some(Normal::new(0.0, self.config.velocity_noise).map_err(|_| {
219                RoboticsError::InvalidParameter(
220                    "MCL velocity_noise must be a valid standard deviation".to_string(),
221                )
222            })?)
223        } else {
224            None
225        };
226        let normal_yaw = if self.config.yaw_rate_noise > 0.0 {
227            Some(Normal::new(0.0, self.config.yaw_rate_noise).map_err(|_| {
228                RoboticsError::InvalidParameter(
229                    "MCL yaw_rate_noise must be a valid standard deviation".to_string(),
230                )
231            })?)
232        } else {
233            None
234        };
235
236        for particle in &mut self.particles {
237            let v_noise = normal_v
238                .as_ref()
239                .map(|normal| normal.sample(&mut rng))
240                .unwrap_or(0.0);
241            let yaw_noise = normal_yaw
242                .as_ref()
243                .map(|normal| normal.sample(&mut rng))
244                .unwrap_or(0.0);
245
246            let v_noisy = control[0] + v_noise;
247            let yaw_rate_noisy = control[1] + yaw_noise;
248
249            particle.x += v_noisy * particle.yaw.cos() * self.config.dt;
250            particle.y += v_noisy * particle.yaw.sin() * self.config.dt;
251            particle.yaw += yaw_rate_noisy * self.config.dt;
252            particle.v = v_noisy;
253        }
254
255        self.refresh_cache();
256        Ok(())
257    }
258
259    /// Update particle weights with range observations.
260    pub fn try_update_with_observations(
261        &mut self,
262        observations: &MCLMeasurement,
263    ) -> RoboticsResult<()> {
264        if observations
265            .iter()
266            .any(|(d, x, y)| !d.is_finite() || !x.is_finite() || !y.is_finite() || *d < 0.0)
267        {
268            return Err(RoboticsError::InvalidParameter(
269                "MCL observations must have finite, non-negative distances".to_string(),
270            ));
271        }
272
273        for particle in &mut self.particles {
274            let mut w = 1.0;
275            for &(d_obs, lx, ly) in observations {
276                let dx = particle.x - lx;
277                let dy = particle.y - ly;
278                let d_pred = (dx * dx + dy * dy).sqrt();
279                let diff = d_obs - d_pred;
280                w *= Self::gauss_likelihood(diff, self.config.range_noise);
281            }
282            particle.w = w;
283        }
284
285        self.normalize_weights();
286        self.refresh_cache();
287        Ok(())
288    }
289
290    /// Full estimation step.
291    pub fn try_step(
292        &mut self,
293        control: &MCLControl,
294        observations: &MCLMeasurement,
295    ) -> RoboticsResult<MCLState> {
296        self.try_predict_with_control(control)?;
297        self.try_update_with_observations(observations)?;
298        self.resample_adaptive();
299        Ok(self.estimate())
300    }
301
302    /// Get estimated state.
303    pub fn estimate(&self) -> MCLState {
304        self.state_estimate
305    }
306
307    /// Get estimated state as common type.
308    pub fn state_2d(&self) -> State2D {
309        State2D::new(
310            self.state_estimate[0],
311            self.state_estimate[1],
312            self.state_estimate[2],
313            self.state_estimate[3],
314        )
315    }
316
317    /// Current number of particles.
318    pub fn particle_count(&self) -> usize {
319        self.particles.len()
320    }
321
322    fn resample_adaptive(&mut self) {
323        let n_current = self.particles.len();
324        if n_current == 0 {
325            return;
326        }
327
328        let mut cumulative_weights = Vec::with_capacity(n_current);
329        let mut cum_sum = 0.0;
330        for particle in &self.particles {
331            cum_sum += particle.w;
332            cumulative_weights.push(cum_sum);
333        }
334        if let Some(last) = cumulative_weights.last_mut() {
335            *last = 1.0;
336        }
337
338        let mut rng = rand::rng();
339        let mut bins: HashSet<(i32, i32, i32)> = HashSet::new();
340        let mut new_particles = Vec::with_capacity(self.config.max_particles);
341        let mut required = self.config.min_particles;
342
343        while new_particles.len() < self.config.max_particles {
344            let idx = Self::sample_index(&cumulative_weights, rng.random::<f64>());
345            let sampled = &self.particles[idx];
346
347            bins.insert(Self::quantize_particle(sampled));
348            let k = bins.len();
349            required = required.max(self.kld_required_particles(k));
350
351            new_particles.push(sampled.clone());
352            if new_particles.len() >= self.config.min_particles && new_particles.len() >= required {
353                break;
354            }
355        }
356
357        let n_new = new_particles.len();
358        let uniform_weight = 1.0 / n_new as f64;
359        for particle in &mut new_particles {
360            particle.w = uniform_weight;
361        }
362
363        self.particles = new_particles;
364        self.refresh_cache();
365    }
366
367    fn kld_required_particles(&self, k_bins: usize) -> usize {
368        if k_bins <= 1 {
369            return self.config.min_particles;
370        }
371
372        let k_minus_one = (k_bins - 1) as f64;
373        let term = 1.0 - 2.0 / (9.0 * k_minus_one)
374            + self.config.kld_z * (2.0 / (9.0 * k_minus_one)).sqrt();
375        let n = (k_minus_one / (2.0 * self.config.kld_epsilon)) * term.powi(3);
376
377        (n.ceil() as usize).clamp(self.config.min_particles, self.config.max_particles)
378    }
379
380    fn quantize_particle(particle: &Particle) -> (i32, i32, i32) {
381        let x_bin = (particle.x / X_BIN_SIZE).floor() as i32;
382        let y_bin = (particle.y / Y_BIN_SIZE).floor() as i32;
383        let yaw_bin = (particle.yaw / YAW_BIN_SIZE).floor() as i32;
384        (x_bin, y_bin, yaw_bin)
385    }
386
387    fn sample_index(cumulative_weights: &[f64], r: f64) -> usize {
388        cumulative_weights
389            .iter()
390            .position(|&w| r <= w)
391            .unwrap_or(cumulative_weights.len() - 1)
392    }
393
394    fn normalize_weights(&mut self) {
395        let sum_w: f64 = self.particles.iter().map(|p| p.w).sum();
396        if sum_w > 0.0 {
397            for particle in &mut self.particles {
398                particle.w /= sum_w;
399            }
400        } else {
401            let uniform_weight = 1.0 / self.particles.len() as f64;
402            for particle in &mut self.particles {
403                particle.w = uniform_weight;
404            }
405        }
406    }
407
408    fn gauss_likelihood(x: f64, sigma: f64) -> f64 {
409        let coeff = 1.0 / (2.0 * PI * sigma.powi(2)).sqrt();
410        coeff * (-x.powi(2) / (2.0 * sigma.powi(2))).exp()
411    }
412
413    fn compute_estimate(&self) -> MCLState {
414        let mut x_est = 0.0;
415        let mut y_est = 0.0;
416        let mut yaw_est = 0.0;
417        let mut v_est = 0.0;
418
419        for particle in &self.particles {
420            x_est += particle.w * particle.x;
421            y_est += particle.w * particle.y;
422            yaw_est += particle.w * particle.yaw;
423            v_est += particle.w * particle.v;
424        }
425
426        Vector4::new(x_est, y_est, yaw_est, v_est)
427    }
428
429    fn compute_covariance(&self, x_est: &MCLState) -> Matrix4<f64> {
430        let mut cov = Matrix4::zeros();
431        for particle in &self.particles {
432            let dx = Vector4::new(
433                particle.x - x_est[0],
434                particle.y - x_est[1],
435                particle.yaw - x_est[2],
436                particle.v - x_est[3],
437            );
438            cov += particle.w * dx * dx.transpose();
439        }
440        cov
441    }
442
443    fn refresh_cache(&mut self) {
444        self.state_estimate = self.compute_estimate();
445        let covariance = self.compute_covariance(&self.state_estimate);
446        self.covariance_dyn = nalgebra::DMatrix::from_fn(4, 4, |i, j| covariance[(i, j)]);
447    }
448}
449
450impl StateEstimator for MonteCarloLocalizer {
451    type State = MCLState;
452    type Measurement = MCLMeasurement;
453    type Control = MCLControl;
454
455    fn predict(&mut self, control: &Self::Control, _dt: f64) {
456        let _ = self.try_predict_with_control(control);
457    }
458
459    fn update(&mut self, measurement: &Self::Measurement) {
460        let _ = self.try_update_with_observations(measurement);
461        self.resample_adaptive();
462    }
463
464    fn get_state(&self) -> &Self::State {
465        &self.state_estimate
466    }
467
468    fn get_covariance(&self) -> Option<&nalgebra::DMatrix<f64>> {
469        Some(&self.covariance_dyn)
470    }
471}
472
473#[cfg(test)]
474mod tests {
475    use super::*;
476
477    fn synthetic_observations(state: &MCLState, landmarks: &[(f64, f64)]) -> MCLMeasurement {
478        landmarks
479            .iter()
480            .map(|&(lx, ly)| {
481                let dx = state[0] - lx;
482                let dy = state[1] - ly;
483                ((dx * dx + dy * dy).sqrt(), lx, ly)
484            })
485            .collect()
486    }
487
488    #[test]
489    fn test_mcl_converges() {
490        let config = MonteCarloLocalizationConfig {
491            min_particles: 250,
492            max_particles: 1200,
493            range_noise: 0.25,
494            velocity_noise: 0.05,
495            yaw_rate_noise: 0.02,
496            dt: 0.1,
497            ..Default::default()
498        };
499        let mut mcl = MonteCarloLocalizer::with_initial_state(MCLState::zeros(), config);
500        let landmarks = vec![(0.0, 0.0), (10.0, 0.0), (5.0, 8.0)];
501        let control = MCLControl::new(1.0, 0.03);
502        let mut truth = MCLState::zeros();
503
504        for _ in 0..60 {
505            truth[0] += control[0] * truth[2].cos() * 0.1;
506            truth[1] += control[0] * truth[2].sin() * 0.1;
507            truth[2] += control[1] * 0.1;
508            truth[3] = control[0];
509            let obs = synthetic_observations(&truth, &landmarks);
510            let _ = mcl.try_step(&control, &obs).unwrap();
511        }
512
513        let est = mcl.estimate();
514        let pos_err = ((est[0] - truth[0]).powi(2) + (est[1] - truth[1]).powi(2)).sqrt();
515        assert!(pos_err < 1.0, "position error too high: {pos_err}");
516    }
517
518    #[test]
519    fn test_mcl_particle_count_adapts() {
520        let config = MonteCarloLocalizationConfig {
521            min_particles: 100,
522            max_particles: 1500,
523            ..Default::default()
524        };
525        let mut mcl = MonteCarloLocalizer::new(config);
526
527        mcl.particles.clear();
528        let multimodal_n = 800;
529        for i in 0..multimodal_n {
530            let cluster = i % 4;
531            let base_x = cluster as f64 * 3.0;
532            let base_y = cluster as f64 * 2.0;
533            let mut p = Particle::new(base_x + (i as f64) * 0.002, base_y, 0.0, 0.0, multimodal_n);
534            p.w = 1.0 / multimodal_n as f64;
535            mcl.particles.push(p);
536        }
537        mcl.resample_adaptive();
538        let expanded_count = mcl.particle_count();
539        assert!(expanded_count > mcl.config.min_particles);
540
541        let concentrated_n = 600;
542        mcl.particles.clear();
543        for _ in 0..concentrated_n {
544            let mut p = Particle::new(1.0, 1.0, 0.1, 0.0, concentrated_n);
545            p.w = 1.0 / concentrated_n as f64;
546            mcl.particles.push(p);
547        }
548        mcl.resample_adaptive();
549        let reduced_count = mcl.particle_count();
550        assert!(reduced_count <= expanded_count);
551    }
552
553    #[test]
554    fn test_mcl_particle_count_stays_within_bounds() {
555        let config = MonteCarloLocalizationConfig {
556            min_particles: 120,
557            max_particles: 600,
558            velocity_noise: 0.5,
559            yaw_rate_noise: 0.2,
560            ..Default::default()
561        };
562        let mut mcl = MonteCarloLocalizer::new(config);
563        let landmarks = vec![(0.0, 0.0), (15.0, 0.0), (8.0, 12.0)];
564        let control = MCLControl::new(0.8, 0.05);
565        let mut truth = MCLState::zeros();
566
567        for _ in 0..40 {
568            truth[0] += control[0] * truth[2].cos() * 0.1;
569            truth[1] += control[0] * truth[2].sin() * 0.1;
570            truth[2] += control[1] * 0.1;
571            let obs = synthetic_observations(&truth, &landmarks);
572            let _ = mcl.try_step(&control, &obs).unwrap();
573            assert!(mcl.particle_count() >= mcl.config.min_particles);
574            assert!(mcl.particle_count() <= mcl.config.max_particles);
575        }
576    }
577
578    #[test]
579    fn test_mcl_config_defaults() {
580        let config = MonteCarloLocalizationConfig::default();
581        assert_eq!(config.min_particles, 100);
582        assert_eq!(config.max_particles, 5000);
583        assert!((config.kld_epsilon - 0.05).abs() < f64::EPSILON);
584        assert!((config.kld_z - 2.326).abs() < f64::EPSILON);
585    }
586}
rust_robotics_localization/monte_carlo_localization.rs

rust_robotics_localization/
monte_carlo_localization.rs
