From d834725af515dce752c23afe670847abcc3e3e9a Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Sat, 9 May 2026 01:27:37 +0900 Subject: [PATCH 01/10] add: ekf_localizer with test case Signed-off-by: nokosaaan --- .../test_autoware/ekf_localizer/Cargo.toml | 11 + .../test_autoware/ekf_localizer/src/lib.rs | 640 ++++++++++++++++++ 2 files changed, 651 insertions(+) create mode 100644 applications/tests/test_autoware/ekf_localizer/Cargo.toml create mode 100644 applications/tests/test_autoware/ekf_localizer/src/lib.rs diff --git a/applications/tests/test_autoware/ekf_localizer/Cargo.toml b/applications/tests/test_autoware/ekf_localizer/Cargo.toml new file mode 100644 index 000000000..e2b8866b6 --- /dev/null +++ b/applications/tests/test_autoware/ekf_localizer/Cargo.toml @@ -0,0 +1,11 @@ +[package] +name = "ekf_localizer" +version = "0.1.0" +edition = "2021" + +[dependencies] +libm = "0.2" +nalgebra = { version = "0.32", default-features = false} +approx = "0.5" +common_types = { path = "../common_types", default-features = false } +vehicle_velocity_converter = { path = "../vehicle_velocity_converter", default-features = false} \ No newline at end of file diff --git a/applications/tests/test_autoware/ekf_localizer/src/lib.rs b/applications/tests/test_autoware/ekf_localizer/src/lib.rs new file mode 100644 index 000000000..038a2cbf1 --- /dev/null +++ b/applications/tests/test_autoware/ekf_localizer/src/lib.rs @@ -0,0 +1,640 @@ +#![no_std] +#![allow(non_snake_case)] + +extern crate alloc; + +use alloc::{vec, vec::Vec}; +pub use common_types::Header; +use core::ptr::null_mut; +use core::sync::atomic::{AtomicPtr, Ordering as AtomicOrdering}; +use libm::{atan2, cos, sin}; +use nalgebra::{Matrix6, Vector3, Vector6}; +pub use vehicle_velocity_converter::TwistWithCovariance; + +static EKF_MODULE_INSTANCE: AtomicPtr = AtomicPtr::new(null_mut()); + +#[derive(Debug, Clone, Copy, PartialEq)] +pub enum StateIndex { + X = 0, + Y = 1, + Yaw = 2, + YawBias = 3, + Vx = 4, + Wz = 5, +} + +pub type StateVector = Vector6; +pub type StateCovariance = Matrix6; + +#[derive(Debug, Clone, Copy)] +pub struct Point3D { + pub x: f64, + pub y: f64, + pub z: f64, +} + +#[derive(Debug, Clone, Copy)] +pub struct Quaternion { + pub x: f64, + pub y: f64, + pub z: f64, + pub w: f64, +} + +#[derive(Debug, Clone, Copy)] +pub struct Pose { + pub position: Point3D, + pub orientation: Quaternion, +} + +#[derive(Debug, Clone, Copy)] +pub struct Twist { + pub linear: Vector3, + pub angular: Vector3, +} + +#[derive(Debug, Clone, Copy)] +pub struct PoseWithCovariance { + pub pose: Pose, + pub covariance: [f64; 36], +} + +#[derive(Debug, Clone)] +pub struct EKFOdometry { + pub header: common_types::Header, + pub child_frame_id: &'static str, + pub pose: PoseWithCovariance, + pub twist: TwistWithCovariance, +} + +#[derive(Debug, Clone)] +pub struct EKFParameters { + pub enable_yaw_bias_estimation: bool, + pub extend_state_step: usize, + pub proc_stddev_vx_c: f64, + pub proc_stddev_wz_c: f64, + pub proc_stddev_yaw_c: f64, + pub z_filter_proc_dev: f64, + pub roll_filter_proc_dev: f64, + pub pitch_filter_proc_dev: f64, +} + +impl Default for EKFParameters { + fn default() -> Self { + Self { + enable_yaw_bias_estimation: true, + extend_state_step: 50, + proc_stddev_vx_c: 2.0, + proc_stddev_wz_c: 1.0, + proc_stddev_yaw_c: 0.005, + z_filter_proc_dev: 1.0, + roll_filter_proc_dev: 0.1, + pitch_filter_proc_dev: 0.1, + } + } +} + +#[derive(Debug, Clone)] +pub struct Simple1DFilter { + initialized: bool, + x: f64, + var: f64, + proc_var_x_c: f64, +} + +impl Simple1DFilter { + pub fn new() -> Self { + Self { + initialized: false, + x: 0.0, + var: 1e9, + proc_var_x_c: 0.0, + } + } + + pub fn init(&mut self, init_obs: f64, obs_var: f64) { + self.x = init_obs; + self.var = obs_var; + self.initialized = true; + } + + pub fn update(&mut self, obs: f64, obs_var: f64, dt: f64) { + if !self.initialized { + self.init(obs, obs_var); + return; + } + + let proc_var_x_d = self.proc_var_x_c * dt * dt; + self.var = self.var + proc_var_x_d; + + let kalman_gain = self.var / (self.var + obs_var); + self.x = self.x + kalman_gain * (obs - self.x); + self.var = (1.0 - kalman_gain) * self.var; + } + + pub fn set_proc_var(&mut self, proc_var: f64) { + self.proc_var_x_c = proc_var; + } + + pub fn get_x(&self) -> f64 { + self.x + } + + pub fn get_var(&self) -> f64 { + self.var + } +} + +#[derive(Debug, Clone)] +pub struct EKFModule { + params: EKFParameters, + state: StateVector, + covariance: StateCovariance, + z_filter: Simple1DFilter, + roll_filter: Simple1DFilter, + pitch_filter: Simple1DFilter, + accumulated_delay_times: Vec, + // When true, only prediction is performed (no measurement updates) + is_mrm_mode: bool, +} + +impl EKFModule { + pub fn new(params: EKFParameters) -> Self { + let state = StateVector::zeros(); + let mut covariance = StateCovariance::identity() * 1e15; + + covariance[(StateIndex::Yaw as usize, StateIndex::Yaw as usize)] = 50.0; + if params.enable_yaw_bias_estimation { + covariance[(StateIndex::YawBias as usize, StateIndex::YawBias as usize)] = 50.0; + } + covariance[(StateIndex::Vx as usize, StateIndex::Vx as usize)] = 1000.0; + covariance[(StateIndex::Wz as usize, StateIndex::Wz as usize)] = 50.0; + + let mut z_filter = Simple1DFilter::new(); + let mut roll_filter = Simple1DFilter::new(); + let mut pitch_filter = Simple1DFilter::new(); + + z_filter.set_proc_var(params.z_filter_proc_dev * params.z_filter_proc_dev); + roll_filter.set_proc_var(params.roll_filter_proc_dev * params.roll_filter_proc_dev); + pitch_filter.set_proc_var(params.pitch_filter_proc_dev * params.pitch_filter_proc_dev); + + let accumulated_delay_times = vec![1e15; params.extend_state_step]; + + Self { + params, + state, + covariance, + z_filter, + roll_filter, + pitch_filter, + accumulated_delay_times, + is_mrm_mode: false, + } + } + + pub fn initialize(&mut self, initial_pose: Pose) { + self.state[StateIndex::X as usize] = initial_pose.position.x; + self.state[StateIndex::Y as usize] = initial_pose.position.y; + self.state[StateIndex::Yaw as usize] = self.quaternion_to_yaw(initial_pose.orientation); + self.state[StateIndex::YawBias as usize] = 0.0; + self.state[StateIndex::Vx as usize] = 0.0; + self.state[StateIndex::Wz as usize] = 0.0; + + self.covariance = StateCovariance::identity() * 0.01; + if self.params.enable_yaw_bias_estimation { + self.covariance[(StateIndex::YawBias as usize, StateIndex::YawBias as usize)] = 0.0001; + } + + self.z_filter.init(initial_pose.position.z, 0.01); + self.roll_filter.init(0.0, 0.01); + self.pitch_filter.init(0.0, 0.01); + } + + fn predict_next_state(&self, dt: f64) -> StateVector { + let mut x_next = self.state.clone(); + let x = self.state[StateIndex::X as usize]; + let y = self.state[StateIndex::Y as usize]; + let yaw = self.state[StateIndex::Yaw as usize]; + let yaw_bias = self.state[StateIndex::YawBias as usize]; + let vx = self.state[StateIndex::Vx as usize]; + let wz = self.state[StateIndex::Wz as usize]; + + x_next[StateIndex::X as usize] = x + vx * cos(yaw + yaw_bias) * dt; + x_next[StateIndex::Y as usize] = y + vx * sin(yaw + yaw_bias) * dt; + let yaw_next = yaw + wz * dt; + x_next[StateIndex::Yaw as usize] = atan2(sin(yaw_next), cos(yaw_next)); + x_next[StateIndex::YawBias as usize] = yaw_bias; + x_next[StateIndex::Vx as usize] = vx; + x_next[StateIndex::Wz as usize] = wz; + + x_next + } + + fn create_state_transition_matrix(&self, dt: f64) -> Matrix6 { + let mut F = Matrix6::identity(); + let yaw = self.state[StateIndex::Yaw as usize]; + let yaw_bias = self.state[StateIndex::YawBias as usize]; + let vx = self.state[StateIndex::Vx as usize]; + + F[(StateIndex::X as usize, StateIndex::Yaw as usize)] = -vx * sin(yaw + yaw_bias) * dt; + F[(StateIndex::X as usize, StateIndex::YawBias as usize)] = -vx * sin(yaw + yaw_bias) * dt; + F[(StateIndex::X as usize, StateIndex::Vx as usize)] = cos(yaw + yaw_bias) * dt; + + F[(StateIndex::Y as usize, StateIndex::Yaw as usize)] = vx * cos(yaw + yaw_bias) * dt; + F[(StateIndex::Y as usize, StateIndex::YawBias as usize)] = vx * cos(yaw + yaw_bias) * dt; + F[(StateIndex::Y as usize, StateIndex::Vx as usize)] = sin(yaw + yaw_bias) * dt; + + F[(StateIndex::Yaw as usize, StateIndex::Wz as usize)] = dt; + + F + } + + fn process_noise_covariance(&self, dt: f64) -> Matrix6 { + let mut Q = Matrix6::zeros(); + + Q[(StateIndex::Vx as usize, StateIndex::Vx as usize)] = + self.params.proc_stddev_vx_c * self.params.proc_stddev_vx_c * dt * dt; + Q[(StateIndex::Wz as usize, StateIndex::Wz as usize)] = + self.params.proc_stddev_wz_c * self.params.proc_stddev_wz_c * dt * dt; + Q[(StateIndex::Yaw as usize, StateIndex::Yaw as usize)] = + self.params.proc_stddev_yaw_c * self.params.proc_stddev_yaw_c * dt * dt; + + Q[(StateIndex::X as usize, StateIndex::X as usize)] = 0.0; + Q[(StateIndex::Y as usize, StateIndex::Y as usize)] = 0.0; + Q[(StateIndex::YawBias as usize, StateIndex::YawBias as usize)] = 0.0; + + Q + } + + pub fn predict(&mut self, dt: f64) { + self.state = self.predict_next_state(dt); + let F = self.create_state_transition_matrix(dt); + let Q = self.process_noise_covariance(dt); + self.covariance = F * self.covariance * F.transpose() + Q; + self.accumulate_delay_time(dt); + } + + pub fn predict_with_delay(&mut self, dt: f64) { + self.predict(dt); + } + + pub fn predict_only(&mut self, dt: f64) { + self.predict(dt); + } + + pub fn get_current_pose(&self, get_biased_yaw: bool) -> Pose { + let z = self.z_filter.get_x(); + let roll = self.roll_filter.get_x(); + let pitch = self.pitch_filter.get_x(); + + let x = self.state[StateIndex::X as usize]; + let y = self.state[StateIndex::Y as usize]; + let biased_yaw = self.state[StateIndex::Yaw as usize]; + let yaw_bias = self.state[StateIndex::YawBias as usize]; + + let yaw = if get_biased_yaw { + biased_yaw + } else { + biased_yaw + yaw_bias + }; + + Pose { + position: Point3D { x, y, z }, + orientation: self.rpy_to_quaternion(roll, pitch, yaw), + } + } + + pub fn get_current_twist(&self) -> Twist { + let vx = self.state[StateIndex::Vx as usize]; + let wz = self.state[StateIndex::Wz as usize]; + + Twist { + linear: Vector3::new(vx, 0.0, 0.0), + angular: Vector3::new(0.0, 0.0, wz), + } + } + + pub fn get_yaw_bias(&self) -> f64 { + self.state[StateIndex::YawBias as usize] + } + + pub fn get_current_pose_with_covariance(&self) -> PoseWithCovariance { + let pose = self.get_current_pose(false); + let pose_covariance = self.get_current_pose_covariance(); + PoseWithCovariance { + pose, + covariance: pose_covariance, + } + } + + pub fn get_current_pose_covariance(&self) -> [f64; 36] { + let mut cov = [0.0; 36]; + + for i in 0..6 { + for j in 0..6 { + cov[i * 6 + j] = self.covariance[(i, j)]; + } + } + + cov[14] = self.z_filter.get_var(); + cov[21] = self.roll_filter.get_var(); + cov[28] = self.pitch_filter.get_var(); + + cov + } + + pub fn get_current_twist_covariance(&self) -> [f64; 36] { + let mut cov = [0.0; 36]; + + cov[0] = self.covariance[(StateIndex::Vx as usize, StateIndex::Vx as usize)]; + cov[35] = self.covariance[(StateIndex::Wz as usize, StateIndex::Wz as usize)]; + + cov + } + + pub fn update_velocity(&mut self, vx_measurement: f64, wz_measurement: f64) { + if self.is_mrm_mode { + return; + } + + let vx_obs_var = 1.0; + let wz_obs_var = 0.1; + + let vx_var = self.covariance[(StateIndex::Vx as usize, StateIndex::Vx as usize)]; + let vx_gain = vx_var / (vx_var + vx_obs_var); + self.state[StateIndex::Vx as usize] = self.state[StateIndex::Vx as usize] + + vx_gain * (vx_measurement - self.state[StateIndex::Vx as usize]); + self.covariance[(StateIndex::Vx as usize, StateIndex::Vx as usize)] = + (1.0 - vx_gain) * vx_var; + + let wz_var = self.covariance[(StateIndex::Wz as usize, StateIndex::Wz as usize)]; + let wz_gain = wz_var / (wz_var + wz_obs_var); + self.state[StateIndex::Wz as usize] = self.state[StateIndex::Wz as usize] + + wz_gain * (wz_measurement - self.state[StateIndex::Wz as usize]); + self.covariance[(StateIndex::Wz as usize, StateIndex::Wz as usize)] = + (1.0 - wz_gain) * wz_var; + } + + pub fn set_mrm_mode(&mut self, is_mrm: bool) { + self.is_mrm_mode = is_mrm; + } + + pub fn is_mrm(&self) -> bool { + self.is_mrm_mode + } + + pub fn accumulate_delay_time(&mut self, dt: f64) { + let len = self.accumulated_delay_times.len(); + if len == 0 { + return; + } + + let last_time = self.accumulated_delay_times[len - 1]; + let new_time = last_time + dt; + + for i in 0..len - 1 { + self.accumulated_delay_times[i] = self.accumulated_delay_times[i + 1]; + } + self.accumulated_delay_times[len - 1] = new_time; + } + + pub fn find_closest_delay_time_index(&self, target_value: f64) -> usize { + let len = self.accumulated_delay_times.len(); + if len == 0 { + return 0; + } + + if target_value > self.accumulated_delay_times[len - 1] { + return len; + } + + let mut closest_index = 0; + let mut min_diff = f64::MAX; + + for i in 0..len { + let time = self.accumulated_delay_times[i]; + let diff = (target_value - time).abs(); + if diff < min_diff { + min_diff = diff; + closest_index = i; + } + } + + closest_index + } + + fn quaternion_to_yaw(&self, q: Quaternion) -> f64 { + atan2( + 2.0 * (q.w * q.z + q.x * q.y), + 1.0 - 2.0 * (q.y * q.y + q.z * q.z), + ) + } + + fn rpy_to_quaternion(&self, roll: f64, pitch: f64, yaw: f64) -> Quaternion { + let cy = cos(yaw * 0.5); + let sy = sin(yaw * 0.5); + let cp = cos(pitch * 0.5); + let sp = sin(pitch * 0.5); + let cr = cos(roll * 0.5); + let sr = sin(roll * 0.5); + + Quaternion { + w: cr * cp * cy + sr * sp * sy, + x: sr * cp * cy - cr * sp * sy, + y: cr * sp * cy + sr * cp * sy, + z: cr * cp * sy - sr * sp * cy, + } + } +} + +pub fn get_or_initialize_default_module() -> &'static mut EKFModule { + let existing = EKF_MODULE_INSTANCE.load(AtomicOrdering::Acquire); + if !existing.is_null() { + return unsafe { &mut *existing }; + } + + let boxed = alloc::boxed::Box::new(EKFModule::new(EKFParameters::default())); + let ptr = alloc::boxed::Box::into_raw(boxed); + + match EKF_MODULE_INSTANCE.compare_exchange( + null_mut(), + ptr, + AtomicOrdering::AcqRel, + AtomicOrdering::Acquire, + ) { + Ok(_) => unsafe { &mut *ptr }, + Err(existing_ptr) => unsafe { + let _ = alloc::boxed::Box::from_raw(ptr); + &mut *existing_ptr + }, + } +} + +#[cfg(test)] +mod tests { + use super::*; + use core::f64::consts::PI; + use nalgebra::{Matrix6, Vector6}; + + #[test] + fn predict_next_state_matches_formula() { + let params = EKFParameters::default(); + let mut ekf = EKFModule::new(params); + + let x_curr = Vector6::new(2.0, 3.0, PI / 2.0, PI / 4.0, 10.0, 2.0 * PI / 3.0); + + ekf.state = x_curr.clone(); + + let dt = 0.5; + let x_next = ekf.predict_next_state(dt); + + let tol = 1e-10; + assert!((x_next[0] - (2.0 + 10.0 * (PI / 2.0 + PI / 4.0).cos() * dt)).abs() < tol); + assert!((x_next[1] - (3.0 + 10.0 * (PI / 2.0 + PI / 4.0).sin() * dt)).abs() < tol); + let yaw_next = PI / 2.0 + (2.0 * PI / 3.0) * dt; + let expected_yaw = yaw_next.sin().atan2(yaw_next.cos()); + assert!((x_next[2] - expected_yaw).abs() < 1e-6); + assert!((x_next[3] - x_curr[3]).abs() < tol); + assert!((x_next[4] - x_curr[4]).abs() < tol); + assert!((x_next[5] - x_curr[5]).abs() < tol); + } + + #[test] + fn create_state_transition_matrix_numeric_approximation() { + let params = EKFParameters::default(); + let mut ekf = EKFModule::new(params); + + // check around zero + let dt = 0.1; + let dx = Vector6::from_element(0.1); + let x = Vector6::zeros(); + + ekf.state = x.clone(); + let a = ekf.create_state_transition_matrix(dt); + + ekf.state = x.clone() + dx.clone(); + let x1 = ekf.predict_next_state(dt); + ekf.state = x.clone(); + let x0 = ekf.predict_next_state(dt); + let df = x1 - x0; + + { + let mut s = 0.0; + let v = df - a * dx; + for i in 0..6 { + let val = v[i]; + s += val * val; + } + assert!(s.sqrt() < 2e-3); + } + + // check around a non-zero state + let dx = Vector6::from_element(0.1); + let x = Vector6::new(0.1, 0.2, 0.1, 0.4, 0.1, 0.3); + + ekf.state = x.clone(); + let a = ekf.create_state_transition_matrix(dt); + + ekf.state = x.clone() + dx.clone(); + let x1 = ekf.predict_next_state(dt); + ekf.state = x.clone(); + let x0 = ekf.predict_next_state(dt); + let df = x1 - x0; + + { + let mut s = 0.0; + let v = df - a * dx; + for i in 0..6 { + let val = v[i]; + s += val * val; + } + assert!(s.sqrt() < 5e-3); + } + } + + #[test] + fn process_noise_covariance_values() { + let mut params = EKFParameters::default(); + params.proc_stddev_yaw_c = 1.0; + params.proc_stddev_vx_c = 2.0; + params.proc_stddev_wz_c = 3.0; + + let ekf = EKFModule::new(params); + + let q = ekf.process_noise_covariance(1.0); + + // indices: yaw = 2, vx = 4, wz = 5 + assert!((q[(2, 2)] - 1.0_f64.powi(2)).abs() < 1e-12); + assert!((q[(4, 4)] - 2.0_f64.powi(2)).abs() < 1e-12); + assert!((q[(5, 5)] - 3.0_f64.powi(2)).abs() < 1e-12); + + // zero case + let mut params = EKFParameters::default(); + params.proc_stddev_yaw_c = 0.0; + params.proc_stddev_vx_c = 0.0; + params.proc_stddev_wz_c = 0.0; + let ekf2 = EKFModule::new(params); + let q2 = ekf2.process_noise_covariance(1.0); + { + let mut s = 0.0; + for i in 0..6 { + for j in 0..6 { + let val = q2[(i, j)]; + s += val * val; + } + } + assert!(s == 0.0); + } + } + + #[test] + fn pose_and_twist_covariance_mapping() { + let params = EKFParameters::default(); + let mut ekf = EKFModule::new(params); + + // prepare a covariance matrix with top-left 3x3 = 1..9 + let mut p = Matrix6::::zeros(); + p[(0, 0)] = 1.0; + p[(0, 1)] = 2.0; + p[(0, 2)] = 3.0; + p[(1, 0)] = 4.0; + p[(1, 1)] = 5.0; + p[(1, 2)] = 6.0; + p[(2, 0)] = 7.0; + p[(2, 1)] = 8.0; + p[(2, 2)] = 9.0; + + ekf.covariance = p; + + // override the filter variances so those indices are replaced + ekf.z_filter.var = 100.0; + ekf.roll_filter.var = 200.0; + ekf.pitch_filter.var = 300.0; + + let cov = ekf.get_current_pose_covariance(); + + // check a few mapped entries according to get_current_pose_covariance implementation + assert_eq!(cov[0], 1.0); + assert_eq!(cov[1], 2.0); + assert_eq!(cov[2], 3.0); + assert_eq!(cov[6], 4.0); + assert_eq!(cov[7], 5.0); + assert_eq!(cov[8], 6.0); + assert_eq!(cov[12], 7.0); + assert_eq!(cov[13], 8.0); + // index 14 is overwritten by z_filter.var + assert_eq!(cov[14], 100.0); + + // twist covariance mapping (Vx -> index 0, Wz -> index 35) + let mut p2 = Matrix6::::zeros(); + p2[(4, 4)] = 1.0; + p2[(4, 5)] = 2.0; + p2[(5, 4)] = 3.0; + p2[(5, 5)] = 4.0; + ekf.covariance = p2; + + let tcov = ekf.get_current_twist_covariance(); + assert_eq!(tcov[0], 1.0); + assert_eq!(tcov[35], 4.0); + } +} From 6f21fd29f7ebb88b441fa50f81b46132cb03a271 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 11 May 2026 10:15:22 +0900 Subject: [PATCH 02/10] fix: dependics path 2 Signed-off-by: nokosaaan --- Cargo.toml | 1 + applications/tests/test_autoware/Cargo.toml | 6 +++--- userland/Cargo.toml | 10 +++++----- userland/src/lib.rs | 6 +++--- 4 files changed, 12 insertions(+), 11 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 6f4af7aab..3e46c056e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -13,6 +13,7 @@ members = [ "applications/awkernel_services", "applications/awkernel_shell", "applications/awkernel_display", + "applications/autoware", "applications/rd_gen_to_dags", "applications/tests/*", "smoltcp", diff --git a/applications/tests/test_autoware/Cargo.toml b/applications/tests/test_autoware/Cargo.toml index f745c6851..96d8a9c7f 100644 --- a/applications/tests/test_autoware/Cargo.toml +++ b/applications/tests/test_autoware/Cargo.toml @@ -1,5 +1,5 @@ [package] -name = "test_autoware" +name = "autoware" version = "0.1.0" edition = "2021" @@ -11,8 +11,8 @@ crate-type = ["rlib"] log = "0.4" libm = "0.2" csv-core = "0.1" -awkernel_async_lib = { path = "../../../awkernel_async_lib", default-features = false } -awkernel_lib = { path = "../../../awkernel_lib", default-features = false } +awkernel_async_lib = { path = "../../awkernel_async_lib", default-features = false } +awkernel_lib = { path = "../../awkernel_lib", default-features = false } imu_driver = { path = "./imu_driver", default-features = false } imu_corrector = { path = "./imu_corrector", default-features = false } vehicle_velocity_converter = { path = "./vehicle_velocity_converter", default-features = false } diff --git a/userland/Cargo.toml b/userland/Cargo.toml index 222256a19..6b0d5ed84 100644 --- a/userland/Cargo.toml +++ b/userland/Cargo.toml @@ -14,6 +14,10 @@ path = "../awkernel_async_lib" [dependencies.awkernel_services] path = "../applications/awkernel_services" +[dependencies.autoware] +path = "../applications/autoware" +optional = true + [dependencies.rd_gen_to_dags] path = "../applications/rd_gen_to_dags" optional = true @@ -66,10 +70,6 @@ optional = true path = "../applications/tests/test_dag" optional = true -[dependencies.test_autoware] -path = "../applications/tests/test_autoware" -optional = true - [dependencies.test_dvfs] path = "../applications/tests/test_dvfs" optional = true @@ -84,6 +84,7 @@ perf = ["awkernel_services/perf"] # Evaluation applications rd_gen_to_dags = ["dep:rd_gen_to_dags"] +autoware = ["dep:autoware"] # Test applications test_network = ["dep:test_network"] @@ -97,5 +98,4 @@ test_gedf = ["dep:test_gedf"] test_measure_channel = ["dep:test_measure_channel"] test_measure_channel_heavy = ["dep:test_measure_channel_heavy"] test_dag = ["dep:test_dag"] -test_autoware = ["dep:test_autoware"] test_voluntary_preemption = ["dep:test_voluntary_preemption"] diff --git a/userland/src/lib.rs b/userland/src/lib.rs index 7082e1eba..bdad77b5b 100644 --- a/userland/src/lib.rs +++ b/userland/src/lib.rs @@ -7,6 +7,9 @@ use alloc::borrow::Cow; pub async fn main() -> Result<(), Cow<'static, str>> { awkernel_services::run().await; + #[cfg(feature = "autoware")] + autoware::run().await; // run the autoware application + #[cfg(feature = "rd_gen_to_dags")] rd_gen_to_dags::run().await; // run the rd_gen_to_dags application @@ -46,9 +49,6 @@ pub async fn main() -> Result<(), Cow<'static, str>> { #[cfg(feature = "test_dag")] test_dag::run().await; // test for DAG - #[cfg(feature = "test_autoware")] - test_autoware::run().await; // test for Autoware - #[cfg(feature = "test_dvfs")] test_dvfs::run().await; // test for DVFS From 04592a3884641ed463605e31fb06e7afb4711327 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 11 May 2026 10:21:11 +0900 Subject: [PATCH 03/10] fix: move autoware position 3 Signed-off-by: nokosaaan --- applications/{tests/test_autoware => autoware}/Cargo.toml | 0 .../{tests/test_autoware => autoware}/common_types/Cargo.toml | 0 .../{tests/test_autoware => autoware}/common_types/src/lib.rs | 0 .../{tests/test_autoware => autoware}/ekf_localizer/Cargo.toml | 0 .../{tests/test_autoware => autoware}/ekf_localizer/src/lib.rs | 0 .../{tests/test_autoware => autoware}/gyro_odometer/Cargo.toml | 0 .../{tests/test_autoware => autoware}/gyro_odometer/src/lib.rs | 0 .../{tests/test_autoware => autoware}/imu_corrector/Cargo.toml | 0 .../{tests/test_autoware => autoware}/imu_corrector/src/lib.rs | 0 .../{tests/test_autoware => autoware}/imu_driver/Cargo.toml | 0 .../{tests/test_autoware => autoware}/imu_driver/src/lib.rs | 0 applications/{tests/test_autoware => autoware}/src/lib.rs | 0 .../vehicle_velocity_converter/Cargo.toml | 0 .../vehicle_velocity_converter/src/lib.rs | 0 14 files changed, 0 insertions(+), 0 deletions(-) rename applications/{tests/test_autoware => autoware}/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/common_types/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/common_types/src/lib.rs (100%) rename applications/{tests/test_autoware => autoware}/ekf_localizer/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/ekf_localizer/src/lib.rs (100%) rename applications/{tests/test_autoware => autoware}/gyro_odometer/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/gyro_odometer/src/lib.rs (100%) rename applications/{tests/test_autoware => autoware}/imu_corrector/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/imu_corrector/src/lib.rs (100%) rename applications/{tests/test_autoware => autoware}/imu_driver/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/imu_driver/src/lib.rs (100%) rename applications/{tests/test_autoware => autoware}/src/lib.rs (100%) rename applications/{tests/test_autoware => autoware}/vehicle_velocity_converter/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/vehicle_velocity_converter/src/lib.rs (100%) diff --git a/applications/tests/test_autoware/Cargo.toml b/applications/autoware/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/Cargo.toml rename to applications/autoware/Cargo.toml diff --git a/applications/tests/test_autoware/common_types/Cargo.toml b/applications/autoware/common_types/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/common_types/Cargo.toml rename to applications/autoware/common_types/Cargo.toml diff --git a/applications/tests/test_autoware/common_types/src/lib.rs b/applications/autoware/common_types/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/common_types/src/lib.rs rename to applications/autoware/common_types/src/lib.rs diff --git a/applications/tests/test_autoware/ekf_localizer/Cargo.toml b/applications/autoware/ekf_localizer/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/ekf_localizer/Cargo.toml rename to applications/autoware/ekf_localizer/Cargo.toml diff --git a/applications/tests/test_autoware/ekf_localizer/src/lib.rs b/applications/autoware/ekf_localizer/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/ekf_localizer/src/lib.rs rename to applications/autoware/ekf_localizer/src/lib.rs diff --git a/applications/tests/test_autoware/gyro_odometer/Cargo.toml b/applications/autoware/gyro_odometer/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/gyro_odometer/Cargo.toml rename to applications/autoware/gyro_odometer/Cargo.toml diff --git a/applications/tests/test_autoware/gyro_odometer/src/lib.rs b/applications/autoware/gyro_odometer/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/gyro_odometer/src/lib.rs rename to applications/autoware/gyro_odometer/src/lib.rs diff --git a/applications/tests/test_autoware/imu_corrector/Cargo.toml b/applications/autoware/imu_corrector/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/imu_corrector/Cargo.toml rename to applications/autoware/imu_corrector/Cargo.toml diff --git a/applications/tests/test_autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/imu_corrector/src/lib.rs rename to applications/autoware/imu_corrector/src/lib.rs diff --git a/applications/tests/test_autoware/imu_driver/Cargo.toml b/applications/autoware/imu_driver/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/imu_driver/Cargo.toml rename to applications/autoware/imu_driver/Cargo.toml diff --git a/applications/tests/test_autoware/imu_driver/src/lib.rs b/applications/autoware/imu_driver/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/imu_driver/src/lib.rs rename to applications/autoware/imu_driver/src/lib.rs diff --git a/applications/tests/test_autoware/src/lib.rs b/applications/autoware/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/src/lib.rs rename to applications/autoware/src/lib.rs diff --git a/applications/tests/test_autoware/vehicle_velocity_converter/Cargo.toml b/applications/autoware/vehicle_velocity_converter/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/vehicle_velocity_converter/Cargo.toml rename to applications/autoware/vehicle_velocity_converter/Cargo.toml diff --git a/applications/tests/test_autoware/vehicle_velocity_converter/src/lib.rs b/applications/autoware/vehicle_velocity_converter/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/vehicle_velocity_converter/src/lib.rs rename to applications/autoware/vehicle_velocity_converter/src/lib.rs From b6d22bb2774d817f40950d18fea5509befd2d037 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 11 May 2026 10:51:15 +0900 Subject: [PATCH 04/10] fix: comment 3 Signed-off-by: nokosaaan --- applications/autoware/gyro_odometer/src/lib.rs | 6 ++++++ applications/autoware/imu_corrector/src/lib.rs | 6 ++++++ applications/autoware/imu_driver/src/lib.rs | 6 ++++++ .../vehicle_velocity_converter/src/lib.rs | 18 +++++------------- 4 files changed, 23 insertions(+), 13 deletions(-) diff --git a/applications/autoware/gyro_odometer/src/lib.rs b/applications/autoware/gyro_odometer/src/lib.rs index 834924786..7d6ae0d47 100644 --- a/applications/autoware/gyro_odometer/src/lib.rs +++ b/applications/autoware/gyro_odometer/src/lib.rs @@ -1,3 +1,9 @@ +// Ported from the following versions of the original C++ code: +// core/autoware_core: +// type: git +// url: https://github.com/autowarefoundation/autoware_core.git +// version: 1.8.0 + #![no_std] extern crate alloc; diff --git a/applications/autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs index abf265a33..ff61b769b 100644 --- a/applications/autoware/imu_corrector/src/lib.rs +++ b/applications/autoware/imu_corrector/src/lib.rs @@ -1,3 +1,9 @@ +// Ported from the following versions of the original C++ code: +// universe/autoware_universe: +// type: git +// url: https://github.com/autowarefoundation/autoware_universe.git +// version: 0.51.0 + #![no_std] extern crate alloc; diff --git a/applications/autoware/imu_driver/src/lib.rs b/applications/autoware/imu_driver/src/lib.rs index 15ef58e24..bdbf28c5a 100644 --- a/applications/autoware/imu_driver/src/lib.rs +++ b/applications/autoware/imu_driver/src/lib.rs @@ -1,3 +1,9 @@ +// Ported from the following versions of the original C++ code: +// tamagawa_imu_driver +// type: git +// url: https://github.com/tier4/tamagawa_imu_driver +// version: 0.1.0 + #![no_std] extern crate alloc; diff --git a/applications/autoware/vehicle_velocity_converter/src/lib.rs b/applications/autoware/vehicle_velocity_converter/src/lib.rs index 2b523c95f..cbb2632b8 100644 --- a/applications/autoware/vehicle_velocity_converter/src/lib.rs +++ b/applications/autoware/vehicle_velocity_converter/src/lib.rs @@ -1,16 +1,8 @@ -// Copyright 2021 TierIV -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. +// Ported from the following versions of the original C++ code: +// core/autoware_core: +// type: git +// url: https://github.com/autowarefoundation/autoware_core.git +// version: 1.8.0 #![no_std] From 308bb3504f498c1290ad5536cb8863c8e8dba75d Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 11 May 2026 10:53:04 +0900 Subject: [PATCH 05/10] fix: comment 4 Signed-off-by: nokosaaan --- applications/autoware/ekf_localizer/src/lib.rs | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/applications/autoware/ekf_localizer/src/lib.rs b/applications/autoware/ekf_localizer/src/lib.rs index 038a2cbf1..eaf88a658 100644 --- a/applications/autoware/ekf_localizer/src/lib.rs +++ b/applications/autoware/ekf_localizer/src/lib.rs @@ -1,3 +1,9 @@ +// Ported from the following versions of the original C++ code: +// core/autoware_core: +// type: git +// url: https://github.com/autowarefoundation/autoware_core.git +// version: 1.8.0 + #![no_std] #![allow(non_snake_case)] From affb3625df17b52c65d8b40e98c6717a5b14e6b1 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Tue, 9 Jun 2026 16:25:14 +0900 Subject: [PATCH 06/10] fix: Cargo.toml Signed-off-by: nokosaaan --- applications/autoware/ekf_localizer/Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/applications/autoware/ekf_localizer/Cargo.toml b/applications/autoware/ekf_localizer/Cargo.toml index e2b8866b6..1ec683b19 100644 --- a/applications/autoware/ekf_localizer/Cargo.toml +++ b/applications/autoware/ekf_localizer/Cargo.toml @@ -8,4 +8,4 @@ libm = "0.2" nalgebra = { version = "0.32", default-features = false} approx = "0.5" common_types = { path = "../common_types", default-features = false } -vehicle_velocity_converter = { path = "../vehicle_velocity_converter", default-features = false} \ No newline at end of file +vehicle_velocity_converter = { path = "../vehicle_velocity_converter", default-features = false} From 98649e13cb26934e162a31928d6671588b056db2 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Fri, 24 Jul 2026 03:55:32 +0900 Subject: [PATCH 07/10] fix: add licence Signed-off-by: nokosaaan --- applications/autoware/ekf_localizer/src/lib.rs | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/applications/autoware/ekf_localizer/src/lib.rs b/applications/autoware/ekf_localizer/src/lib.rs index eaf88a658..3fdfbf286 100644 --- a/applications/autoware/ekf_localizer/src/lib.rs +++ b/applications/autoware/ekf_localizer/src/lib.rs @@ -1,7 +1,23 @@ +// Copyright 2018-2019 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// // Ported from the following versions of the original C++ code: // core/autoware_core: // type: git // url: https://github.com/autowarefoundation/autoware_core.git +// original file path: localization/autoware_ekf_localizer/src/ekf_localizer.cpp +// test code: localization/autoware_ekf_localizer/test/test_ekf_module.cpp // version: 1.8.0 #![no_std] From 1e79b0196233977a0a11eb16ea8433280f91d223 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Fri, 31 Jul 2026 16:17:21 +0900 Subject: [PATCH 08/10] fix: ignore log file Signed-off-by: nokosaaan --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index b2771824f..e3294ce99 100644 --- a/.gitignore +++ b/.gitignore @@ -42,6 +42,7 @@ *.su *.idb *.pdb +log/ # Kernel Module Compile Results *.mod* From 89719fc2e63a0993e55070b6f23f8bff89f9e78d Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Sat, 1 Aug 2026 10:25:55 +0900 Subject: [PATCH 09/10] feat: implement main 5 features of ekf localizer Signed-off-by: nokosaaan --- applications/autoware/Cargo.toml | 1 + .../autoware/ekf_localizer/Cargo.toml | 6 +- .../autoware/ekf_localizer/src/covariance.rs | 145 ++ .../ekf_localizer/src/kalman_filter.rs | 847 ++++++++++++ .../autoware/ekf_localizer/src/lib.rs | 1170 ++++++++++++----- .../autoware/ekf_localizer/src/mahalanobis.rs | 123 ++ .../autoware/ekf_localizer/src/measurement.rs | 199 +++ .../autoware/ekf_localizer/src/numeric.rs | 68 + .../ekf_localizer/src/state_transition.rs | 200 +++ .../ekf_localizer/src/warn_throttle.rs | 143 ++ 10 files changed, 2596 insertions(+), 306 deletions(-) create mode 100644 applications/autoware/ekf_localizer/src/covariance.rs create mode 100644 applications/autoware/ekf_localizer/src/kalman_filter.rs create mode 100644 applications/autoware/ekf_localizer/src/mahalanobis.rs create mode 100644 applications/autoware/ekf_localizer/src/measurement.rs create mode 100644 applications/autoware/ekf_localizer/src/numeric.rs create mode 100644 applications/autoware/ekf_localizer/src/state_transition.rs create mode 100644 applications/autoware/ekf_localizer/src/warn_throttle.rs diff --git a/applications/autoware/Cargo.toml b/applications/autoware/Cargo.toml index 31b7485c2..265f6b0ab 100644 --- a/applications/autoware/Cargo.toml +++ b/applications/autoware/Cargo.toml @@ -17,4 +17,5 @@ imu_driver = { path = "./imu_driver", default-features = false } imu_corrector = { path = "./imu_corrector", default-features = false } vehicle_velocity_converter = { path = "./vehicle_velocity_converter", default-features = false } gyro_odometer = { path = "./gyro_odometer", default-features = false} +ekf_localizer = { path = "./ekf_localizer", default-features = false } diff --git a/applications/autoware/ekf_localizer/Cargo.toml b/applications/autoware/ekf_localizer/Cargo.toml index 1ec683b19..08b348e5f 100644 --- a/applications/autoware/ekf_localizer/Cargo.toml +++ b/applications/autoware/ekf_localizer/Cargo.toml @@ -5,7 +5,9 @@ edition = "2021" [dependencies] libm = "0.2" -nalgebra = { version = "0.32", default-features = false} -approx = "0.5" +nalgebra = { version = "0.32", default-features = false, features = ["alloc", "libm"] } +approx = "0.5" common_types = { path = "../common_types", default-features = false } vehicle_velocity_converter = { path = "../vehicle_velocity_converter", default-features = false} +imu_corrector = { path = "../imu_corrector", default-features = false } +log = { version = "0.4", default-features = false } diff --git a/applications/autoware/ekf_localizer/src/covariance.rs b/applications/autoware/ekf_localizer/src/covariance.rs new file mode 100644 index 000000000..c333cd45f --- /dev/null +++ b/applications/autoware/ekf_localizer/src/covariance.rs @@ -0,0 +1,145 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Ported from the following versions of the original C++ code: +// core/autoware_core: +// type: git +// url: https://github.com/autowarefoundation/autoware_core.git +// original file path: localization/autoware_ekf_localizer/src/covariance.cpp +// test code: localization/autoware_ekf_localizer/test/test_covariance.cpp +// version: 1.8.0 + +use nalgebra::DMatrix; + +use crate::StateIndex; + +// XYZRPY (6x6, row-major) covariance array indices, same layout as measurement.rs. +// Only the 9 entries this file's two functions actually use are pulled out of upstream's +// XYZRPY_COV_IDX enum (which defines all 36: X_Z, X_ROLL, Z_Z, ROLL_ROLL, PITCH_PITCH, +// etc. are defined there but unused here) -- not an incomplete port. +const X_X: usize = 0; +const X_Y: usize = 1; +const X_YAW: usize = 5; +const Y_X: usize = 6; +const Y_Y: usize = 7; +const Y_YAW: usize = 11; +const YAW_X: usize = 30; +const YAW_Y: usize = 31; +const YAW_YAW: usize = 35; + +/// Converts the 6x6 EKF state covariance into a ROS-style 6x6 (XYZRPY) flattened +/// pose covariance, filling in only the X/Y/YAW block (Z/ROLL/PITCH come from the +/// Simple1DFilters and are overwritten by the caller). +pub fn ekf_covariance_to_pose_message_covariance(p: &DMatrix) -> [f64; 36] { + let x = StateIndex::X as usize; + let y = StateIndex::Y as usize; + let yaw = StateIndex::Yaw as usize; + + let mut covariance = [0.0; 36]; + covariance[X_X] = p[(x, x)]; + covariance[X_Y] = p[(x, y)]; + covariance[X_YAW] = p[(x, yaw)]; + covariance[Y_X] = p[(y, x)]; + covariance[Y_Y] = p[(y, y)]; + covariance[Y_YAW] = p[(y, yaw)]; + covariance[YAW_X] = p[(yaw, x)]; + covariance[YAW_Y] = p[(yaw, y)]; + covariance[YAW_YAW] = p[(yaw, yaw)]; + covariance +} + +/// Converts the 6x6 EKF state covariance into a ROS-style flattened twist covariance, +/// mapping VX -> linear.x and WZ -> angular.z. +pub fn ekf_covariance_to_twist_message_covariance(p: &DMatrix) -> [f64; 36] { + let vx = StateIndex::Vx as usize; + let wz = StateIndex::Wz as usize; + + let mut covariance = [0.0; 36]; + covariance[X_X] = p[(vx, vx)]; + covariance[X_YAW] = p[(vx, wz)]; + covariance[YAW_X] = p[(wz, vx)]; + covariance[YAW_YAW] = p[(wz, wz)]; + covariance +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn pose_covariance_mapping() { + let mut p = DMatrix::::zeros(6, 6); + p[(0, 0)] = 1.0; + p[(0, 1)] = 2.0; + p[(0, 2)] = 3.0; + p[(1, 0)] = 4.0; + p[(1, 1)] = 5.0; + p[(1, 2)] = 6.0; + p[(2, 0)] = 7.0; + p[(2, 1)] = 8.0; + p[(2, 2)] = 9.0; + + let cov = ekf_covariance_to_pose_message_covariance(&p); + assert_eq!(cov[0], 1.0); + assert_eq!(cov[1], 2.0); + assert_eq!(cov[5], 3.0); + assert_eq!(cov[6], 4.0); + assert_eq!(cov[7], 5.0); + assert_eq!(cov[11], 6.0); + assert_eq!(cov[30], 7.0); + assert_eq!(cov[31], 8.0); + assert_eq!(cov[35], 9.0); + } + + // Matches upstream test_covariance.cpp's "ensure other elements are zero" sub-case: + // with an all-zero input, every one of the 36 output slots (not just the 9 mapped + // ones) must be zero -- catches accidental garbage/uninitialized values in the + // unmapped Z/ROLL/PITCH slots this function doesn't touch. + #[test] + fn pose_covariance_mapping_zero_input_yields_zero_output() { + let p = DMatrix::::zeros(6, 6); + let cov = ekf_covariance_to_pose_message_covariance(&p); + for e in cov { + assert_eq!(e, 0.0); + } + } + + // Matches upstream's `EKFCovarianceToTwistMessageCovariance.SmokeTest` exactly, + // including the X_YAW/YAW_X off-diagonal terms (deliberately given different values, + // 2 vs 3, so a transposition bug would be caught). + #[test] + fn twist_covariance_mapping() { + let mut p = DMatrix::::zeros(6, 6); + p[(4, 4)] = 1.0; + p[(4, 5)] = 2.0; + p[(5, 4)] = 3.0; + p[(5, 5)] = 4.0; + + let cov = ekf_covariance_to_twist_message_covariance(&p); + assert_eq!(cov[0], 1.0); + assert_eq!(cov[5], 2.0); + assert_eq!(cov[30], 3.0); + assert_eq!(cov[35], 4.0); + } + + // Matches upstream's "ensure other elements are zero" sub-case for the twist mapping. + #[test] + fn twist_covariance_mapping_zero_input_yields_zero_output() { + let p = DMatrix::::zeros(6, 6); + let cov = ekf_covariance_to_twist_message_covariance(&p); + for e in cov { + assert_eq!(e, 0.0); + } + } +} diff --git a/applications/autoware/ekf_localizer/src/kalman_filter.rs b/applications/autoware/ekf_localizer/src/kalman_filter.rs new file mode 100644 index 000000000..5b04d370d --- /dev/null +++ b/applications/autoware/ekf_localizer/src/kalman_filter.rs @@ -0,0 +1,847 @@ +// Copyright 2018-2019 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Ported from the following versions of the original C++ code: +// core/autoware_core: +// type: git +// url: https://github.com/autowarefoundation/autoware_core.git +// original file path: common/autoware_kalman_filter/{include/autoware/kalman_filter/time_delay_kalman_filter.hpp, src/time_delay_kalman_filter.cpp} +// test code: common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp +// version: 1.8.0 +// +// NOTE: this is not part of the autoware_ekf_localizer package upstream. It is a shared +// Autoware utility (`autoware_kalman_filter`) that `ekf_module.cpp` depends on via +// `#include `. It is ported here, +// scoped to this crate, because no standalone kalman_filter crate exists in this +// repository yet. +// +// RT NOTE: `predict_with_delay`/`update_with_delay` run on the EKF's periodic predict/update +// tick, so they are treated as RT-critical. Every buffer they touch is preallocated once in +// `init()`; the two methods only read/write into those buffers (via `copy_from`/`gemm`/ +// `mul_to`, and a small number of fixed-bound element-wise loops) instead of calling +// `DMatrix::zeros`/`.clone_owned()` on every call. See each method's WCET contract for the +// one remaining bounded exception. + +extern crate alloc; + +use nalgebra::{DMatrix, DVector}; + +/// Upper bound on `max_delay_step` (i.e. upstream's `extend_state_step`) accepted by +/// `init`. This bounds the O(dim_x^2 * max_delay_step^2) memory used by `p_ex` and its +/// scratch buffers so a misconfigured caller cannot silently blow up allocation size at +/// `init()` time. Upstream's own shipped default is 50; 200 leaves generous headroom +/// while still being a fixed, documented cap. +pub const MAX_DELAY_STEP: usize = 200; + +/// Upper bound on the measurement dimension (`dim_y`) accepted by `update_with_delay`. +/// Fixing this bound lets `update_with_delay`'s scratch buffers (`e`, `c_transpose`, +/// `c_p_dd`, `s`, `p_ct`, `k_transpose`, `k`) be sized once in `init()` instead of +/// reallocating on every call. This crate's only two callers use `dim_y = 3` (pose: x, +/// y, yaw) and `dim_y = 2` (twist: vx, wz), so 3 covers both with no slack to spare for +/// a third caller — bump this (and re-check the scratch buffer sizes below) before +/// adding one. +const MAX_DIM_Y: usize = 3; + +/// Kalman filter that keeps an extended state history so that measurements which arrive +/// with a known delay can be fused against the state as it was `delay_step` predict-ticks +/// ago, instead of being (incorrectly) fused against the *current* state. +/// +/// All scratch buffers used by `predict_with_delay`/`update_with_delay` are preallocated +/// by `init()`; see the WCET contract on each method. +#[derive(Debug, Clone)] +pub struct DelayCompensatedKalmanFilter { + dim_x: usize, + max_delay_step: usize, + x_ex: DVector, + p_ex: DMatrix, + + // --- predict_with_delay scratch (sized in `init()`) --- + x_next_ex: DVector, // dim_x_ex + p_next_ex: DMatrix, // dim_x_ex x dim_x_ex; also reused by update_with_delay + a_transpose: DMatrix, // dim_x x dim_x + ap11: DMatrix, // dim_x x dim_x + + // --- update_with_delay scratch (sized in `init()`) --- + x_d: DVector, // dim_x + e: DVector, // MAX_DIM_Y, use rows(0, dim_y) + c_transpose: DMatrix, // dim_x x MAX_DIM_Y + c_p_dd: DMatrix, // MAX_DIM_Y x dim_x + s: DMatrix, // MAX_DIM_Y x MAX_DIM_Y + p_ct: DMatrix, // dim_x_ex x MAX_DIM_Y + k_transpose: DMatrix, // MAX_DIM_Y x dim_x_ex + k: DMatrix, // dim_x_ex x MAX_DIM_Y +} + +impl DelayCompensatedKalmanFilter { + pub fn new() -> Self { + Self { + dim_x: 0, + max_delay_step: 0, + x_ex: DVector::zeros(0), + p_ex: DMatrix::zeros(0, 0), + x_next_ex: DVector::zeros(0), + p_next_ex: DMatrix::zeros(0, 0), + a_transpose: DMatrix::zeros(0, 0), + ap11: DMatrix::zeros(0, 0), + x_d: DVector::zeros(0), + e: DVector::zeros(MAX_DIM_Y), + c_transpose: DMatrix::zeros(0, MAX_DIM_Y), + c_p_dd: DMatrix::zeros(MAX_DIM_Y, 0), + s: DMatrix::zeros(MAX_DIM_Y, MAX_DIM_Y), + p_ct: DMatrix::zeros(0, MAX_DIM_Y), + k_transpose: DMatrix::zeros(MAX_DIM_Y, 0), + k: DMatrix::zeros(0, MAX_DIM_Y), + } + } + + /// Initializes (or re-initializes) the filter and (re)allocates every scratch buffer + /// used by `predict_with_delay`/`update_with_delay`. This is a setup-phase operation + /// (called at construction, and again on a re-localization event) — allocation here is + /// expected and fine; it is the *only* place in this type that allocates. + /// + /// `max_delay_step` is clamped to `MAX_DELAY_STEP` (logged as an error if it was over + /// the cap) so a misconfigured caller cannot make every later `predict_with_delay` call + /// allocate-free but arbitrarily large. + pub fn init(&mut self, x: &DVector, p0: &DMatrix, max_delay_step: usize) { + let dim_x = x.len(); + let max_delay_step = if max_delay_step > MAX_DELAY_STEP { + log::error!( + "requested max_delay_step {max_delay_step} exceeds the fixed cap {MAX_DELAY_STEP}; clamping." + ); + MAX_DELAY_STEP + } else { + max_delay_step + }; + let dim_x_ex = dim_x * max_delay_step; + + let mut x_ex = DVector::zeros(dim_x_ex); + let mut p_ex = DMatrix::zeros(dim_x_ex, dim_x_ex); + for i in 0..max_delay_step { + let offset = i * dim_x; + x_ex.rows_mut(offset, dim_x).copy_from(x); + p_ex.view_mut((offset, offset), (dim_x, dim_x)).copy_from(p0); + } + + self.dim_x = dim_x; + self.max_delay_step = max_delay_step; + self.x_ex = x_ex; + self.p_ex = p_ex; + + self.x_next_ex = DVector::zeros(dim_x_ex); + self.p_next_ex = DMatrix::zeros(dim_x_ex, dim_x_ex); + self.a_transpose = DMatrix::zeros(dim_x, dim_x); + self.ap11 = DMatrix::zeros(dim_x, dim_x); + + self.x_d = DVector::zeros(dim_x); + self.e = DVector::zeros(MAX_DIM_Y); + self.c_transpose = DMatrix::zeros(dim_x, MAX_DIM_Y); + self.c_p_dd = DMatrix::zeros(MAX_DIM_Y, dim_x); + self.s = DMatrix::zeros(MAX_DIM_Y, MAX_DIM_Y); + self.p_ct = DMatrix::zeros(dim_x_ex, MAX_DIM_Y); + self.k_transpose = DMatrix::zeros(MAX_DIM_Y, dim_x_ex); + self.k = DMatrix::zeros(dim_x_ex, MAX_DIM_Y); + } + + /// Current-time state estimate (the first `dim_x` block of the extended state). + pub fn latest_x(&self) -> DVector { + self.x_ex.rows(0, self.dim_x).clone_owned() + } + + /// Current-time state covariance (the first `dim_x x dim_x` block). + pub fn latest_p(&self) -> DMatrix { + self.p_ex.view((0, 0), (self.dim_x, self.dim_x)).clone_owned() + } + + /// Reads a single element of the state as it was `delay_step` predict-ticks ago. + pub fn x_element(&self, delay_step: usize, i: usize) -> f64 { + self.x_ex[delay_step * self.dim_x + i] + } + + /// Advances the extended state by one predict-tick: `x_next`/`a`/`q` describe the + /// (possibly nonlinear, already-linearized) process model for the *current* time step + /// only; older history blocks are shifted back and kept correlated with the new + /// current-time block through `a`, exactly as in `TimeDelayKalmanFilter::predictWithDelay`. + /// + /// WCET contract: + /// - No heap allocation (all buffers were preallocated by `init()`; the new/old state + /// are exchanged via `core::mem::swap`, not reallocated). + /// - No panics for `x_next`/`a`/`q` matching the `dim_x` passed to `init()` (this crate's + /// only caller, `EKFModule`, always builds them from that same `dim_x`). + /// - Cost is O(dim_x^2 * max_delay_step) for the state slide and the two cross-term + /// blocks, plus one O(dim_x_ex^2) `copy_from` (a memcpy, not an allocation) to carry + /// the untouched history forward. + /// - Does not log, format, block, or call unknown code. + pub fn predict_with_delay(&mut self, x_next: &DVector, a: &DMatrix, q: &DMatrix) { + let dim_x = self.dim_x; + let dim_x_ex = dim_x * self.max_delay_step; + let d_dim_x = dim_x_ex - dim_x; + + // Slide the state: x_next_ex = [x_next; x_ex[0..d_dim_x]]. + self.x_next_ex.rows_mut(0, dim_x).copy_from(x_next); + if d_dim_x > 0 { + self.x_next_ex + .rows_mut(dim_x, d_dim_x) + .copy_from(&self.x_ex.rows(0, d_dim_x)); + } + + // a_transpose = a^T (dim_x x dim_x, fixed-size regardless of max_delay_step). + for i in 0..dim_x { + for j in 0..dim_x { + self.a_transpose[(i, j)] = a[(j, i)]; + } + } + + // ap11 = a * p11 + { + let p11 = self.p_ex.view((0, 0), (dim_x, dim_x)); + a.mul_to(&p11, &mut self.ap11); + } + // p_next_ex[0..dim_x, 0..dim_x] = ap11 * a^T + q + { + let mut dest = self.p_next_ex.view_mut((0, 0), (dim_x, dim_x)); + dest.copy_from(q); + dest.gemm(1.0, &self.ap11, &self.a_transpose, 1.0); + } + + if d_dim_x > 0 { + // p_next_ex[0..dim_x, dim_x..] = a * p_ex[0..dim_x, 0..d_dim_x] + { + let p_top_strip = self.p_ex.view((0, 0), (dim_x, d_dim_x)); + let mut dest = self.p_next_ex.view_mut((0, dim_x), (dim_x, d_dim_x)); + a.mul_to(&p_top_strip, &mut dest); + } + // p_next_ex[dim_x.., 0..dim_x] = p_ex[0..d_dim_x, 0..dim_x] * a^T + { + let p_left_strip = self.p_ex.view((0, 0), (d_dim_x, dim_x)); + let mut dest = self.p_next_ex.view_mut((dim_x, 0), (d_dim_x, dim_x)); + p_left_strip.mul_to(&self.a_transpose, &mut dest); + } + // p_next_ex[dim_x.., dim_x..] = p_ex[0..d_dim_x, 0..d_dim_x] (unchanged history) + { + let p_history = self.p_ex.view((0, 0), (d_dim_x, d_dim_x)); + self.p_next_ex + .view_mut((dim_x, dim_x), (d_dim_x, d_dim_x)) + .copy_from(&p_history); + } + } + + core::mem::swap(&mut self.x_ex, &mut self.x_next_ex); + core::mem::swap(&mut self.p_ex, &mut self.p_next_ex); + } + + /// Fuses a measurement `y` (with observation matrix `c` and noise covariance `r`) against + /// the state as it was `delay_step` predict-ticks ago. Returns `false` (and leaves the + /// filter untouched) on any dimension mismatch, an out-of-range `delay_step`, a + /// non-invertible innovation covariance, or a NaN/Inf Kalman gain — matching upstream + /// `TimeDelayKalmanFilter::updateWithDelay`. + /// + /// WCET contract: + /// - No heap allocation, with one bounded exception: inverting the `dim_y x dim_y` + /// (dim_y <= `MAX_DIM_Y` = 3) innovation covariance goes through nalgebra's + /// `try_inverse()`, which internally clones into an owned matrix of at most 3x3 = 9 + /// `f64` (72 bytes). This allocation is fixed-size and independent of + /// `max_delay_step`/`extend_state_step` — it does not grow with history depth. + /// - Returns `false` instead of panicking on any dimension mismatch, `dim_y > + /// MAX_DIM_Y`, or an out-of-range `delay_step`. + /// - Cost is O(dim_x_ex * dim_y) to build the gain and O(dim_x_ex^2) for the final + /// covariance subtraction (an element-wise loop over preallocated buffers, not an + /// allocation). + /// - Does not log more than once per rejected call, does not block, does not call + /// unknown code. + pub fn update_with_delay( + &mut self, + y: &DVector, + c: &DMatrix, + r: &DMatrix, + delay_step: usize, + ) -> bool { + let dim_y = y.nrows(); + if dim_y == 0 || dim_y > MAX_DIM_Y { + log::error!("Unsupported measurement dimension: {dim_y} (max {MAX_DIM_Y})."); + return false; + } + if delay_step >= self.max_delay_step { + log::error!( + "Invalid delay step: {delay_step}. max_delay_step is {}. Update ignored.", + self.max_delay_step + ); + return false; + } + if c.ncols() != self.dim_x { + log::error!( + "Dimension mismatch in C matrix: expected {} columns, got {}.", + self.dim_x, + c.ncols() + ); + return false; + } + if y.nrows() != c.nrows() { + log::error!( + "Dimension mismatch between y and C: y.rows()={}, C.rows()={}.", + y.nrows(), + c.nrows() + ); + return false; + } + if r.nrows() != r.ncols() || r.nrows() != c.nrows() { + log::error!("Dimension mismatch in R matrix."); + return false; + } + + let dim_x = self.dim_x; + let dim_x_ex = dim_x * self.max_delay_step; + let start_idx = dim_x * delay_step; + + self.x_d.copy_from(&self.x_ex.rows(start_idx, dim_x)); + + // e[0..dim_y] = y - c * x_d + { + let mut e = self.e.rows_mut(0, dim_y); + c.mul_to(&self.x_d, &mut e); + for i in 0..dim_y { + e[i] = y[i] - e[i]; + } + } + + // c_transpose[0..dim_x, 0..dim_y] = c^T + { + let mut ct = self.c_transpose.view_mut((0, 0), (dim_x, dim_y)); + for i in 0..dim_x { + for j in 0..dim_y { + ct[(i, j)] = c[(j, i)]; + } + } + } + + // s[0..dim_y, 0..dim_y] = r + c * p_dd * c^T + { + let p_dd = self.p_ex.view((start_idx, start_idx), (dim_x, dim_x)); + let mut c_p_dd = self.c_p_dd.view_mut((0, 0), (dim_y, dim_x)); + c.mul_to(&p_dd, &mut c_p_dd); + } + { + let c_p_dd = self.c_p_dd.view((0, 0), (dim_y, dim_x)); + let ct = self.c_transpose.view((0, 0), (dim_x, dim_y)); + let mut s = self.s.view_mut((0, 0), (dim_y, dim_y)); + s.copy_from(r); + s.gemm(1.0, &c_p_dd, &ct, 1.0); + } + + // p_ct[0..dim_x_ex, 0..dim_y] = p_ex[:, start_idx..start_idx+dim_x] * c^T + { + let p_star_d = self.p_ex.columns(start_idx, dim_x); + let ct = self.c_transpose.view((0, 0), (dim_x, dim_y)); + let mut p_ct = self.p_ct.view_mut((0, 0), (dim_x_ex, dim_y)); + p_star_d.mul_to(&ct, &mut p_ct); + } + + // Bounded exception (documented in the WCET contract above): inverting a <=3x3 + // matrix clones it into an owned buffer internally. + let s_view = self.s.view((0, 0), (dim_y, dim_y)); + let s_inv = match s_view.clone_owned().try_inverse() { + Some(inv) => inv, + None => { + log::error!("Innovation covariance S is not invertible. Update ignored."); + return false; + } + }; + + // k[0..dim_x_ex, 0..dim_y] = p_ct * s_inv + { + let p_ct = self.p_ct.view((0, 0), (dim_x_ex, dim_y)); + let mut k = self.k.view_mut((0, 0), (dim_x_ex, dim_y)); + p_ct.mul_to(&s_inv, &mut k); + } + + { + let k = self.k.view((0, 0), (dim_x_ex, dim_y)); + if k.iter().any(|v| v.is_nan() || v.is_infinite()) { + log::error!("Kalman gain contains NaN or Inf. Aborting update."); + return false; + } + } + + // x_ex += k * e + { + let k = self.k.view((0, 0), (dim_x_ex, dim_y)); + let e = self.e.rows(0, dim_y); + self.x_ex.gemm(1.0, &k, &e, 1.0); + } + + // p_ex -= p_ct * k^T, computed in the same order as upstream + // (`P_.noalias() -= P_CT * K.transpose();`) rather than the mathematically-equal + // `k * p_ct^T` (which would introduce a floating-point rounding difference from a + // different operand order, even though it is provably the same value since S^-1, + // and hence S^-1's role in K, is symmetric). k_transpose + p_next_ex are reused + // scratch buffers, so this still performs no allocation. + { + let k = self.k.view((0, 0), (dim_x_ex, dim_y)); + let mut k_t = self.k_transpose.view_mut((0, 0), (dim_y, dim_x_ex)); + for i in 0..dim_y { + for j in 0..dim_x_ex { + k_t[(i, j)] = k[(j, i)]; + } + } + } + { + let p_ct = self.p_ct.view((0, 0), (dim_x_ex, dim_y)); + let k_t = self.k_transpose.view((0, 0), (dim_y, dim_x_ex)); + let mut delta = self.p_next_ex.view_mut((0, 0), (dim_x_ex, dim_x_ex)); + p_ct.mul_to(&k_t, &mut delta); + } + for i in 0..dim_x_ex { + for j in 0..dim_x_ex { + self.p_ex[(i, j)] -= self.p_next_ex[(i, j)]; + } + } + + true + } +} + +impl Default for DelayCompensatedKalmanFilter { + fn default() -> Self { + Self::new() + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn identity_setup(dim_x: usize, max_delay_step: usize) -> DelayCompensatedKalmanFilter { + let mut kf = DelayCompensatedKalmanFilter::new(); + let x0 = DVector::from_element(dim_x, 1.0); + let p0 = DMatrix::::identity(dim_x, dim_x); + kf.init(&x0, &p0, max_delay_step); + kf + } + + // [own test] no upstream equivalent. + #[test] + fn init_replicates_initial_state_across_all_delay_blocks() { + let kf = identity_setup(2, 3); + assert_eq!(kf.latest_x().len(), 2); + for step in 0..3 { + assert_eq!(kf.x_element(step, 0), 1.0); + assert_eq!(kf.x_element(step, 1), 1.0); + } + } + + // [own test] no upstream equivalent (MAX_DELAY_STEP is this crate's own cap). + #[test] + fn init_clamps_max_delay_step_to_the_documented_cap() { + let mut kf = DelayCompensatedKalmanFilter::new(); + let x0 = DVector::from_element(1, 0.0); + let p0 = DMatrix::::identity(1, 1); + kf.init(&x0, &p0, MAX_DELAY_STEP + 50); + + // Direct check on the clamped field, not just an indirect consequence of it. + assert_eq!(kf.max_delay_step, MAX_DELAY_STEP); + // dim_x = 1, so dim_x_ex == max_delay_step: confirms the buffer itself was sized + // to the clamped value, not just that the field says so. + assert_eq!(kf.x_ex.len(), MAX_DELAY_STEP); + // The last valid block is at index MAX_DELAY_STEP - 1; anything beyond that would + // have panicked on out-of-bounds access if the cap were not enforced. + assert_eq!(kf.x_element(MAX_DELAY_STEP - 1, 0), 0.0); + } + + // [own test] no upstream equivalent. + #[test] + fn predict_with_delay_shifts_history_back() { + let mut kf = identity_setup(1, 3); + let a = DMatrix::::identity(1, 1); + let q = DMatrix::::zeros(1, 1); + + kf.predict_with_delay(&DVector::from_element(1, 2.0), &a, &q); + assert_eq!(kf.x_element(0, 0), 2.0); // newest + assert_eq!(kf.x_element(1, 0), 1.0); // previous "now" pushed back + assert_eq!(kf.x_element(2, 0), 1.0); // oldest history, still the initial value + + kf.predict_with_delay(&DVector::from_element(1, 3.0), &a, &q); + assert_eq!(kf.x_element(0, 0), 3.0); + assert_eq!(kf.x_element(1, 0), 2.0); + assert_eq!(kf.x_element(2, 0), 1.0); + } + + // [own test] no upstream equivalent. + #[test] + fn update_with_delay_without_correlation_only_touches_targeted_block() { + // Right after init(), each delay block's covariance is independent (`init` only + // fills the diagonal blocks, see upstream `TimeDelayKalmanFilter::init`), so a + // correction to block 1 must not leak into block 0. + let mut kf = identity_setup(1, 2); + let c = DMatrix::::identity(1, 1); + let r = DMatrix::from_element(1, 1, 0.01); + + let ok = kf.update_with_delay(&DVector::from_element(1, 5.0), &c, &r, 1); + assert!(ok); + assert!(kf.x_element(1, 0) > 1.0); + assert_eq!(kf.x_element(0, 0), 1.0); + } + + // [own test] no upstream equivalent. + #[test] + fn update_with_delay_propagates_correction_to_current_block_via_correlation() { + // A predict tick correlates the new current block with the (now one-step-older) + // history block through the `A*P11` cross terms, so a correction to the delayed + // block should ripple forward into the current-time estimate too. + let mut kf = identity_setup(1, 2); + let a = DMatrix::::identity(1, 1); + let q = DMatrix::::zeros(1, 1); + kf.predict_with_delay(&DVector::from_element(1, 1.0), &a, &q); + + let c = DMatrix::::identity(1, 1); + let r = DMatrix::from_element(1, 1, 0.01); + + let ok = kf.update_with_delay(&DVector::from_element(1, 5.0), &c, &r, 1); + assert!(ok); + assert!(kf.x_element(1, 0) > 1.0); + assert!(kf.x_element(0, 0) > 1.0); + } + + // Matches upstream test_time_delay_kalman_filter.cpp's + // `UpdateWithInvalidDelayStepExceedsMax`. Upstream also has a separate + // `UpdateWithNegativeDelayStep` (passing `delay_step = -1`, since upstream's + // `delay_step` is a plain `int`): this crate uses `delay_step: usize`, so a negative + // delay step is not a value that can be constructed at all -- the type system rules + // it out at compile time instead of needing a runtime check/test. + #[test] + fn update_with_delay_rejects_out_of_range_delay_step() { + let mut kf = identity_setup(1, 2); + let c = DMatrix::::identity(1, 1); + let r = DMatrix::from_element(1, 1, 0.01); + + let ok = kf.update_with_delay(&DVector::from_element(1, 5.0), &c, &r, 2); + assert!(!ok); + assert_eq!(kf.x_element(0, 0), 1.0); + } + + // Matches upstream's `UpdateWithDimensionMismatchInC` (C with `dim_x + 1` columns + // instead of `dim_x` must be rejected, not panic). + #[test] + fn update_with_delay_rejects_c_with_wrong_column_count() { + let mut kf = identity_setup(3, 2); + let c_wrong = DMatrix::::identity(3, 4); // dim_x is 3, this has 4 columns + let r = DMatrix::::identity(3, 3); + + let ok = kf.update_with_delay(&DVector::from_element(3, 1.0), &c_wrong, &r, 0); + assert!(!ok); + } + + // [own test] no upstream equivalent (MAX_DIM_Y is this crate's own cap). + #[test] + fn update_with_delay_rejects_measurement_dimension_over_the_cap() { + let mut kf = identity_setup(2, 2); + let c = DMatrix::::identity(4, 2); // dim_y = 4 > MAX_DIM_Y + let r = DMatrix::::identity(4, 4); + let ok = kf.update_with_delay(&DVector::from_element(4, 1.0), &c, &r, 0); + assert!(!ok); + } + + // [own test] no upstream equivalent. Same underlying scalar Kalman update as + // `update_with_delay_matches_hand_computed_scalar_kalman_update`, applied independently + // across 2 decoupled dimensions (C, P0, and R are all diagonal/identity here, so there + // is no cross-term coupling the two measurement rows): x0=1.0, P0=1.0, C=1, R=0.01, + // y=5.0 for each dimension, giving the same K = 1/1.01 as the dim_y=1 case. + #[test] + fn update_with_delay_matches_a_2x2_measurement() { + let mut kf = identity_setup(2, 1); + let c = DMatrix::::identity(2, 2); + let r = DMatrix::::identity(2, 2) * 0.01; + + let ok = kf.update_with_delay(&DVector::from_vec(alloc::vec![5.0, 5.0]), &c, &r, 0); + assert!(ok); + + let k = 1.0 / 1.01; + let expected_x = 1.0 + k * 4.0; + assert!((kf.x_element(0, 0) - expected_x).abs() < 1e-12); + assert!((kf.x_element(0, 1) - expected_x).abs() < 1e-12); + + let expected_p = 1.0 - k * 1.0; + let p = kf.latest_p(); + assert!((p[(0, 0)] - expected_p).abs() < 1e-12); + assert!((p[(1, 1)] - expected_p).abs() < 1e-12); + // Still decoupled: no cross-term should have appeared between the two dimensions. + assert_eq!(p[(0, 1)], 0.0); + assert_eq!(p[(1, 0)], 0.0); + } + + // [own test] no upstream equivalent. + #[test] + fn update_with_delay_matches_hand_computed_scalar_kalman_update() { + // Golden-value check against the textbook scalar Kalman update, independent of + // this file's implementation, to catch any operand-order regression in the + // gemm/mul_to rewrite (e.g. accidentally computing K*P_CT^T instead of + // P_CT*K^T -- provably the same value, but a good place for a copy-paste bug to + // hide). x0=1.0, P0=1.0, C=1, R=0.01, y=5.0: + // e = y - C*x0 = 5.0 - 1.0 = 4.0 + // S = R + C*P0*C^T = 0.01 + 1.0 = 1.01 + // P_CT = P0*C^T = 1.0 + // K = P_CT / S = 1.0 / 1.01 + // x1 = x0 + K*e = 1.0 + (1.0/1.01)*4.0 + // P1 = P0 - P_CT*K = 1.0 - (1.0/1.01)*1.0 + let mut kf = identity_setup(1, 1); + let c = DMatrix::::identity(1, 1); + let r = DMatrix::from_element(1, 1, 0.01); + + let ok = kf.update_with_delay(&DVector::from_element(1, 5.0), &c, &r, 0); + assert!(ok); + + let k = 1.0 / 1.01; + let expected_x = 1.0 + k * 4.0; + let expected_p = 1.0 - k * 1.0; + + assert!((kf.x_element(0, 0) - expected_x).abs() < 1e-12); + assert!((kf.latest_p()[(0, 0)] - expected_p).abs() < 1e-12); + } + + // --- Ground-truth cross-check, mirroring upstream test_time_delay_kalman_filter.cpp --- + // + // Upstream's `TimeDelayKalmanFilterTest` fixture re-derives predict/update with a + // *second, independently written* implementation (`ground_truth_predict`/ + // `ground_truth_update`, operating on a plain full-size `Eigen::MatrixXd` with no + // shared code with `TimeDelayKalmanFilter` itself) and checks the real implementation + // against it, using dim_x=3/max_delay_step=5 -- large enough that a transposed block + // or a wrong slice offset would actually be visible (the dim_x=1 tests above cannot + // catch that class of bug, since every "matrix" involved is a 1x1 scalar). This + // section ports that fixture and methodology, including the exact numeric values + // upstream uses (`kInitialCovariance`=0.1, `kProcessNoise`=0.01, + // `kMeasurementNoise`=0.001, `kStateTransitionScale`=2.0, `kObservationScale`=0.5). + mod ground_truth { + use nalgebra::{DMatrix, DVector}; + + pub fn predict( + x_ex: &mut DVector, + p_ex: &mut DMatrix, + x_next: &DVector, + a: &DMatrix, + q: &DMatrix, + dim_x: usize, + dim_x_ex: usize, + ) { + let d = dim_x_ex - dim_x; + + let mut x_shifted = DVector::zeros(dim_x_ex); + x_shifted.rows_mut(dim_x, d).copy_from(&x_ex.rows(0, d)); + x_shifted.rows_mut(0, dim_x).copy_from(x_next); + *x_ex = x_shifted; + + let mut p_tmp = DMatrix::zeros(dim_x_ex, dim_x_ex); + let p00 = p_ex.view((0, 0), (dim_x, dim_x)).clone_owned(); + p_tmp + .view_mut((0, 0), (dim_x, dim_x)) + .copy_from(&(a * &p00 * a.transpose() + q)); + let p0d = p_ex.view((0, 0), (dim_x, d)).clone_owned(); + p_tmp.view_mut((0, dim_x), (dim_x, d)).copy_from(&(a * &p0d)); + let pd0 = p_ex.view((0, 0), (d, dim_x)).clone_owned(); + p_tmp + .view_mut((dim_x, 0), (d, dim_x)) + .copy_from(&(&pd0 * a.transpose())); + let pdd = p_ex.view((0, 0), (d, d)).clone_owned(); + p_tmp.view_mut((dim_x, dim_x), (d, d)).copy_from(&pdd); + *p_ex = p_tmp; + } + + pub fn update( + x_ex: &mut DVector, + p_ex: &mut DMatrix, + y: &DVector, + c: &DMatrix, + r: &DMatrix, + delay_step: usize, + dim_x: usize, + dim_y: usize, + dim_x_ex: usize, + ) { + let mut c_ex = DMatrix::zeros(dim_y, dim_x_ex); + c_ex.view_mut((0, delay_step * dim_x), (dim_y, dim_x)) + .copy_from(c); + + let pct = &*p_ex * c_ex.transpose(); + let s = r + &c_ex * &pct; + let s_inv = s + .try_inverse() + .expect("test fixture's S must be invertible"); + let k = &pct * s_inv; + let y_pred = &c_ex * &*x_ex; + + *x_ex = &*x_ex + &k * (y - y_pred); + *p_ex = &*p_ex - &k * (&c_ex * &*p_ex); + } + } + + struct GroundTruthFixture { + kf: DelayCompensatedKalmanFilter, + x_ex_gt: DVector, + p_ex_gt: DMatrix, + a: DMatrix, + q: DMatrix, + c: DMatrix, + r: DMatrix, + } + + const GT_DIM_X: usize = 3; + const GT_MAX_DELAY_STEP: usize = 5; + const GT_DIM_X_EX: usize = GT_DIM_X * GT_MAX_DELAY_STEP; + const GT_EPSILON: f64 = 1e-5; + + impl GroundTruthFixture { + fn new() -> Self { + let x_t = DVector::from_vec(alloc::vec![1.0, 2.0, 3.0]); + let p_t = DMatrix::::identity(GT_DIM_X, GT_DIM_X) * 0.1; + + let mut kf = DelayCompensatedKalmanFilter::new(); + kf.init(&x_t, &p_t, GT_MAX_DELAY_STEP); + + let mut x_ex_gt = DVector::zeros(GT_DIM_X_EX); + let mut p_ex_gt = DMatrix::zeros(GT_DIM_X_EX, GT_DIM_X_EX); + for i in 0..GT_MAX_DELAY_STEP { + x_ex_gt.rows_mut(i * GT_DIM_X, GT_DIM_X).copy_from(&x_t); + p_ex_gt + .view_mut((i * GT_DIM_X, i * GT_DIM_X), (GT_DIM_X, GT_DIM_X)) + .copy_from(&p_t); + } + + Self { + kf, + x_ex_gt, + p_ex_gt, + a: DMatrix::::identity(GT_DIM_X, GT_DIM_X) * 2.0, + q: DMatrix::::identity(GT_DIM_X, GT_DIM_X) * 0.01, + c: DMatrix::::identity(GT_DIM_X, GT_DIM_X) * 0.5, + r: DMatrix::::identity(GT_DIM_X, GT_DIM_X) * 0.001, + } + } + + fn predict(&mut self, x_next: &DVector) { + ground_truth::predict( + &mut self.x_ex_gt, + &mut self.p_ex_gt, + x_next, + &self.a, + &self.q, + GT_DIM_X, + GT_DIM_X_EX, + ); + self.kf.predict_with_delay(x_next, &self.a, &self.q); + } + + fn update(&mut self, y: &DVector, delay_step: usize) -> bool { + let c = self.c.clone(); + let r = self.r.clone(); + ground_truth::update( + &mut self.x_ex_gt, + &mut self.p_ex_gt, + y, + &c, + &r, + delay_step, + GT_DIM_X, + GT_DIM_X, + GT_DIM_X_EX, + ); + self.kf.update_with_delay(y, &c, &r, delay_step) + } + + // Deliberately element-wise absolute-error, not upstream's `isApprox`-style + // whole-vector relative error (`||A-B|| <= prec * min(||A||,||B||)`): a + // relative/aggregate check can mask a single badly-off element when the rest of + // the vector is large, so per-element absolute comparison is strictly more + // sensitive here, at the cost of literal fidelity to `isApprox`'s formula. + fn assert_matches_ground_truth(&self) { + let x_check = self.kf.latest_x(); + let x_gt = self.x_ex_gt.rows(0, GT_DIM_X).clone_owned(); + for i in 0..GT_DIM_X { + assert!( + (x_check[i] - x_gt[i]).abs() < GT_EPSILON, + "x[{i}]: {} vs ground truth {}", + x_check[i], + x_gt[i] + ); + } + + let p_check = self.kf.latest_p(); + let p_gt = self.p_ex_gt.view((0, 0), (GT_DIM_X, GT_DIM_X)).clone_owned(); + for i in 0..GT_DIM_X { + for j in 0..GT_DIM_X { + assert!( + (p_check[(i, j)] - p_gt[(i, j)]).abs() < GT_EPSILON, + "P[{i},{j}]: {} vs ground truth {}", + p_check[(i, j)], + p_gt[(i, j)] + ); + } + } + } + } + + // Matches upstream's `TimeDelayKalmanFilterTest.Prediction`. + #[test] + fn predict_matches_ground_truth_reimplementation() { + let mut fixture = GroundTruthFixture::new(); + let x_next = DVector::from_vec(alloc::vec![2.0, 4.0, 6.0]); + fixture.predict(&x_next); + fixture.assert_matches_ground_truth(); + } + + // Matches upstream's `TimeDelayKalmanFilterTest.UpdateWithDelay` (delay_step = 2). + #[test] + fn update_with_delay_at_step_2_matches_ground_truth_reimplementation() { + let mut fixture = GroundTruthFixture::new(); + fixture.predict(&DVector::from_vec(alloc::vec![2.0, 4.0, 6.0])); + + let y_delayed = DVector::from_vec(alloc::vec![1.05, 2.05, 3.05]); + assert!(fixture.update(&y_delayed, 2)); + fixture.assert_matches_ground_truth(); + } + + // Matches upstream's `TimeDelayKalmanFilterTest.UpdateWithZeroDelay`. + #[test] + fn update_with_zero_delay_matches_ground_truth_reimplementation() { + let mut fixture = GroundTruthFixture::new(); + fixture.predict(&DVector::from_vec(alloc::vec![2.0, 4.0, 6.0])); + + let y_current = DVector::from_vec(alloc::vec![2.1, 4.1, 6.1]); + assert!(fixture.update(&y_current, 0)); + fixture.assert_matches_ground_truth(); + } + + // Matches upstream's `TimeDelayKalmanFilterTest.UpdateWithMaxDelay` (updating the + // oldest block in the buffer, index `max_delay_step - 1`). + #[test] + fn update_with_max_delay_matches_ground_truth_reimplementation() { + let mut fixture = GroundTruthFixture::new(); + fixture.predict(&DVector::from_vec(alloc::vec![2.0, 4.0, 6.0])); + + let y_old = DVector::from_vec(alloc::vec![0.9, 1.9, 2.9]); + assert!(fixture.update(&y_old, GT_MAX_DELAY_STEP - 1)); + fixture.assert_matches_ground_truth(); + } + + // Matches upstream's `TimeDelayKalmanFilterTest.MultiplePredictionsBeforeUpdate`. + #[test] + fn multiple_predictions_before_update_matches_ground_truth_reimplementation() { + let mut fixture = GroundTruthFixture::new(); + + for i in 0..3 { + let scale = (i + 1) as f64; + fixture.predict(&DVector::from_vec(alloc::vec![2.0 * scale, 4.0 * scale, 6.0 * scale])); + } + + let y = DVector::from_vec(alloc::vec![1.0, 2.0, 3.0]); + assert!(fixture.update(&y, 2)); + fixture.assert_matches_ground_truth(); + } +} diff --git a/applications/autoware/ekf_localizer/src/lib.rs b/applications/autoware/ekf_localizer/src/lib.rs index 3fdfbf286..1675ddace 100644 --- a/applications/autoware/ekf_localizer/src/lib.rs +++ b/applications/autoware/ekf_localizer/src/lib.rs @@ -16,25 +16,64 @@ // core/autoware_core: // type: git // url: https://github.com/autowarefoundation/autoware_core.git -// original file path: localization/autoware_ekf_localizer/src/ekf_localizer.cpp -// test code: localization/autoware_ekf_localizer/test/test_ekf_module.cpp +// original file path: localization/autoware_ekf_localizer/src/ekf_module.cpp // version: 1.8.0 +// +// NOTE: upstream has no `test_ekf_module.cpp` at this tag (confirmed against the actual +// `localization/autoware_ekf_localizer/test/` tree) -- `EKFModule` itself is not unit +// tested upstream. The `#[cfg(test)] mod tests` below is therefore this crate's own +// integration-level test suite for `EKFModule`, not a port of an upstream test file. +// +// See src/kalman_filter.rs, src/state_transition.rs, src/measurement.rs, +// src/mahalanobis.rs, src/covariance.rs and src/numeric.rs for the upstream files each +// module corresponds to (each of those does have an upstream test file, ported alongside). #![no_std] #![allow(non_snake_case)] extern crate alloc; +mod covariance; +mod kalman_filter; +mod mahalanobis; +mod measurement; +mod numeric; +mod state_transition; +mod warn_throttle; + use alloc::{vec, vec::Vec}; pub use common_types::Header; use core::ptr::null_mut; use core::sync::atomic::{AtomicPtr, Ordering as AtomicOrdering}; use libm::{atan2, cos, sin}; -use nalgebra::{Matrix6, Vector3, Vector6}; -pub use vehicle_velocity_converter::TwistWithCovariance; +use nalgebra::{DMatrix, DVector, Matrix6, Quaternion as NQuaternion, Unit, UnitQuaternion, Vector3, Vector6}; + +pub use imu_corrector::Transform; +pub use vehicle_velocity_converter::{TwistWithCovariance, TwistWithCovarianceStamped}; + +use covariance::{ekf_covariance_to_pose_message_covariance, ekf_covariance_to_twist_message_covariance}; +use kalman_filter::DelayCompensatedKalmanFilter; +use mahalanobis::mahalanobis; +use measurement::{ + pose_measurement_covariance, pose_measurement_matrix, twist_measurement_covariance, + twist_measurement_matrix, +}; +use numeric::{has_inf, has_nan}; +use state_transition::{ + create_state_transition_matrix, normalize_yaw, predict_next_state, process_noise_covariance, +}; +use warn_throttle::WarnThrottle; static EKF_MODULE_INSTANCE: AtomicPtr = AtomicPtr::new(null_mut()); +// XYZRPY (6x6, row-major) covariance array indices, same layout as measurement.rs/covariance.rs. +const POSE_COV_X_X: usize = 0; +const POSE_COV_Y_Y: usize = 7; +const POSE_COV_Z_Z: usize = 14; +const POSE_COV_ROLL_ROLL: usize = 21; +const POSE_COV_PITCH_PITCH: usize = 28; +const POSE_COV_YAW_YAW: usize = 35; + #[derive(Debug, Clone, Copy, PartialEq)] pub enum StateIndex { X = 0, @@ -69,18 +108,50 @@ pub struct Pose { pub orientation: Quaternion, } +/// Equivalent to ROS `geometry_msgs/PoseStamped` (`ros2/common_interfaces`, the message +/// definitions repo -- not `ros2/geometry2`, which only consumes these types): +/// `EKFModule::get_current_pose`'s output, as opposed to `Pose` which is the +/// (header-less) value itself. +#[derive(Debug, Clone)] +pub struct PoseStamped { + pub header: common_types::Header, + pub pose: Pose, +} + #[derive(Debug, Clone, Copy)] pub struct Twist { pub linear: Vector3, pub angular: Vector3, } +/// Equivalent to ROS `geometry_msgs/TwistStamped` (`ros2/common_interfaces`): +/// `EKFModule::get_current_twist`'s output. +#[derive(Debug, Clone)] +pub struct TwistStamped { + pub header: common_types::Header, + pub twist: Twist, +} + #[derive(Debug, Clone, Copy)] pub struct PoseWithCovariance { pub pose: Pose, pub covariance: [f64; 36], } +/// Equivalent to ROS `geometry_msgs/PoseWithCovarianceStamped` (`ros2/common_interfaces`): +/// a timestamped pose measurement, as opposed to `PoseWithCovariance` which is the +/// (header-less) payload embedded in `EKFOdometry` output. +#[derive(Debug, Clone)] +pub struct PoseWithCovarianceStamped { + pub header: common_types::Header, + pub pose: PoseWithCovariance, +} + +/// Equivalent to ROS `nav_msgs/Odometry` (`ros2/common_interfaces`): the aggregated +/// output message combining pose, twist, and frame metadata, as opposed to `EKFModule`'s +/// individual getters +/// (`get_current_pose_with_covariance`, `get_current_twist_covariance`, etc.) which this +/// type is meant to be assembled from at the pub/sub layer. #[derive(Debug, Clone)] pub struct EKFOdometry { pub header: common_types::Header, @@ -99,6 +170,18 @@ pub struct EKFParameters { pub z_filter_proc_dev: f64, pub roll_filter_proc_dev: f64, pub pitch_filter_proc_dev: f64, + pub pose_frame_id: &'static str, + pub pose_additional_delay: f64, + pub pose_gate_dist: f64, + pub pose_smoothing_steps: usize, + pub twist_additional_delay: f64, + pub twist_gate_dist: f64, + pub twist_smoothing_steps: usize, + /// Below this |vx| [m/s], the vx observation is considered unreliable (wheel-speed + /// sensor quantization/slip near zero speed) and its variance should be inflated by + /// the caller via `apply_twist_observability_gate` before calling + /// `measurement_update_twist`. `0.0` disables the gate (upstream's own default). + pub threshold_observable_velocity_mps: f64, } impl Default for EKFParameters { @@ -106,12 +189,20 @@ impl Default for EKFParameters { Self { enable_yaw_bias_estimation: true, extend_state_step: 50, - proc_stddev_vx_c: 2.0, - proc_stddev_wz_c: 1.0, + proc_stddev_vx_c: 10.0, + proc_stddev_wz_c: 5.0, proc_stddev_yaw_c: 0.005, - z_filter_proc_dev: 1.0, + z_filter_proc_dev: 5.0, roll_filter_proc_dev: 0.1, pitch_filter_proc_dev: 0.1, + pose_frame_id: "map", + pose_additional_delay: 0.0, + pose_gate_dist: 49.5, + pose_smoothing_steps: 5, + twist_additional_delay: 0.0, + twist_gate_dist: 46.1, + twist_smoothing_steps: 2, + threshold_observable_velocity_mps: 0.0, } } } @@ -147,10 +238,10 @@ impl Simple1DFilter { } let proc_var_x_d = self.proc_var_x_c * dt * dt; - self.var = self.var + proc_var_x_d; + self.var += proc_var_x_d; let kalman_gain = self.var / (self.var + obs_var); - self.x = self.x + kalman_gain * (obs - self.x); + self.x += kalman_gain * (obs - self.x); self.var = (1.0 - kalman_gain) * self.var; } @@ -167,30 +258,82 @@ impl Simple1DFilter { } } +impl Default for Simple1DFilter { + fn default() -> Self { + Self::new() + } +} + +fn to_dvector(v: &StateVector) -> DVector { + let mut out = DVector::zeros(6); + for i in 0..6 { + out[i] = v[i]; + } + out +} + +fn to_state_vector(v: &DVector) -> StateVector { + let mut out = StateVector::zeros(); + for i in 0..6 { + out[i] = v[i]; + } + out +} + +fn to_dmatrix(m: &StateCovariance) -> DMatrix { + let mut out = DMatrix::zeros(6, 6); + for i in 0..6 { + for j in 0..6 { + out[(i, j)] = m[(i, j)]; + } + } + out +} + #[derive(Debug, Clone)] pub struct EKFModule { params: EKFParameters, - state: StateVector, - covariance: StateCovariance, + kf: DelayCompensatedKalmanFilter, z_filter: Simple1DFilter, roll_filter: Simple1DFilter, pitch_filter: Simple1DFilter, accumulated_delay_times: Vec, - // When true, only prediction is performed (no measurement updates) - is_mrm_mode: bool, + /// Angular velocity from the most recent successful twist update, used by + /// `compensate_rph_with_delay` to extrapolate roll/pitch/z across the pose's delay. + last_angular_velocity: Vector3, + ekf_dt: f64, + // RT NOTE: the measurement matrices C are constant (only the state layout picks which + // components are observed), so they are built once here instead of every + // measurement_update_pose/twist call. + pose_measurement_matrix: DMatrix, + twist_measurement_matrix: DMatrix, + // Mirrors upstream's per-call-site `RCLCPP_WARN_THROTTLE` durations (see + // `warning_message.cpp`): one independent throttle per warning site so a stuck sensor + // failing one gate every tick doesn't also suppress warnings from a different gate. + pose_frame_id_warn: WarnThrottle, + pose_delay_time_warn: WarnThrottle, + pose_delay_step_warn: WarnThrottle, + pose_mahalanobis_warn: WarnThrottle, + twist_frame_id_warn: WarnThrottle, + twist_delay_time_warn: WarnThrottle, + twist_delay_step_warn: WarnThrottle, + twist_mahalanobis_warn: WarnThrottle, } impl EKFModule { pub fn new(params: EKFParameters) -> Self { - let state = StateVector::zeros(); - let mut covariance = StateCovariance::identity() * 1e15; + let x0 = StateVector::zeros(); + let mut p0 = StateCovariance::identity() * 1e15; - covariance[(StateIndex::Yaw as usize, StateIndex::Yaw as usize)] = 50.0; + p0[(StateIndex::Yaw as usize, StateIndex::Yaw as usize)] = 50.0; if params.enable_yaw_bias_estimation { - covariance[(StateIndex::YawBias as usize, StateIndex::YawBias as usize)] = 50.0; + p0[(StateIndex::YawBias as usize, StateIndex::YawBias as usize)] = 50.0; } - covariance[(StateIndex::Vx as usize, StateIndex::Vx as usize)] = 1000.0; - covariance[(StateIndex::Wz as usize, StateIndex::Wz as usize)] = 50.0; + p0[(StateIndex::Vx as usize, StateIndex::Vx as usize)] = 1000.0; + p0[(StateIndex::Wz as usize, StateIndex::Wz as usize)] = 50.0; + + let mut kf = DelayCompensatedKalmanFilter::new(); + kf.init(&to_dvector(&x0), &to_dmatrix(&p0), params.extend_state_step); let mut z_filter = Simple1DFilter::new(); let mut roll_filter = Simple1DFilter::new(); @@ -204,115 +347,109 @@ impl EKFModule { Self { params, - state, - covariance, + kf, z_filter, roll_filter, pitch_filter, accumulated_delay_times, - is_mrm_mode: false, + last_angular_velocity: Vector3::zeros(), + ekf_dt: 0.0, + pose_measurement_matrix: pose_measurement_matrix(), + twist_measurement_matrix: twist_measurement_matrix(), + // Durations match upstream's literal `warn_throttle(..., N)` call sites. + pose_frame_id_warn: WarnThrottle::new(2000), + pose_delay_time_warn: WarnThrottle::new(1000), + pose_delay_step_warn: WarnThrottle::new(2000), + pose_mahalanobis_warn: WarnThrottle::new(2000), + twist_frame_id_warn: WarnThrottle::new(2000), + twist_delay_time_warn: WarnThrottle::new(1000), + twist_delay_step_warn: WarnThrottle::new(2000), + twist_mahalanobis_warn: WarnThrottle::new(2000), } } - pub fn initialize(&mut self, initial_pose: Pose) { - self.state[StateIndex::X as usize] = initial_pose.position.x; - self.state[StateIndex::Y as usize] = initial_pose.position.y; - self.state[StateIndex::Yaw as usize] = self.quaternion_to_yaw(initial_pose.orientation); - self.state[StateIndex::YawBias as usize] = 0.0; - self.state[StateIndex::Vx as usize] = 0.0; - self.state[StateIndex::Wz as usize] = 0.0; - - self.covariance = StateCovariance::identity() * 0.01; + /// TF-aware initialization: `transform` is the already-resolved + /// `initial_pose.header.frame_id -> base_link` (or map -> odom, depending on wiring) + /// transform, applied the same way upstream's `initialize()` does by adding it to the + /// pose before seeding the filter. Looking up the transform itself is out of scope here + /// (see `imu_corrector::TransformListener` for that half of the existing port pattern). + pub fn initialize(&mut self, initial_pose: &PoseWithCovarianceStamped, transform: &Transform) { + let mut x0 = StateVector::zeros(); + x0[StateIndex::X as usize] = initial_pose.pose.pose.position.x + transform.translation.x; + x0[StateIndex::Y as usize] = initial_pose.pose.pose.position.y + transform.translation.y; + + let transform_yaw = self.quaternion_to_yaw(Quaternion { + x: transform.rotation.x, + y: transform.rotation.y, + z: transform.rotation.z, + w: transform.rotation.w, + }); + x0[StateIndex::Yaw as usize] = + self.quaternion_to_yaw(initial_pose.pose.pose.orientation) + transform_yaw; + x0[StateIndex::YawBias as usize] = 0.0; + x0[StateIndex::Vx as usize] = 0.0; + x0[StateIndex::Wz as usize] = 0.0; + + let mut p0 = StateCovariance::zeros(); + p0[(StateIndex::X as usize, StateIndex::X as usize)] = + initial_pose.pose.covariance[POSE_COV_X_X]; + p0[(StateIndex::Y as usize, StateIndex::Y as usize)] = + initial_pose.pose.covariance[POSE_COV_Y_Y]; + p0[(StateIndex::Yaw as usize, StateIndex::Yaw as usize)] = + initial_pose.pose.covariance[POSE_COV_YAW_YAW]; if self.params.enable_yaw_bias_estimation { - self.covariance[(StateIndex::YawBias as usize, StateIndex::YawBias as usize)] = 0.0001; + p0[(StateIndex::YawBias as usize, StateIndex::YawBias as usize)] = 0.0001; } + p0[(StateIndex::Vx as usize, StateIndex::Vx as usize)] = 0.01; + p0[(StateIndex::Wz as usize, StateIndex::Wz as usize)] = 0.01; - self.z_filter.init(initial_pose.position.z, 0.01); - self.roll_filter.init(0.0, 0.01); - self.pitch_filter.init(0.0, 0.01); - } - - fn predict_next_state(&self, dt: f64) -> StateVector { - let mut x_next = self.state.clone(); - let x = self.state[StateIndex::X as usize]; - let y = self.state[StateIndex::Y as usize]; - let yaw = self.state[StateIndex::Yaw as usize]; - let yaw_bias = self.state[StateIndex::YawBias as usize]; - let vx = self.state[StateIndex::Vx as usize]; - let wz = self.state[StateIndex::Wz as usize]; - - x_next[StateIndex::X as usize] = x + vx * cos(yaw + yaw_bias) * dt; - x_next[StateIndex::Y as usize] = y + vx * sin(yaw + yaw_bias) * dt; - let yaw_next = yaw + wz * dt; - x_next[StateIndex::Yaw as usize] = atan2(sin(yaw_next), cos(yaw_next)); - x_next[StateIndex::YawBias as usize] = yaw_bias; - x_next[StateIndex::Vx as usize] = vx; - x_next[StateIndex::Wz as usize] = wz; - - x_next - } - - fn create_state_transition_matrix(&self, dt: f64) -> Matrix6 { - let mut F = Matrix6::identity(); - let yaw = self.state[StateIndex::Yaw as usize]; - let yaw_bias = self.state[StateIndex::YawBias as usize]; - let vx = self.state[StateIndex::Vx as usize]; - - F[(StateIndex::X as usize, StateIndex::Yaw as usize)] = -vx * sin(yaw + yaw_bias) * dt; - F[(StateIndex::X as usize, StateIndex::YawBias as usize)] = -vx * sin(yaw + yaw_bias) * dt; - F[(StateIndex::X as usize, StateIndex::Vx as usize)] = cos(yaw + yaw_bias) * dt; + self.kf + .init(&to_dvector(&x0), &to_dmatrix(&p0), self.params.extend_state_step); - F[(StateIndex::Y as usize, StateIndex::Yaw as usize)] = vx * cos(yaw + yaw_bias) * dt; - F[(StateIndex::Y as usize, StateIndex::YawBias as usize)] = vx * cos(yaw + yaw_bias) * dt; - F[(StateIndex::Y as usize, StateIndex::Vx as usize)] = sin(yaw + yaw_bias) * dt; + let z = initial_pose.pose.pose.position.z; + let (roll, pitch, _yaw) = self.quaternion_to_rpy(initial_pose.pose.pose.orientation); - F[(StateIndex::Yaw as usize, StateIndex::Wz as usize)] = dt; + let z_var = initial_pose.pose.covariance[POSE_COV_Z_Z]; + let roll_var = initial_pose.pose.covariance[POSE_COV_ROLL_ROLL]; + let pitch_var = initial_pose.pose.covariance[POSE_COV_PITCH_PITCH]; - F + self.z_filter.init(z, z_var); + self.roll_filter.init(roll, roll_var); + self.pitch_filter.init(pitch, pitch_var); } - fn process_noise_covariance(&self, dt: f64) -> Matrix6 { - let mut Q = Matrix6::zeros(); - - Q[(StateIndex::Vx as usize, StateIndex::Vx as usize)] = - self.params.proc_stddev_vx_c * self.params.proc_stddev_vx_c * dt * dt; - Q[(StateIndex::Wz as usize, StateIndex::Wz as usize)] = - self.params.proc_stddev_wz_c * self.params.proc_stddev_wz_c * dt * dt; - Q[(StateIndex::Yaw as usize, StateIndex::Yaw as usize)] = - self.params.proc_stddev_yaw_c * self.params.proc_stddev_yaw_c * dt * dt; + /// Matches upstream `EKFModule::predict_with_delay` exactly: it does *not* touch the + /// delay-time buffer. Upstream's node (`EKFLocalizer::timer_callback`) calls + /// `accumulate_delay_time(dt)` and `predict_with_delay(dt)` as two separate sibling + /// calls (see `update_predict_frequency` vs. the prediction block in `timer_callback`); + /// callers of this crate must do the same, with the same `dt`, once per predict tick. + pub fn predict_with_delay(&mut self, dt: f64) { + let x_curr = to_state_vector(&self.kf.latest_x()); - Q[(StateIndex::X as usize, StateIndex::X as usize)] = 0.0; - Q[(StateIndex::Y as usize, StateIndex::Y as usize)] = 0.0; - Q[(StateIndex::YawBias as usize, StateIndex::YawBias as usize)] = 0.0; + let vx_term = self.params.proc_stddev_vx_c * dt; + let wz_term = self.params.proc_stddev_wz_c * dt; + let yaw_term = self.params.proc_stddev_yaw_c * dt; - Q - } + let x_next = predict_next_state(&x_curr, dt); + let a = create_state_transition_matrix(&x_curr, dt); + let q = process_noise_covariance(yaw_term * yaw_term, vx_term * vx_term, wz_term * wz_term); - pub fn predict(&mut self, dt: f64) { - self.state = self.predict_next_state(dt); - let F = self.create_state_transition_matrix(dt); - let Q = self.process_noise_covariance(dt); - self.covariance = F * self.covariance * F.transpose() + Q; - self.accumulate_delay_time(dt); - } + self.kf + .predict_with_delay(&to_dvector(&x_next), &to_dmatrix(&a), &to_dmatrix(&q)); - pub fn predict_with_delay(&mut self, dt: f64) { - self.predict(dt); + self.ekf_dt = dt; } - pub fn predict_only(&mut self, dt: f64) { - self.predict(dt); - } - - pub fn get_current_pose(&self, get_biased_yaw: bool) -> Pose { + pub fn get_current_pose(&self, get_biased_yaw: bool, current_time: u64) -> PoseStamped { let z = self.z_filter.get_x(); let roll = self.roll_filter.get_x(); let pitch = self.pitch_filter.get_x(); - let x = self.state[StateIndex::X as usize]; - let y = self.state[StateIndex::Y as usize]; - let biased_yaw = self.state[StateIndex::Yaw as usize]; - let yaw_bias = self.state[StateIndex::YawBias as usize]; + let x_vec = self.kf.latest_x(); + let x = x_vec[StateIndex::X as usize]; + let y = x_vec[StateIndex::Y as usize]; + let biased_yaw = x_vec[StateIndex::Yaw as usize]; + let yaw_bias = x_vec[StateIndex::YawBias as usize]; let yaw = if get_biased_yaw { biased_yaw @@ -320,28 +457,44 @@ impl EKFModule { biased_yaw + yaw_bias }; - Pose { - position: Point3D { x, y, z }, - orientation: self.rpy_to_quaternion(roll, pitch, yaw), + PoseStamped { + header: common_types::Header { + frame_id: self.params.pose_frame_id, + timestamp: current_time, + }, + pose: Pose { + position: Point3D { x, y, z }, + orientation: self.rpy_to_quaternion(roll, pitch, yaw), + }, } } - pub fn get_current_twist(&self) -> Twist { - let vx = self.state[StateIndex::Vx as usize]; - let wz = self.state[StateIndex::Wz as usize]; - - Twist { - linear: Vector3::new(vx, 0.0, 0.0), - angular: Vector3::new(0.0, 0.0, wz), + pub fn get_current_twist(&self, current_time: u64) -> TwistStamped { + let x_vec = self.kf.latest_x(); + let vx = x_vec[StateIndex::Vx as usize]; + let wz = x_vec[StateIndex::Wz as usize]; + + TwistStamped { + header: common_types::Header { + frame_id: "base_link", + timestamp: current_time, + }, + twist: Twist { + linear: Vector3::new(vx, 0.0, 0.0), + angular: Vector3::new(0.0, 0.0, wz), + }, } } pub fn get_yaw_bias(&self) -> f64 { - self.state[StateIndex::YawBias as usize] + self.kf.latest_x()[StateIndex::YawBias as usize] } - pub fn get_current_pose_with_covariance(&self) -> PoseWithCovariance { - let pose = self.get_current_pose(false); + // Not a port of any upstream `EKFModule` method (see `ekf_module.hpp`'s public API); a + // convenience wrapper bundling `get_current_pose`/`get_current_pose_covariance` for + // `EKFOdometry` assembly. + pub fn get_current_pose_with_covariance(&self, current_time: u64) -> PoseWithCovariance { + let pose = self.get_current_pose(false, current_time).pose; let pose_covariance = self.get_current_pose_covariance(); PoseWithCovariance { pose, @@ -350,74 +503,261 @@ impl EKFModule { } pub fn get_current_pose_covariance(&self) -> [f64; 36] { - let mut cov = [0.0; 36]; + let mut cov = ekf_covariance_to_pose_message_covariance(&self.kf.latest_p()); + cov[POSE_COV_Z_Z] = self.z_filter.get_var(); + cov[POSE_COV_ROLL_ROLL] = self.roll_filter.get_var(); + cov[POSE_COV_PITCH_PITCH] = self.pitch_filter.get_var(); + cov + } - for i in 0..6 { - for j in 0..6 { - cov[i * 6 + j] = self.covariance[(i, j)]; + pub fn get_current_twist_covariance(&self) -> [f64; 36] { + ekf_covariance_to_twist_message_covariance(&self.kf.latest_p()) + } + + /// Fuses an external pose measurement (e.g. NDT/GNSS localization) at delay step + /// `find_closest_delay_time_index(t_curr - pose.header.timestamp)`. Returns `false` + /// (state left untouched) if the delay exceeds `extend_state_step`, the measurement + /// fails the Mahalanobis gate, or it contains NaN/Inf. + /// + /// There is deliberately no "MRM mode" gate here (or on `measurement_update_twist`). + /// Switching into dead-reckoning is done entirely by whether `pose_with_covariance` is + /// being published at all -- upstream's node only calls this when its pose queue is + /// non-empty, so during MRM the queue is simply always empty and this is never + /// invoked. `measurement_update_twist` keeps running throughout (Dead Reckoning + /// explicitly still uses twist). That decision belongs entirely to the pub/sub wiring + /// layer, not to `EKFModule`. + pub fn measurement_update_pose(&mut self, pose: &PoseWithCovarianceStamped, t_curr: u64) -> bool { + if pose.header.frame_id != self.params.pose_frame_id && self.pose_frame_id_warn.should_emit(t_curr) { + log::warn!( + "pose frame_id is {}, but pose_frame is set as {}. They must be same.", + pose.header.frame_id, + self.params.pose_frame_id + ); + } + + let mut delay_time = + nanos_to_seconds_delta(t_curr, pose.header.timestamp) + self.params.pose_additional_delay; + if delay_time < 0.0 && self.pose_delay_time_warn.should_emit(t_curr) { + log::warn!("[EKF] pose delay time is negative: {delay_time}. Treated as 0."); + } + delay_time = delay_time.max(0.0); + + let delay_step = self.find_closest_delay_time_index(delay_time); + if delay_step >= self.params.extend_state_step { + if self.pose_delay_step_warn.should_emit(t_curr) { + log::warn!( + "[EKF] pose delay step {delay_step} exceeds extend_state_step {}. Ignoring measurement.", + self.params.extend_state_step + ); } + return false; } - cov[14] = self.z_filter.get_var(); - cov[21] = self.roll_filter.get_var(); - cov[28] = self.pitch_filter.get_var(); + let ekf_yaw = self.kf.x_element(delay_step, StateIndex::Yaw as usize); + let raw_yaw = self.quaternion_to_yaw(pose.pose.pose.orientation); + let yaw_error = normalize_yaw(raw_yaw - ekf_yaw); + let yaw = yaw_error + ekf_yaw; + + let y = DVector::from_vec(vec![ + pose.pose.pose.position.x, + pose.pose.pose.position.y, + yaw, + ]); + if has_nan(&y) || has_inf(&y) { + log::warn!("[EKF] pose measurement includes NaN or Inf. ignore update."); + return false; + } - cov - } + let y_ekf = DVector::from_vec(vec![ + self.kf.x_element(delay_step, StateIndex::X as usize), + self.kf.x_element(delay_step, StateIndex::Y as usize), + ekf_yaw, + ]); + let p_curr = self.kf.latest_p(); + let p_y = p_curr.view((0, 0), (3, 3)).clone_owned(); + + let distance = mahalanobis(&y_ekf, &y, &p_y); + if distance > self.params.pose_gate_dist { + if self.pose_mahalanobis_warn.should_emit(t_curr) { + log::warn!( + "[EKF] pose Mahalanobis distance {distance} exceeds gate {}. Ignore the measurement data.", + self.params.pose_gate_dist + ); + } + return false; + } - pub fn get_current_twist_covariance(&self) -> [f64; 36] { - let mut cov = [0.0; 36]; + let r = pose_measurement_covariance(&pose.pose.covariance, self.params.pose_smoothing_steps); - cov[0] = self.covariance[(StateIndex::Vx as usize, StateIndex::Vx as usize)]; - cov[35] = self.covariance[(StateIndex::Wz as usize, StateIndex::Wz as usize)]; + if !self + .kf + .update_with_delay(&y, &self.pose_measurement_matrix, &r, delay_step) + { + return false; + } - cov + let pose_with_delay = self.compensate_rph_with_delay(pose, delay_time); + self.update_simple_1d_filters(&pose_with_delay, self.params.pose_smoothing_steps); + + true } - pub fn update_velocity(&mut self, vx_measurement: f64, wz_measurement: f64) { - if self.is_mrm_mode { - return; - } + /// Extrapolates `pose`'s orientation/z forward by `delay_time` using + /// `last_angular_velocity` (from the most recent twist update), so that the + /// Simple1DFilters for z/roll/pitch are updated against a value consistent with + /// "now" rather than the pose's original (delayed) timestamp. + fn compensate_rph_with_delay( + &self, + pose: &PoseWithCovarianceStamped, + delay_time: f64, + ) -> PoseWithCovarianceStamped { + let av = self.last_angular_velocity; + let av_len = av.norm(); + + let delta_orientation = if av_len > 0.0 { + let axis = Unit::new_normalize(av); + UnitQuaternion::from_axis_angle(&axis, av_len * delay_time) + } else { + UnitQuaternion::identity() + }; - let vx_obs_var = 1.0; - let wz_obs_var = 0.1; + let prev_orientation = UnitQuaternion::new_normalize(NQuaternion::new( + pose.pose.pose.orientation.w, + pose.pose.pose.orientation.x, + pose.pose.pose.orientation.y, + pose.pose.pose.orientation.z, + )); + let curr_orientation = (prev_orientation * delta_orientation).into_inner(); + + let mut pose_with_delay = pose.clone(); + pose_with_delay.header.timestamp = pose + .header + .timestamp + .saturating_add((delay_time.max(0.0) * 1_000_000_000.0) as u64); + pose_with_delay.pose.pose.orientation = Quaternion { + x: curr_orientation.coords.x, + y: curr_orientation.coords.y, + z: curr_orientation.coords.z, + w: curr_orientation.coords.w, + }; - let vx_var = self.covariance[(StateIndex::Vx as usize, StateIndex::Vx as usize)]; - let vx_gain = vx_var / (vx_var + vx_obs_var); - self.state[StateIndex::Vx as usize] = self.state[StateIndex::Vx as usize] - + vx_gain * (vx_measurement - self.state[StateIndex::Vx as usize]); - self.covariance[(StateIndex::Vx as usize, StateIndex::Vx as usize)] = - (1.0 - vx_gain) * vx_var; + let (_roll, pitch, _yaw) = self.quaternion_to_rpy(pose_with_delay.pose.pose.orientation); + let vx = self.kf.x_element(0, StateIndex::Vx as usize); + pose_with_delay.pose.pose.position.z += vx * delay_time * sin(-pitch); - let wz_var = self.covariance[(StateIndex::Wz as usize, StateIndex::Wz as usize)]; - let wz_gain = wz_var / (wz_var + wz_obs_var); - self.state[StateIndex::Wz as usize] = self.state[StateIndex::Wz as usize] - + wz_gain * (wz_measurement - self.state[StateIndex::Wz as usize]); - self.covariance[(StateIndex::Wz as usize, StateIndex::Wz as usize)] = - (1.0 - wz_gain) * wz_var; + pose_with_delay } - pub fn set_mrm_mode(&mut self, is_mrm: bool) { - self.is_mrm_mode = is_mrm; + fn update_simple_1d_filters(&mut self, pose: &PoseWithCovarianceStamped, smoothing_step: usize) { + let z = pose.pose.pose.position.z; + let (roll, pitch, _yaw) = self.quaternion_to_rpy(pose.pose.pose.orientation); + + let smoothing_step = smoothing_step as f64; + let z_var = pose.pose.covariance[POSE_COV_Z_Z] * smoothing_step; + let roll_var = pose.pose.covariance[POSE_COV_ROLL_ROLL] * smoothing_step; + let pitch_var = pose.pose.covariance[POSE_COV_PITCH_PITCH] * smoothing_step; + + self.z_filter.update(z, z_var, self.ekf_dt); + self.roll_filter.update(roll, roll_var, self.ekf_dt); + self.pitch_filter.update(pitch, pitch_var, self.ekf_dt); } - pub fn is_mrm(&self) -> bool { - self.is_mrm_mode + /// Fuses a (vx, wz) twist measurement using the message's own covariance and a + /// Mahalanobis gate, matching upstream. Callers that want the "don't trust vx at low + /// speed" behaviour must call `apply_twist_observability_gate` on `twist` first. This + /// keeps running during MRM Dead Reckoning -- see the note on `measurement_update_pose`. + pub fn measurement_update_twist(&mut self, twist: &TwistWithCovarianceStamped, t_curr: u64) -> bool { + if twist.header.frame_id != "base_link" && self.twist_frame_id_warn.should_emit(t_curr) { + log::warn!("twist frame_id must be base_link, got {}", twist.header.frame_id); + } + + self.last_angular_velocity = Vector3::zeros(); + + let mut delay_time = nanos_to_seconds_delta(t_curr, twist.header.timestamp) + + self.params.twist_additional_delay; + if delay_time < 0.0 && self.twist_delay_time_warn.should_emit(t_curr) { + log::warn!("[EKF] twist delay time is negative: {delay_time}. Treated as 0."); + } + delay_time = delay_time.max(0.0); + + let delay_step = self.find_closest_delay_time_index(delay_time); + if delay_step >= self.params.extend_state_step { + if self.twist_delay_step_warn.should_emit(t_curr) { + log::warn!( + "[EKF] twist delay step {delay_step} exceeds extend_state_step {}. Ignoring measurement.", + self.params.extend_state_step + ); + } + return false; + } + + let y = DVector::from_vec(vec![ + twist.twist.twist.linear.x, + twist.twist.twist.angular.z, + ]); + if has_nan(&y) || has_inf(&y) { + log::warn!("[EKF] twist measurement includes NaN or Inf. ignore update."); + return false; + } + + let y_ekf = DVector::from_vec(vec![ + self.kf.x_element(delay_step, StateIndex::Vx as usize), + self.kf.x_element(delay_step, StateIndex::Wz as usize), + ]); + let p_curr = self.kf.latest_p(); + let p_y = p_curr.view((4, 4), (2, 2)).clone_owned(); + + let distance = mahalanobis(&y_ekf, &y, &p_y); + if distance > self.params.twist_gate_dist { + if self.twist_mahalanobis_warn.should_emit(t_curr) { + log::warn!( + "[EKF] twist Mahalanobis distance {distance} exceeds gate {}. Ignore the measurement data.", + self.params.twist_gate_dist + ); + } + return false; + } + + let r = twist_measurement_covariance(&twist.twist.covariance, self.params.twist_smoothing_steps); + + if !self + .kf + .update_with_delay(&y, &self.twist_measurement_matrix, &r, delay_step) + { + return false; + } + + self.last_angular_velocity = Vector3::new( + twist.twist.twist.angular.x, + twist.twist.twist.angular.y, + twist.twist.twist.angular.z, + ); + + true } + /// Ages the delay-time buffer by one predict-tick: the just-predicted state becomes + /// index 0 (age 0), and every older entry (which just got pushed back one slot in the + /// kalman filter's extended state, see `kalman_filter::predict_with_delay`) gets `dt` + /// added to its age. Must shift toward *higher* indices to stay aligned with how the + /// extended state itself shifts. + /// + /// Matches upstream `EKFModule::accumulate_delay_time`: it is *not* called by + /// `predict_with_delay`. Callers must call this once per predict tick, with the same + /// `dt`, alongside `predict_with_delay(dt)` (see that method's doc comment). pub fn accumulate_delay_time(&mut self, dt: f64) { let len = self.accumulated_delay_times.len(); if len == 0 { return; } - let last_time = self.accumulated_delay_times[len - 1]; - let new_time = last_time + dt; - - for i in 0..len - 1 { - self.accumulated_delay_times[i] = self.accumulated_delay_times[i + 1]; + for i in (1..len).rev() { + self.accumulated_delay_times[i] = self.accumulated_delay_times[i - 1]; + } + self.accumulated_delay_times[0] = 0.0; + for time in self.accumulated_delay_times.iter_mut().skip(1) { + *time += dt; } - self.accumulated_delay_times[len - 1] = new_time; } pub fn find_closest_delay_time_index(&self, target_value: f64) -> usize { @@ -430,26 +770,92 @@ impl EKFModule { return len; } - let mut closest_index = 0; - let mut min_diff = f64::MAX; + // Matches `std::lower_bound`'s semantics via an actual binary search (not a linear + // scan): `partition_point` returns the index of the first entry >= target_value, + // relying on `accumulated_delay_times` being kept sorted ascending by + // `accumulate_delay_time`. + let lower = self + .accumulated_delay_times + .partition_point(|&time| time < target_value); - for i in 0..len { - let time = self.accumulated_delay_times[i]; - let diff = (target_value - time).abs(); - if diff < min_diff { - min_diff = diff; - closest_index = i; - } + if lower == 0 { + return 0; + } + if lower == len { + return len - 1; } - closest_index + let prev = lower - 1; + let diff_prev = target_value - self.accumulated_delay_times[prev]; + let diff_lower = self.accumulated_delay_times[lower] - target_value; + if diff_prev < diff_lower { + prev + } else { + lower + } } + /// Ported from `tf2::getYaw` (`ros2/geometry2`, `tf2/include/tf2/impl/utils.hpp`, + /// `humble` branch) rather than `autoware_core` -- `tf2::getYaw` is a ROS 2 core + /// library function, not an Autoware one. Unlike a plain + /// `atan2(2*(w*z+x*y), 1-2*(y²+z²))`, tf2's version falls back to a different formula + /// near the pitch = +/-90 deg gimbal-lock singularity (where yaw and roll become + /// coupled and the "normal" formula loses precision), and normalizes by + /// `sqx+sqy+sqz+sqw` instead of assuming an already-unit quaternion. fn quaternion_to_yaw(&self, q: Quaternion) -> f64 { - atan2( - 2.0 * (q.w * q.z + q.x * q.y), - 1.0 - 2.0 * (q.y * q.y + q.z * q.z), - ) + let sqx = q.x * q.x; + let sqy = q.y * q.y; + let sqz = q.z * q.z; + let sqw = q.w * q.w; + + let sarg = -2.0 * (q.x * q.z - q.w * q.y) / (sqx + sqy + sqz + sqw); + + if sarg <= -0.99999 { + -2.0 * atan2(q.y, q.x) + } else if sarg >= 0.99999 { + 2.0 * atan2(q.y, q.x) + } else { + atan2(2.0 * (q.x * q.y + q.w * q.z), sqw + sqx - sqy - sqz) + } + } + + /// Returns `(roll, pitch, yaw)` in radians (ZYX Euler convention). `autoware_utils_geometry` + /// lives in the separate `autowarefoundation/autoware_utils` repo, not `autoware_core`; + /// its `get_rpy` delegates entirely to `tf2::Matrix3x3(q).getRPY(...)` + /// (`ros2/geometry2`, `tf2/include/tf2/LinearMath/Matrix3x3.hpp`, `humble` branch, + /// `getEulerYPR`). That function's "normal case" formula is algebraically identical to + /// the one below, but it also has an explicit gimbal-lock branch (pitch == +/-90 deg, + /// where roll and yaw become coupled and only their sum/difference is defined) that a + /// plain `asin` clamp does not reproduce -- ported here as the `m20.abs() >= 1.0` + /// branch instead of clamping the `asin` argument. + fn quaternion_to_rpy(&self, q: Quaternion) -> (f64, f64, f64) { + // == tf2::Matrix3x3::setRotation's m_el[2].x() == -sin(pitch). + let m20 = 2.0 * (q.x * q.z - q.w * q.y); + + if m20.abs() >= 1.0 { + let delta = atan2( + 2.0 * (q.y * q.z + q.w * q.x), + 1.0 - 2.0 * (q.x * q.x + q.y * q.y), + ); + let pitch = if m20 < 0.0 { + core::f64::consts::FRAC_PI_2 + } else { + -core::f64::consts::FRAC_PI_2 + }; + (delta, pitch, 0.0) + } else { + let sinr_cosp = 2.0 * (q.w * q.x + q.y * q.z); + let cosr_cosp = 1.0 - 2.0 * (q.x * q.x + q.y * q.y); + let roll = atan2(sinr_cosp, cosr_cosp); + + let pitch = libm::asin(-m20); + + let siny_cosp = 2.0 * (q.w * q.z + q.x * q.y); + let cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z); + let yaw = atan2(siny_cosp, cosy_cosp); + + (roll, pitch, yaw) + } } fn rpy_to_quaternion(&self, roll: f64, pitch: f64, yaw: f64) -> Quaternion { @@ -469,6 +875,38 @@ impl EKFModule { } } +// Subtracts in exact `u64` nanoseconds first, then converts to `f64`, instead of +// converting each side to `f64` before subtracting: realistic epoch nanosecond values +// (~1e18) already lose precision past `f64`'s 2^53 exact-integer range, but the +// difference itself (realistically well under a minute, i.e. ~1e9-1e10 ns) fits exactly. +// This also matches how `rclcpp::Time`/`Duration` work internally (nanoseconds kept as an +// integer until the final `.seconds()` conversion), which this crate has no equivalent of. +fn nanos_to_seconds_delta(t_curr: u64, t_prev: u64) -> f64 { + if t_curr >= t_prev { + (t_curr - t_prev) as f64 / 1_000_000_000.0 + } else { + -((t_prev - t_curr) as f64) / 1_000_000_000.0 + } +} + +/// Awkernel node-layer helper mirroring upstream `EKFLocalizer::callback_twist_with_covariance`: +/// below `threshold_observable_velocity_mps`, the vx observation is not trusted, so its +/// variance is inflated (the wz variance is untouched). Call this before +/// `EKFModule::measurement_update_twist`. A `threshold` of `0.0` disables the gate. +pub fn apply_twist_observability_gate( + twist: &mut TwistWithCovarianceStamped, + threshold_observable_velocity_mps: f64, +) { + if twist.twist.twist.linear.x.abs() < threshold_observable_velocity_mps { + twist.twist.covariance[0] = 10000.0; + } +} + +/// Stand-in for the node-layer ownership `EKFModule` doesn't have yet (upstream's +/// `EKFLocalizer` owns its `EKFModule` via `std::make_unique`, as a regular member). Once +/// DAG/pub-sub wiring gives this crate an equivalent task/node struct that owns an +/// `EKFModule` instance directly, callers should go through that instead of this lazily +/// initialized global singleton, and this function should be removed. pub fn get_or_initialize_default_module() -> &'static mut EKFModule { let existing = EKF_MODULE_INSTANCE.load(AtomicOrdering::Acquire); if !existing.is_null() { @@ -495,168 +933,292 @@ pub fn get_or_initialize_default_module() -> &'static mut EKFModule { #[cfg(test)] mod tests { use super::*; - use core::f64::consts::PI; - use nalgebra::{Matrix6, Vector6}; - #[test] - fn predict_next_state_matches_formula() { - let params = EKFParameters::default(); - let mut ekf = EKFModule::new(params); + fn identity_pose_stamped(timestamp: u64) -> PoseWithCovarianceStamped { + pose_stamped_at(0.0, 0.0, 0.0, 0.0, timestamp) + } - let x_curr = Vector6::new(2.0, 3.0, PI / 2.0, PI / 4.0, 10.0, 2.0 * PI / 3.0); + fn pose_stamped_at(x: f64, y: f64, z: f64, yaw: f64, timestamp: u64) -> PoseWithCovarianceStamped { + let mut covariance = [0.0; 36]; + covariance[POSE_COV_X_X] = 0.01; + covariance[POSE_COV_Y_Y] = 0.01; + covariance[POSE_COV_YAW_YAW] = 0.01; + covariance[POSE_COV_Z_Z] = 0.01; + covariance[POSE_COV_ROLL_ROLL] = 0.01; + covariance[POSE_COV_PITCH_PITCH] = 0.01; + + PoseWithCovarianceStamped { + header: common_types::Header { + frame_id: "map", + timestamp, + }, + pose: PoseWithCovariance { + pose: Pose { + position: Point3D { x, y, z }, + orientation: Quaternion { + x: 0.0, + y: 0.0, + z: sin(yaw * 0.5), + w: cos(yaw * 0.5), + }, + }, + covariance, + }, + } + } - ekf.state = x_curr.clone(); + fn twist_stamped_at(vx: f64, wz: f64, timestamp: u64) -> TwistWithCovarianceStamped { + vehicle_velocity_converter::reactor_helpers::create_empty_twist(timestamp).apply(|t| { + t.twist.twist.linear.x = vx; + t.twist.twist.angular.z = wz; + t.twist.covariance[0] = 0.04; + t.twist.covariance[35] = 0.01; + }) + } - let dt = 0.5; - let x_next = ekf.predict_next_state(dt); + trait Apply { + fn apply(self, f: impl FnOnce(&mut Self)) -> Self; + } + impl Apply for T { + fn apply(mut self, f: impl FnOnce(&mut Self)) -> Self { + f(&mut self); + self + } + } + + // [own test] no upstream equivalent. + #[test] + fn initialize_applies_tf_transform_and_message_covariance() { + let mut ekf = EKFModule::new(EKFParameters::default()); + let mut pose = identity_pose_stamped(0); + pose.pose.pose.position.x = 1.0; + pose.pose.pose.position.y = 2.0; + pose.pose.covariance[POSE_COV_X_X] = 0.02; + pose.pose.covariance[POSE_COV_Y_Y] = 0.03; + pose.pose.covariance[POSE_COV_YAW_YAW] = 0.04; + + // A 90 degree yaw transform: w=cos(45deg), z=sin(45deg). + let mut transform = Transform::identity(); + transform.translation.x = 10.0; + transform.translation.y = 20.0; + transform.rotation.z = core::f64::consts::FRAC_1_SQRT_2; + transform.rotation.w = core::f64::consts::FRAC_1_SQRT_2; + + ekf.initialize(&pose, &transform); + + let p = ekf.get_current_pose(true, 0); + assert!((p.pose.position.x - 11.0).abs() < 1e-9); + assert!((p.pose.position.y - 22.0).abs() < 1e-9); + + // The 90 degree yaw transform should show up directly in the returned orientation + // (pose itself has yaw=0, so biased_yaw = 0 + 90deg == the transform's own rotation). + assert!((p.pose.orientation.z - core::f64::consts::FRAC_1_SQRT_2).abs() < 1e-9); + assert!((p.pose.orientation.w - core::f64::consts::FRAC_1_SQRT_2).abs() < 1e-9); + assert!(p.pose.orientation.x.abs() < 1e-9); + assert!(p.pose.orientation.y.abs() < 1e-9); + + let cov = ekf.get_current_pose_covariance(); + assert_eq!(cov[POSE_COV_X_X], 0.02); + assert_eq!(cov[POSE_COV_Y_Y], 0.03); + assert_eq!(cov[POSE_COV_YAW_YAW], 0.04); + } - let tol = 1e-10; - assert!((x_next[0] - (2.0 + 10.0 * (PI / 2.0 + PI / 4.0).cos() * dt)).abs() < tol); - assert!((x_next[1] - (3.0 + 10.0 * (PI / 2.0 + PI / 4.0).sin() * dt)).abs() < tol); - let yaw_next = PI / 2.0 + (2.0 * PI / 3.0) * dt; - let expected_yaw = yaw_next.sin().atan2(yaw_next.cos()); - assert!((x_next[2] - expected_yaw).abs() < 1e-6); - assert!((x_next[3] - x_curr[3]).abs() < tol); - assert!((x_next[4] - x_curr[4]).abs() < tol); - assert!((x_next[5] - x_curr[5]).abs() < tol); + // [own test] no upstream equivalent. + #[test] + fn measurement_update_pose_moves_state_toward_measurement() { + let mut ekf = EKFModule::new(EKFParameters::default()); + ekf.initialize(&identity_pose_stamped(0), &Transform::identity()); + + let pose = pose_stamped_at(1.0, 0.0, 0.0, 0.0, 0); + let accepted = ekf.measurement_update_pose(&pose, 0); + assert!(accepted); + assert!(ekf.get_current_pose(false, 0).pose.position.x > 0.0); } + // [own test] no upstream equivalent. #[test] - fn create_state_transition_matrix_numeric_approximation() { - let params = EKFParameters::default(); + fn measurement_update_pose_rejects_excessive_delay() { + let mut params = EKFParameters::default(); + params.extend_state_step = 3; let mut ekf = EKFModule::new(params); + ekf.initialize(&identity_pose_stamped(0), &Transform::identity()); - // check around zero - let dt = 0.1; - let dx = Vector6::from_element(0.1); - let x = Vector6::zeros(); + for _ in 0..3 { + // Mirrors upstream: the node calls both once per tick with the same dt. + ekf.accumulate_delay_time(0.1); + ekf.predict_with_delay(0.1); + } - ekf.state = x.clone(); - let a = ekf.create_state_transition_matrix(dt); + let before = ekf.get_current_pose(false, 0).pose.position.x; + let pose = pose_stamped_at(1.0, 0.0, 0.0, 0.0, 0); + // 10 seconds of delay is far beyond the ~0.3s of history the buffer holds. + let accepted = ekf.measurement_update_pose(&pose, 10_000_000_000); + assert!(!accepted); + assert_eq!(ekf.get_current_pose(false, 0).pose.position.x, before); + } - ekf.state = x.clone() + dx.clone(); - let x1 = ekf.predict_next_state(dt); - ekf.state = x.clone(); - let x0 = ekf.predict_next_state(dt); - let df = x1 - x0; + // [own test] no upstream equivalent. + #[test] + fn measurement_update_pose_rejects_mahalanobis_outlier() { + let mut ekf = EKFModule::new(EKFParameters::default()); + ekf.initialize(&identity_pose_stamped(0), &Transform::identity()); + + let before = ekf.get_current_pose(false, 0).pose.position.x; + let pose = pose_stamped_at(1000.0, 0.0, 0.0, 0.0, 0); + let accepted = ekf.measurement_update_pose(&pose, 0); + assert!(!accepted); + assert_eq!(ekf.get_current_pose(false, 0).pose.position.x, before); + } - { - let mut s = 0.0; - let v = df - a * dx; - for i in 0..6 { - let val = v[i]; - s += val * val; - } - assert!(s.sqrt() < 2e-3); - } + // [own test] no upstream equivalent. + #[test] + fn measurement_update_pose_rejects_nan() { + let mut ekf = EKFModule::new(EKFParameters::default()); + ekf.initialize(&identity_pose_stamped(0), &Transform::identity()); - // check around a non-zero state - let dx = Vector6::from_element(0.1); - let x = Vector6::new(0.1, 0.2, 0.1, 0.4, 0.1, 0.3); + let mut pose = pose_stamped_at(1.0, 0.0, 0.0, 0.0, 0); + pose.pose.pose.position.x = f64::NAN; + assert!(!ekf.measurement_update_pose(&pose, 0)); + } - ekf.state = x.clone(); - let a = ekf.create_state_transition_matrix(dt); + // [own test] no upstream equivalent. Also serves as a regression guard for the + // MRM/Dead Reckoning design: `EKFModule` has no "MRM mode" of its own (see the note + // on `measurement_update_pose`), so this must keep working via + // `measurement_update_twist` alone even though no pose update has ever been applied + // in this test. + #[test] + fn measurement_update_twist_moves_state_toward_measurement() { + let mut ekf = EKFModule::new(EKFParameters::default()); + ekf.initialize(&identity_pose_stamped(0), &Transform::identity()); - ekf.state = x.clone() + dx.clone(); - let x1 = ekf.predict_next_state(dt); - ekf.state = x.clone(); - let x0 = ekf.predict_next_state(dt); - let df = x1 - x0; + let twist = twist_stamped_at(2.0, 0.0, 0); + assert!(ekf.measurement_update_twist(&twist, 0)); + assert!(ekf.get_current_twist(0).twist.linear.x > 0.0); + } - { - let mut s = 0.0; - let v = df - a * dx; - for i in 0..6 { - let val = v[i]; - s += val * val; - } - assert!(s.sqrt() < 5e-3); - } + // [own test] no upstream equivalent. + #[test] + fn measurement_update_twist_rejects_mahalanobis_outlier() { + let mut ekf = EKFModule::new(EKFParameters::default()); + ekf.initialize(&identity_pose_stamped(0), &Transform::identity()); + + let before = ekf.get_current_twist(0).twist.linear.x; + let twist = twist_stamped_at(1000.0, 0.0, 0); + assert!(!ekf.measurement_update_twist(&twist, 0)); + assert_eq!(ekf.get_current_twist(0).twist.linear.x, before); } + // [own test] no upstream equivalent. #[test] - fn process_noise_covariance_values() { - let mut params = EKFParameters::default(); - params.proc_stddev_yaw_c = 1.0; - params.proc_stddev_vx_c = 2.0; - params.proc_stddev_wz_c = 3.0; + fn apply_twist_observability_gate_inflates_low_speed_variance_only() { + let mut low_speed = twist_stamped_at(0.01, 0.5, 0); + apply_twist_observability_gate(&mut low_speed, 0.05); + assert_eq!(low_speed.twist.covariance[0], 10000.0); + assert_ne!(low_speed.twist.covariance[35], 10000.0); + + let mut normal_speed = twist_stamped_at(5.0, 0.5, 0); + let original_vx_var = normal_speed.twist.covariance[0]; + apply_twist_observability_gate(&mut normal_speed, 0.05); + assert_eq!(normal_speed.twist.covariance[0], original_vx_var); + } + + // [own test] no upstream equivalent. + #[test] + fn apply_twist_observability_gate_suppresses_low_speed_correction() { + let mut gated_ekf = EKFModule::new(EKFParameters::default()); + gated_ekf.initialize(&identity_pose_stamped(0), &Transform::identity()); + let mut ungated_ekf = EKFModule::new(EKFParameters::default()); + ungated_ekf.initialize(&identity_pose_stamped(0), &Transform::identity()); + + let mut gated_twist = twist_stamped_at(0.01, 0.0, 0); + apply_twist_observability_gate(&mut gated_twist, 0.05); + let ungated_twist = twist_stamped_at(0.01, 0.0, 0); + + gated_ekf.measurement_update_twist(&gated_twist, 0); + ungated_ekf.measurement_update_twist(&ungated_twist, 0); + + assert!( + gated_ekf.get_current_twist(0).twist.linear.x + < ungated_ekf.get_current_twist(0).twist.linear.x + ); + } + + // [own test] no upstream equivalent. + #[test] + fn find_closest_delay_time_index_prefers_upper_bound_on_exact_tie() { + let mut ekf = EKFModule::new(EKFParameters::default()); + // Force a known, small buffer: [0.0, 1.0, 2.0]. + ekf.accumulated_delay_times = alloc::vec![0.0, 1.0, 2.0]; + // Exactly halfway between index 0 (0.0) and index 1 (1.0): upstream's + // lower_bound-based tie-break prefers the upper (later) index. + assert_eq!(ekf.find_closest_delay_time_index(0.5), 1); + assert_eq!(ekf.find_closest_delay_time_index(0.0), 0); + assert_eq!(ekf.find_closest_delay_time_index(2.0), 2); + assert_eq!(ekf.find_closest_delay_time_index(2.1), 3); + } - let ekf = EKFModule::new(params); + // [own test] no upstream equivalent. + #[test] + fn accumulate_delay_time_tracks_block_age_in_ascending_order() { + let mut ekf = EKFModule::new(EKFParameters::default()); + ekf.accumulated_delay_times = alloc::vec![1e15; 4]; - let q = ekf.process_noise_covariance(1.0); + ekf.accumulate_delay_time(0.1); + assert_eq!(ekf.accumulated_delay_times[0], 0.0); - // indices: yaw = 2, vx = 4, wz = 5 - assert!((q[(2, 2)] - 1.0_f64.powi(2)).abs() < 1e-12); - assert!((q[(4, 4)] - 2.0_f64.powi(2)).abs() < 1e-12); - assert!((q[(5, 5)] - 3.0_f64.powi(2)).abs() < 1e-12); + ekf.accumulate_delay_time(0.1); + assert_eq!(ekf.accumulated_delay_times[0], 0.0); + assert!((ekf.accumulated_delay_times[1] - 0.1).abs() < 1e-12); - // zero case - let mut params = EKFParameters::default(); - params.proc_stddev_yaw_c = 0.0; - params.proc_stddev_vx_c = 0.0; - params.proc_stddev_wz_c = 0.0; - let ekf2 = EKFModule::new(params); - let q2 = ekf2.process_noise_covariance(1.0); - { - let mut s = 0.0; - for i in 0..6 { - for j in 0..6 { - let val = q2[(i, j)]; - s += val * val; - } - } - assert!(s == 0.0); + ekf.accumulate_delay_time(0.1); + assert_eq!(ekf.accumulated_delay_times[0], 0.0); + assert!((ekf.accumulated_delay_times[1] - 0.1).abs() < 1e-12); + assert!((ekf.accumulated_delay_times[2] - 0.2).abs() < 1e-12); + + // Ages are monotonically non-decreasing: block 0 is always "now". + for pair in ekf.accumulated_delay_times.windows(2) { + assert!(pair[0] <= pair[1]); } } + /// [own test] no upstream equivalent. `predict_with_delay` must not touch the + /// delay-time buffer, matching upstream where + /// `EKFModule::predict_with_delay` and `EKFModule::accumulate_delay_time` are two + /// separate calls made by the node (`update_predict_frequency` vs. `timer_callback`'s + /// prediction block). If `predict_with_delay` called `accumulate_delay_time` + /// internally, a caller that (correctly, per upstream) also calls + /// `accumulate_delay_time` itself would age the buffer twice per tick. #[test] - fn pose_and_twist_covariance_mapping() { - let params = EKFParameters::default(); - let mut ekf = EKFModule::new(params); + fn predict_with_delay_does_not_age_the_delay_time_buffer() { + let mut ekf = EKFModule::new(EKFParameters::default()); + let before = ekf.accumulated_delay_times.clone(); - // prepare a covariance matrix with top-left 3x3 = 1..9 - let mut p = Matrix6::::zeros(); - p[(0, 0)] = 1.0; - p[(0, 1)] = 2.0; - p[(0, 2)] = 3.0; - p[(1, 0)] = 4.0; - p[(1, 1)] = 5.0; - p[(1, 2)] = 6.0; - p[(2, 0)] = 7.0; - p[(2, 1)] = 8.0; - p[(2, 2)] = 9.0; - - ekf.covariance = p; - - // override the filter variances so those indices are replaced - ekf.z_filter.var = 100.0; - ekf.roll_filter.var = 200.0; - ekf.pitch_filter.var = 300.0; + ekf.predict_with_delay(0.1); - let cov = ekf.get_current_pose_covariance(); + assert_eq!(ekf.accumulated_delay_times, before); + } - // check a few mapped entries according to get_current_pose_covariance implementation - assert_eq!(cov[0], 1.0); - assert_eq!(cov[1], 2.0); - assert_eq!(cov[2], 3.0); - assert_eq!(cov[6], 4.0); - assert_eq!(cov[7], 5.0); - assert_eq!(cov[8], 6.0); - assert_eq!(cov[12], 7.0); - assert_eq!(cov[13], 8.0); - // index 14 is overwritten by z_filter.var - assert_eq!(cov[14], 100.0); - - // twist covariance mapping (Vx -> index 0, Wz -> index 35) - let mut p2 = Matrix6::::zeros(); - p2[(4, 4)] = 1.0; - p2[(4, 5)] = 2.0; - p2[(5, 4)] = 3.0; - p2[(5, 5)] = 4.0; - ekf.covariance = p2; - - let tcov = ekf.get_current_twist_covariance(); - assert_eq!(tcov[0], 1.0); - assert_eq!(tcov[35], 4.0); + /// [own test] no upstream equivalent. `EKFParameters::default()` must track + /// upstream's shipped `config/ekf_localizer.param.yaml` (autoware_core 1.8.0) + /// field-for-field. + #[test] + fn default_parameters_match_upstream_shipped_yaml() { + let p = EKFParameters::default(); + assert!(p.enable_yaw_bias_estimation); + assert_eq!(p.extend_state_step, 50); + assert_eq!(p.proc_stddev_vx_c, 10.0); + assert_eq!(p.proc_stddev_wz_c, 5.0); + assert_eq!(p.proc_stddev_yaw_c, 0.005); + assert_eq!(p.z_filter_proc_dev, 5.0); + assert_eq!(p.roll_filter_proc_dev, 0.1); + assert_eq!(p.pitch_filter_proc_dev, 0.1); + assert_eq!(p.pose_frame_id, "map"); + assert_eq!(p.pose_additional_delay, 0.0); + assert_eq!(p.pose_gate_dist, 49.5); + assert_eq!(p.pose_smoothing_steps, 5); + assert_eq!(p.twist_additional_delay, 0.0); + assert_eq!(p.twist_gate_dist, 46.1); + assert_eq!(p.twist_smoothing_steps, 2); + assert_eq!(p.threshold_observable_velocity_mps, 0.0); } } diff --git a/applications/autoware/ekf_localizer/src/mahalanobis.rs b/applications/autoware/ekf_localizer/src/mahalanobis.rs new file mode 100644 index 000000000..cc04f2158 --- /dev/null +++ b/applications/autoware/ekf_localizer/src/mahalanobis.rs @@ -0,0 +1,123 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Ported from the following versions of the original C++ code: +// core/autoware_core: +// type: git +// url: https://github.com/autowarefoundation/autoware_core.git +// original file path: localization/autoware_ekf_localizer/src/mahalanobis.cpp +// test code: localization/autoware_ekf_localizer/test/test_mahalanobis.cpp +// version: 1.8.0 + +use libm::sqrt; +use nalgebra::{DMatrix, DVector}; + +/// Squared Mahalanobis distance between `x` and `y` under covariance `c`. +/// +/// NOTE: upstream calls `C.inverse()` unconditionally (Eigen does not check +/// invertibility). Here a singular `C` is treated as "infinitely far apart" so that +/// callers gating on a distance threshold reject the measurement instead of dividing +/// by a garbage value. +pub fn squared_mahalanobis(x: &DVector, y: &DVector, c: &DMatrix) -> f64 { + let d = x - y; + match c.clone().try_inverse() { + Some(c_inv) => d.dot(&(c_inv * &d)), + None => f64::INFINITY, + } +} + +pub fn mahalanobis(x: &DVector, y: &DVector, c: &DMatrix) -> f64 { + sqrt(squared_mahalanobis(x, y, c)) +} + +#[cfg(test)] +mod tests { + use super::*; + + // Exact fixtures from upstream test_mahalanobis.cpp's `squared_mahalanobis.SmokeTest` + // (x=(0,1), y=(3,2), c=diag(10,10) -> 1.0; x=(4,1), y=(1,5), c=diag(5,5) -> 5.0). + #[test] + fn squared_mahalanobis_matches_hand_computed_values_for_diagonal_covariance() { + let c1 = DMatrix::from_diagonal(&DVector::from_vec(alloc::vec![10.0, 10.0])); + let x1 = DVector::from_vec(alloc::vec![0.0, 1.0]); + let y1 = DVector::from_vec(alloc::vec![3.0, 2.0]); + assert!((squared_mahalanobis(&x1, &y1, &c1) - 1.0).abs() <= 1e-8); + + let c2 = DMatrix::from_diagonal(&DVector::from_vec(alloc::vec![5.0, 5.0])); + let x2 = DVector::from_vec(alloc::vec![4.0, 1.0]); + let y2 = DVector::from_vec(alloc::vec![1.0, 5.0]); + assert!((squared_mahalanobis(&x2, &y2, &c2) - 5.0).abs() <= 1e-8); + } + + // Exact fixtures from upstream's `mahalanobis.SmokeTest` (same two cases as above, + // through the sqrt-wrapping `mahalanobis` function). + #[test] + fn mahalanobis_matches_hand_computed_values_for_diagonal_covariance() { + let c1 = DMatrix::from_diagonal(&DVector::from_vec(alloc::vec![10.0, 10.0])); + let x1 = DVector::from_vec(alloc::vec![0.0, 1.0]); + let y1 = DVector::from_vec(alloc::vec![3.0, 2.0]); + assert!((mahalanobis(&x1, &y1, &c1) - 1.0).abs() <= 1e-8); + + let c2 = DMatrix::from_diagonal(&DVector::from_vec(alloc::vec![5.0, 5.0])); + let x2 = DVector::from_vec(alloc::vec![4.0, 1.0]); + let y2 = DVector::from_vec(alloc::vec![1.0, 5.0]); + assert!((mahalanobis(&x2, &y2, &c2) - sqrt(5.0)).abs() <= 1e-8); + } + + // [own test] no upstream equivalent; covers the x==y degenerate case (distance must + // be exactly 0, not just "small"), which neither upstream fixture happens to exercise. + #[test] + fn zero_distance_when_points_match() { + let x = DVector::from_vec(alloc::vec![1.0, 2.0]); + let y = x.clone(); + let c = DMatrix::::identity(2, 2); + assert_eq!(mahalanobis(&x, &y, &c), 0.0); + } + + // [own test] no upstream equivalent; pins down the specific "identity covariance + // reduces to plain Euclidean distance" interpretation using a well-known 3-4-5 + // triangle, which is easier to sanity-check by eye than upstream's + // diag(10,10)/diag(5,5) fixtures. + #[test] + fn identity_covariance_is_euclidean_distance() { + let x = DVector::from_vec(alloc::vec![3.0, 0.0]); + let y = DVector::from_vec(alloc::vec![0.0, 4.0]); + let c = DMatrix::::identity(2, 2); + assert!((mahalanobis(&x, &y, &c) - 5.0).abs() < 1e-12); + } + + // [own test] no upstream equivalent; verifies the monotonic direction (looser + // covariance -> smaller distance) that upstream's two independent point fixtures + // don't directly compare against each other. + #[test] + fn larger_variance_shrinks_the_distance() { + let x = DVector::from_vec(alloc::vec![2.0]); + let y = DVector::from_vec(alloc::vec![0.0]); + let tight = DMatrix::from_element(1, 1, 1.0); + let loose = DMatrix::from_element(1, 1, 100.0); + assert!(mahalanobis(&x, &y, &loose) < mahalanobis(&x, &y, &tight)); + } + + // [own test] no upstream equivalent (Eigen's `.inverse()` on a singular matrix + // silently returns garbage rather than erroring); locks in this crate's deliberate + // divergence -- see the NOTE on `squared_mahalanobis` above -- of treating a singular + // covariance as "infinitely far" instead. + #[test] + fn singular_covariance_is_treated_as_infinitely_far() { + let x = DVector::from_vec(alloc::vec![1.0, 0.0]); + let y = DVector::from_vec(alloc::vec![0.0, 0.0]); + let c = DMatrix::::zeros(2, 2); + assert!(mahalanobis(&x, &y, &c).is_infinite()); + } +} diff --git a/applications/autoware/ekf_localizer/src/measurement.rs b/applications/autoware/ekf_localizer/src/measurement.rs new file mode 100644 index 000000000..423cbda31 --- /dev/null +++ b/applications/autoware/ekf_localizer/src/measurement.rs @@ -0,0 +1,199 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Ported from the following versions of the original C++ code: +// core/autoware_core: +// type: git +// url: https://github.com/autowarefoundation/autoware_core.git +// original file path: localization/autoware_ekf_localizer/src/measurement.cpp +// test code: localization/autoware_ekf_localizer/test/test_measurement.cpp +// version: 1.8.0 + +use nalgebra::DMatrix; + +use crate::StateIndex; + +// Every function below returns `DMatrix` even though its shape (3x6, 2x6, 3x3, 2x2) +// is fixed and known here -- a fixed-size nalgebra type (`Matrix3`, etc., mirroring +// upstream's `Eigen::Matrix3d`) would type-check just as well locally. `DMatrix` is used +// instead so the result can be passed directly as the `c`/`r` argument of +// `kalman_filter::DelayCompensatedKalmanFilter::update_with_delay`, which is itself +// `DMatrix`-typed because *its* dimensions (`dim_x_ex = dim_x * max_delay_step`) are only +// known at runtime. See kalman_filter.rs for that constraint. + +// XYZRPY (6x6, row-major) covariance array indices, matching +// autoware_utils_geometry::xyzrpy_covariance_index::XYZRPY_COV_IDX used upstream. +const X_X: usize = 0; +const X_Y: usize = 1; +const X_YAW: usize = 5; +const Y_X: usize = 6; +const Y_Y: usize = 7; +const Y_YAW: usize = 11; +const YAW_X: usize = 30; +const YAW_Y: usize = 31; +const YAW_YAW: usize = 35; + +pub fn pose_measurement_matrix() -> DMatrix { + let mut c = DMatrix::zeros(3, 6); + c[(0, StateIndex::X as usize)] = 1.0; + c[(1, StateIndex::Y as usize)] = 1.0; + c[(2, StateIndex::Yaw as usize)] = 1.0; + c +} + +pub fn twist_measurement_matrix() -> DMatrix { + let mut c = DMatrix::zeros(2, 6); + c[(0, StateIndex::Vx as usize)] = 1.0; + c[(1, StateIndex::Wz as usize)] = 1.0; + c +} + +pub fn pose_measurement_covariance(covariance: &[f64; 36], smoothing_step: usize) -> DMatrix { + let mut r = DMatrix::zeros(3, 3); + r[(0, 0)] = covariance[X_X]; + r[(0, 1)] = covariance[X_Y]; + r[(0, 2)] = covariance[X_YAW]; + r[(1, 0)] = covariance[Y_X]; + r[(1, 1)] = covariance[Y_Y]; + r[(1, 2)] = covariance[Y_YAW]; + r[(2, 0)] = covariance[YAW_X]; + r[(2, 1)] = covariance[YAW_Y]; + r[(2, 2)] = covariance[YAW_YAW]; + r * smoothing_step as f64 +} + +pub fn twist_measurement_covariance(covariance: &[f64; 36], smoothing_step: usize) -> DMatrix { + let mut r = DMatrix::zeros(2, 2); + r[(0, 0)] = covariance[X_X]; + r[(0, 1)] = covariance[X_YAW]; + r[(1, 0)] = covariance[YAW_X]; + r[(1, 1)] = covariance[YAW_YAW]; + r * smoothing_step as f64 +} + +#[cfg(test)] +mod tests { + use super::*; + + // Matches upstream test_measurement.cpp's `Measurement.pose_measurement_matrix` + // fixture (`expected << 1,0,0,0,0,0, 0,1,0,0,0,0, 0,0,1,0,0,0;`), checked element-wise + // (including every entry that should be zero) instead of building the full expected + // matrix and comparing norms, since this crate doesn't need an Eigen-style matrix + // literal builder for a one-off test. + #[test] + fn pose_measurement_matrix_picks_x_y_yaw() { + let c = pose_measurement_matrix(); + assert_eq!(c.shape(), (3, 6)); + for i in 0..3 { + for j in 0..6 { + let expected = if (i, j) == (0, StateIndex::X as usize) + || (i, j) == (1, StateIndex::Y as usize) + || (i, j) == (2, StateIndex::Yaw as usize) + { + 1.0 + } else { + 0.0 + }; + assert_eq!(c[(i, j)], expected, "mismatch at ({i},{j})"); + } + } + } + + // Matches upstream's `Measurement.twist_measurement_matrix` + // (`expected << 0,0,0,0,1,0, 0,0,0,0,0,1;`). + #[test] + fn twist_measurement_matrix_picks_vx_wz() { + let c = twist_measurement_matrix(); + assert_eq!(c.shape(), (2, 6)); + for i in 0..2 { + for j in 0..6 { + let expected = if (i, j) == (0, StateIndex::Vx as usize) + || (i, j) == (1, StateIndex::Wz as usize) + { + 1.0 + } else { + 0.0 + }; + assert_eq!(c[(i, j)], expected, "mismatch at ({i},{j})"); + } + } + } + + // Matches upstream's `Measurement.pose_measurement_covariance` exactly (same + // covariance array fixture and smoothing_step=2, same expected 3x3), including the + // off-diagonal cross-terms (X_Y, X_YAW, Y_X, Y_YAW, YAW_X, YAW_Y). A diagonal-only + // check cannot catch a transposition bug (e.g. reading Y_X into the X_Y slot), since + // that would still leave every diagonal entry correct. + #[test] + fn pose_measurement_covariance_preserves_cross_terms_and_scales_by_smoothing_step() { + let mut cov = [0.0; 36]; + cov[X_X] = 1.0; + cov[X_Y] = 2.0; + cov[X_YAW] = 3.0; + cov[Y_X] = 4.0; + cov[Y_Y] = 5.0; + cov[Y_YAW] = 6.0; + cov[YAW_X] = 7.0; + cov[YAW_Y] = 8.0; + cov[YAW_YAW] = 9.0; + + let r = pose_measurement_covariance(&cov, 2); + let expected = [[2.0, 4.0, 6.0], [8.0, 10.0, 12.0], [14.0, 16.0, 18.0]]; + for i in 0..3 { + for j in 0..3 { + assert_eq!(r[(i, j)], expected[i][j], "mismatch at ({i},{j})"); + } + } + } + + // Matches upstream's "make sure that other elements are not changed" sub-case. + #[test] + fn pose_measurement_covariance_zero_input_yields_zero_output() { + let cov = [0.0; 36]; + let r = pose_measurement_covariance(&cov, 2); + assert_eq!(r.iter().map(|v| v * v).sum::(), 0.0); + } + + // Matches upstream's `Measurement.twist_measurement_covariance` exactly, including + // the X_YAW/YAW_X off-diagonal terms (deliberately given different values, 2 vs 3, so + // a transposition bug would be caught). + // + // Omits upstream's extra `covariance[11] = 6` (Y_YAW, a slot this function never + // reads): an unset (zero) slot there still gets caught by the assertion below if it + // were accidentally read, so a non-zero noise value isn't needed to detect that bug + // class. + #[test] + fn twist_measurement_covariance_preserves_cross_terms_and_scales_by_smoothing_step() { + let mut cov = [0.0; 36]; + cov[X_X] = 1.0; + cov[X_YAW] = 2.0; + cov[YAW_X] = 3.0; + cov[YAW_YAW] = 4.0; + + let r = twist_measurement_covariance(&cov, 2); + let expected = [[2.0, 4.0], [6.0, 8.0]]; + for i in 0..2 { + for j in 0..2 { + assert_eq!(r[(i, j)], expected[i][j], "mismatch at ({i},{j})"); + } + } + } + + #[test] + fn twist_measurement_covariance_zero_input_yields_zero_output() { + let cov = [0.0; 36]; + let r = twist_measurement_covariance(&cov, 2); + assert_eq!(r.iter().map(|v| v * v).sum::(), 0.0); + } +} diff --git a/applications/autoware/ekf_localizer/src/numeric.rs b/applications/autoware/ekf_localizer/src/numeric.rs new file mode 100644 index 000000000..5349a0b88 --- /dev/null +++ b/applications/autoware/ekf_localizer/src/numeric.rs @@ -0,0 +1,68 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Ported from the following versions of the original C++ code: +// core/autoware_core: +// type: git +// url: https://github.com/autowarefoundation/autoware_core.git +// original file path: localization/autoware_ekf_localizer/src/include/numeric.hpp +// test code: localization/autoware_ekf_localizer/test/test_numeric.cpp +// version: 1.8.0 +// +// Kept as its own module, matching upstream's `numeric.hpp` file boundary, so +// `test_numeric.cpp`'s exact fixtures can be ported as real unit tests instead of only +// being exercised indirectly through `EKFModule` integration tests. + +use nalgebra::DVector; + +pub fn has_nan(v: &DVector) -> bool { + v.iter().any(|x| x.is_nan()) +} + +pub fn has_inf(v: &DVector) -> bool { + v.iter().any(|x| x.is_infinite()) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn has_nan_detects_nan_but_not_inf_or_large_finite_values() { + let empty = DVector::from_vec(alloc::vec![]); + let inf = f64::INFINITY; + let nan = f64::NAN; + + assert!(!has_nan(&empty)); + assert!(!has_nan(&DVector::from_vec(alloc::vec![0.0, 0.0, 1.0]))); + assert!(!has_nan(&DVector::from_vec(alloc::vec![1e16, 0.0, 1.0]))); + assert!(!has_nan(&DVector::from_vec(alloc::vec![0.0, 1.0, inf]))); + + assert!(has_nan(&DVector::from_vec(alloc::vec![nan, 1.0, 0.0]))); + } + + #[test] + fn has_inf_detects_inf_but_not_nan_or_large_finite_values() { + let empty = DVector::from_vec(alloc::vec![]); + let inf = f64::INFINITY; + let nan = f64::NAN; + + assert!(!has_inf(&empty)); + assert!(!has_inf(&DVector::from_vec(alloc::vec![0.0, 0.0, 1.0]))); + assert!(!has_inf(&DVector::from_vec(alloc::vec![1e16, 0.0, 1.0]))); + assert!(!has_inf(&DVector::from_vec(alloc::vec![nan, 1.0, 0.0]))); + + assert!(has_inf(&DVector::from_vec(alloc::vec![0.0, 1.0, inf]))); + } +} diff --git a/applications/autoware/ekf_localizer/src/state_transition.rs b/applications/autoware/ekf_localizer/src/state_transition.rs new file mode 100644 index 000000000..dd72a6ded --- /dev/null +++ b/applications/autoware/ekf_localizer/src/state_transition.rs @@ -0,0 +1,200 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Ported from the following versions of the original C++ code: +// core/autoware_core: +// type: git +// url: https://github.com/autowarefoundation/autoware_core.git +// original file path: localization/autoware_ekf_localizer/src/state_transition.cpp +// test code: localization/autoware_ekf_localizer/test/test_state_transition.cpp +// version: 1.8.0 + +use libm::{atan2, cos, sin}; + +use crate::{StateCovariance, StateIndex, StateVector}; + +pub fn normalize_yaw(yaw: f64) -> f64 { + atan2(sin(yaw), cos(yaw)) +} + +/* == Nonlinear model == + * + * x_{k+1} = x_k + vx_k * cos(yaw_k + b_k) * dt + * y_{k+1} = y_k + vx_k * sin(yaw_k + b_k) * dt + * yaw_{k+1} = yaw_k + (wz_k) * dt + * b_{k+1} = b_k + * vx_{k+1} = vx_k + * wz_{k+1} = wz_k + * + * (b_k : yaw_bias_k) + */ +pub fn predict_next_state(x_curr: &StateVector, dt: f64) -> StateVector { + let x = x_curr[StateIndex::X as usize]; + let y = x_curr[StateIndex::Y as usize]; + let yaw = x_curr[StateIndex::Yaw as usize]; + let yaw_bias = x_curr[StateIndex::YawBias as usize]; + let vx = x_curr[StateIndex::Vx as usize]; + let wz = x_curr[StateIndex::Wz as usize]; + + let mut x_next = *x_curr; + x_next[StateIndex::X as usize] = x + vx * cos(yaw + yaw_bias) * dt; + x_next[StateIndex::Y as usize] = y + vx * sin(yaw + yaw_bias) * dt; + x_next[StateIndex::Yaw as usize] = normalize_yaw(yaw + wz * dt); + x_next[StateIndex::YawBias as usize] = yaw_bias; + x_next[StateIndex::Vx as usize] = vx; + x_next[StateIndex::Wz as usize] = wz; + x_next +} + +/* == Linearized model == + * + * A = [ 1, 0, -vx*sin(yaw+b)*dt, -vx*sin(yaw+b)*dt, cos(yaw+b)*dt, 0] + * [ 0, 1, vx*cos(yaw+b)*dt, vx*cos(yaw+b)*dt, sin(yaw+b)*dt, 0] + * [ 0, 0, 1, 0, 0, dt] + * [ 0, 0, 0, 1, 0, 0] + * [ 0, 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 0, 1] + */ +pub fn create_state_transition_matrix(x_curr: &StateVector, dt: f64) -> StateCovariance { + let yaw = x_curr[StateIndex::Yaw as usize]; + let yaw_bias = x_curr[StateIndex::YawBias as usize]; + let vx = x_curr[StateIndex::Vx as usize]; + + let mut a = StateCovariance::identity(); + a[(StateIndex::X as usize, StateIndex::Yaw as usize)] = -vx * sin(yaw + yaw_bias) * dt; + a[(StateIndex::X as usize, StateIndex::YawBias as usize)] = -vx * sin(yaw + yaw_bias) * dt; + a[(StateIndex::X as usize, StateIndex::Vx as usize)] = cos(yaw + yaw_bias) * dt; + a[(StateIndex::Y as usize, StateIndex::Yaw as usize)] = vx * cos(yaw + yaw_bias) * dt; + a[(StateIndex::Y as usize, StateIndex::YawBias as usize)] = vx * cos(yaw + yaw_bias) * dt; + a[(StateIndex::Y as usize, StateIndex::Vx as usize)] = sin(yaw + yaw_bias) * dt; + a[(StateIndex::Yaw as usize, StateIndex::Wz as usize)] = dt; + a +} + +pub fn process_noise_covariance( + proc_cov_yaw_d: f64, + proc_cov_vx_d: f64, + proc_cov_wz_d: f64, +) -> StateCovariance { + let mut q = StateCovariance::zeros(); + q[(StateIndex::Yaw as usize, StateIndex::Yaw as usize)] = proc_cov_yaw_d; + q[(StateIndex::Vx as usize, StateIndex::Vx as usize)] = proc_cov_vx_d; + q[(StateIndex::Wz as usize, StateIndex::Wz as usize)] = proc_cov_wz_d; + q +} + +#[cfg(test)] +mod tests { + use super::*; + use core::f64::consts::PI; + use libm::sqrt; + use nalgebra::Vector6; + + // Expected values are computed via the same `libm` free functions `predict_next_state` + // itself uses (`cos`/`sin`/`atan2`, imported above through `use super::*`), not `f64`'s + // std-backed `.cos()/.sin()/.atan2()` methods. Those std methods resolve here too -- + // `cargo test` links `std` for the test harness even though this crate is `#![no_std]` + // -- but doing so would silently compare this crate's `libm` output against the host's + // glibc `libm.so` (confirmed via `nm -D`/`ldd` on the test binary: it pulls in + // `cos@GLIBC`/`sin@GLIBC`/`atan2@GLIBC`), two independent implementations that are not + // guaranteed to agree bit-for-bit. Using the same implementation on both sides, like + // upstream does (its test reuses its own `normalize_yaw` for the expected yaw), lets + // every component share the same tight 1e-10 tolerance instead of loosening yaw's. + #[test] + fn predict_next_state_matches_formula() { + let x_curr = Vector6::new(2.0, 3.0, PI / 2.0, PI / 4.0, 10.0, 2.0 * PI / 3.0); + let dt = 0.5; + let x_next = predict_next_state(&x_curr, dt); + + let tol = 1e-10; + assert!((x_next[0] - (2.0 + 10.0 * cos(PI / 2.0 + PI / 4.0) * dt)).abs() < tol); + assert!((x_next[1] - (3.0 + 10.0 * sin(PI / 2.0 + PI / 4.0) * dt)).abs() < tol); + let yaw_next = PI / 2.0 + (2.0 * PI / 3.0) * dt; + let expected_yaw = atan2(sin(yaw_next), cos(yaw_next)); + assert!((x_next[2] - expected_yaw).abs() < tol); + assert!((x_next[3] - x_curr[3]).abs() < tol); + assert!((x_next[4] - x_curr[4]).abs() < tol); + assert!((x_next[5] - x_curr[5]).abs() < tol); + } + + // Matches upstream test_state_transition.cpp's + // `create_state_transition_matrix.NumericalApproximation` exactly, including its + // per-case tolerance (2e-3 around x=0, 5e-3 around the non-zero x): a single shared + // tolerance would be looser than upstream for the x=0 case. + #[test] + fn create_state_transition_matrix_numeric_approximation() { + let dt = 0.1; + let dx = Vector6::from_element(0.1); + + for (x, tolerance) in [ + (Vector6::zeros(), 2e-3), + (Vector6::new(0.1, 0.2, 0.1, 0.4, 0.1, 0.3), 5e-3), + ] { + let a = create_state_transition_matrix(&x, dt); + let x1 = predict_next_state(&(x + dx), dt); + let x0 = predict_next_state(&x, dt); + let df = x1 - x0; + + let v = df - a * dx; + let mut s = 0.0; + for i in 0..6 { + s += v[i] * v[i]; + } + assert!(sqrt(s) < tolerance); + } + } + + // Matches upstream's `process_noise_covariance.process_noise_covariance` exactly: + // `process_noise_covariance(1., 2., 3.)`. The function already takes pre-computed + // proc_cov_*_d (variance-level) values, not stddevs to be squared, so the arguments + // are used as-is -- squaring them here would misleadingly imply a squaring step that + // doesn't belong in this test. + #[test] + fn process_noise_covariance_values() { + let q = process_noise_covariance(1.0, 2.0, 3.0); + + // indices: yaw = 2, vx = 4, wz = 5 + assert_eq!(q[(2, 2)], 1.0); + assert_eq!(q[(4, 4)], 2.0); + assert_eq!(q[(5, 5)], 3.0); + + let q2 = process_noise_covariance(0.0, 0.0, 0.0); + let mut s = 0.0; + for i in 0..6 { + for j in 0..6 { + let val = q2[(i, j)]; + s += val * val; + } + } + assert_eq!(s, 0.0); + } + + #[test] + fn normalize_yaw_wraps_into_pi_range() { + let tol = 1e-6; + // Exact fixtures from upstream test_state_transition.cpp's `StateTransition.normalize_yaw`. + assert!((normalize_yaw(PI * 4.0 / 3.0) - (-PI * 2.0 / 3.0)).abs() < tol); + assert!((normalize_yaw(-PI * 4.0 / 3.0) - (PI * 2.0 / 3.0)).abs() < tol); + assert!((normalize_yaw(PI * 9.0 / 2.0) - (PI * 1.0 / 2.0)).abs() < tol); + assert!((normalize_yaw(PI * 4.0) - 0.0).abs() < tol); + + // [own additions] no upstream equivalent below; covers the trivial 0 input and + // the exact-π boundary (where atan2's branch cut sits), neither of which + // upstream's four fixtures above happen to land on. + let tol_tight = 1e-9; + assert!((normalize_yaw(0.0) - 0.0).abs() < tol_tight); + assert!((normalize_yaw(2.0 * PI) - 0.0).abs() < tol_tight); + assert!((normalize_yaw(PI + 0.1) - (-PI + 0.1)).abs() < tol); + } +} diff --git a/applications/autoware/ekf_localizer/src/warn_throttle.rs b/applications/autoware/ekf_localizer/src/warn_throttle.rs new file mode 100644 index 000000000..41126f5cb --- /dev/null +++ b/applications/autoware/ekf_localizer/src/warn_throttle.rs @@ -0,0 +1,143 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Ported from the following versions of the original C++ code: +// core/autoware_core: +// type: git +// url: https://github.com/autowarefoundation/autoware_core.git +// original file path: localization/autoware_ekf_localizer/src/include/warning.hpp +// version: 1.8.0 +// +// The actual throttle *algorithm* upstream's `Warning::warn_throttle` runs (via +// `RCLCPP_WARN_THROTTLE`) is not defined in autoware_ekf_localizer at all -- it lives in +// ROS 2 itself: +// - rclcpp (humble): rclcpp/resource/logging.hpp.em, the `throttle` feature combination: +// auto get_time_point = [&c=clock](rcutils_time_point_value_t * time_point) { ... +// *time_point = c.now().nanoseconds(); ... }; +// RCUTILS_LOG_WARN_THROTTLE_NAMED(get_time_point, duration, ...); +// - rcutils (humble): rcutils/resource/logging_macros.h.em, +// `RCUTILS_LOG_CONDITION_THROTTLE_BEFORE(get_time_point_value, duration)`: +// static rcutils_duration_value_t __rcutils_logging_duration = MS_TO_NS(duration); +// static rcutils_time_point_value_t __rcutils_logging_last_logged = 0; +// rcutils_time_point_value_t __rcutils_logging_now = 0; +// get_time_point_value(&__rcutils_logging_now); +// condition = __rcutils_logging_now >= __rcutils_logging_last_logged + __rcutils_logging_duration; +// if (condition) { __rcutils_logging_last_logged = __rcutils_logging_now; ... log ... } +// `WarnThrottle` below is a direct port of that condition, using `now_ns` (this crate's u64 +// timestamp convention) in place of `clock.now().nanoseconds()`. This crate has no ROS node, +// so the caller passes "now" explicitly instead of a node-owned `rclcpp::Clock`. +// +// RT NOTE: unlike upstream -- where e.g. `mahalanobis_warning_message(distance, ...)` is +// built (and its `std::string` allocated) as an eager argument to `warn_throttle`, so the +// formatting cost is paid on every call regardless of whether the throttle actually lets +// the message through -- `should_emit` is checked *before* the caller formats/logs +// anything, so the `log::warn!`/`log::error!` allocation (`alloc::format!` in awkernel's +// buffered logger) is itself skipped while throttled, not just the eventual UART write. +// This matters because these call sites live inside `EKFModule::measurement_update_pose`/ +// `measurement_update_twist`, which are RT-critical (see `kalman_filter.rs`'s WCET +// contracts): under a sustained fault (a sensor failing the same gate every tick), +// unthrottled logging would otherwise allocate on every single tick. + +#[derive(Debug, Clone)] +pub struct WarnThrottle { + /// Matches upstream's `__rcutils_logging_last_logged`, which is a `static` initialized + /// to `0`, not an "unset" sentinel -- see `should_emit` for why that matters on the + /// very first call. + last_logged_ns: u64, + interval_ns: u64, +} + +impl WarnThrottle { + pub fn new(interval_ms: u64) -> Self { + Self { + last_logged_ns: 0, + interval_ns: interval_ms * 1_000_000, + } + } + + /// Returns `true` (and records `now_ns` as the new last-logged time) exactly when + /// upstream's `__rcutils_logging_now >= __rcutils_logging_last_logged + __rcutils_logging_duration` + /// would be true. Two consequences of matching this literally, both confirmed against + /// the rcutils source above rather than assumed: + /// - Since `last_logged_ns` starts at `0` (not "never logged"), the very first call + /// only emits if `now_ns >= interval_ns`. With a small/near-zero `now_ns` (e.g. replaying + /// a recorded run whose timestamps start near zero, as this project's own evaluation + /// data does), the first would-be warning is suppressed, exactly like upstream -- it is + /// *not* unconditionally emitted the way a "first call always logs" design would. + /// - A backwards time jump (`now_ns` older than `last_logged_ns`) makes the sum + /// `last_logged_ns + interval_ns` exceed `now_ns`, so the condition is false: upstream + /// *suppresses* on a clock jump back, it does not fail open. + /// + /// WCET contract: no heap allocation, no panic (uses `saturating_add` to rule out + /// overflow even though realistic timestamps never approach `u64::MAX`), O(1), no + /// logging/formatting/I/O of its own -- this only decides whether the *caller* should + /// log. + pub fn should_emit(&mut self, now_ns: u64) -> bool { + let condition = now_ns >= self.last_logged_ns.saturating_add(self.interval_ns); + if condition { + self.last_logged_ns = now_ns; + } + condition + } +} + +#[cfg(test)] +mod tests { + use super::*; + + // None of the tests below port an upstream `test_*.cpp` file -- as noted in this + // file's header, the throttle algorithm belongs to ROS 2's `rcutils`, not + // `autoware_ekf_localizer`, and `rcutils`'s own unit tests are out of scope for this + // repository. These tests instead verify this crate's `WarnThrottle` against the + // `RCUTILS_LOG_CONDITION_THROTTLE_BEFORE` macro semantics quoted above. + + #[test] + fn suppresses_the_first_call_when_now_is_below_the_interval() { + // last_logged_ns starts at 0, so now=0 with a 2s interval must NOT emit -- + // matching rcutils' `0 >= 0 + duration` being false for any positive duration. + let mut t = WarnThrottle::new(2000); + assert!(!t.should_emit(0)); + assert!(!t.should_emit(1_999_999_999)); + } + + #[test] + fn emits_the_first_call_once_now_reaches_the_interval() { + let mut t = WarnThrottle::new(2000); + assert!(t.should_emit(2_000_000_000)); + } + + #[test] + fn suppresses_within_the_interval_after_an_emit() { + let mut t = WarnThrottle::new(2000); + assert!(t.should_emit(2_000_000_000)); + assert!(!t.should_emit(3_000_000_000)); // 1s later, interval is 2s + assert!(!t.should_emit(3_999_999_999)); + } + + #[test] + fn emits_again_once_the_interval_elapses() { + let mut t = WarnThrottle::new(2000); + assert!(t.should_emit(2_000_000_000)); + assert!(t.should_emit(4_000_000_000)); // exactly 2s after the last emit + } + + #[test] + fn suppresses_on_a_backwards_time_jump() { + // Matches rcutils exactly: now < last_logged makes `now >= last_logged + duration` + // false, so a clock jump back suppresses rather than fail-opening. + let mut t = WarnThrottle::new(2000); + assert!(t.should_emit(5_000_000_000)); + assert!(!t.should_emit(1_000_000_000)); + } +} From 0e5852e1f2a2d3b83102e003dfd51f5e7dd62d37 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Sat, 1 Aug 2026 10:36:49 +0900 Subject: [PATCH 10/10] fix: apply cargo fmt 1 Signed-off-by: nokosaaan --- .../ekf_localizer/src/kalman_filter.rs | 45 +++++++++----- .../autoware/ekf_localizer/src/lib.rs | 62 ++++++++++++++----- 2 files changed, 79 insertions(+), 28 deletions(-) diff --git a/applications/autoware/ekf_localizer/src/kalman_filter.rs b/applications/autoware/ekf_localizer/src/kalman_filter.rs index 5b04d370d..05d23ff33 100644 --- a/applications/autoware/ekf_localizer/src/kalman_filter.rs +++ b/applications/autoware/ekf_localizer/src/kalman_filter.rs @@ -73,14 +73,14 @@ pub struct DelayCompensatedKalmanFilter { ap11: DMatrix, // dim_x x dim_x // --- update_with_delay scratch (sized in `init()`) --- - x_d: DVector, // dim_x - e: DVector, // MAX_DIM_Y, use rows(0, dim_y) - c_transpose: DMatrix, // dim_x x MAX_DIM_Y - c_p_dd: DMatrix, // MAX_DIM_Y x dim_x - s: DMatrix, // MAX_DIM_Y x MAX_DIM_Y - p_ct: DMatrix, // dim_x_ex x MAX_DIM_Y - k_transpose: DMatrix, // MAX_DIM_Y x dim_x_ex - k: DMatrix, // dim_x_ex x MAX_DIM_Y + x_d: DVector, // dim_x + e: DVector, // MAX_DIM_Y, use rows(0, dim_y) + c_transpose: DMatrix, // dim_x x MAX_DIM_Y + c_p_dd: DMatrix, // MAX_DIM_Y x dim_x + s: DMatrix, // MAX_DIM_Y x MAX_DIM_Y + p_ct: DMatrix, // dim_x_ex x MAX_DIM_Y + k_transpose: DMatrix, // MAX_DIM_Y x dim_x_ex + k: DMatrix, // dim_x_ex x MAX_DIM_Y } impl DelayCompensatedKalmanFilter { @@ -130,7 +130,8 @@ impl DelayCompensatedKalmanFilter { for i in 0..max_delay_step { let offset = i * dim_x; x_ex.rows_mut(offset, dim_x).copy_from(x); - p_ex.view_mut((offset, offset), (dim_x, dim_x)).copy_from(p0); + p_ex.view_mut((offset, offset), (dim_x, dim_x)) + .copy_from(p0); } self.dim_x = dim_x; @@ -160,7 +161,9 @@ impl DelayCompensatedKalmanFilter { /// Current-time state covariance (the first `dim_x x dim_x` block). pub fn latest_p(&self) -> DMatrix { - self.p_ex.view((0, 0), (self.dim_x, self.dim_x)).clone_owned() + self.p_ex + .view((0, 0), (self.dim_x, self.dim_x)) + .clone_owned() } /// Reads a single element of the state as it was `delay_step` predict-ticks ago. @@ -182,7 +185,12 @@ impl DelayCompensatedKalmanFilter { /// blocks, plus one O(dim_x_ex^2) `copy_from` (a memcpy, not an allocation) to carry /// the untouched history forward. /// - Does not log, format, block, or call unknown code. - pub fn predict_with_delay(&mut self, x_next: &DVector, a: &DMatrix, q: &DMatrix) { + pub fn predict_with_delay( + &mut self, + x_next: &DVector, + a: &DMatrix, + q: &DMatrix, + ) { let dim_x = self.dim_x; let dim_x_ex = dim_x * self.max_delay_step; let d_dim_x = dim_x_ex - dim_x; @@ -644,7 +652,9 @@ mod tests { .view_mut((0, 0), (dim_x, dim_x)) .copy_from(&(a * &p00 * a.transpose() + q)); let p0d = p_ex.view((0, 0), (dim_x, d)).clone_owned(); - p_tmp.view_mut((0, dim_x), (dim_x, d)).copy_from(&(a * &p0d)); + p_tmp + .view_mut((0, dim_x), (dim_x, d)) + .copy_from(&(a * &p0d)); let pd0 = p_ex.view((0, 0), (d, dim_x)).clone_owned(); p_tmp .view_mut((dim_x, 0), (d, dim_x)) @@ -773,7 +783,10 @@ mod tests { } let p_check = self.kf.latest_p(); - let p_gt = self.p_ex_gt.view((0, 0), (GT_DIM_X, GT_DIM_X)).clone_owned(); + let p_gt = self + .p_ex_gt + .view((0, 0), (GT_DIM_X, GT_DIM_X)) + .clone_owned(); for i in 0..GT_DIM_X { for j in 0..GT_DIM_X { assert!( @@ -837,7 +850,11 @@ mod tests { for i in 0..3 { let scale = (i + 1) as f64; - fixture.predict(&DVector::from_vec(alloc::vec![2.0 * scale, 4.0 * scale, 6.0 * scale])); + fixture.predict(&DVector::from_vec(alloc::vec![ + 2.0 * scale, + 4.0 * scale, + 6.0 * scale + ])); } let y = DVector::from_vec(alloc::vec![1.0, 2.0, 3.0]); diff --git a/applications/autoware/ekf_localizer/src/lib.rs b/applications/autoware/ekf_localizer/src/lib.rs index 1675ddace..1dd223a1d 100644 --- a/applications/autoware/ekf_localizer/src/lib.rs +++ b/applications/autoware/ekf_localizer/src/lib.rs @@ -46,12 +46,16 @@ pub use common_types::Header; use core::ptr::null_mut; use core::sync::atomic::{AtomicPtr, Ordering as AtomicOrdering}; use libm::{atan2, cos, sin}; -use nalgebra::{DMatrix, DVector, Matrix6, Quaternion as NQuaternion, Unit, UnitQuaternion, Vector3, Vector6}; +use nalgebra::{ + DMatrix, DVector, Matrix6, Quaternion as NQuaternion, Unit, UnitQuaternion, Vector3, Vector6, +}; pub use imu_corrector::Transform; pub use vehicle_velocity_converter::{TwistWithCovariance, TwistWithCovarianceStamped}; -use covariance::{ekf_covariance_to_pose_message_covariance, ekf_covariance_to_twist_message_covariance}; +use covariance::{ + ekf_covariance_to_pose_message_covariance, ekf_covariance_to_twist_message_covariance, +}; use kalman_filter::DelayCompensatedKalmanFilter; use mahalanobis::mahalanobis; use measurement::{ @@ -403,8 +407,11 @@ impl EKFModule { p0[(StateIndex::Vx as usize, StateIndex::Vx as usize)] = 0.01; p0[(StateIndex::Wz as usize, StateIndex::Wz as usize)] = 0.01; - self.kf - .init(&to_dvector(&x0), &to_dmatrix(&p0), self.params.extend_state_step); + self.kf.init( + &to_dvector(&x0), + &to_dmatrix(&p0), + self.params.extend_state_step, + ); let z = initial_pose.pose.pose.position.z; let (roll, pitch, _yaw) = self.quaternion_to_rpy(initial_pose.pose.pose.orientation); @@ -526,8 +533,14 @@ impl EKFModule { /// invoked. `measurement_update_twist` keeps running throughout (Dead Reckoning /// explicitly still uses twist). That decision belongs entirely to the pub/sub wiring /// layer, not to `EKFModule`. - pub fn measurement_update_pose(&mut self, pose: &PoseWithCovarianceStamped, t_curr: u64) -> bool { - if pose.header.frame_id != self.params.pose_frame_id && self.pose_frame_id_warn.should_emit(t_curr) { + pub fn measurement_update_pose( + &mut self, + pose: &PoseWithCovarianceStamped, + t_curr: u64, + ) -> bool { + if pose.header.frame_id != self.params.pose_frame_id + && self.pose_frame_id_warn.should_emit(t_curr) + { log::warn!( "pose frame_id is {}, but pose_frame is set as {}. They must be same.", pose.header.frame_id, @@ -535,8 +548,8 @@ impl EKFModule { ); } - let mut delay_time = - nanos_to_seconds_delta(t_curr, pose.header.timestamp) + self.params.pose_additional_delay; + let mut delay_time = nanos_to_seconds_delta(t_curr, pose.header.timestamp) + + self.params.pose_additional_delay; if delay_time < 0.0 && self.pose_delay_time_warn.should_emit(t_curr) { log::warn!("[EKF] pose delay time is negative: {delay_time}. Treated as 0."); } @@ -587,7 +600,8 @@ impl EKFModule { return false; } - let r = pose_measurement_covariance(&pose.pose.covariance, self.params.pose_smoothing_steps); + let r = + pose_measurement_covariance(&pose.pose.covariance, self.params.pose_smoothing_steps); if !self .kf @@ -648,7 +662,11 @@ impl EKFModule { pose_with_delay } - fn update_simple_1d_filters(&mut self, pose: &PoseWithCovarianceStamped, smoothing_step: usize) { + fn update_simple_1d_filters( + &mut self, + pose: &PoseWithCovarianceStamped, + smoothing_step: usize, + ) { let z = pose.pose.pose.position.z; let (roll, pitch, _yaw) = self.quaternion_to_rpy(pose.pose.pose.orientation); @@ -666,9 +684,16 @@ impl EKFModule { /// Mahalanobis gate, matching upstream. Callers that want the "don't trust vx at low /// speed" behaviour must call `apply_twist_observability_gate` on `twist` first. This /// keeps running during MRM Dead Reckoning -- see the note on `measurement_update_pose`. - pub fn measurement_update_twist(&mut self, twist: &TwistWithCovarianceStamped, t_curr: u64) -> bool { + pub fn measurement_update_twist( + &mut self, + twist: &TwistWithCovarianceStamped, + t_curr: u64, + ) -> bool { if twist.header.frame_id != "base_link" && self.twist_frame_id_warn.should_emit(t_curr) { - log::warn!("twist frame_id must be base_link, got {}", twist.header.frame_id); + log::warn!( + "twist frame_id must be base_link, got {}", + twist.header.frame_id + ); } self.last_angular_velocity = Vector3::zeros(); @@ -718,7 +743,10 @@ impl EKFModule { return false; } - let r = twist_measurement_covariance(&twist.twist.covariance, self.params.twist_smoothing_steps); + let r = twist_measurement_covariance( + &twist.twist.covariance, + self.params.twist_smoothing_steps, + ); if !self .kf @@ -938,7 +966,13 @@ mod tests { pose_stamped_at(0.0, 0.0, 0.0, 0.0, timestamp) } - fn pose_stamped_at(x: f64, y: f64, z: f64, yaw: f64, timestamp: u64) -> PoseWithCovarianceStamped { + fn pose_stamped_at( + x: f64, + y: f64, + z: f64, + yaw: f64, + timestamp: u64, + ) -> PoseWithCovarianceStamped { let mut covariance = [0.0; 36]; covariance[POSE_COV_X_X] = 0.01; covariance[POSE_COV_Y_Y] = 0.01;