1#![allow(dead_code)]
2
3use nalgebra::{Matrix2, Vector2, Vector3};
9use rand_distr::{Distribution, Normal, Uniform};
10use rust_robotics_core::normalize_angle;
11use std::f64::consts::PI;
12
13const DT: f64 = 0.1; const MAX_RANGE: f64 = 20.0; const N_PARTICLE: usize = 100; const NTH: f64 = N_PARTICLE as f64 / 1.5; const Q_SIM: [[f64; 2]; 2] = [[0.3, 0.0], [0.0, 0.0305]]; const R_SIM: [[f64; 2]; 2] = [[0.5, 0.0], [0.0, 0.0305]]; #[derive(Clone)]
28pub struct Landmark {
29 pub x: f64,
30 pub y: f64,
31 pub cov: Matrix2<f64>, }
33
34impl Landmark {
35 fn new() -> Self {
36 Landmark {
37 x: 0.0,
38 y: 0.0,
39 cov: Matrix2::identity() * 1000.0, }
41 }
42}
43
44#[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
70fn 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
80fn 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
90fn 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
101fn 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
106fn 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
111fn 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 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
128fn update_landmark(particle: &mut Particle, z: &Vector2<f64>, lm_id: usize, r: &Matrix2<f64>) {
130 let lm = &particle.landmarks[lm_id];
131
132 if lm.cov[(0, 0)] > 100.0 {
134 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 let z_pred = observation_model(particle, lm_id);
148
149 let y = Vector2::new(z[0] - z_pred[0], normalize_angle(z[1] - z_pred[1]));
151
152 let h = compute_jacobian(particle, lm_id);
154
155 let s = h * particle.landmarks[lm_id].cov * h.transpose() + r;
157
158 let s_inv = s.try_inverse().unwrap_or(Matrix2::identity());
160 let k = particle.landmarks[lm_id].cov * h.transpose() * s_inv;
161
162 let delta = k * y;
164 particle.landmarks[lm_id].x += delta[0];
165 particle.landmarks[lm_id].y += delta[1];
166
167 let i = Matrix2::identity();
169 particle.landmarks[lm_id].cov = (i - k * h) * particle.landmarks[lm_id].cov;
170
171 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
180fn 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
190fn 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
200fn 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 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 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
231pub fn fastslam_update(
233 particles: &mut Vec<Particle>,
234 u: Vector2<f64>,
235 z: &[(f64, f64, usize)], ) {
237 let r = get_r();
238
239 for particle in particles.iter_mut() {
241 predict_particle(particle, u);
242 }
243
244 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(particles);
255
256 let neff = compute_neff(particles);
258 if neff < NTH {
259 resample(particles);
260 }
261}
262
263pub 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
271pub 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 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
296pub 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 let x_true = Vector3::new(0.0, 0.0, 0.0);
323 let landmarks = vec![(5.0, 0.0), (100.0, 100.0)];
325 let observations = get_observations(&x_true, &landmarks);
326
327 assert_eq!(observations.len(), 1);
329 let (d, _angle, lm_id) = observations[0];
330 assert_eq!(lm_id, 0);
331 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 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); 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 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 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 assert!((p.weight - 1.0 / N_PARTICLE as f64).abs() < f64::EPSILON);
411 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}