1use alloc::format;
7use alloc::string::ToString;
8use nalgebra::{Matrix2, Matrix2x4, Matrix4, Vector2, Vector4};
9#[cfg(not(feature = "std"))]
10#[allow(unused_imports)]
11use num_traits::Float;
13use rust_robotics_core::{
14 ControlInput, Point2D, RoboticsError, RoboticsResult, State2D, StateEstimator,
15};
16
17pub type EKFState = Vector4<f64>;
19
20pub type EKFMeasurement = Vector2<f64>;
22
23pub type EKFControl = Vector2<f64>;
25
26#[derive(Debug, Clone)]
28pub struct EKFConfig {
29 pub q: Matrix4<f64>,
31 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
80pub struct EKFLocalizer {
82 state: EKFState,
84 covariance: Matrix4<f64>,
86 config: EKFConfig,
88}
89
90impl EKFLocalizer {
91 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 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 pub fn with_defaults() -> Self {
110 Self::new(EKFConfig::default())
111 }
112
113 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 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 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 pub fn get_covariance_matrix(&self) -> &Matrix4<f64> {
143 &self.covariance
144 }
145
146 pub fn state_2d(&self) -> State2D {
148 State2D::new(self.state[0], self.state[1], self.state[2], self.state[3])
149 }
150
151 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 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 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 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 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 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 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 fn jacobian_h() -> Matrix2x4<f64> {
244 Matrix2x4::new(1., 0., 0., 0., 0., 1., 0., 0.)
245 }
246
247 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 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 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 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 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 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 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); ekf.predict(&control, 0.1);
429 let state = ekf.get_state();
430 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 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}