1#![allow(dead_code)]
2
3use 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
19const DT: f64 = 0.1;
21const MAX_RANGE: f64 = 20.0;
22
23const N_PARTICLE: usize = 100;
25const NTH: f64 = N_PARTICLE as f64 / 1.5;
26
27const 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
31const 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
95fn 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
120fn 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
129fn 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; 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
161fn 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 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 return (x_pred, p_pred);
181 }
182
183 let h_pose = obs_jacobian_pose(x_pred[0], x_pred[1], x_pred[2], lm.x, lm.y);
185
186 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 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 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
208fn 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 let l = cov.cholesky().map(|c| c.l()).unwrap_or_else(|| {
218 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
231fn 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 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; }
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 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 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
316fn fastslam2_update_with_rng<R: Rng + ?Sized>(
322 particles: &mut Vec<Particle>,
323 u: Vector2<f64>,
324 z: &[(f64, f64, usize)], rng: &mut R,
326) {
327 let r = get_r();
328
329 for particle in particles.iter_mut() {
330 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 let sampled_pose = sample_pose_with_rng(&proposal_mean, &proposal_cov, rng);
337 particle.set_pose(&sampled_pose);
338 } else {
339 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 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)], ) {
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 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); let r = get_r();
459
460 let (mean, cov) = compute_proposal(&p, &u, &z, 0, &r);
461
462 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 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}