Skip to main content

rust_robotics_slam/
fastslam1.rs

1#![allow(dead_code)]
2
3// FastSLAM 1.0
4// author: Atsushi Sakai (@Atsushi_twi)
5//         Ryohei Sasaki (@rsasaki0109)
6//         Rust port
7
8use nalgebra::{Matrix2, Vector2, Vector3};
9use rand_distr::{Distribution, Normal, Uniform};
10use rust_robotics_core::normalize_angle;
11use std::f64::consts::PI;
12
13// Simulation parameters
14const DT: f64 = 0.1; // time step [s]
15const MAX_RANGE: f64 = 20.0; // maximum observation range [m]
16
17// Particle filter parameters
18const N_PARTICLE: usize = 100; // number of particles
19const NTH: f64 = N_PARTICLE as f64 / 1.5; // resampling threshold
20
21// Noise parameters
22// (10.0 * PI / 180.0)^2 ≈ 0.0305
23const Q_SIM: [[f64; 2]; 2] = [[0.3, 0.0], [0.0, 0.0305]]; // simulation noise
24const R_SIM: [[f64; 2]; 2] = [[0.5, 0.0], [0.0, 0.0305]]; // observation noise
25
26/// Landmark for EKF
27#[derive(Clone)]
28pub struct Landmark {
29    pub x: f64,
30    pub y: f64,
31    pub cov: Matrix2<f64>, // covariance
32}
33
34impl Landmark {
35    fn new() -> Self {
36        Landmark {
37            x: 0.0,
38            y: 0.0,
39            cov: Matrix2::identity() * 1000.0, // large initial uncertainty
40        }
41    }
42}
43
44/// Particle for FastSLAM
45#[derive(Clone)]
46pub struct Particle {
47    pub weight: f64,
48    pub x: f64,
49    pub y: f64,
50    pub yaw: f64,
51    pub landmarks: Vec<Landmark>,
52}
53
54impl Particle {
55    fn new(n_landmarks: usize) -> Self {
56        Particle {
57            weight: 1.0 / N_PARTICLE as f64,
58            x: 0.0,
59            y: 0.0,
60            yaw: 0.0,
61            landmarks: vec![Landmark::new(); n_landmarks],
62        }
63    }
64
65    fn pose(&self) -> Vector3<f64> {
66        Vector3::new(self.x, self.y, self.yaw)
67    }
68}
69
70/// Motion model for robot
71fn motion_model(x: Vector3<f64>, u: Vector2<f64>) -> Vector3<f64> {
72    let yaw = x[2];
73    Vector3::new(
74        x[0] + u[0] * DT * yaw.cos(),
75        x[1] + u[0] * DT * yaw.sin(),
76        normalize_angle(x[2] + u[1] * DT),
77    )
78}
79
80/// Observation model: predict observation from particle pose and landmark
81fn observation_model(particle: &Particle, lm_id: usize) -> Vector2<f64> {
82    let lm = &particle.landmarks[lm_id];
83    let dx = lm.x - particle.x;
84    let dy = lm.y - particle.y;
85    let d = (dx * dx + dy * dy).sqrt();
86    let angle = normalize_angle(dy.atan2(dx) - particle.yaw);
87    Vector2::new(d, angle)
88}
89
90/// Compute Jacobian of observation model w.r.t. landmark position
91fn compute_jacobian(particle: &Particle, lm_id: usize) -> Matrix2<f64> {
92    let lm = &particle.landmarks[lm_id];
93    let dx = lm.x - particle.x;
94    let dy = lm.y - particle.y;
95    let d2 = dx * dx + dy * dy;
96    let d = d2.sqrt();
97
98    Matrix2::new(dx / d, dy / d, -dy / d2, dx / d2)
99}
100
101/// Process noise covariance
102fn get_q() -> Matrix2<f64> {
103    Matrix2::new(Q_SIM[0][0], Q_SIM[0][1], Q_SIM[1][0], Q_SIM[1][1])
104}
105
106/// Observation noise covariance
107fn get_r() -> Matrix2<f64> {
108    Matrix2::new(R_SIM[0][0], R_SIM[0][1], R_SIM[1][0], R_SIM[1][1])
109}
110
111/// Update particle with control input (motion update)
112fn predict_particle(particle: &mut Particle, u: Vector2<f64>) {
113    let normal = Normal::new(0.0, 1.0).unwrap();
114    let q = get_q();
115
116    // Add noise to control
117    let u_noisy = Vector2::new(
118        u[0] + normal.sample(&mut rand::rng()) * q[(0, 0)].sqrt(),
119        u[1] + normal.sample(&mut rand::rng()) * q[(1, 1)].sqrt(),
120    );
121
122    let pose = motion_model(particle.pose(), u_noisy);
123    particle.x = pose[0];
124    particle.y = pose[1];
125    particle.yaw = pose[2];
126}
127
128/// Update landmark with EKF
129fn update_landmark(particle: &mut Particle, z: &Vector2<f64>, lm_id: usize, r: &Matrix2<f64>) {
130    let lm = &particle.landmarks[lm_id];
131
132    // First observation of this landmark
133    if lm.cov[(0, 0)] > 100.0 {
134        // Initialize the landmark from the observation, with the observation
135        // noise mapped into the map frame: Σ = H⁻¹ R H⁻ᵀ. Leaving the prior
136        // covariance in place would re-initialize it on every sighting.
137        particle.landmarks[lm_id].x = particle.x + z[0] * (particle.yaw + z[1]).cos();
138        particle.landmarks[lm_id].y = particle.y + z[0] * (particle.yaw + z[1]).sin();
139        let h = compute_jacobian(particle, lm_id);
140        if let Some(h_inv) = h.try_inverse() {
141            particle.landmarks[lm_id].cov = h_inv * r * h_inv.transpose();
142        }
143        return;
144    }
145
146    // Predicted observation
147    let z_pred = observation_model(particle, lm_id);
148
149    // Innovation
150    let y = Vector2::new(z[0] - z_pred[0], normalize_angle(z[1] - z_pred[1]));
151
152    // Jacobian
153    let h = compute_jacobian(particle, lm_id);
154
155    // Innovation covariance
156    let s = h * particle.landmarks[lm_id].cov * h.transpose() + r;
157
158    // Kalman gain
159    let s_inv = s.try_inverse().unwrap_or(Matrix2::identity());
160    let k = particle.landmarks[lm_id].cov * h.transpose() * s_inv;
161
162    // Update landmark position
163    let delta = k * y;
164    particle.landmarks[lm_id].x += delta[0];
165    particle.landmarks[lm_id].y += delta[1];
166
167    // Update covariance
168    let i = Matrix2::identity();
169    particle.landmarks[lm_id].cov = (i - k * h) * particle.landmarks[lm_id].cov;
170
171    // Update weight using likelihood
172    let det_s = s.determinant();
173    if det_s > 0.0 {
174        let mahal = y.transpose() * s_inv * y;
175        let likelihood = (-0.5 * mahal[(0, 0)]).exp() / (2.0 * PI * det_s.sqrt());
176        particle.weight *= likelihood;
177    }
178}
179
180/// Compute effective number of particles
181fn compute_neff(particles: &[Particle]) -> f64 {
182    let sum_w2: f64 = particles.iter().map(|p| p.weight * p.weight).sum();
183    if sum_w2 > 0.0 {
184        1.0 / sum_w2
185    } else {
186        0.0
187    }
188}
189
190/// Normalize particle weights
191fn normalize_weights(particles: &mut [Particle]) {
192    let sum_w: f64 = particles.iter().map(|p| p.weight).sum();
193    if sum_w > 0.0 {
194        for p in particles.iter_mut() {
195            p.weight /= sum_w;
196        }
197    }
198}
199
200/// Low variance resampling
201fn resample(particles: &mut Vec<Particle>) {
202    normalize_weights(particles);
203
204    let n = particles.len();
205    let mut new_particles = Vec::with_capacity(n);
206
207    // Cumulative sum of weights
208    let mut cum_sum = vec![0.0; n + 1];
209    for (i, p) in particles.iter().enumerate() {
210        cum_sum[i + 1] = cum_sum[i] + p.weight;
211    }
212
213    // Systematic resampling
214    let uniform = Uniform::new(0.0, 1.0 / n as f64).expect("valid resampling range");
215    let mut r = uniform.sample(&mut rand::rng());
216
217    let mut j = 0;
218    for _ in 0..n {
219        while r > cum_sum[j + 1] && j < n - 1 {
220            j += 1;
221        }
222        let mut new_p = particles[j].clone();
223        new_p.weight = 1.0 / n as f64;
224        new_particles.push(new_p);
225        r += 1.0 / n as f64;
226    }
227
228    *particles = new_particles;
229}
230
231/// FastSLAM update step
232pub fn fastslam_update(
233    particles: &mut Vec<Particle>,
234    u: Vector2<f64>,
235    z: &[(f64, f64, usize)], // (distance, angle, landmark_id)
236) {
237    let r = get_r();
238
239    // Prediction step
240    for particle in particles.iter_mut() {
241        predict_particle(particle, u);
242    }
243
244    // Update step for each observation
245    for (d, angle, lm_id) in z {
246        let z_vec = Vector2::new(*d, *angle);
247
248        for particle in particles.iter_mut() {
249            update_landmark(particle, &z_vec, *lm_id, &r);
250        }
251    }
252
253    // Normalize weights
254    normalize_weights(particles);
255
256    // Resample if needed
257    let neff = compute_neff(particles);
258    if neff < NTH {
259        resample(particles);
260    }
261}
262
263/// Get best particle (highest weight)
264pub fn get_best_particle(particles: &[Particle]) -> &Particle {
265    particles
266        .iter()
267        .max_by(|a, b| a.weight.partial_cmp(&b.weight).unwrap())
268        .unwrap()
269}
270
271/// Simulate observations
272pub fn get_observations(x_true: &Vector3<f64>, landmarks: &[(f64, f64)]) -> Vec<(f64, f64, usize)> {
273    let normal = Normal::new(0.0, 1.0).unwrap();
274    let r = get_r();
275    let mut z = Vec::new();
276
277    for (lm_id, (lx, ly)) in landmarks.iter().enumerate() {
278        let dx = lx - x_true[0];
279        let dy = ly - x_true[1];
280        let d = (dx * dx + dy * dy).sqrt();
281
282        if d <= MAX_RANGE {
283            let angle = normalize_angle(dy.atan2(dx) - x_true[2]);
284
285            // Add noise
286            let d_noisy = d + normal.sample(&mut rand::rng()) * r[(0, 0)].sqrt();
287            let angle_noisy = angle + normal.sample(&mut rand::rng()) * r[(1, 1)].sqrt();
288
289            z.push((d_noisy, angle_noisy, lm_id));
290        }
291    }
292
293    z
294}
295
296/// Create a new set of particles for FastSLAM
297pub fn create_particles(n_particles: usize, n_landmarks: usize) -> Vec<Particle> {
298    (0..n_particles)
299        .map(|_| Particle::new(n_landmarks))
300        .collect()
301}
302
303#[cfg(test)]
304mod tests {
305    use super::*;
306
307    #[test]
308    fn test_create_particles() {
309        let n_particles = 50;
310        let n_landmarks = 5;
311        let particles = create_particles(n_particles, n_landmarks);
312
313        assert_eq!(particles.len(), n_particles);
314        for p in &particles {
315            assert_eq!(p.landmarks.len(), n_landmarks);
316        }
317    }
318
319    #[test]
320    fn test_get_observations() {
321        // Robot at origin facing +x
322        let x_true = Vector3::new(0.0, 0.0, 0.0);
323        // One landmark within range, one outside
324        let landmarks = vec![(5.0, 0.0), (100.0, 100.0)];
325        let observations = get_observations(&x_true, &landmarks);
326
327        // Only the first landmark should be observed (within MAX_RANGE=20)
328        assert_eq!(observations.len(), 1);
329        let (d, _angle, lm_id) = observations[0];
330        assert_eq!(lm_id, 0);
331        // Distance should be roughly 5.0 (noisy)
332        assert!(
333            (d - 5.0).abs() < 5.0,
334            "distance {d} too far from expected 5.0"
335        );
336    }
337
338    #[test]
339    fn test_get_best_particle() {
340        let n_landmarks = 3;
341        let mut particles = create_particles(5, n_landmarks);
342        // Assign distinct weights
343        particles[0].weight = 0.1;
344        particles[1].weight = 0.5;
345        particles[2].weight = 0.9;
346        particles[3].weight = 0.3;
347        particles[4].weight = 0.2;
348
349        let best = get_best_particle(&particles);
350        assert!(
351            (best.weight - 0.9).abs() < f64::EPSILON,
352            "expected best weight 0.9, got {}",
353            best.weight
354        );
355    }
356
357    #[test]
358    fn test_fastslam_update_does_not_panic() {
359        let n_landmarks = 3;
360        let mut particles = create_particles(20, n_landmarks);
361        let u = Vector2::new(1.0, 0.1); // linear vel, angular vel
362
363        // Simulate a few observations
364        let landmarks = vec![(10.0, 0.0), (0.0, 10.0), (10.0, 10.0)];
365        let x_true = Vector3::new(0.0, 0.0, 0.0);
366
367        for _ in 0..5 {
368            let z = get_observations(&x_true, &landmarks);
369            fastslam_update(&mut particles, u, &z);
370        }
371
372        // All particles should still exist
373        assert_eq!(particles.len(), 20);
374    }
375
376    #[test]
377    fn a_landmark_is_initialized_once_and_then_filtered() {
378        let r = get_r();
379        let mut particle = Particle::new(1);
380        let z = Vector2::new(5.0, 0.0);
381
382        update_landmark(&mut particle, &z, 0, &r);
383        let first = particle.landmarks[0].cov;
384        assert!((particle.landmarks[0].x - 5.0).abs() < 1e-12);
385        assert!(
386            first[(0, 0)] < 100.0,
387            "first sighting keeps the prior: {first}"
388        );
389        let weight = particle.weight;
390
391        // A second, slightly different reading is fused, not copied, and it
392        // scores the particle.
393        update_landmark(&mut particle, &Vector2::new(5.4, 0.0), 0, &r);
394        let lm = &particle.landmarks[0];
395        assert!(lm.x > 5.0 && lm.x < 5.4, "landmark x = {}", lm.x);
396        assert!(lm.cov[(0, 0)] < first[(0, 0)]);
397        assert!(particle.weight != weight);
398    }
399
400    #[test]
401    fn test_particle_initial_state() {
402        let n_landmarks = 4;
403        let particles = create_particles(10, n_landmarks);
404
405        for p in &particles {
406            assert_eq!(p.x, 0.0);
407            assert_eq!(p.y, 0.0);
408            assert_eq!(p.yaw, 0.0);
409            // Weight should be 1/N_PARTICLE
410            assert!((p.weight - 1.0 / N_PARTICLE as f64).abs() < f64::EPSILON);
411            // Each landmark should have large initial covariance
412            for lm in &p.landmarks {
413                assert_eq!(lm.x, 0.0);
414                assert_eq!(lm.y, 0.0);
415                assert_eq!(lm.cov[(0, 0)], 1000.0);
416                assert_eq!(lm.cov[(1, 1)], 1000.0);
417            }
418        }
419    }
420}