Skip to main content

rust_robotics_localization/
ekf.rs

1//! Extended Kalman Filter (EKF) localization
2//!
3//! Implements state estimation using the Extended Kalman Filter algorithm
4//! for robot localization with nonlinear motion and observation models.
5
6use alloc::format;
7use alloc::string::ToString;
8use nalgebra::{Matrix2, Matrix2x4, Matrix4, Vector2, Vector4};
9#[cfg(not(feature = "std"))]
10#[allow(unused_imports)]
11// f64 math via libm on no_std targets; on std hosts the inherent methods win
12use num_traits::Float;
13use rust_robotics_core::{
14    ControlInput, Point2D, RoboticsError, RoboticsResult, State2D, StateEstimator,
15};
16
17/// State representation for EKF (x, y, yaw, velocity)
18pub type EKFState = Vector4<f64>;
19
20/// Measurement representation (x, y position)
21pub type EKFMeasurement = Vector2<f64>;
22
23/// Control input (velocity, yaw rate)
24pub type EKFControl = Vector2<f64>;
25
26/// Configuration for EKF
27#[derive(Debug, Clone)]
28pub struct EKFConfig {
29    /// Process noise covariance matrix
30    pub q: Matrix4<f64>,
31    /// Measurement noise covariance matrix
32    pub r: Matrix2<f64>,
33}
34
35impl Default for EKFConfig {
36    fn default() -> Self {
37        let mut q = Matrix4::<f64>::identity();
38        q[(0, 0)] = 0.1_f64.powi(2);
39        q[(1, 1)] = 0.1_f64.powi(2);
40        q[(2, 2)] = (1.0_f64.to_radians()).powi(2);
41        q[(3, 3)] = 0.1_f64.powi(2);
42
43        let r = Matrix2::<f64>::identity();
44
45        Self { q, r }
46    }
47}
48
49impl EKFConfig {
50    pub fn validate(&self) -> RoboticsResult<()> {
51        if self.q.iter().any(|value| !value.is_finite()) {
52            return Err(RoboticsError::InvalidParameter(
53                "EKF process noise matrix must contain only finite values".to_string(),
54            ));
55        }
56        if self.r.iter().any(|value| !value.is_finite()) {
57            return Err(RoboticsError::InvalidParameter(
58                "EKF measurement noise matrix must contain only finite values".to_string(),
59            ));
60        }
61        for i in 0..4 {
62            if self.q[(i, i)] < 0.0 {
63                return Err(RoboticsError::InvalidParameter(
64                    "EKF process noise diagonal entries must be non-negative".to_string(),
65                ));
66            }
67        }
68        for i in 0..2 {
69            if self.r[(i, i)] < 0.0 {
70                return Err(RoboticsError::InvalidParameter(
71                    "EKF measurement noise diagonal entries must be non-negative".to_string(),
72                ));
73            }
74        }
75
76        Ok(())
77    }
78}
79
80/// Extended Kalman Filter for robot localization
81pub struct EKFLocalizer {
82    /// Current state estimate [x, y, yaw, v]
83    state: EKFState,
84    /// State covariance matrix
85    covariance: Matrix4<f64>,
86    /// Configuration
87    config: EKFConfig,
88}
89
90impl EKFLocalizer {
91    /// Create a new EKF localizer
92    pub fn new(config: EKFConfig) -> Self {
93        Self::try_new(config).expect(
94            "invalid EKF configuration: noise matrices must be finite and have non-negative diagonals",
95        )
96    }
97
98    /// Create a new validated EKF localizer
99    pub fn try_new(config: EKFConfig) -> RoboticsResult<Self> {
100        config.validate()?;
101        Ok(EKFLocalizer {
102            state: EKFState::zeros(),
103            covariance: Matrix4::identity(),
104            config,
105        })
106    }
107
108    /// Create with default configuration
109    pub fn with_defaults() -> Self {
110        Self::new(EKFConfig::default())
111    }
112
113    /// Create with initial state
114    pub fn with_initial_state(initial_state: EKFState, config: EKFConfig) -> Self {
115        Self::try_with_initial_state(initial_state, config)
116            .expect("invalid EKF initialization: state must be finite and config must be valid")
117    }
118
119    /// Create with validated initial state
120    pub fn try_with_initial_state(
121        initial_state: EKFState,
122        config: EKFConfig,
123    ) -> RoboticsResult<Self> {
124        config.validate()?;
125        Self::validate_state_vector(&initial_state)?;
126        Ok(EKFLocalizer {
127            state: initial_state,
128            covariance: Matrix4::identity(),
129            config,
130        })
131    }
132
133    /// Create with common State2D type
134    pub fn with_initial_state_2d(
135        initial_state: State2D,
136        config: EKFConfig,
137    ) -> RoboticsResult<Self> {
138        Self::try_with_initial_state(initial_state.to_vector(), config)
139    }
140
141    /// Get reference to state covariance
142    pub fn get_covariance_matrix(&self) -> &Matrix4<f64> {
143        &self.covariance
144    }
145
146    /// Get current estimate as State2D
147    pub fn state_2d(&self) -> State2D {
148        State2D::new(self.state[0], self.state[1], self.state[2], self.state[3])
149    }
150
151    /// Set process noise covariance
152    pub fn set_process_noise(&mut self, q: Matrix4<f64>) {
153        self.try_set_process_noise(q)
154            .expect("invalid EKF process noise matrix")
155    }
156
157    /// Set process noise covariance with validation
158    pub fn try_set_process_noise(&mut self, q: Matrix4<f64>) -> RoboticsResult<()> {
159        if q.iter().any(|value| !value.is_finite()) {
160            return Err(RoboticsError::InvalidParameter(
161                "EKF process noise matrix must contain only finite values".to_string(),
162            ));
163        }
164        for i in 0..4 {
165            if q[(i, i)] < 0.0 {
166                return Err(RoboticsError::InvalidParameter(
167                    "EKF process noise diagonal entries must be non-negative".to_string(),
168                ));
169            }
170        }
171
172        self.config.q = q;
173        Ok(())
174    }
175
176    /// Set measurement noise covariance
177    pub fn set_measurement_noise(&mut self, r: Matrix2<f64>) {
178        self.try_set_measurement_noise(r)
179            .expect("invalid EKF measurement noise matrix")
180    }
181
182    /// Set measurement noise covariance with validation
183    pub fn try_set_measurement_noise(&mut self, r: Matrix2<f64>) -> RoboticsResult<()> {
184        if r.iter().any(|value| !value.is_finite()) {
185            return Err(RoboticsError::InvalidParameter(
186                "EKF measurement noise matrix must contain only finite values".to_string(),
187            ));
188        }
189        for i in 0..2 {
190            if r[(i, i)] < 0.0 {
191                return Err(RoboticsError::InvalidParameter(
192                    "EKF measurement noise diagonal entries must be non-negative".to_string(),
193                ));
194            }
195        }
196
197        self.config.r = r;
198        Ok(())
199    }
200
201    /// Motion model: predict state based on control input
202    fn motion_model(x: &EKFState, u: &EKFControl, dt: f64) -> EKFState {
203        let yaw = x[2];
204        EKFState::new(
205            x[0] + dt * u[0] * yaw.cos(),
206            x[1] + dt * u[0] * yaw.sin(),
207            x[2] + dt * u[1],
208            u[0],
209        )
210    }
211
212    /// Jacobian of motion model with respect to state
213    fn jacobian_f(x: &EKFState, u: &EKFControl, dt: f64) -> Matrix4<f64> {
214        let yaw = x[2];
215        let v = u[0];
216        Matrix4::new(
217            1.,
218            0.,
219            -dt * v * yaw.sin(),
220            0.,
221            0.,
222            1.,
223            dt * v * yaw.cos(),
224            0.,
225            0.,
226            0.,
227            1.,
228            0.,
229            0.,
230            0.,
231            0.,
232            0.,
233        )
234    }
235
236    /// Observation model: predict measurement from state
237    fn observation_model(x: &EKFState) -> EKFMeasurement {
238        let h = Matrix2x4::new(1., 0., 0., 0., 0., 1., 0., 0.);
239        h * x
240    }
241
242    /// Jacobian of observation model
243    fn jacobian_h() -> Matrix2x4<f64> {
244        Matrix2x4::new(1., 0., 0., 0., 0., 1., 0., 0.)
245    }
246
247    /// Full EKF estimation step (predict + update)
248    pub fn estimate(
249        &mut self,
250        measurement: &EKFMeasurement,
251        control: &EKFControl,
252        dt: f64,
253    ) -> Result<&EKFState, RoboticsError> {
254        Self::validate_measurement_vector(measurement)?;
255        Self::validate_control_vector(control)?;
256        Self::validate_dt(dt)?;
257
258        // Predict (linearized at the prior estimate, like PythonRobotics'
259        // jacob_f(xEst, u), not at the predicted state)
260        let j_f = Self::jacobian_f(&self.state, control, dt);
261        let x_pred = Self::motion_model(&self.state, control, dt);
262        let p_pred = j_f * self.covariance * j_f.transpose() + self.config.q;
263
264        // Update
265        let j_h = Self::jacobian_h();
266        let z_pred = Self::observation_model(&x_pred);
267        let y = measurement - z_pred;
268        let s = j_h * p_pred * j_h.transpose() + self.config.r;
269
270        let s_inv = s.try_inverse().ok_or_else(|| {
271            RoboticsError::NumericalError("Failed to invert S matrix".to_string())
272        })?;
273
274        let k = p_pred * j_h.transpose() * s_inv;
275        self.state = x_pred + k * y;
276        self.covariance = (Matrix4::identity() - k * j_h) * p_pred;
277
278        Ok(&self.state)
279    }
280
281    /// EKF estimate step using common crate types
282    pub fn estimate_state(
283        &mut self,
284        measurement: Point2D,
285        control: ControlInput,
286        dt: f64,
287    ) -> RoboticsResult<State2D> {
288        self.estimate(&measurement.to_vector(), &control.to_vector(), dt)?;
289        Ok(self.state_2d())
290    }
291
292    /// Legacy interface for EKF estimation (standalone function style)
293    pub fn ekf_estimation(
294        x_est: EKFState,
295        p_est: Matrix4<f64>,
296        z: EKFMeasurement,
297        u: EKFControl,
298        q: Matrix4<f64>,
299        r: Matrix2<f64>,
300        dt: f64,
301    ) -> (EKFState, Matrix4<f64>) {
302        let x_pred = Self::motion_model(&x_est, &u, dt);
303        let j_f = Self::jacobian_f(&x_est, &u, dt);
304        let p_pred = j_f * p_est * j_f.transpose() + q;
305
306        let j_h = Self::jacobian_h();
307        let z_pred = Self::observation_model(&x_pred);
308        let y = z - z_pred;
309        let s = j_h * p_pred * j_h.transpose() + r;
310        let k = p_pred * j_h.transpose() * s.try_inverse().unwrap();
311        let new_x_est = x_pred + k * y;
312        let new_p_est = (Matrix4::identity() - k * j_h) * p_pred;
313
314        (new_x_est, new_p_est)
315    }
316
317    fn validate_state_vector(state: &EKFState) -> RoboticsResult<()> {
318        if state.iter().any(|value| !value.is_finite()) {
319            return Err(RoboticsError::InvalidParameter(
320                "EKF state must contain only finite values".to_string(),
321            ));
322        }
323
324        Ok(())
325    }
326
327    fn validate_measurement_vector(measurement: &EKFMeasurement) -> RoboticsResult<()> {
328        if measurement.iter().any(|value| !value.is_finite()) {
329            return Err(RoboticsError::InvalidParameter(
330                "EKF measurement must contain only finite values".to_string(),
331            ));
332        }
333
334        Ok(())
335    }
336
337    fn validate_control_vector(control: &EKFControl) -> RoboticsResult<()> {
338        if control.iter().any(|value| !value.is_finite()) {
339            return Err(RoboticsError::InvalidParameter(
340                "EKF control input must contain only finite values".to_string(),
341            ));
342        }
343
344        Ok(())
345    }
346
347    fn validate_dt(dt: f64) -> RoboticsResult<()> {
348        if !dt.is_finite() || dt <= 0.0 {
349            return Err(RoboticsError::InvalidParameter(format!(
350                "EKF dt must be positive and finite, got {}",
351                dt
352            )));
353        }
354
355        Ok(())
356    }
357}
358
359impl StateEstimator for EKFLocalizer {
360    type State = EKFState;
361    type Measurement = EKFMeasurement;
362    type Control = EKFControl;
363
364    fn predict(&mut self, control: &Self::Control, dt: f64) {
365        let x_pred = Self::motion_model(&self.state, control, dt);
366        let j_f = Self::jacobian_f(&self.state, control, dt);
367        self.covariance = j_f * self.covariance * j_f.transpose() + self.config.q;
368        self.state = x_pred;
369    }
370
371    fn update(&mut self, measurement: &Self::Measurement) {
372        let j_h = Self::jacobian_h();
373        let z_pred = Self::observation_model(&self.state);
374        let y = measurement - z_pred;
375        let s = j_h * self.covariance * j_h.transpose() + self.config.r;
376
377        if let Some(s_inv) = s.try_inverse() {
378            let k = self.covariance * j_h.transpose() * s_inv;
379            self.state += k * y;
380            self.covariance = (Matrix4::identity() - k * j_h) * self.covariance;
381        }
382    }
383
384    fn get_state(&self) -> &Self::State {
385        &self.state
386    }
387
388    fn get_covariance(&self) -> Option<&nalgebra::DMatrix<f64>> {
389        // Note: We use fixed-size matrix internally, so we don't implement this
390        None
391    }
392}
393
394#[cfg(test)]
395mod tests {
396    use super::*;
397
398    #[test]
399    fn the_covariance_prediction_is_linearized_at_the_prior_estimate() {
400        use rust_robotics_core::StateEstimator;
401        // Heading 0, turning fast: at the predicted heading (0.5 rad) the
402        // Jacobian's x-yaw term would be -dt v sin(0.5); at the prior, 0.
403        let config = EKFConfig::default();
404        let q = config.q;
405        let mut ekf = EKFLocalizer::with_initial_state(Vector4::new(0.0, 0.0, 0.0, 1.0), config);
406        let dt = 0.5;
407        ekf.predict(&Vector2::new(1.0, 1.0), dt);
408        let mut jacobian = Matrix4::identity();
409        jacobian[(1, 2)] = dt;
410        jacobian[(3, 3)] = 0.0;
411        let expected = jacobian * jacobian.transpose() + q;
412        let got = ekf.get_covariance_matrix();
413        assert!((got - expected).norm() < 1e-12, "{got} vs {expected}");
414    }
415
416    #[test]
417    fn test_ekf_creation() {
418        let ekf = EKFLocalizer::with_defaults();
419        let state = ekf.get_state();
420        assert_eq!(state[0], 0.0);
421        assert_eq!(state[1], 0.0);
422    }
423
424    #[test]
425    fn test_ekf_predict() {
426        let mut ekf = EKFLocalizer::with_defaults();
427        let control = EKFControl::new(1.0, 0.0); // move forward at 1 m/s
428        ekf.predict(&control, 0.1);
429        let state = ekf.get_state();
430        // Should move in x direction (yaw is 0)
431        assert!(state[0] > 0.0);
432        assert!(state[1].abs() < 0.001);
433    }
434
435    #[test]
436    fn test_ekf_update() {
437        let mut ekf = EKFLocalizer::with_defaults();
438        let measurement = EKFMeasurement::new(1.0, 1.0);
439        ekf.update(&measurement);
440        let state = ekf.get_state();
441        // State should move towards measurement
442        assert!(state[0] > 0.0);
443        assert!(state[1] > 0.0);
444    }
445
446    #[test]
447    fn test_ekf_estimate() {
448        let mut ekf = EKFLocalizer::with_defaults();
449        let control = EKFControl::new(1.0, 0.1);
450        let measurement = EKFMeasurement::new(0.1, 0.01);
451
452        let result = ekf.estimate(&measurement, &control, 0.1);
453        assert!(result.is_ok());
454    }
455
456    #[test]
457    fn test_ekf_legacy_interface() {
458        let x_est = EKFState::zeros();
459        let p_est = Matrix4::identity();
460        let z = EKFMeasurement::new(0.1, 0.0);
461        let u = EKFControl::new(1.0, 0.0);
462        let q = EKFConfig::default().q;
463        let r = EKFConfig::default().r;
464
465        let (new_x, new_p) = EKFLocalizer::ekf_estimation(x_est, p_est, z, u, q, r, 0.1);
466
467        assert!(new_x[0] > 0.0);
468        assert!(new_p[(0, 0)] > 0.0);
469    }
470
471    #[test]
472    fn test_ekf_try_new_rejects_invalid_config() {
473        let mut config = EKFConfig::default();
474        config.q[(0, 0)] = -1.0;
475
476        let err = match EKFLocalizer::try_new(config) {
477            Ok(_) => panic!("expected invalid config to be rejected"),
478            Err(err) => err,
479        };
480        assert!(matches!(err, RoboticsError::InvalidParameter(_)));
481    }
482
483    #[test]
484    fn test_ekf_with_initial_state_2d() {
485        let ekf = EKFLocalizer::with_initial_state_2d(
486            State2D::new(1.0, 2.0, 0.3, 0.5),
487            EKFConfig::default(),
488        )
489        .unwrap();
490
491        let state = ekf.state_2d();
492        assert_eq!(state.x, 1.0);
493        assert_eq!(state.y, 2.0);
494        assert_eq!(state.yaw, 0.3);
495        assert_eq!(state.v, 0.5);
496    }
497
498    #[test]
499    fn test_ekf_estimate_state_with_common_types() {
500        let mut ekf = EKFLocalizer::with_defaults();
501        let state = ekf
502            .estimate_state(Point2D::new(0.1, 0.0), ControlInput::new(1.0, 0.0), 0.1)
503            .unwrap();
504
505        assert!(state.x > 0.0);
506    }
507
508    #[test]
509    fn test_ekf_velocity_tracks_control_without_accumulating() {
510        let mut ekf = EKFLocalizer::with_defaults();
511
512        for step in 1..=20 {
513            let state = ekf
514                .estimate_state(
515                    Point2D::new(step as f64 * 0.05, 0.0),
516                    ControlInput::new(0.5, 0.0),
517                    0.1,
518                )
519                .unwrap();
520
521            assert!((state.v - 0.5).abs() < 1e-9);
522        }
523    }
524
525    #[test]
526    fn test_ekf_estimate_rejects_invalid_dt() {
527        let mut ekf = EKFLocalizer::with_defaults();
528        let err = match ekf.estimate(&EKFMeasurement::new(0.0, 0.0), &EKFControl::zeros(), 0.0) {
529            Ok(_) => panic!("expected invalid dt to be rejected"),
530            Err(err) => err,
531        };
532
533        assert!(matches!(err, RoboticsError::InvalidParameter(_)));
534    }
535}