rust_robotics_localization/
iterated_ekf.rs

1//! Iterated Extended Kalman Filter (IEKF) localization
2//!
3//! Improves on the standard EKF by iterating the linearization point during
4//! the update step, which gives better accuracy for highly nonlinear models.
5
6use alloc::format;
7use alloc::string::ToString;
8use nalgebra::{Matrix2, Matrix2x4, Matrix4, Matrix4x2, 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::{RoboticsError, RoboticsResult, State2D, StateEstimator};
14
15/// State representation for IEKF (x, y, yaw, velocity)
16pub type IEKFState = Vector4<f64>;
17
18/// Measurement representation (x, y position)
19pub type IEKFMeasurement = Vector2<f64>;
20
21/// Control input (velocity, yaw rate)
22pub type IEKFControl = Vector2<f64>;
23
24/// Configuration for IEKF
25#[derive(Debug, Clone)]
26pub struct IEKFConfig {
27    /// Process noise covariance matrix
28    pub q: Matrix4<f64>,
29    /// Measurement noise covariance matrix
30    pub r: Matrix2<f64>,
31    /// Maximum number of linearization iterations in the update step
32    pub max_iterations: usize,
33    /// Convergence tolerance for the iteration (norm of state change)
34    pub tolerance: f64,
35}
36
37impl Default for IEKFConfig {
38    fn default() -> Self {
39        let mut q = Matrix4::<f64>::identity();
40        q[(0, 0)] = 0.1_f64.powi(2);
41        q[(1, 1)] = (1.0_f64.to_radians()).powi(2);
42        q[(2, 2)] = 0.1_f64.powi(2);
43        q[(3, 3)] = 0.1_f64.powi(2);
44
45        let r = Matrix2::<f64>::identity();
46
47        Self {
48            q,
49            r,
50            max_iterations: 5,
51            tolerance: 1e-6,
52        }
53    }
54}
55
56impl IEKFConfig {
57    /// Validate that the configuration is numerically sane.
58    pub fn validate(&self) -> RoboticsResult<()> {
59        if self.q.iter().any(|v| !v.is_finite()) {
60            return Err(RoboticsError::InvalidParameter(
61                "IEKF process noise matrix must contain only finite values".to_string(),
62            ));
63        }
64        if self.r.iter().any(|v| !v.is_finite()) {
65            return Err(RoboticsError::InvalidParameter(
66                "IEKF measurement noise matrix must contain only finite values".to_string(),
67            ));
68        }
69        for i in 0..4 {
70            if self.q[(i, i)] < 0.0 {
71                return Err(RoboticsError::InvalidParameter(
72                    "IEKF process noise diagonal entries must be non-negative".to_string(),
73                ));
74            }
75        }
76        for i in 0..2 {
77            if self.r[(i, i)] < 0.0 {
78                return Err(RoboticsError::InvalidParameter(
79                    "IEKF measurement noise diagonal entries must be non-negative".to_string(),
80                ));
81            }
82        }
83        if self.max_iterations == 0 {
84            return Err(RoboticsError::InvalidParameter(
85                "IEKF max_iterations must be greater than zero".to_string(),
86            ));
87        }
88        Ok(())
89    }
90}
91
92/// Iterated Extended Kalman Filter for robot localization
93pub struct IEKFLocalizer {
94    /// Current state estimate \[x, y, yaw, v\]
95    pub state: IEKFState,
96    /// State covariance matrix
97    pub covariance: Matrix4<f64>,
98    /// Configuration
99    pub config: IEKFConfig,
100}
101
102impl IEKFLocalizer {
103    /// Create a new IEKF localizer; panics on invalid config.
104    pub fn new(config: IEKFConfig) -> Self {
105        Self::try_new(config).expect(
106            "invalid IEKF configuration: noise matrices must be finite and have non-negative diagonals, max_iterations > 0",
107        )
108    }
109
110    /// Create a new validated IEKF localizer.
111    pub fn try_new(config: IEKFConfig) -> RoboticsResult<Self> {
112        config.validate()?;
113        Ok(Self {
114            state: IEKFState::zeros(),
115            covariance: Matrix4::identity(),
116            config,
117        })
118    }
119
120    /// Create with an explicit initial state; panics on invalid inputs.
121    pub fn with_initial_state(initial_state: IEKFState, config: IEKFConfig) -> Self {
122        config.validate().expect("invalid IEKF configuration");
123        Self {
124            state: initial_state,
125            covariance: Matrix4::identity(),
126            config,
127        }
128    }
129
130    /// Get the current estimate as a [`State2D`].
131    pub fn state_2d(&self) -> State2D {
132        State2D::new(self.state[0], self.state[1], self.state[2], self.state[3])
133    }
134
135    // -----------------------------------------------------------------------
136    // Models (identical to EKF)
137    // -----------------------------------------------------------------------
138
139    fn motion_model(x: &IEKFState, u: &IEKFControl, dt: f64) -> IEKFState {
140        let yaw = x[2];
141        let f = Matrix4::new(
142            1., 0., 0., 0., 0., 1., 0., 0., 0., 0., 1., 0., 0., 0., 0., 1.,
143        );
144        let b = Matrix4x2::new(dt * yaw.cos(), 0., dt * yaw.sin(), 0., 0., dt, 1., 0.);
145        f * x + b * u
146    }
147
148    fn jacobian_f(x: &IEKFState, u: &IEKFControl, dt: f64) -> Matrix4<f64> {
149        let yaw = x[2];
150        let v = u[0];
151        Matrix4::new(
152            1.,
153            0.,
154            -dt * v * yaw.sin(),
155            dt * yaw.cos(),
156            0.,
157            1.,
158            dt * v * yaw.cos(),
159            dt * yaw.sin(),
160            0.,
161            0.,
162            1.,
163            0.,
164            0.,
165            0.,
166            0.,
167            1.,
168        )
169    }
170
171    fn observation_model(x: &IEKFState) -> IEKFMeasurement {
172        let h = Matrix2x4::new(1., 0., 0., 0., 0., 1., 0., 0.);
173        h * x
174    }
175
176    fn jacobian_h() -> Matrix2x4<f64> {
177        Matrix2x4::new(1., 0., 0., 0., 0., 1., 0., 0.)
178    }
179
180    // -----------------------------------------------------------------------
181    // Iterated update (with saved predicted state / covariance)
182    // -----------------------------------------------------------------------
183
184    fn iterated_update(
185        &mut self,
186        measurement: &IEKFMeasurement,
187        x_pred: IEKFState,
188        p_pred: Matrix4<f64>,
189    ) -> RoboticsResult<()> {
190        let mut x_i = x_pred;
191        let mut k = Matrix4x2::zeros();
192        let mut j_h = Self::jacobian_h();
193
194        for _ in 0..self.config.max_iterations {
195            j_h = Self::jacobian_h();
196            let z_pred = Self::observation_model(&x_i);
197            // Innovation linearized around x_i rather than x_pred
198            let y = measurement - z_pred - j_h * (x_pred - x_i);
199            let s = j_h * p_pred * j_h.transpose() + self.config.r;
200
201            let s_inv = s.try_inverse().ok_or_else(|| {
202                RoboticsError::NumericalError(
203                    "Failed to invert innovation covariance S".to_string(),
204                )
205            })?;
206
207            k = p_pred * j_h.transpose() * s_inv;
208            let x_new = x_pred + k * y;
209
210            if (x_new - x_i).norm() < self.config.tolerance {
211                x_i = x_new;
212                break;
213            }
214            x_i = x_new;
215        }
216
217        self.state = x_i;
218        self.covariance = (Matrix4::identity() - k * j_h) * p_pred;
219        Ok(())
220    }
221
222    // -----------------------------------------------------------------------
223    // Public combined estimate
224    // -----------------------------------------------------------------------
225
226    /// Full IEKF estimation step (predict + iterated update).
227    pub fn estimate(
228        &mut self,
229        measurement: &IEKFMeasurement,
230        control: &IEKFControl,
231        dt: f64,
232    ) -> Result<&IEKFState, RoboticsError> {
233        if measurement.iter().any(|v| !v.is_finite()) {
234            return Err(RoboticsError::InvalidParameter(
235                "IEKF measurement must contain only finite values".to_string(),
236            ));
237        }
238        if control.iter().any(|v| !v.is_finite()) {
239            return Err(RoboticsError::InvalidParameter(
240                "IEKF control input must contain only finite values".to_string(),
241            ));
242        }
243        if !dt.is_finite() || dt <= 0.0 {
244            return Err(RoboticsError::InvalidParameter(format!(
245                "IEKF dt must be positive and finite, got {}",
246                dt
247            )));
248        }
249
250        let x_pred = Self::motion_model(&self.state, control, dt);
251        let j_f = Self::jacobian_f(&x_pred, control, dt);
252        let p_pred = j_f * self.covariance * j_f.transpose() + self.config.q;
253
254        self.iterated_update(measurement, x_pred, p_pred)?;
255        Ok(&self.state)
256    }
257}
258
259// ---------------------------------------------------------------------------
260// StateEstimator trait
261// ---------------------------------------------------------------------------
262
263impl StateEstimator for IEKFLocalizer {
264    type State = IEKFState;
265    type Measurement = IEKFMeasurement;
266    type Control = IEKFControl;
267
268    /// Prediction step — identical to EKF predict.
269    fn predict(&mut self, control: &Self::Control, dt: f64) {
270        let x_pred = Self::motion_model(&self.state, control, dt);
271        let j_f = Self::jacobian_f(&x_pred, control, dt);
272        self.covariance = j_f * self.covariance * j_f.transpose() + self.config.q;
273        self.state = x_pred;
274    }
275
276    /// Update step using iterated linearization.
277    ///
278    /// Silently skips the update if the innovation covariance S is singular.
279    fn update(&mut self, measurement: &Self::Measurement) {
280        let x_pred = self.state;
281        let p_pred = self.covariance;
282        // Ignore numerical errors in the trait update (no return type)
283        let _ = self.iterated_update(measurement, x_pred, p_pred);
284    }
285
286    fn get_state(&self) -> &Self::State {
287        &self.state
288    }
289
290    fn get_covariance(&self) -> Option<&nalgebra::DMatrix<f64>> {
291        None
292    }
293}
294
295// ---------------------------------------------------------------------------
296// Tests
297// ---------------------------------------------------------------------------
298
299#[cfg(test)]
300mod tests {
301    use super::*;
302
303    #[test]
304    fn test_iekf_creation_with_defaults() {
305        let iekf = IEKFLocalizer::new(IEKFConfig::default());
306        let state = iekf.get_state();
307        assert_eq!(state[0], 0.0);
308        assert_eq!(state[1], 0.0);
309        assert_eq!(state[2], 0.0);
310        assert_eq!(state[3], 0.0);
311        assert_eq!(iekf.config.max_iterations, 5);
312        assert!((iekf.config.tolerance - 1e-6).abs() < 1e-12);
313    }
314
315    #[test]
316    fn test_iekf_predict_moves_state() {
317        let mut iekf = IEKFLocalizer::new(IEKFConfig::default());
318        let control = IEKFControl::new(1.0, 0.0); // 1 m/s forward, yaw rate 0
319        iekf.predict(&control, 0.1);
320        let state = iekf.get_state();
321        // With zero initial yaw the robot moves along x
322        assert!(state[0] > 0.0, "x should increase after forward motion");
323        assert!(state[1].abs() < 1e-10, "y should remain ~0");
324    }
325
326    #[test]
327    fn test_iekf_update_toward_measurement() {
328        let mut iekf = IEKFLocalizer::new(IEKFConfig::default());
329        // Start at origin; update with a measurement at (1, 1)
330        let measurement = IEKFMeasurement::new(1.0, 1.0);
331        iekf.update(&measurement);
332        let state = iekf.get_state();
333        assert!(state[0] > 0.0, "x should move toward measurement");
334        assert!(state[1] > 0.0, "y should move toward measurement");
335    }
336
337    #[test]
338    fn test_iekf_estimate_combined() {
339        let mut iekf = IEKFLocalizer::new(IEKFConfig::default());
340        let control = IEKFControl::new(1.0, 0.1);
341        let measurement = IEKFMeasurement::new(0.1, 0.01);
342
343        let result = iekf.estimate(&measurement, &control, 0.1);
344        assert!(result.is_ok(), "estimate should succeed with valid inputs");
345        let state = result.unwrap();
346        assert!(
347            state.iter().all(|v| v.is_finite()),
348            "state should be finite"
349        );
350    }
351
352    #[test]
353    fn test_iekf_try_new_rejects_invalid_config() {
354        // Negative diagonal in Q
355        let mut bad_q = IEKFConfig::default();
356        bad_q.q[(0, 0)] = -1.0;
357        assert!(matches!(
358            IEKFLocalizer::try_new(bad_q),
359            Err(RoboticsError::InvalidParameter(_))
360        ));
361
362        // max_iterations == 0
363        let bad_iter = IEKFConfig {
364            max_iterations: 0,
365            ..IEKFConfig::default()
366        };
367        assert!(matches!(
368            IEKFLocalizer::try_new(bad_iter),
369            Err(RoboticsError::InvalidParameter(_))
370        ));
371    }
372}
rust_robotics_localization/iterated_ekf.rs

rust_robotics_localization/
iterated_ekf.rs
