Skip to main content

rust_robotics_slam/
ekf_slam.rs

1#![allow(dead_code)]
2
3// EKF SLAM (Extended Kalman Filter SLAM)
4// author: Atsushi Sakai (@Atsushi_twi)
5//         Ryohei Sasaki (@rsasaki0109)
6//         Rust port
7//
8// Reference:
9// - Probabilistic Robotics (Thrun, Burgard, Fox)
10// - https://github.com/AtsushiSakai/PythonRobotics
11
12use nalgebra::{DMatrix, DVector, Matrix2, Matrix3, Vector2, Vector3};
13use rand_distr::{Distribution, Normal};
14use rust_robotics_core::normalize_angle;
15use std::f64::consts::PI;
16
17// Simulation parameters
18const DT: f64 = 0.1; // time step [s]
19const MAX_RANGE: f64 = 20.0; // maximum observation range [m]
20const M_DIST_TH: f64 = 4.0; // Mahalanobis distance gate for data association [σ] (compared squared: 16, looser than the 95% χ²(2) value 5.99)
21
22// State dimension
23const STATE_SIZE: usize = 3; // robot state [x, y, yaw]
24const LM_SIZE: usize = 2; // landmark state [x, y]
25
26// Noise parameters
27const Q_SIM: [[f64; 2]; 2] = [[0.2, 0.0], [0.0, (5.0 * PI / 180.0) * (5.0 * PI / 180.0)]]; // process noise (reduced)
28const R_SIM: [[f64; 2]; 2] = [[0.3, 0.0], [0.0, (5.0 * PI / 180.0) * (5.0 * PI / 180.0)]]; // observation noise (reduced)
29
30/// Process noise covariance for control input
31fn 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
35/// Observation noise covariance
36fn 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
40/// EKF SLAM state
41/// State vector: [x, y, yaw, lm1_x, lm1_y, lm2_x, lm2_y, ...]
42pub struct EKFSLAMState {
43    /// State vector
44    pub x: DVector<f64>,
45    /// Covariance matrix
46    pub p: DMatrix<f64>,
47    /// Number of observed landmarks
48    pub n_lm: usize,
49}
50
51impl EKFSLAMState {
52    /// Create a new EKF SLAM state
53    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    /// Get robot pose [x, y, yaw]
62    pub fn get_robot_pose(&self) -> Vector3<f64> {
63        Vector3::new(self.x[0], self.x[1], self.x[2])
64    }
65
66    /// Get landmark position by index
67    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    /// Get number of landmarks
77    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
88/// Motion model for robot
89/// x_t = f(x_{t-1}, u_t)
90fn 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
98/// Jacobian of motion model with respect to state
99fn 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    // Jacobian with respect to state (G matrix)
104    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    // Jacobian with respect to control (V matrix): 3x2
117    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
122/// Calculate observation from robot pose to landmark
123fn 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
131/// Jacobian of observation model with respect to robot pose and landmark position
132fn 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    // Jacobian with respect to robot pose [x, y, yaw]
142    let h_robot = nalgebra::Matrix2x3::new(-dx / d, -dy / d, 0.0, dy / d2, -dx / d2, -1.0);
143
144    // Jacobian with respect to landmark position [lm_x, lm_y]
145    let h_lm = Matrix2::new(dx / d, dy / d, -dy / d2, dx / d2);
146
147    (h_robot, h_lm)
148}
149
150/// EKF SLAM prediction step
151fn ekf_slam_predict(state: &mut EKFSLAMState, u: &Vector2<f64>) {
152    let _n = state.x.len();
153
154    // Get current robot pose
155    let robot_pose = state.get_robot_pose();
156
157    // Jacobians (computed before state update, using current state)
158    let (g, v_mat) = jacob_motion(&robot_pose, u);
159
160    // Predict robot pose using motion model
161    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    // Process noise in control space
167    let q_control = get_q_control();
168
169    // Process noise in state space (robot part only): V * Q * V^T (3x2 * 2x2 * 2x3 = 3x3)
170    let q_robot = v_mat * q_control * v_mat.transpose();
171
172    // Update covariance following EKF formula:
173    // P_new = F @ P @ F^T + Q_augmented
174    // where F is the Jacobian of full state transition (identity for landmarks)
175
176    // For EKF-SLAM, we need to propagate covariance properly:
177    // P_rr = G @ P_rr @ G^T + Q
178    // P_rm = G @ P_rm (cross-covariance between robot and landmarks)
179    // P_mr = P_rm^T
180    // P_mm = P_mm (landmarks covariance unchanged)
181
182    // Extract robot covariance (3x3)
183    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    // Update P_rr
191    let p_rr_new = g * p_rr * g.transpose() + q_robot;
192
193    // Update state covariance
194    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    // Update cross-covariance P_rm = G @ P_rm
201    for lm in 0..state.n_lm {
202        let lm_idx = STATE_SIZE + lm * LM_SIZE;
203
204        // Extract P_rm for this landmark (3x2)
205        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        // Update: P_rm_new = G @ P_rm
213        let p_rm_new = g * p_rm;
214
215        // Write back
216        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)]; // Keep symmetry
220            }
221        }
222    }
223}
224
225/// Calculate innovation (measurement residual) for a landmark observation
226fn 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    // Predicted observation
235    let z_pred = calc_observation(&robot_pose, &landmark);
236
237    // Innovation
238    let y = Vector2::new(z[0] - z_pred[0], normalize_angle(z[1] - z_pred[1]));
239
240    // Jacobians
241    let (h_robot, h_lm) = jacob_observation(&robot_pose, &landmark);
242
243    // Build full Jacobian H
244    let n = state.x.len();
245    let mut h_full = DMatrix::zeros(2, n);
246
247    // Robot part
248    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    // Landmark part
255    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    // Innovation covariance
263    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
273/// Search for corresponding landmark using Mahalanobis distance
274fn 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        // Mahalanobis distance
282        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    // Return match only if below threshold
292    if min_dist < M_DIST_TH * M_DIST_TH {
293        min_id
294    } else {
295        None
296    }
297}
298
299/// Add a new landmark to the state
300fn add_new_landmark(state: &mut EKFSLAMState, z: &Vector2<f64>) {
301    // Extend the state and covariance with a zero block, then fill it in.
302    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
310/// Initializes the landmark at state index `lm_idx` from its first
311/// observation `z`, linearizing `l = r + d·(cos, sin)(yaw + θ)`:
312/// `P_ll = G_r P_rr G_rᵀ + G_z R G_zᵀ` and `P_l· = G_r P_r·`, so the
313/// landmark inherits the robot's uncertainty and stays correlated with it.
314fn 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    // G_r: Jacobian w.r.t. robot pose [x, y, yaw]; G_z: w.r.t. [d, angle].
321    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    // Cross-covariance with every other state entry: G_r P_r·.
325    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
342/// EKF SLAM update step for a single observation
343fn 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    // Kalman gain
347    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    // State update
354    let y_dvec = DVector::from_vec(vec![y[0], y[1]]);
355    state.x = &state.x + &k * y_dvec;
356
357    // Normalize yaw
358    state.x[2] = normalize_angle(state.x[2]);
359
360    // Covariance update
361    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    // Ensure symmetry
366    state.p = (&state.p + state.p.transpose()) * 0.5;
367}
368
369/// Full EKF SLAM step (prediction + update) with unknown data association
370pub fn ekf_slam(
371    state: &mut EKFSLAMState,
372    u: &Vector2<f64>,
373    observations: &[(f64, f64)], // (distance, angle)
374) {
375    // Prediction step
376    ekf_slam_predict(state, u);
377
378    // Update step for each observation
379    for (d, angle) in observations {
380        let z = Vector2::new(*d, *angle);
381
382        // Data association
383        let lm_idx = search_correspond_landmark_id(state, &z);
384
385        match lm_idx {
386            Some(idx) => {
387                // Update existing landmark
388                ekf_slam_update(state, &z, idx);
389            }
390            None => {
391                // Add new landmark
392                add_new_landmark(state, &z);
393            }
394        }
395    }
396}
397
398/// Full EKF SLAM step (prediction + update) with known data association
399/// This version uses landmark IDs from observations (ideal case)
400pub fn ekf_slam_known_correspondences(
401    state: &mut EKFSLAMState,
402    u: &Vector2<f64>,
403    observations: &[(f64, f64, usize)], // (distance, angle, landmark_id)
404    n_landmarks: usize,
405) {
406    // Prediction step
407    ekf_slam_predict(state, u);
408
409    // Pre-allocate space for all landmarks on first observation
410    if state.n_lm == 0 && !observations.is_empty() {
411        // Initialize state to hold all potential landmarks
412        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        // Initialize landmarks to (0, 0) with large covariance
418        state.x = new_x;
419
420        let mut new_p = DMatrix::identity(n, n) * 1e6; // Large initial uncertainty
421                                                       // Copy robot covariance
422        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    // Update step for each observation
432    for (d, angle, lm_id) in observations {
433        let z = Vector2::new(*d, *angle);
434
435        // Check if this is a valid landmark ID
436        if *lm_id < state.n_lm {
437            // Check if landmark is initialized (covariance is reasonable)
438            let lm_idx = STATE_SIZE + lm_id * LM_SIZE;
439            if state.p[(lm_idx, lm_idx)] > 1e5 {
440                // First observation of this landmark - initialize it
441                initialize_landmark(state, &z, lm_idx);
442            } else {
443                // Update existing landmark
444                ekf_slam_update(state, &z, *lm_id);
445            }
446        }
447    }
448}
449
450/// Simulate observations from true robot pose to landmarks (without IDs)
451pub 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            // Add noise
465            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
475/// Simulate observations from true robot pose to landmarks (with IDs - known correspondences)
476pub 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            // Add noise
493            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        // 3π = π (after normalization), both π and -π are equivalent at boundary
521        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); // move forward at 1 m/s
528        let new_x = motion_model(&x, &u);
529
530        // Should move in x direction (yaw is 0)
531        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        // Robot pose uncertain (P_rr = I); the landmark seen 5 m ahead must be
539        // at least that uncertain and correlated with the robot.
540        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        // The still-unseen landmark keeps its prior and no correlation.
551        assert!(known.p[(lm + 2, lm + 2)] > 1e5);
552        assert_eq!(known.p[(lm + 2, lm)], 0.0);
553
554        // Same block as the unknown-association path from the same prior.
555        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); // landmark at 5m ahead
569
570        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        // Landmark should be at (5, 0) since robot is at origin facing +x
576        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        // Robot should have moved forward and turned slightly
589        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        // Should have added 2 landmarks
602        assert_eq!(state.n_lm, 2);
603        assert_eq!(state.x.len(), STATE_SIZE + 2 * LM_SIZE);
604    }
605}