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

rust_robotics_slam/
fastslam1.rs
