Skip to main content

rust_robotics_slam/
fastslam2.rs

1#![allow(dead_code)]
2
3//! FastSLAM 2.0
4//!
5//! Improves on FastSLAM 1.0 by incorporating observations into the proposal
6//! distribution for particle pose sampling, resulting in better particle
7//! efficiency.
8//!
9//! author: Atsushi Sakai (@Atsushi_twi)
10//!         Ryohei Sasaki (@rsasaki0109)
11//!         Rust port
12
13use nalgebra::{Matrix2, Matrix2x3, Matrix3, Vector2, Vector3};
14use rand::Rng;
15use rand_distr::{Distribution, Normal, Uniform};
16use rust_robotics_core::normalize_angle;
17use std::f64::consts::PI;
18
19// Simulation parameters
20const DT: f64 = 0.1;
21const MAX_RANGE: f64 = 20.0;
22
23// Particle filter parameters
24const N_PARTICLE: usize = 100;
25const NTH: f64 = N_PARTICLE as f64 / 1.5;
26
27// Noise parameters
28const Q_SIM: [[f64; 2]; 2] = [[0.3, 0.0], [0.0, 0.0305]];
29const R_SIM: [[f64; 2]; 2] = [[0.5, 0.0], [0.0, 0.0305]];
30
31// Motion noise (for proposal distribution)
32const MOTION_COV: [[f64; 3]; 3] = [[0.1, 0.0, 0.0], [0.0, 0.1, 0.0], [0.0, 0.0, 0.01]];
33
34#[derive(Clone)]
35pub struct Landmark {
36    pub x: f64,
37    pub y: f64,
38    pub cov: Matrix2<f64>,
39}
40
41impl Landmark {
42    fn new() -> Self {
43        Landmark {
44            x: 0.0,
45            y: 0.0,
46            cov: Matrix2::identity() * 1000.0,
47        }
48    }
49
50    fn is_initialized(&self) -> bool {
51        self.cov[(0, 0)] < 100.0
52    }
53}
54
55#[derive(Clone)]
56pub struct Particle {
57    pub weight: f64,
58    pub x: f64,
59    pub y: f64,
60    pub yaw: f64,
61    pub landmarks: Vec<Landmark>,
62}
63
64impl Particle {
65    fn new(n_landmarks: usize) -> Self {
66        Particle {
67            weight: 1.0 / N_PARTICLE as f64,
68            x: 0.0,
69            y: 0.0,
70            yaw: 0.0,
71            landmarks: vec![Landmark::new(); n_landmarks],
72        }
73    }
74
75    fn pose(&self) -> Vector3<f64> {
76        Vector3::new(self.x, self.y, self.yaw)
77    }
78
79    fn set_pose(&mut self, pose: &Vector3<f64>) {
80        self.x = pose[0];
81        self.y = pose[1];
82        self.yaw = normalize_angle(pose[2]);
83    }
84}
85
86fn motion_model(x: Vector3<f64>, u: Vector2<f64>) -> Vector3<f64> {
87    let yaw = x[2];
88    Vector3::new(
89        x[0] + u[0] * DT * yaw.cos(),
90        x[1] + u[0] * DT * yaw.sin(),
91        normalize_angle(x[2] + u[1] * DT),
92    )
93}
94
95/// Jacobian of motion model w.r.t. state
96fn motion_jacobian(x: &Vector3<f64>, u: &Vector2<f64>) -> Matrix3<f64> {
97    let yaw = x[2];
98    let v = u[0];
99    Matrix3::new(
100        1.0,
101        0.0,
102        -v * DT * yaw.sin(),
103        0.0,
104        1.0,
105        v * DT * yaw.cos(),
106        0.0,
107        0.0,
108        1.0,
109    )
110}
111
112fn observation_model(px: f64, py: f64, pyaw: f64, lm_x: f64, lm_y: f64) -> Vector2<f64> {
113    let dx = lm_x - px;
114    let dy = lm_y - py;
115    let d = (dx * dx + dy * dy).sqrt();
116    let angle = normalize_angle(dy.atan2(dx) - pyaw);
117    Vector2::new(d, angle)
118}
119
120/// Jacobian of observation model w.r.t. landmark position
121fn obs_jacobian_landmark(px: f64, py: f64, lm_x: f64, lm_y: f64) -> Matrix2<f64> {
122    let dx = lm_x - px;
123    let dy = lm_y - py;
124    let d2 = dx * dx + dy * dy;
125    let d = d2.sqrt();
126    Matrix2::new(dx / d, dy / d, -dy / d2, dx / d2)
127}
128
129/// Jacobian of observation model w.r.t. particle pose [x, y, yaw]
130fn obs_jacobian_pose(px: f64, py: f64, pyaw: f64, lm_x: f64, lm_y: f64) -> Matrix2x3<f64> {
131    let dx = lm_x - px;
132    let dy = lm_y - py;
133    let d2 = dx * dx + dy * dy;
134    let d = d2.sqrt();
135    let _ = pyaw; // yaw only affects angle via subtraction
136    Matrix2x3::new(-dx / d, -dy / d, 0.0, dy / d2, -dx / d2, -1.0)
137}
138
139fn get_q() -> Matrix2<f64> {
140    Matrix2::new(Q_SIM[0][0], Q_SIM[0][1], Q_SIM[1][0], Q_SIM[1][1])
141}
142
143fn get_r() -> Matrix2<f64> {
144    Matrix2::new(R_SIM[0][0], R_SIM[0][1], R_SIM[1][0], R_SIM[1][1])
145}
146
147fn get_motion_cov() -> Matrix3<f64> {
148    Matrix3::new(
149        MOTION_COV[0][0],
150        MOTION_COV[0][1],
151        MOTION_COV[0][2],
152        MOTION_COV[1][0],
153        MOTION_COV[1][1],
154        MOTION_COV[1][2],
155        MOTION_COV[2][0],
156        MOTION_COV[2][1],
157        MOTION_COV[2][2],
158    )
159}
160
161/// FastSLAM 2.0: compute the improved proposal distribution for a particle
162/// given an observation of a known landmark, and sample from it.
163fn compute_proposal(
164    particle: &Particle,
165    u: &Vector2<f64>,
166    z: &Vector2<f64>,
167    lm_id: usize,
168    r: &Matrix2<f64>,
169) -> (Vector3<f64>, Matrix3<f64>) {
170    let lm = &particle.landmarks[lm_id];
171
172    // Prior: motion model prediction
173    let x_pred = motion_model(particle.pose(), *u);
174    let g = motion_jacobian(&particle.pose(), u);
175    let motion_cov = get_motion_cov();
176    let p_pred = g * motion_cov * g.transpose();
177
178    if !lm.is_initialized() {
179        // No observation update possible for uninitialized landmark
180        return (x_pred, p_pred);
181    }
182
183    // Observation Jacobian w.r.t. pose at predicted pose
184    let h_pose = obs_jacobian_pose(x_pred[0], x_pred[1], x_pred[2], lm.x, lm.y);
185
186    // Innovation covariance contribution from landmark
187    let h_lm = obs_jacobian_landmark(x_pred[0], x_pred[1], lm.x, lm.y);
188    let q_obs = h_lm * lm.cov * h_lm.transpose() + r;
189
190    // Fuse prior (motion) with observation likelihood
191    // Posterior precision = prior precision + observation precision
192    let h_pose_t = h_pose.transpose();
193    let q_obs_inv = q_obs.try_inverse().unwrap_or(Matrix2::identity());
194
195    let p_pred_inv = p_pred.try_inverse().unwrap_or(Matrix3::identity() * 1e-6);
196    let p_post_inv = p_pred_inv + h_pose_t * q_obs_inv * h_pose;
197    let p_post = p_post_inv.try_inverse().unwrap_or(p_pred);
198
199    // Posterior mean
200    let z_pred = observation_model(x_pred[0], x_pred[1], x_pred[2], lm.x, lm.y);
201    let innovation = Vector2::new(z[0] - z_pred[0], normalize_angle(z[1] - z_pred[1]));
202
203    let x_post = x_pred + p_post * h_pose_t * q_obs_inv * innovation;
204
205    (x_post, p_post)
206}
207
208/// Sample a 3D pose from a Gaussian distribution
209fn sample_pose_with_rng<R: Rng + ?Sized>(
210    mean: &Vector3<f64>,
211    cov: &Matrix3<f64>,
212    rng: &mut R,
213) -> Vector3<f64> {
214    let normal = Normal::new(0.0, 1.0).unwrap();
215
216    // Cholesky decomposition for sampling
217    let l = cov.cholesky().map(|c| c.l()).unwrap_or_else(|| {
218        // Fallback: use diagonal sqrt
219        Matrix3::from_diagonal(&Vector3::new(
220            cov[(0, 0)].max(0.0).sqrt(),
221            cov[(1, 1)].max(0.0).sqrt(),
222            cov[(2, 2)].max(0.0).sqrt(),
223        ))
224    });
225
226    let noise = Vector3::new(normal.sample(rng), normal.sample(rng), normal.sample(rng));
227
228    mean + l * noise
229}
230
231/// Update landmark EKF and compute weight (FastSLAM 2.0 version)
232fn update_landmark_and_weight(
233    particle: &mut Particle,
234    z: &Vector2<f64>,
235    lm_id: usize,
236    r: &Matrix2<f64>,
237) -> f64 {
238    let lm = &particle.landmarks[lm_id];
239
240    if !lm.is_initialized() {
241        // Initialize landmark
242        particle.landmarks[lm_id].x = particle.x + z[0] * (particle.yaw + z[1]).cos();
243        particle.landmarks[lm_id].y = particle.y + z[0] * (particle.yaw + z[1]).sin();
244        particle.landmarks[lm_id].cov = Matrix2::identity() * 10.0;
245        return 1.0; // neutral weight for new landmark
246    }
247
248    let z_pred = observation_model(particle.x, particle.y, particle.yaw, lm.x, lm.y);
249    let innovation = Vector2::new(z[0] - z_pred[0], normalize_angle(z[1] - z_pred[1]));
250
251    let h = obs_jacobian_landmark(particle.x, particle.y, lm.x, lm.y);
252    let s = h * particle.landmarks[lm_id].cov * h.transpose() + r;
253    let s_inv = s.try_inverse().unwrap_or(Matrix2::identity());
254    let k = particle.landmarks[lm_id].cov * h.transpose() * s_inv;
255
256    // Update landmark
257    let delta = k * innovation;
258    particle.landmarks[lm_id].x += delta[0];
259    particle.landmarks[lm_id].y += delta[1];
260    particle.landmarks[lm_id].cov = (Matrix2::identity() - k * h) * particle.landmarks[lm_id].cov;
261
262    // Compute weight from likelihood
263    let det_s = s.determinant();
264    if det_s > 0.0 {
265        let mahal = innovation.transpose() * s_inv * innovation;
266        (-0.5 * mahal[(0, 0)]).exp() / (2.0 * PI * det_s.sqrt())
267    } else {
268        1e-10
269    }
270}
271
272fn compute_neff(particles: &[Particle]) -> f64 {
273    let sum_w2: f64 = particles.iter().map(|p| p.weight * p.weight).sum();
274    if sum_w2 > 0.0 {
275        1.0 / sum_w2
276    } else {
277        0.0
278    }
279}
280
281fn normalize_weights(particles: &mut [Particle]) {
282    let sum_w: f64 = particles.iter().map(|p| p.weight).sum();
283    if sum_w > 0.0 {
284        for p in particles.iter_mut() {
285            p.weight /= sum_w;
286        }
287    }
288}
289
290fn resample_with_rng<R: Rng + ?Sized>(particles: &mut Vec<Particle>, rng: &mut R) {
291    normalize_weights(particles);
292    let n = particles.len();
293    let mut new_particles = Vec::with_capacity(n);
294
295    let mut cum_sum = vec![0.0; n + 1];
296    for (i, p) in particles.iter().enumerate() {
297        cum_sum[i + 1] = cum_sum[i] + p.weight;
298    }
299
300    let uniform = Uniform::new(0.0, 1.0 / n as f64).expect("valid resampling range");
301    let mut r = uniform.sample(rng);
302    let mut j = 0;
303    for _ in 0..n {
304        while r > cum_sum[j + 1] && j < n - 1 {
305            j += 1;
306        }
307        let mut new_p = particles[j].clone();
308        new_p.weight = 1.0 / n as f64;
309        new_particles.push(new_p);
310        r += 1.0 / n as f64;
311    }
312
313    *particles = new_particles;
314}
315
316/// FastSLAM 2.0 update step.
317///
318/// Key difference from FastSLAM 1.0: the particle pose is sampled from a
319/// proposal distribution that incorporates the observation, not just the
320/// motion model.
321fn fastslam2_update_with_rng<R: Rng + ?Sized>(
322    particles: &mut Vec<Particle>,
323    u: Vector2<f64>,
324    z: &[(f64, f64, usize)], // (distance, angle, landmark_id)
325    rng: &mut R,
326) {
327    let r = get_r();
328
329    for particle in particles.iter_mut() {
330        // FastSLAM 2.0: compute proposal using first observation (if any)
331        if let Some(&(d, angle, lm_id)) = z.first() {
332            let z_vec = Vector2::new(d, angle);
333            let (proposal_mean, proposal_cov) = compute_proposal(particle, &u, &z_vec, lm_id, &r);
334
335            // Sample pose from improved proposal
336            let sampled_pose = sample_pose_with_rng(&proposal_mean, &proposal_cov, rng);
337            particle.set_pose(&sampled_pose);
338        } else {
339            // No observations: fall back to motion model sampling
340            let normal = Normal::new(0.0, 1.0).unwrap();
341            let q = get_q();
342            let u_noisy = Vector2::new(
343                u[0] + normal.sample(rng) * q[(0, 0)].sqrt(),
344                u[1] + normal.sample(rng) * q[(1, 1)].sqrt(),
345            );
346            let pose = motion_model(particle.pose(), u_noisy);
347            particle.set_pose(&pose);
348        }
349
350        // Update landmarks and compute weights
351        for &(d, angle, lm_id) in z {
352            let z_vec = Vector2::new(d, angle);
353            let w = update_landmark_and_weight(particle, &z_vec, lm_id, &r);
354            particle.weight *= w;
355        }
356    }
357
358    normalize_weights(particles);
359
360    let neff = compute_neff(particles);
361    if neff < NTH {
362        resample_with_rng(particles, rng);
363    }
364}
365
366pub fn fastslam2_update(
367    particles: &mut Vec<Particle>,
368    u: Vector2<f64>,
369    z: &[(f64, f64, usize)], // (distance, angle, landmark_id)
370) {
371    let mut rng = rand::rng();
372    fastslam2_update_with_rng(particles, u, z, &mut rng);
373}
374
375pub fn get_best_particle(particles: &[Particle]) -> &Particle {
376    particles
377        .iter()
378        .max_by(|a, b| a.weight.partial_cmp(&b.weight).unwrap())
379        .unwrap()
380}
381
382fn get_observations_with_rng<R: Rng + ?Sized>(
383    x_true: &Vector3<f64>,
384    landmarks: &[(f64, f64)],
385    rng: &mut R,
386) -> Vec<(f64, f64, usize)> {
387    let normal = Normal::new(0.0, 1.0).unwrap();
388    let r = get_r();
389    let mut z = Vec::new();
390
391    for (lm_id, (lx, ly)) in landmarks.iter().enumerate() {
392        let dx = lx - x_true[0];
393        let dy = ly - x_true[1];
394        let d = (dx * dx + dy * dy).sqrt();
395
396        if d <= MAX_RANGE {
397            let angle = normalize_angle(dy.atan2(dx) - x_true[2]);
398            let d_noisy = d + normal.sample(rng) * r[(0, 0)].sqrt();
399            let angle_noisy = angle + normal.sample(rng) * r[(1, 1)].sqrt();
400            z.push((d_noisy, angle_noisy, lm_id));
401        }
402    }
403
404    z
405}
406
407pub fn get_observations(x_true: &Vector3<f64>, landmarks: &[(f64, f64)]) -> Vec<(f64, f64, usize)> {
408    let mut rng = rand::rng();
409    get_observations_with_rng(x_true, landmarks, &mut rng)
410}
411
412pub fn create_particles(n_particles: usize, n_landmarks: usize) -> Vec<Particle> {
413    (0..n_particles)
414        .map(|_| Particle::new(n_landmarks))
415        .collect()
416}
417
418#[cfg(test)]
419mod tests {
420    use super::*;
421    use rand::{rngs::StdRng, SeedableRng};
422
423    #[test]
424    fn test_create_particles() {
425        let particles = create_particles(50, 5);
426        assert_eq!(particles.len(), 50);
427        for p in &particles {
428            assert_eq!(p.landmarks.len(), 5);
429        }
430    }
431
432    #[test]
433    fn test_fastslam2_update_does_not_panic() {
434        let mut particles = create_particles(20, 3);
435        let u = Vector2::new(1.0, 0.1);
436        let landmarks = vec![(10.0, 0.0), (0.0, 10.0), (10.0, 10.0)];
437        let x_true = Vector3::new(0.0, 0.0, 0.0);
438        let mut rng = StdRng::seed_from_u64(7);
439
440        for _ in 0..5 {
441            let z = get_observations_with_rng(&x_true, &landmarks, &mut rng);
442            fastslam2_update_with_rng(&mut particles, u, &z, &mut rng);
443        }
444
445        assert_eq!(particles.len(), 20);
446    }
447
448    #[test]
449    fn test_proposal_improves_with_observation() {
450        let mut p = Particle::new(1);
451        // Initialize a landmark at (5, 0)
452        p.landmarks[0].x = 5.0;
453        p.landmarks[0].y = 0.0;
454        p.landmarks[0].cov = Matrix2::identity() * 0.5;
455
456        let u = Vector2::new(1.0, 0.0);
457        let z = Vector2::new(5.0, 0.0); // observe landmark at distance=5, angle=0
458        let r = get_r();
459
460        let (mean, cov) = compute_proposal(&p, &u, &z, 0, &r);
461
462        // Proposal covariance should be smaller than motion-only covariance
463        let g = motion_jacobian(&p.pose(), &u);
464        let motion_only_cov = g * get_motion_cov() * g.transpose();
465
466        assert!(
467            cov.determinant() < motion_only_cov.determinant(),
468            "proposal should have less uncertainty than motion-only"
469        );
470
471        // Mean should be close to predicted (no large deviation for consistent obs)
472        let x_pred = motion_model(p.pose(), u);
473        let diff = (mean - x_pred).norm();
474        assert!(
475            diff < 1.0,
476            "proposal mean should be near prediction: diff={diff}"
477        );
478    }
479
480    #[test]
481    fn test_landmark_convergence() {
482        let mut particles = create_particles(120, 1);
483        let landmarks = vec![(5.0, 5.0)];
484        let mut x_true = Vector3::new(0.0, 0.0, PI / 4.0);
485        let u = Vector2::new(0.5, 0.0);
486        let mut rng = StdRng::seed_from_u64(17);
487
488        for _ in 0..60 {
489            x_true = motion_model(x_true, u);
490            let z = get_observations_with_rng(&x_true, &landmarks, &mut rng);
491            fastslam2_update_with_rng(&mut particles, u, &z, &mut rng);
492        }
493
494        let initialized: Vec<&Particle> = particles
495            .iter()
496            .filter(|particle| particle.landmarks[0].is_initialized())
497            .collect();
498        assert!(
499            !initialized.is_empty(),
500            "at least one particle should initialize the landmark"
501        );
502
503        let total_weight: f64 = initialized.iter().map(|particle| particle.weight).sum();
504        let (mean_x, mean_y) = if total_weight > 0.0 {
505            let weighted_x = initialized
506                .iter()
507                .map(|particle| particle.weight * particle.landmarks[0].x)
508                .sum::<f64>()
509                / total_weight;
510            let weighted_y = initialized
511                .iter()
512                .map(|particle| particle.weight * particle.landmarks[0].y)
513                .sum::<f64>()
514                / total_weight;
515            (weighted_x, weighted_y)
516        } else {
517            let avg_x = initialized
518                .iter()
519                .map(|particle| particle.landmarks[0].x)
520                .sum::<f64>()
521                / initialized.len() as f64;
522            let avg_y = initialized
523                .iter()
524                .map(|particle| particle.landmarks[0].y)
525                .sum::<f64>()
526                / initialized.len() as f64;
527            (avg_x, avg_y)
528        };
529
530        let lm_err = ((mean_x - 5.0).powi(2) + (mean_y - 5.0).powi(2)).sqrt();
531        assert!(
532            lm_err < 6.0,
533            "landmark estimate should converge: err={lm_err}"
534        );
535    }
536}