1#![allow(dead_code)]
2
3use nalgebra::{DMatrix, DVector, Matrix2, Matrix3, Vector2, Vector3};
13use rand_distr::{Distribution, Normal};
14use rust_robotics_core::normalize_angle;
15use std::f64::consts::PI;
16
17const DT: f64 = 0.1; const MAX_RANGE: f64 = 20.0; const M_DIST_TH: f64 = 4.0; const STATE_SIZE: usize = 3; const LM_SIZE: usize = 2; const Q_SIM: [[f64; 2]; 2] = [[0.2, 0.0], [0.0, (5.0 * PI / 180.0) * (5.0 * PI / 180.0)]]; const R_SIM: [[f64; 2]; 2] = [[0.3, 0.0], [0.0, (5.0 * PI / 180.0) * (5.0 * PI / 180.0)]]; fn get_q_control() -> Matrix2<f64> {
32 Matrix2::new(Q_SIM[0][0], Q_SIM[0][1], Q_SIM[1][0], Q_SIM[1][1])
33}
34
35fn get_r() -> Matrix2<f64> {
37 Matrix2::new(R_SIM[0][0], R_SIM[0][1], R_SIM[1][0], R_SIM[1][1])
38}
39
40pub struct EKFSLAMState {
43 pub x: DVector<f64>,
45 pub p: DMatrix<f64>,
47 pub n_lm: usize,
49}
50
51impl EKFSLAMState {
52 pub fn new() -> Self {
54 EKFSLAMState {
55 x: DVector::zeros(STATE_SIZE),
56 p: DMatrix::identity(STATE_SIZE, STATE_SIZE),
57 n_lm: 0,
58 }
59 }
60
61 pub fn get_robot_pose(&self) -> Vector3<f64> {
63 Vector3::new(self.x[0], self.x[1], self.x[2])
64 }
65
66 pub fn get_landmark(&self, idx: usize) -> Option<Vector2<f64>> {
68 if idx < self.n_lm {
69 let lm_idx = STATE_SIZE + idx * LM_SIZE;
70 Some(Vector2::new(self.x[lm_idx], self.x[lm_idx + 1]))
71 } else {
72 None
73 }
74 }
75
76 pub fn n_landmarks(&self) -> usize {
78 self.n_lm
79 }
80}
81
82impl Default for EKFSLAMState {
83 fn default() -> Self {
84 Self::new()
85 }
86}
87
88fn motion_model(x: &Vector3<f64>, u: &Vector2<f64>) -> Vector3<f64> {
91 Vector3::new(
92 x[0] + u[0] * DT * x[2].cos(),
93 x[1] + u[0] * DT * x[2].sin(),
94 normalize_angle(x[2] + u[1] * DT),
95 )
96}
97
98fn jacob_motion(x: &Vector3<f64>, u: &Vector2<f64>) -> (Matrix3<f64>, nalgebra::Matrix3x2<f64>) {
100 let yaw = x[2];
101 let v = u[0];
102
103 let g = Matrix3::new(
105 1.0,
106 0.0,
107 -DT * v * yaw.sin(),
108 0.0,
109 1.0,
110 DT * v * yaw.cos(),
111 0.0,
112 0.0,
113 1.0,
114 );
115
116 let v_mat = nalgebra::Matrix3x2::new(DT * yaw.cos(), 0.0, DT * yaw.sin(), 0.0, 0.0, DT);
118
119 (g, v_mat)
120}
121
122fn calc_observation(robot_pose: &Vector3<f64>, landmark: &Vector2<f64>) -> Vector2<f64> {
124 let dx = landmark[0] - robot_pose[0];
125 let dy = landmark[1] - robot_pose[1];
126 let d = (dx * dx + dy * dy).sqrt();
127 let angle = normalize_angle(dy.atan2(dx) - robot_pose[2]);
128 Vector2::new(d, angle)
129}
130
131fn jacob_observation(
133 robot_pose: &Vector3<f64>,
134 landmark: &Vector2<f64>,
135) -> (nalgebra::Matrix2x3<f64>, Matrix2<f64>) {
136 let dx = landmark[0] - robot_pose[0];
137 let dy = landmark[1] - robot_pose[1];
138 let d2 = dx * dx + dy * dy;
139 let d = d2.sqrt();
140
141 let h_robot = nalgebra::Matrix2x3::new(-dx / d, -dy / d, 0.0, dy / d2, -dx / d2, -1.0);
143
144 let h_lm = Matrix2::new(dx / d, dy / d, -dy / d2, dx / d2);
146
147 (h_robot, h_lm)
148}
149
150fn ekf_slam_predict(state: &mut EKFSLAMState, u: &Vector2<f64>) {
152 let _n = state.x.len();
153
154 let robot_pose = state.get_robot_pose();
156
157 let (g, v_mat) = jacob_motion(&robot_pose, u);
159
160 let new_pose = motion_model(&robot_pose, u);
162 state.x[0] = new_pose[0];
163 state.x[1] = new_pose[1];
164 state.x[2] = new_pose[2];
165
166 let q_control = get_q_control();
168
169 let q_robot = v_mat * q_control * v_mat.transpose();
171
172 let mut p_rr = Matrix3::zeros();
184 for i in 0..STATE_SIZE {
185 for j in 0..STATE_SIZE {
186 p_rr[(i, j)] = state.p[(i, j)];
187 }
188 }
189
190 let p_rr_new = g * p_rr * g.transpose() + q_robot;
192
193 for i in 0..STATE_SIZE {
195 for j in 0..STATE_SIZE {
196 state.p[(i, j)] = p_rr_new[(i, j)];
197 }
198 }
199
200 for lm in 0..state.n_lm {
202 let lm_idx = STATE_SIZE + lm * LM_SIZE;
203
204 let mut p_rm = nalgebra::Matrix3x2::zeros();
206 for i in 0..STATE_SIZE {
207 for j in 0..LM_SIZE {
208 p_rm[(i, j)] = state.p[(i, lm_idx + j)];
209 }
210 }
211
212 let p_rm_new = g * p_rm;
214
215 for i in 0..STATE_SIZE {
217 for j in 0..LM_SIZE {
218 state.p[(i, lm_idx + j)] = p_rm_new[(i, j)];
219 state.p[(lm_idx + j, i)] = p_rm_new[(i, j)]; }
221 }
222 }
223}
224
225fn calc_innovation(
227 state: &EKFSLAMState,
228 lm_idx: usize,
229 z: &Vector2<f64>,
230) -> (Vector2<f64>, Matrix2<f64>, DMatrix<f64>) {
231 let robot_pose = state.get_robot_pose();
232 let landmark = state.get_landmark(lm_idx).unwrap();
233
234 let z_pred = calc_observation(&robot_pose, &landmark);
236
237 let y = Vector2::new(z[0] - z_pred[0], normalize_angle(z[1] - z_pred[1]));
239
240 let (h_robot, h_lm) = jacob_observation(&robot_pose, &landmark);
242
243 let n = state.x.len();
245 let mut h_full = DMatrix::zeros(2, n);
246
247 for i in 0..2 {
249 for j in 0..STATE_SIZE {
250 h_full[(i, j)] = h_robot[(i, j)];
251 }
252 }
253
254 let lm_state_idx = STATE_SIZE + lm_idx * LM_SIZE;
256 for i in 0..2 {
257 for j in 0..LM_SIZE {
258 h_full[(i, lm_state_idx + j)] = h_lm[(i, j)];
259 }
260 }
261
262 let r = get_r();
264 let s = &h_full * &state.p * h_full.transpose() + DMatrix::from_fn(2, 2, |i, j| r[(i, j)]);
265
266 (
267 y,
268 Matrix2::new(s[(0, 0)], s[(0, 1)], s[(1, 0)], s[(1, 1)]),
269 h_full,
270 )
271}
272
273fn search_correspond_landmark_id(state: &EKFSLAMState, z: &Vector2<f64>) -> Option<usize> {
275 let mut min_dist = f64::MAX;
276 let mut min_id = None;
277
278 for i in 0..state.n_lm {
279 let (y, s, _) = calc_innovation(state, i, z);
280
281 if let Some(s_inv) = s.try_inverse() {
283 let mahal = (y.transpose() * s_inv * y)[(0, 0)];
284 if mahal < min_dist {
285 min_dist = mahal;
286 min_id = Some(i);
287 }
288 }
289 }
290
291 if min_dist < M_DIST_TH * M_DIST_TH {
293 min_id
294 } else {
295 None
296 }
297}
298
299fn add_new_landmark(state: &mut EKFSLAMState, z: &Vector2<f64>) {
301 let old_n = state.x.len();
303 let new_n = old_n + LM_SIZE;
304 state.x = state.x.clone().resize_vertically(new_n, 0.0);
305 state.p = state.p.clone().resize(new_n, new_n, 0.0);
306 initialize_landmark(state, z, old_n);
307 state.n_lm += 1;
308}
309
310fn initialize_landmark(state: &mut EKFSLAMState, z: &Vector2<f64>, lm_idx: usize) {
315 let robot_pose = state.get_robot_pose();
316 let (s, c) = (robot_pose[2] + z[1]).sin_cos();
317 state.x[lm_idx] = robot_pose[0] + z[0] * c;
318 state.x[lm_idx + 1] = robot_pose[1] + z[0] * s;
319
320 let g_r = nalgebra::Matrix2x3::new(1.0, 0.0, -z[0] * s, 0.0, 1.0, z[0] * c);
322 let g_z = Matrix2::new(c, -z[0] * s, s, z[0] * c);
323
324 let n = state.x.len();
326 let p_r = state.p.rows(0, STATE_SIZE).into_owned();
327 let cross = g_r * p_r;
328 for j in (0..n).filter(|j| !(lm_idx..lm_idx + LM_SIZE).contains(j)) {
329 for i in 0..LM_SIZE {
330 state.p[(lm_idx + i, j)] = cross[(i, j)];
331 state.p[(j, lm_idx + i)] = cross[(i, j)];
332 }
333 }
334 let p_rr = state.p.fixed_view::<3, 3>(0, 0).into_owned();
335 let p_ll = g_r * p_rr * g_r.transpose() + g_z * get_r() * g_z.transpose();
336 state
337 .p
338 .fixed_view_mut::<2, 2>(lm_idx, lm_idx)
339 .copy_from(&p_ll);
340}
341
342fn ekf_slam_update(state: &mut EKFSLAMState, z: &Vector2<f64>, lm_idx: usize) {
344 let (y, s, h_full) = calc_innovation(state, lm_idx, z);
345
346 let s_dmatrix = DMatrix::from_fn(2, 2, |i, j| s[(i, j)]);
348 let s_inv = s_dmatrix
349 .try_inverse()
350 .unwrap_or_else(|| DMatrix::identity(2, 2));
351 let k = &state.p * h_full.transpose() * s_inv;
352
353 let y_dvec = DVector::from_vec(vec![y[0], y[1]]);
355 state.x = &state.x + &k * y_dvec;
356
357 state.x[2] = normalize_angle(state.x[2]);
359
360 let n = state.x.len();
362 let i_kh = DMatrix::identity(n, n) - &k * h_full;
363 state.p = &i_kh * &state.p;
364
365 state.p = (&state.p + state.p.transpose()) * 0.5;
367}
368
369pub fn ekf_slam(
371 state: &mut EKFSLAMState,
372 u: &Vector2<f64>,
373 observations: &[(f64, f64)], ) {
375 ekf_slam_predict(state, u);
377
378 for (d, angle) in observations {
380 let z = Vector2::new(*d, *angle);
381
382 let lm_idx = search_correspond_landmark_id(state, &z);
384
385 match lm_idx {
386 Some(idx) => {
387 ekf_slam_update(state, &z, idx);
389 }
390 None => {
391 add_new_landmark(state, &z);
393 }
394 }
395 }
396}
397
398pub fn ekf_slam_known_correspondences(
401 state: &mut EKFSLAMState,
402 u: &Vector2<f64>,
403 observations: &[(f64, f64, usize)], n_landmarks: usize,
405) {
406 ekf_slam_predict(state, u);
408
409 if state.n_lm == 0 && !observations.is_empty() {
411 let n = STATE_SIZE + n_landmarks * LM_SIZE;
413 let mut new_x = DVector::zeros(n);
414 for i in 0..STATE_SIZE {
415 new_x[i] = state.x[i];
416 }
417 state.x = new_x;
419
420 let mut new_p = DMatrix::identity(n, n) * 1e6; for i in 0..STATE_SIZE {
423 for j in 0..STATE_SIZE {
424 new_p[(i, j)] = state.p[(i, j)];
425 }
426 }
427 state.p = new_p;
428 state.n_lm = n_landmarks;
429 }
430
431 for (d, angle, lm_id) in observations {
433 let z = Vector2::new(*d, *angle);
434
435 if *lm_id < state.n_lm {
437 let lm_idx = STATE_SIZE + lm_id * LM_SIZE;
439 if state.p[(lm_idx, lm_idx)] > 1e5 {
440 initialize_landmark(state, &z, lm_idx);
442 } else {
443 ekf_slam_update(state, &z, *lm_id);
445 }
446 }
447 }
448}
449
450pub fn get_observations(x_true: &Vector3<f64>, landmarks: &[(f64, f64)]) -> Vec<(f64, f64)> {
452 let normal = Normal::new(0.0, 1.0).unwrap();
453 let r = get_r();
454 let mut z = Vec::new();
455
456 for (lx, ly) in landmarks.iter() {
457 let dx = lx - x_true[0];
458 let dy = ly - x_true[1];
459 let d = (dx * dx + dy * dy).sqrt();
460
461 if d <= MAX_RANGE {
462 let angle = normalize_angle(dy.atan2(dx) - x_true[2]);
463
464 let d_noisy = d + normal.sample(&mut rand::rng()) * r[(0, 0)].sqrt();
466 let angle_noisy = angle + normal.sample(&mut rand::rng()) * r[(1, 1)].sqrt();
467
468 z.push((d_noisy, angle_noisy));
469 }
470 }
471
472 z
473}
474
475pub fn get_observations_with_id(
477 x_true: &Vector3<f64>,
478 landmarks: &[(f64, f64)],
479) -> Vec<(f64, f64, usize)> {
480 let normal = Normal::new(0.0, 1.0).unwrap();
481 let r = get_r();
482 let mut z = Vec::new();
483
484 for (lm_id, (lx, ly)) in landmarks.iter().enumerate() {
485 let dx = lx - x_true[0];
486 let dy = ly - x_true[1];
487 let d = (dx * dx + dy * dy).sqrt();
488
489 if d <= MAX_RANGE {
490 let angle = normalize_angle(dy.atan2(dx) - x_true[2]);
491
492 let d_noisy = d + normal.sample(&mut rand::rng()) * r[(0, 0)].sqrt();
494 let angle_noisy = angle + normal.sample(&mut rand::rng()) * r[(1, 1)].sqrt();
495
496 z.push((d_noisy, angle_noisy, lm_id));
497 }
498 }
499
500 z
501}
502
503#[cfg(test)]
504mod tests {
505 use super::*;
506
507 #[test]
508 fn test_ekf_slam_state_creation() {
509 let state = EKFSLAMState::new();
510 assert_eq!(state.x.len(), STATE_SIZE);
511 assert_eq!(state.n_lm, 0);
512 }
513
514 #[test]
515 fn test_normalize_angle() {
516 assert!((normalize_angle(0.0) - 0.0).abs() < 1e-10);
517 assert!((normalize_angle(PI) - PI).abs() < 1e-10);
518 assert!((normalize_angle(2.0 * PI) - 0.0).abs() < 1e-10);
519 assert!((normalize_angle(-2.0 * PI) - 0.0).abs() < 1e-10);
520 assert!((normalize_angle(3.0 * PI).abs() - PI).abs() < 1e-10);
522 }
523
524 #[test]
525 fn test_motion_model() {
526 let x = Vector3::new(0.0, 0.0, 0.0);
527 let u = Vector2::new(1.0, 0.0); let new_x = motion_model(&x, &u);
529
530 assert!(new_x[0] > 0.0);
532 assert!(new_x[1].abs() < 1e-10);
533 assert!(new_x[2].abs() < 1e-10);
534 }
535
536 #[test]
537 fn a_known_landmark_inherits_the_robot_uncertainty_on_first_sighting() {
538 let mut known = EKFSLAMState::new();
541 ekf_slam_known_correspondences(&mut known, &Vector2::zeros(), &[(5.0, 0.0, 0)], 2);
542 let lm = STATE_SIZE;
543 assert!(known.p[(lm, lm)] > known.p[(0, 0)] * 0.99, "{}", known.p);
544 assert!(
545 known.p[(lm, 0)] > 0.5,
546 "robot-landmark correlation {}",
547 known.p[(lm, 0)]
548 );
549 assert_eq!(known.p[(lm, 0)], known.p[(0, lm)]);
550 assert!(known.p[(lm + 2, lm + 2)] > 1e5);
552 assert_eq!(known.p[(lm + 2, lm)], 0.0);
553
554 let mut unknown = EKFSLAMState::new();
556 ekf_slam_predict(&mut unknown, &Vector2::zeros());
557 add_new_landmark(&mut unknown, &Vector2::new(5.0, 0.0));
558 for i in 0..STATE_SIZE + LM_SIZE {
559 for j in 0..STATE_SIZE + LM_SIZE {
560 assert!((known.p[(i, j)] - unknown.p[(i, j)]).abs() < 1e-12);
561 }
562 }
563 }
564
565 #[test]
566 fn test_add_landmark() {
567 let mut state = EKFSLAMState::new();
568 let z = Vector2::new(5.0, 0.0); add_new_landmark(&mut state, &z);
571
572 assert_eq!(state.n_lm, 1);
573 assert_eq!(state.x.len(), STATE_SIZE + LM_SIZE);
574
575 let lm = state.get_landmark(0).unwrap();
577 assert!((lm[0] - 5.0).abs() < 0.1);
578 assert!(lm[1].abs() < 0.1);
579 }
580
581 #[test]
582 fn test_ekf_slam_prediction() {
583 let mut state = EKFSLAMState::new();
584 let u = Vector2::new(1.0, 0.1);
585
586 ekf_slam_predict(&mut state, &u);
587
588 assert!(state.x[0] > 0.0);
590 assert!(state.x[2].abs() > 0.0);
591 }
592
593 #[test]
594 fn test_ekf_slam_full() {
595 let mut state = EKFSLAMState::new();
596 let u = Vector2::new(1.0, 0.0);
597 let observations = vec![(5.0, 0.0), (5.0, PI / 2.0)];
598
599 ekf_slam(&mut state, &u, &observations);
600
601 assert_eq!(state.n_lm, 2);
603 assert_eq!(state.x.len(), STATE_SIZE + 2 * LM_SIZE);
604 }
605}