diff --git a/applications/tests/test_autoware/Cargo.toml b/applications/tests/test_autoware/Cargo.toml
new file mode 100644
index 000000000..f745c6851
--- /dev/null
+++ b/applications/tests/test_autoware/Cargo.toml
@@ -0,0 +1,18 @@
+[package]
+name = "test_autoware"
+version = "0.1.0"
+edition = "2021"
+
+[lib]
+path = "src/lib.rs"
+crate-type = ["rlib"]
+
+[dependencies]
+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 }
+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/applications/tests/test_autoware/common_types/Cargo.toml b/applications/tests/test_autoware/common_types/Cargo.toml
new file mode 100644
index 000000000..c54b7769b
--- /dev/null
+++ b/applications/tests/test_autoware/common_types/Cargo.toml
@@ -0,0 +1,6 @@
+[package]
+name = "common_types"
+version = "0.1.0"
+edition = "2021"
+
+[dependencies]
diff --git a/applications/tests/test_autoware/common_types/src/lib.rs b/applications/tests/test_autoware/common_types/src/lib.rs
new file mode 100644
index 000000000..2ca030990
--- /dev/null
+++ b/applications/tests/test_autoware/common_types/src/lib.rs
@@ -0,0 +1,21 @@
+#![no_std]
+extern crate alloc;
+
+#[derive(Debug, Clone)]
+pub struct Header {
+    pub frame_id: &'static str,
+    pub timestamp: u64,
+}
+
+#[derive(Debug, Clone)]
+pub struct Vector3 {
+    pub x: f64,
+    pub y: f64,
+    pub z: f64,
+}
+
+impl Vector3 {
+    pub fn new(x: f64, y: f64, z: f64) -> Self {
+        Self { x, y, z }
+    }
+}
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<EKFModule> = 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<f64>;
+pub type StateCovariance = Matrix6<f64>;
+
+#[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<f64>,
+    pub angular: Vector3<f64>,
+}
+
+#[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<f64>,
+    // 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<f64> {
+        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<f64> {
+        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::<f64>::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::<f64>::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);
+    }
+}
diff --git a/applications/tests/test_autoware/gyro_odometer/Cargo.toml b/applications/tests/test_autoware/gyro_odometer/Cargo.toml
new file mode 100644
index 000000000..be11aaefb
--- /dev/null
+++ b/applications/tests/test_autoware/gyro_odometer/Cargo.toml
@@ -0,0 +1,9 @@
+[package]
+name = "gyro_odometer"
+version = "0.1.0"
+edition = "2021"
+
+[dependencies]
+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}
\ No newline at end of file
diff --git a/applications/tests/test_autoware/gyro_odometer/src/lib.rs b/applications/tests/test_autoware/gyro_odometer/src/lib.rs
new file mode 100644
index 000000000..834924786
--- /dev/null
+++ b/applications/tests/test_autoware/gyro_odometer/src/lib.rs
@@ -0,0 +1,461 @@
+#![no_std]
+extern crate alloc;
+
+use alloc::{collections::VecDeque, string::String};
+use core::time::Duration;
+use core::ptr::null_mut;
+use core::sync::atomic::{AtomicPtr, Ordering as AtomicOrdering};
+
+pub use imu_corrector::{transform_covariance, ImuWithCovariance, Transform};
+pub use imu_driver::{Header, ImuMsg, Quaternion, Vector3};
+pub use vehicle_velocity_converter::{
+    Odometry, Twist, TwistWithCovariance, TwistWithCovarianceStamped,
+};
+
+static GYRO_ODOMETER_INSTANCE: AtomicPtr<GyroOdometerCore> = AtomicPtr::new(null_mut());
+
+const COV_IDX_X_X: usize = 0;
+const COV_IDX_Y_Y: usize = 4;
+const COV_IDX_Z_Z: usize = 8;
+const COV_IDX_XYZRPY_X_X: usize = 0;
+const COV_IDX_XYZRPY_Y_Y: usize = 7;
+const COV_IDX_XYZRPY_Z_Z: usize = 14;
+const COV_IDX_XYZRPY_ROLL_ROLL: usize = 21;
+const COV_IDX_XYZRPY_PITCH_PITCH: usize = 28;
+const COV_IDX_XYZRPY_YAW_YAW: usize = 35;
+
+pub struct GyroOdometerCore {
+    pub output_frame: String,
+    pub message_timeout_sec: f64,
+    pub vehicle_twist_arrived: bool,
+    pub imu_arrived: bool,
+    pub vehicle_twist_queue: VecDeque<TwistWithCovarianceStamped>,
+    pub gyro_queue: VecDeque<ImuWithCovariance>,
+    pub config: GyroOdometerConfig,
+}
+
+impl GyroOdometerCore {
+    pub fn new(config: GyroOdometerConfig) -> Result<Self> {
+        let queue_size = config.queue_size;
+        let output_frame = config.output_frame.clone();
+        let message_timeout_sec = config.message_timeout_sec;
+
+        Ok(Self {
+            output_frame,
+            message_timeout_sec,
+            vehicle_twist_arrived: false,
+            imu_arrived: false,
+            vehicle_twist_queue: VecDeque::with_capacity(queue_size),
+            gyro_queue: VecDeque::with_capacity(queue_size),
+            config,
+        })
+    }
+
+    pub fn concat_gyro_and_odometer(
+        &mut self,
+        current_time: u64,
+    ) -> Result<Option<TwistWithCovarianceStamped>> {
+        if !self.vehicle_twist_arrived {
+            self.vehicle_twist_queue.clear();
+            self.gyro_queue.clear();
+            return Ok(None);
+        }
+        if !self.imu_arrived {
+            self.vehicle_twist_queue.clear();
+            self.gyro_queue.clear();
+            return Ok(None);
+        }
+        if !self.vehicle_twist_queue.is_empty() && !self.gyro_queue.is_empty() {
+            let latest_vehicle_twist_stamp =
+                self.vehicle_twist_queue.back().unwrap().header.timestamp;
+            let latest_imu_stamp = self.gyro_queue.back().unwrap().header.timestamp;
+
+            if Self::check_timeout(
+                current_time,
+                latest_vehicle_twist_stamp,
+                self.message_timeout_sec,
+            ) {
+                self.vehicle_twist_queue.clear();
+                self.gyro_queue.clear();
+                return Err(GyroOdometerError::TimeoutError(String::from(
+                    "Vehicle twist message timeout",
+                )));
+            }
+
+            if Self::check_timeout(current_time, latest_imu_stamp, self.message_timeout_sec) {
+                self.vehicle_twist_queue.clear();
+                self.gyro_queue.clear();
+                return Err(GyroOdometerError::TimeoutError(String::from(
+                    "IMU message timeout",
+                )));
+            }
+        }
+
+        if self.vehicle_twist_queue.is_empty() || self.gyro_queue.is_empty() {
+            return Ok(None);
+        }
+
+        let tf = self.get_transform(
+            &self.gyro_queue.front().unwrap().header.frame_id,
+            &self.output_frame,
+        )?;
+
+        for gyro in &mut self.gyro_queue {
+            let transformed_angular_velocity = tf.apply_to_vector(gyro.angular_velocity.clone());
+            gyro.angular_velocity = transformed_angular_velocity;
+        }
+
+        let mut vx_mean = 0.0;
+        let mut gyro_mean = Vector3::new(0.0, 0.0, 0.0);
+        let mut vx_covariance_original = 0.0;
+        let mut gyro_covariance_original = Vector3::new(0.0, 0.0, 0.0);
+
+        for vehicle_twist in &self.vehicle_twist_queue {
+            vx_mean += vehicle_twist.twist.twist.linear.x;
+            vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0];
+        }
+        vx_mean /= self.vehicle_twist_queue.len() as f64;
+        vx_covariance_original /= self.vehicle_twist_queue.len() as f64;
+
+        for gyro in &self.gyro_queue {
+            gyro_mean.x += gyro.angular_velocity.x;
+            gyro_mean.y += gyro.angular_velocity.y;
+            gyro_mean.z += gyro.angular_velocity.z;
+            gyro_covariance_original.x += gyro.angular_velocity_covariance[COV_IDX_X_X];
+            gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_Y_Y];
+            gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_Z_Z];
+        }
+        gyro_mean.x /= self.gyro_queue.len() as f64;
+        gyro_mean.y /= self.gyro_queue.len() as f64;
+        gyro_mean.z /= self.gyro_queue.len() as f64;
+        gyro_covariance_original.x /= self.gyro_queue.len() as f64;
+        gyro_covariance_original.y /= self.gyro_queue.len() as f64;
+        gyro_covariance_original.z /= self.gyro_queue.len() as f64;
+
+        let latest_vehicle_twist_stamp = self.vehicle_twist_queue.back().unwrap().header.timestamp;
+        let latest_imu_stamp = self.gyro_queue.back().unwrap().header.timestamp;
+
+        let result_timestamp = if latest_vehicle_twist_stamp < latest_imu_stamp {
+            latest_imu_stamp
+        } else {
+            latest_vehicle_twist_stamp
+        };
+
+        let mut result = TwistWithCovarianceStamped {
+            header: Header {
+                frame_id: self.gyro_queue.front().unwrap().header.frame_id,
+                timestamp: result_timestamp,
+            },
+            twist: TwistWithCovariance {
+                twist: Twist {
+                    linear: Vector3::new(vx_mean, 0.0, 0.0),
+                    angular: gyro_mean,
+                },
+                covariance: [0.0; 36],
+            },
+        };
+
+        result.twist.covariance[COV_IDX_XYZRPY_X_X] =
+            vx_covariance_original / self.vehicle_twist_queue.len() as f64;
+        result.twist.covariance[COV_IDX_XYZRPY_Y_Y] = 100000.0;
+        result.twist.covariance[COV_IDX_XYZRPY_Z_Z] = 100000.0;
+        result.twist.covariance[COV_IDX_XYZRPY_ROLL_ROLL] =
+            gyro_covariance_original.x / self.gyro_queue.len() as f64;
+        result.twist.covariance[COV_IDX_XYZRPY_PITCH_PITCH] =
+            gyro_covariance_original.y / self.gyro_queue.len() as f64;
+        result.twist.covariance[COV_IDX_XYZRPY_YAW_YAW] =
+            gyro_covariance_original.z / self.gyro_queue.len() as f64;
+
+        self.vehicle_twist_queue.clear();
+        self.gyro_queue.clear();
+
+        Ok(Some(result))
+    }
+
+    pub fn check_timeout(current_timestamp: u64, last_timestamp: u64, timeout_sec: f64) -> bool {
+        let dt = (current_timestamp as f64 - last_timestamp as f64) / 1_000_000_000.0;
+        dt.abs() > timeout_sec
+    }
+    pub fn get_transform(&self, from_frame: &str, to_frame: &str) -> Result<Transform> {
+        if from_frame == to_frame || from_frame == "" || to_frame == "" {
+            Ok(Transform::identity())
+        } else {
+            Ok(Transform::identity())
+        }
+    }
+
+    pub fn process_result(
+        &self,
+        twist_with_cov_raw: TwistWithCovarianceStamped,
+    ) -> TwistWithCovarianceStamped {
+        if twist_with_cov_raw.twist.twist.angular.z.abs() < 0.01
+            && twist_with_cov_raw.twist.twist.linear.x.abs() < 0.01
+        {
+            let mut twist = twist_with_cov_raw;
+            twist.twist.twist.angular.x = 0.0;
+            twist.twist.twist.angular.y = 0.0;
+            twist.twist.twist.angular.z = 0.0;
+            twist
+        } else {
+            twist_with_cov_raw
+        }
+    }
+
+    pub fn convert_vehicle_velocity_to_twist(
+        &self,
+        odometry: &Odometry,
+        timestamp: u64,
+    ) -> TwistWithCovarianceStamped {
+        TwistWithCovarianceStamped {
+            header: Header {
+                frame_id: "base_link",
+                timestamp,
+            },
+            twist: TwistWithCovariance {
+                twist: Twist {
+                    linear: Vector3::new(odometry.velocity, 0.0, 0.0),
+                    angular: Vector3::new(0.0, 0.0, 0.0),
+                },
+                covariance: [0.0; 36],
+            },
+        }
+    }
+
+    pub fn add_vehicle_twist(&mut self, twist: TwistWithCovarianceStamped) {
+        self.vehicle_twist_arrived = true;
+        self.vehicle_twist_queue.push_back(twist);
+    }
+
+    pub fn add_imu(&mut self, imu: ImuWithCovariance) {
+        self.imu_arrived = true;
+        self.gyro_queue.push_back(imu);
+    }
+
+    pub fn process_imu_with_covariance(&mut self, imu: ImuWithCovariance) -> Result<()> {
+        self.add_imu(imu);
+        Ok(())
+    }
+
+    pub fn process_and_get_result(
+        &mut self,
+        current_time: u64,
+    ) -> Option<TwistWithCovarianceStamped> {
+        match self.concat_gyro_and_odometer(current_time) {
+            Ok(result) => result,
+            Err(_) => None,
+        }
+    }
+
+    pub fn get_queue_sizes(&self) -> (usize, usize) {
+        (self.vehicle_twist_queue.len(), self.gyro_queue.len())
+    }
+
+    pub fn clear_queues(&mut self) {
+        self.vehicle_twist_queue.clear();
+        self.gyro_queue.clear();
+    }
+
+    pub fn reset_arrival_flags(&mut self) {
+        self.vehicle_twist_arrived = false;
+        self.imu_arrived = false;
+    }
+}
+
+#[derive(Debug)]
+pub enum GyroOdometerError {
+    TransformError(String),
+    TimeoutError(String),
+    QueueError(String),
+    ParameterError(String),
+}
+
+impl core::fmt::Display for GyroOdometerError {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        match self {
+            GyroOdometerError::TransformError(msg) => write!(f, "Transform error: {}", msg),
+            GyroOdometerError::TimeoutError(msg) => write!(f, "Timeout error: {}", msg),
+            GyroOdometerError::QueueError(msg) => write!(f, "Queue error: {}", msg),
+            GyroOdometerError::ParameterError(msg) => write!(f, "Invalid parameter: {}", msg),
+        }
+    }
+}
+
+impl core::error::Error for GyroOdometerError {}
+
+type Result<T> = core::result::Result<T, GyroOdometerError>;
+
+#[derive(Debug, Clone)]
+pub struct GyroOdometerConfig {
+    pub output_frame: String,
+    pub message_timeout_sec: f64,
+    pub queue_size: usize,
+    pub transform_timeout: Duration,
+    pub min_velocity_threshold: f64,
+    pub covariance_scale: f64,
+}
+
+impl Default for GyroOdometerConfig {
+    fn default() -> Self {
+        Self {
+            output_frame: String::from("base_link"),
+            message_timeout_sec: 1.0,
+            queue_size: 100,
+            transform_timeout: Duration::from_secs(1),
+            min_velocity_threshold: 0.01,
+            covariance_scale: 100000.0,
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    // Equivalent helpers to Autoware C++ test helper.
+    fn generate_sample_imu() -> ImuMsg {
+        ImuMsg {
+            header: Header {
+                frame_id: "base_link",
+                timestamp: 123456789,
+            },
+            orientation: Quaternion {
+                x: 0.0,
+                y: 0.0,
+                z: 0.0,
+                w: 1.0,
+            },
+            angular_velocity: Vector3::new(0.1, 0.2, 0.3),
+            linear_acceleration: Vector3::new(9.8, 0.0, 0.0),
+        }
+    }
+
+    fn generate_sample_velocity() -> TwistWithCovarianceStamped {
+        TwistWithCovarianceStamped {
+            header: Header {
+                frame_id: "base_link",
+                timestamp: 123456789,
+            },
+            twist: TwistWithCovariance {
+                twist: Twist {
+                    linear: Vector3::new(1.0, 0.0, 0.0),
+                    angular: Vector3::new(0.0, 0.0, 0.0),
+                },
+                covariance: [0.0; 36],
+            },
+        }
+    }
+
+    fn get_config_with_default_params() -> GyroOdometerConfig {
+        GyroOdometerConfig {
+            output_frame: String::from("base_link"),
+            message_timeout_sec: 1e12,
+            ..GyroOdometerConfig::default()
+        }
+    }
+
+    #[test]
+    fn test_gyro_odometer_core_creation() {
+        let config = get_config_with_default_params();
+        let core = GyroOdometerCore::new(config);
+        assert!(core.is_ok());
+    }
+
+    #[test]
+    fn test_imu_with_covariance_conversion() {
+        let imu_msg = generate_sample_imu();
+
+        let imu_with_cov = ImuWithCovariance::from_imu_msg(&imu_msg);
+        let converted_back = imu_with_cov.to_imu_msg();
+
+        assert_eq!(imu_msg.header.frame_id, converted_back.header.frame_id);
+        assert_eq!(imu_msg.header.timestamp, converted_back.header.timestamp);
+        assert_eq!(
+            imu_msg.angular_velocity.x,
+            converted_back.angular_velocity.x
+        );
+        assert_eq!(
+            imu_msg.angular_velocity.y,
+            converted_back.angular_velocity.y
+        );
+        assert_eq!(
+            imu_msg.angular_velocity.z,
+            converted_back.angular_velocity.z
+        );
+    }
+
+    #[test]
+    fn test_vehicle_velocity_conversion() {
+        let config = get_config_with_default_params();
+        let core = GyroOdometerCore::new(config).unwrap();
+
+        let sample_twist = generate_sample_velocity();
+        assert_eq!(sample_twist.header.frame_id, "base_link");
+        assert_eq!(sample_twist.twist.twist.linear.x, 1.0);
+
+        let odometry = Odometry {
+            velocity: sample_twist.twist.twist.linear.x,
+        };
+        let twist = core.convert_vehicle_velocity_to_twist(&odometry, sample_twist.header.timestamp);
+
+        assert_eq!(twist.header.frame_id, sample_twist.header.frame_id);
+        assert_eq!(twist.header.timestamp, 123456789);
+        assert_eq!(twist.twist.twist.linear.x, 1.0);
+        assert_eq!(twist.twist.twist.linear.y, 0.0);
+        assert_eq!(twist.twist.twist.linear.z, 0.0);
+    }
+
+    #[test]
+    fn test_imu_corrector_integration() {
+        let config = get_config_with_default_params();
+        let mut core = GyroOdometerCore::new(config).unwrap();
+
+        let imu_msg = generate_sample_imu();
+        let mut imu_with_cov = ImuWithCovariance::from_imu_msg(&imu_msg);
+        imu_with_cov.angular_velocity_covariance =
+            [0.0009, 0.0, 0.0, 0.0, 0.0009, 0.0, 0.0, 0.0, 0.0009];
+        imu_with_cov.linear_acceleration_covariance =
+            [100000000.0, 0.0, 0.0, 0.0, 100000000.0, 0.0, 0.0, 0.0, 100000000.0];
+
+        let result = core.process_imu_with_covariance(imu_with_cov);
+        assert!(result.is_ok());
+    }
+
+    #[test]
+    fn test_transform_covariance_from_imu_corrector() {
+        let input = [1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0];
+        let output = transform_covariance(&input);
+        assert_eq!(output[COV_IDX_X_X], 3.0);
+        assert_eq!(output[COV_IDX_Y_Y], 3.0);
+        assert_eq!(output[COV_IDX_Z_Z], 3.0);
+    }
+}
+
+pub fn get_or_initialize() -> Result<&'static mut GyroOdometerCore> {
+    let ptr = GYRO_ODOMETER_INSTANCE.load(AtomicOrdering::Acquire);
+
+    if !ptr.is_null() {
+        return Ok(unsafe { &mut *ptr });
+    }
+
+    let config = GyroOdometerConfig::default();
+    let core = GyroOdometerCore::new(config)?;
+    let boxed_core = alloc::boxed::Box::new(core);
+    let new_ptr = alloc::boxed::Box::into_raw(boxed_core);
+
+    match GYRO_ODOMETER_INSTANCE.compare_exchange(
+        null_mut(),
+        new_ptr,
+        AtomicOrdering::Acquire,
+        AtomicOrdering::Relaxed,
+    ) {
+        Ok(_) => {
+            Ok(unsafe { &mut *new_ptr })
+        }
+        Err(existing_ptr) => {
+            unsafe {
+                let _ = alloc::boxed::Box::from_raw(new_ptr);
+            }
+            Ok(unsafe { &mut *existing_ptr })
+        }
+    }
+}
\ No newline at end of file
diff --git a/applications/tests/test_autoware/imu_corrector/Cargo.toml b/applications/tests/test_autoware/imu_corrector/Cargo.toml
new file mode 100644
index 000000000..0397eed62
--- /dev/null
+++ b/applications/tests/test_autoware/imu_corrector/Cargo.toml
@@ -0,0 +1,8 @@
+[package]
+name = "imu_corrector"
+version = "0.1.0"
+edition = "2021"
+
+[dependencies]
+nalgebra = { version = "0.32", default-features = false, features = ["libm"] }
+imu_driver = { path = "../imu_driver", default-features = false }
\ No newline at end of file
diff --git a/applications/tests/test_autoware/imu_corrector/src/lib.rs b/applications/tests/test_autoware/imu_corrector/src/lib.rs
new file mode 100644
index 000000000..abf265a33
--- /dev/null
+++ b/applications/tests/test_autoware/imu_corrector/src/lib.rs
@@ -0,0 +1,482 @@
+#![no_std]
+extern crate alloc;
+
+use alloc::{format, string::String};
+use imu_driver::{Header, ImuMsg, Quaternion, Vector3};
+use nalgebra::{Quaternion as NQuaternion, UnitQuaternion, Vector3 as NVector3};
+
+#[derive(Clone, Debug)]
+pub struct Transform {
+    pub translation: Vector3,
+    pub rotation: Quaternion,
+}
+
+impl Transform {
+    pub fn identity() -> Self {
+        Self {
+            translation: imu_driver::Vector3::new(0.0, 0.0, 0.0),
+            rotation: imu_driver::Quaternion {
+                x: 0.0,
+                y: 0.0,
+                z: 0.0,
+                w: 1.0,
+            },
+        }
+    }
+
+    fn to_nalgebra_vector3(&self, vec: &Vector3) -> NVector3<f64> {
+        NVector3::new(vec.x, vec.y, vec.z)
+    }
+
+    fn to_imu_vector3(&self, vec: &NVector3<f64>) -> Vector3 {
+        Vector3::new(vec.x, vec.y, vec.z)
+    }
+
+    fn to_nalgebra_quaternion(&self, quat: &Quaternion) -> UnitQuaternion<f64> {
+        let n_quat = NQuaternion::new(quat.w, quat.x, quat.y, quat.z);
+        UnitQuaternion::from_quaternion(n_quat)
+    }
+
+    pub fn apply_to_vector(&self, vec: Vector3) -> Vector3 {
+        let nalgebra_vec = self.to_nalgebra_vector3(&vec);
+        let nalgebra_quat = self.to_nalgebra_quaternion(&self.rotation);
+        let nalgebra_trans = self.to_nalgebra_vector3(&self.translation);
+        let rotated = nalgebra_quat * nalgebra_vec;
+        let result = rotated + nalgebra_trans;
+        self.to_imu_vector3(&result)
+    }
+}
+
+pub trait TransformListener {
+    fn get_latest_transform(&self, from_frame: &str, to_frame: &str) -> Option<Transform>;
+    fn get_transform_at_time(
+        &self,
+        from_frame: &str,
+        to_frame: &str,
+        timestamp: u64,
+    ) -> Option<Transform>;
+}
+
+pub struct MockTransformListener {
+    transforms: alloc::collections::BTreeMap<String, Transform>,
+}
+
+impl MockTransformListener {
+    pub fn new() -> Self {
+        Self {
+            transforms: alloc::collections::BTreeMap::new(),
+        }
+    }
+
+    pub fn add_transform(&mut self, from_frame: &str, to_frame: &str, transform: Transform) {
+        let key = format!("{}_to_{}", from_frame, to_frame);
+        self.transforms.insert(key, transform);
+    }
+}
+
+impl TransformListener for MockTransformListener {
+    fn get_latest_transform(&self, from_frame: &str, to_frame: &str) -> Option<Transform> {
+        let key = format!("{}_to_{}", from_frame, to_frame);
+        self.transforms.get(&key).cloned()
+    }
+
+    fn get_transform_at_time(
+        &self,
+        from_frame: &str,
+        to_frame: &str,
+        _timestamp: u64,
+    ) -> Option<Transform> {
+        self.get_latest_transform(from_frame, to_frame)
+    }
+}
+
+#[derive(Clone, Debug)]
+pub struct ImuWithCovariance {
+    pub header: Header,
+    pub orientation: Quaternion,
+    pub angular_velocity: Vector3,
+    pub angular_velocity_covariance: [f64; 9],
+    pub linear_acceleration: Vector3,
+    pub linear_acceleration_covariance: [f64; 9],
+}
+
+impl ImuWithCovariance {
+    pub fn from_imu_msg(imu_msg: &ImuMsg) -> Self {
+        Self {
+            header: imu_msg.header.clone(),
+            orientation: imu_msg.orientation.clone(),
+            angular_velocity: imu_msg.angular_velocity.clone(),
+            angular_velocity_covariance: [0.0; 9],
+            linear_acceleration: imu_msg.linear_acceleration.clone(),
+            linear_acceleration_covariance: [0.0; 9],
+        }
+    }
+
+    pub fn to_imu_msg(&self) -> ImuMsg {
+        ImuMsg {
+            header: self.header.clone(),
+            orientation: self.orientation.clone(),
+            angular_velocity: self.angular_velocity.clone(),
+            linear_acceleration: self.linear_acceleration.clone(),
+        }
+    }
+}
+
+pub struct ImuCorrectorConfig {
+    pub angular_velocity_offset_x: f64,
+    pub angular_velocity_offset_y: f64,
+    pub angular_velocity_offset_z: f64,
+    pub angular_velocity_stddev_xx: f64,
+    pub angular_velocity_stddev_yy: f64,
+    pub angular_velocity_stddev_zz: f64,
+    pub acceleration_stddev: f64,
+    pub output_frame: &'static str,
+}
+
+impl Default for ImuCorrectorConfig {
+    fn default() -> Self {
+        Self {
+            angular_velocity_offset_x: 0.0,
+            angular_velocity_offset_y: 0.0,
+            angular_velocity_offset_z: 0.0,
+            angular_velocity_stddev_xx: 0.03,
+            angular_velocity_stddev_yy: 0.03,
+            angular_velocity_stddev_zz: 0.03,
+            acceleration_stddev: 10000.0,
+            output_frame: "base_link",
+        }
+    }
+}
+
+pub struct ImuCorrector<T: TransformListener> {
+    config: ImuCorrectorConfig,
+    transform_listener: T,
+}
+
+impl ImuCorrector<MockTransformListener> {
+    pub fn new() -> Self {
+        Self {
+            config: ImuCorrectorConfig::default(),
+            transform_listener: MockTransformListener::new(),
+        }
+    }
+
+    pub fn with_transform_listener(transform_listener: MockTransformListener) -> Self {
+        Self {
+            config: ImuCorrectorConfig::default(),
+            transform_listener,
+        }
+    }
+}
+
+impl<T: TransformListener> ImuCorrector<T> {
+    pub fn set_config(&mut self, config: ImuCorrectorConfig) {
+        self.config = config;
+    }
+
+    fn to_nalgebra_vector3(&self, vec: &Vector3) -> NVector3<f64> {
+        NVector3::new(vec.x, vec.y, vec.z)
+    }
+
+    fn to_imu_vector3(&self, vec: &NVector3<f64>) -> Vector3 {
+        Vector3::new(vec.x, vec.y, vec.z)
+    }
+
+    fn to_nalgebra_quaternion(&self, quat: &Quaternion) -> UnitQuaternion<f64> {
+        let n_quat = NQuaternion::new(quat.w, quat.x, quat.y, quat.z);
+        UnitQuaternion::from_quaternion(n_quat)
+    }
+
+    fn transform_vector3(&self, vec: &Vector3, transform: &Transform) -> Vector3 {
+        let nalgebra_vec = self.to_nalgebra_vector3(vec);
+        let nalgebra_quat = self.to_nalgebra_quaternion(&transform.rotation);
+        let rotated = nalgebra_quat * nalgebra_vec;
+        self.to_imu_vector3(&rotated)
+    }
+
+    fn transform_covariance(&self, cov: &[f64; 9]) -> [f64; 9] {
+        let max_cov = cov[0].max(cov[4]).max(cov[8]);
+        let mut cov_transformed = [0.0; 9];
+        cov_transformed[0] = max_cov;
+        cov_transformed[4] = max_cov;
+        cov_transformed[8] = max_cov;
+
+        cov_transformed
+    }
+
+    pub fn correct_imu_with_covariance(
+        &self,
+        imu_msg: &ImuMsg,
+        transform: Option<&Transform>,
+    ) -> ImuWithCovariance {
+        let mut corrected_imu = ImuWithCovariance::from_imu_msg(imu_msg);
+        corrected_imu.angular_velocity.x -= self.config.angular_velocity_offset_x;
+        corrected_imu.angular_velocity.y -= self.config.angular_velocity_offset_y;
+        corrected_imu.angular_velocity.z -= self.config.angular_velocity_offset_z;
+        corrected_imu.angular_velocity_covariance[0] =
+            self.config.angular_velocity_stddev_xx * self.config.angular_velocity_stddev_xx;
+        corrected_imu.angular_velocity_covariance[4] =
+            self.config.angular_velocity_stddev_yy * self.config.angular_velocity_stddev_yy;
+        corrected_imu.angular_velocity_covariance[8] =
+            self.config.angular_velocity_stddev_zz * self.config.angular_velocity_stddev_zz;
+        let accel_var = self.config.acceleration_stddev * self.config.acceleration_stddev;
+        corrected_imu.linear_acceleration_covariance[0] = accel_var;
+        corrected_imu.linear_acceleration_covariance[4] = accel_var;
+        corrected_imu.linear_acceleration_covariance[8] = accel_var;
+
+        if let Some(tf) = transform {
+            corrected_imu.linear_acceleration =
+                self.transform_vector3(&corrected_imu.linear_acceleration, tf);
+            corrected_imu.linear_acceleration_covariance =
+                self.transform_covariance(&corrected_imu.linear_acceleration_covariance);
+            corrected_imu.angular_velocity =
+                self.transform_vector3(&corrected_imu.angular_velocity, tf);
+            corrected_imu.angular_velocity_covariance =
+                self.transform_covariance(&corrected_imu.angular_velocity_covariance);
+            corrected_imu.header.frame_id = self.config.output_frame;
+        }
+
+        corrected_imu
+    }
+
+    pub fn correct_imu(&self, imu_msg: &ImuMsg, transform: Option<&Transform>) -> ImuMsg {
+        let corrected_with_cov = self.correct_imu_with_covariance(imu_msg, transform);
+        corrected_with_cov.to_imu_msg()
+    }
+
+    pub fn correct_imu_with_dynamic_tf(&self, imu_msg: &ImuMsg) -> Option<ImuMsg> {
+        let transform = self
+            .transform_listener
+            .get_latest_transform(&imu_msg.header.frame_id, self.config.output_frame)?;
+
+        let corrected_with_cov = self.correct_imu_with_covariance(imu_msg, Some(&transform));
+        Some(corrected_with_cov.to_imu_msg())
+    }
+
+    pub fn correct_imu_with_dynamic_tf_covariance(
+        &self,
+        imu_msg: &ImuMsg,
+    ) -> Option<ImuWithCovariance> {
+        let transform = self
+            .transform_listener
+            .get_latest_transform(&imu_msg.header.frame_id, self.config.output_frame)?;
+
+        Some(self.correct_imu_with_covariance(imu_msg, Some(&transform)))
+    }
+
+    pub fn correct_imu_simple(&self, imu_msg: &ImuMsg) -> ImuMsg {
+        let mut corrected_msg = imu_msg.clone();
+
+        corrected_msg.angular_velocity.x -= self.config.angular_velocity_offset_x;
+        corrected_msg.angular_velocity.y -= self.config.angular_velocity_offset_y;
+        corrected_msg.angular_velocity.z -= self.config.angular_velocity_offset_z;
+
+        corrected_msg.header.frame_id = self.config.output_frame;
+
+        corrected_msg
+    }
+
+    pub fn get_covariance_config(&self) -> (f64, f64, f64, f64) {
+        (
+            self.config.angular_velocity_stddev_xx,
+            self.config.angular_velocity_stddev_yy,
+            self.config.angular_velocity_stddev_zz,
+            self.config.acceleration_stddev,
+        )
+    }
+}
+
+pub fn transform_covariance(cov: &[f64; 9]) -> [f64; 9] {
+    let max_cov = cov[0].max(cov[4]).max(cov[8]);
+    let mut cov_transformed = [0.0; 9];
+    cov_transformed[0] = max_cov;
+    cov_transformed[4] = max_cov;
+    cov_transformed[8] = max_cov;
+    cov_transformed
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn assert_almost_eq(actual: f64, expected: f64) {
+        assert!((actual - expected).abs() < 1e-10);
+    }
+
+    fn sample_imu_msg() -> ImuMsg {
+        ImuMsg {
+            header: Header {
+                frame_id: "imu_link",
+                timestamp: 0,
+            },
+            orientation: Quaternion {
+                x: 0.0,
+                y: 0.0,
+                z: 0.0,
+                w: 1.0,
+            },
+            angular_velocity: Vector3::new(0.1, 0.2, 0.3),
+            linear_acceleration: Vector3::new(9.8, 0.0, 0.0),
+        }
+    }
+
+    #[test]
+    fn static_bias_correction_updates_angular_velocity() {
+        let mut config = ImuCorrectorConfig::default();
+        config.angular_velocity_offset_x = 0.05;
+        config.angular_velocity_offset_y = 0.1;
+        config.angular_velocity_offset_z = 0.15;
+
+        let mut corrector = ImuCorrector::new();
+        corrector.set_config(config);
+
+        let imu_msg = sample_imu_msg();
+        let corrected = corrector.correct_imu(&imu_msg, None);
+
+        assert_eq!(corrected.angular_velocity.x, 0.05);
+        assert_eq!(corrected.angular_velocity.y, 0.1);
+        assert_eq!(corrected.angular_velocity.z, 0.15);
+        assert_eq!(corrected.header.frame_id, "imu_link");
+    }
+
+    #[test]
+    fn simple_path_sets_output_frame() {
+        let corrector = ImuCorrector::new();
+        let imu_msg = sample_imu_msg();
+
+        let corrected_msg = corrector.correct_imu_simple(&imu_msg);
+
+        assert_eq!(corrected_msg.header.frame_id, "base_link");
+        assert_eq!(corrected_msg.angular_velocity.x, 0.1);
+        assert_eq!(corrected_msg.angular_velocity.y, 0.2);
+        assert_eq!(corrected_msg.angular_velocity.z, 0.3);
+    }
+
+    #[test]
+    fn covariance_defaults_are_set_from_config() {
+        let corrector = ImuCorrector::new();
+        let imu_msg = sample_imu_msg();
+
+        let corrected_with_cov = corrector.correct_imu_with_covariance(&imu_msg, None);
+        let expected_angular_var = 0.03 * 0.03;
+        assert_eq!(
+            corrected_with_cov.angular_velocity_covariance[0],
+            expected_angular_var
+        );
+        assert_eq!(
+            corrected_with_cov.angular_velocity_covariance[4],
+            expected_angular_var
+        );
+        assert_eq!(
+            corrected_with_cov.angular_velocity_covariance[8],
+            expected_angular_var
+        );
+        let expected_accel_var = 10000.0 * 10000.0;
+        assert_eq!(
+            corrected_with_cov.linear_acceleration_covariance[0],
+            expected_accel_var
+        );
+        assert_eq!(
+            corrected_with_cov.linear_acceleration_covariance[4],
+            expected_accel_var
+        );
+        assert_eq!(
+            corrected_with_cov.linear_acceleration_covariance[8],
+            expected_accel_var
+        );
+    }
+
+    #[test]
+    fn covariance_transform_uses_max_diagonal() {
+        let corrector = ImuCorrector::new();
+        let input_cov = [1.0, 0.5, 0.3, 0.5, 2.0, 0.4, 0.3, 0.4, 3.0];
+        let transformed_cov = corrector.transform_covariance(&input_cov);
+
+        assert_eq!(transformed_cov[0], 3.0);
+        assert_eq!(transformed_cov[4], 3.0);
+        assert_eq!(transformed_cov[8], 3.0);
+        assert_eq!(transformed_cov[1], 0.0);
+        assert_eq!(transformed_cov[2], 0.0);
+        assert_eq!(transformed_cov[3], 0.0);
+        assert_eq!(transformed_cov[5], 0.0);
+        assert_eq!(transformed_cov[6], 0.0);
+        assert_eq!(transformed_cov[7], 0.0);
+    }
+
+    #[test]
+    fn public_covariance_transform_uses_max_diagonal() {
+        let input_cov = [1.0, 0.5, 0.3, 0.5, 2.0, 0.4, 0.3, 0.4, 3.0];
+        let transformed_cov = transform_covariance(&input_cov);
+        assert_eq!(transformed_cov[0], 3.0);
+        assert_eq!(transformed_cov[4], 3.0);
+        assert_eq!(transformed_cov[8], 3.0);
+        assert_eq!(transformed_cov[1], 0.0);
+        assert_eq!(transformed_cov[2], 0.0);
+        assert_eq!(transformed_cov[3], 0.0);
+        assert_eq!(transformed_cov[5], 0.0);
+        assert_eq!(transformed_cov[6], 0.0);
+        assert_eq!(transformed_cov[7], 0.0);
+    }
+
+    #[test]
+    fn transform_applies_to_vectors_and_sets_output_frame() {
+        let corrector = ImuCorrector::new();
+        let imu_msg = sample_imu_msg();
+
+        let transform = Transform {
+            translation: Vector3::new(1.0, 2.0, 3.0),
+            rotation: Quaternion {
+                x: 0.0,
+                y: 0.0,
+                z: 0.7071067811865475,
+                w: 0.7071067811865476,
+            },
+        };
+
+        let corrected_with_cov = corrector.correct_imu_with_covariance(&imu_msg, Some(&transform));
+        assert_eq!(corrected_with_cov.header.frame_id, "base_link");
+        assert_almost_eq(corrected_with_cov.linear_acceleration.x, 0.0);
+        assert_almost_eq(corrected_with_cov.linear_acceleration.y, 9.8);
+        assert_almost_eq(corrected_with_cov.linear_acceleration.z, 0.0);
+        assert_almost_eq(corrected_with_cov.angular_velocity.x, -0.2);
+        assert_almost_eq(corrected_with_cov.angular_velocity.y, 0.1);
+        assert_almost_eq(corrected_with_cov.angular_velocity.z, 0.3);
+    }
+
+    #[test]
+    fn dynamic_tf_path_applies_transform() {
+        let mut transform_listener = MockTransformListener::new();
+        let transform = Transform {
+            translation: Vector3::new(1.0, 0.0, 0.0),
+            rotation: Quaternion {
+                x: 0.0,
+                y: 0.0,
+                z: 0.0,
+                w: 1.0,
+            },
+        };
+        transform_listener.add_transform("imu_link", "base_link", transform);
+
+        let corrector = ImuCorrector::with_transform_listener(transform_listener);
+        let imu_msg = sample_imu_msg();
+
+        let corrected_msg = corrector.correct_imu_with_dynamic_tf(&imu_msg);
+        assert!(corrected_msg.is_some());
+
+        let corrected_msg = corrected_msg.unwrap();
+        assert_eq!(corrected_msg.header.frame_id, "base_link");
+        assert_eq!(corrected_msg.linear_acceleration.x, 9.8);
+        assert_eq!(corrected_msg.linear_acceleration.y, 0.0);
+        assert_eq!(corrected_msg.linear_acceleration.z, 0.0);
+    }
+
+    #[test]
+    fn dynamic_tf_path_returns_none_when_transform_is_missing() {
+        let transform_listener = MockTransformListener::new();
+        let corrector = ImuCorrector::with_transform_listener(transform_listener);
+        let imu_msg = sample_imu_msg();
+
+        let corrected_msg = corrector.correct_imu_with_dynamic_tf(&imu_msg);
+        assert!(corrected_msg.is_none());
+    }
+}
diff --git a/applications/tests/test_autoware/imu_driver/Cargo.toml b/applications/tests/test_autoware/imu_driver/Cargo.toml
new file mode 100644
index 000000000..ceec72d7a
--- /dev/null
+++ b/applications/tests/test_autoware/imu_driver/Cargo.toml
@@ -0,0 +1,7 @@
+[package]
+name = "imu_driver"
+version = "0.1.0"
+edition = "2021"
+
+[dependencies]
+common_types = { path = "../common_types", default-features = false }
\ No newline at end of file
diff --git a/applications/tests/test_autoware/imu_driver/src/lib.rs b/applications/tests/test_autoware/imu_driver/src/lib.rs
new file mode 100644
index 000000000..15ef58e24
--- /dev/null
+++ b/applications/tests/test_autoware/imu_driver/src/lib.rs
@@ -0,0 +1,339 @@
+#![no_std]
+extern crate alloc;
+
+use alloc::{format, string::String, vec, vec::Vec};
+use core::f64::consts::PI;
+
+pub use common_types::{Header, Vector3};
+
+#[derive(Clone, Debug)]
+pub struct ImuMsg {
+    pub header: Header,
+    pub orientation: Quaternion,
+    pub angular_velocity: Vector3,
+    pub linear_acceleration: Vector3,
+}
+
+#[derive(Clone, Debug)]
+pub struct ImuCsvRow {
+    pub timestamp: u64,
+    pub orientation: Quaternion,
+    pub angular_velocity: Vector3,
+    pub linear_acceleration: Vector3,
+}
+
+#[derive(Debug, Clone)]
+pub struct Quaternion {
+    pub x: f64,
+    pub y: f64,
+    pub z: f64,
+    pub w: f64,
+}
+
+impl Default for ImuMsg {
+    fn default() -> Self {
+        Self {
+            header: Header {
+                frame_id: "imu",
+                timestamp: 0,
+            },
+            orientation: Quaternion {
+                x: 0.0,
+                y: 0.0,
+                z: 0.0,
+                w: 1.0,
+            },
+            angular_velocity: Vector3::new(0.0, 0.0, 0.0),
+            linear_acceleration: Vector3::new(0.0, 0.0, 0.0),
+        }
+    }
+}
+
+pub fn build_imu_msg_from_csv_row(
+    row: &ImuCsvRow,
+    frame_id: &'static str,
+    timestamp: u64,
+) -> ImuMsg {
+    ImuMsg {
+        header: Header {
+            frame_id,
+            timestamp,
+        },
+        orientation: row.orientation.clone(),
+        angular_velocity: row.angular_velocity.clone(),
+        linear_acceleration: row.linear_acceleration.clone(),
+    }
+}
+
+pub struct TamagawaImuParser {
+    frame_id: &'static str,
+    dummy_counter: u16,
+}
+
+impl TamagawaImuParser {
+    pub fn new(frame_id: &'static str) -> Self {
+        Self {
+            frame_id,
+            dummy_counter: 0,
+        }
+    }
+
+    pub fn parse_binary_data(&self, data: &[u8], timestamp: u64) -> Option<ImuMsg> {
+        if data.len() != 58 || data[0..9] != *b"$TSC,BIN," {
+            return None;
+        }
+
+        let mut imu_msg = ImuMsg::default();
+        imu_msg.header.frame_id = self.frame_id;
+        imu_msg.header.timestamp = timestamp;
+
+        let _counter = ((data[11] as u16) << 8) | (data[12] as u16);
+        let raw_data = self.parse_signed_16bit(&data[15..17]);
+        imu_msg.angular_velocity.x = self.convert_angular_velocity(raw_data);
+        let raw_data = self.parse_signed_16bit(&data[17..19]);
+        imu_msg.angular_velocity.y = self.convert_angular_velocity(raw_data);
+        let raw_data = self.parse_signed_16bit(&data[19..21]);
+        imu_msg.angular_velocity.z = self.convert_angular_velocity(raw_data);
+        let raw_data = self.parse_signed_16bit(&data[21..23]);
+        imu_msg.linear_acceleration.x = self.convert_acceleration(raw_data);
+        let raw_data = self.parse_signed_16bit(&data[23..25]);
+        imu_msg.linear_acceleration.y = self.convert_acceleration(raw_data);
+        let raw_data = self.parse_signed_16bit(&data[25..27]);
+        imu_msg.linear_acceleration.z = self.convert_acceleration(raw_data);
+
+        Some(imu_msg)
+    }
+
+    pub fn generate_dummy_binary_data(
+        &mut self,
+        _timestamp: u64,
+        angular_velocity: Vector3,
+        linear_acceleration: Vector3,
+    ) -> Vec<u8> {
+        let mut data = vec![0u8; 58];
+
+        data[0..5].copy_from_slice(b"$TSC,");
+        data[5] = b'B';
+        data[6] = b'I';
+        data[7] = b'N';
+        data[8] = b',';
+        data[11] = (self.dummy_counter >> 8) as u8;
+        data[12] = (self.dummy_counter & 0xFF) as u8;
+        self.dummy_counter = self.dummy_counter.wrapping_add(1);
+
+        let angular_vel_x_lsb = self.convert_angular_velocity_to_lsb(angular_velocity.x);
+        let angular_vel_y_lsb = self.convert_angular_velocity_to_lsb(angular_velocity.y);
+        let angular_vel_z_lsb = self.convert_angular_velocity_to_lsb(angular_velocity.z);
+        let accel_x_lsb = self.convert_acceleration_to_lsb(linear_acceleration.x);
+        let accel_y_lsb = self.convert_acceleration_to_lsb(linear_acceleration.y);
+        let accel_z_lsb = self.convert_acceleration_to_lsb(linear_acceleration.z);
+
+        data[15] = (angular_vel_x_lsb >> 8) as u8;
+        data[16] = (angular_vel_x_lsb & 0xFF) as u8;
+        data[17] = (angular_vel_y_lsb >> 8) as u8;
+        data[18] = (angular_vel_y_lsb & 0xFF) as u8;
+        data[19] = (angular_vel_z_lsb >> 8) as u8;
+        data[20] = (angular_vel_z_lsb & 0xFF) as u8;
+        data[21] = (accel_x_lsb >> 8) as u8;
+        data[22] = (accel_x_lsb & 0xFF) as u8;
+        data[23] = (accel_y_lsb >> 8) as u8;
+        data[24] = (accel_y_lsb & 0xFF) as u8;
+        data[25] = (accel_z_lsb >> 8) as u8;
+        data[26] = (accel_z_lsb & 0xFF) as u8;
+
+        data
+    }
+
+    pub fn generate_static_dummy_data(&mut self, timestamp: u64) -> Vec<u8> {
+        let angular_velocity = Vector3::new(0.1, 0.2, 0.01);
+        let linear_acceleration = Vector3::new(0.0, 0.0, 9.80665);
+
+        self.generate_dummy_binary_data(timestamp, angular_velocity, linear_acceleration)
+    }
+
+    fn convert_angular_velocity_to_lsb(&self, rad_per_sec: f64) -> i16 {
+        let deg_per_sec = rad_per_sec * 180.0 / PI;
+        let lsb = deg_per_sec / (200.0 / (1 << 15) as f64);
+        lsb as i16
+    }
+
+    fn convert_acceleration_to_lsb(&self, m_per_sec_squared: f64) -> i16 {
+        let lsb = m_per_sec_squared / (100.0 / (1 << 15) as f64);
+        lsb as i16
+    }
+
+    fn parse_signed_16bit(&self, data: &[u8]) -> i16 {
+        if data.len() != 2 {
+            return 0;
+        }
+        let high_byte = (data[0] as i32) << 8;
+        let low_byte = data[1] as i32;
+        let result = (high_byte & 0xFFFFFF00u32 as i32) | (low_byte & 0x000000FF);
+        result as i16
+    }
+
+    fn convert_angular_velocity(&self, raw_data: i16) -> f64 {
+        let lsb_to_deg_per_sec = 200.0 / (1 << 15) as f64;
+        let deg_to_rad = PI / 180.0;
+        (raw_data as f64) * lsb_to_deg_per_sec * deg_to_rad
+    }
+
+    fn convert_acceleration(&self, raw_data: i16) -> f64 {
+        let lsb_to_m_per_sec_squared = 100.0 / (1 << 15) as f64;
+        (raw_data as f64) * lsb_to_m_per_sec_squared
+    }
+
+    pub fn generate_version_request() -> &'static [u8] {
+        b"$TSC,VER*29\r\n"
+    }
+
+    pub fn generate_offset_cancel_request(offset_value: i32) -> String {
+        format!("$TSC,OFC,{}\r\n", offset_value)
+    }
+
+    pub fn generate_heading_reset_request() -> &'static [u8] {
+        b"$TSC,HRST*29\r\n"
+    }
+
+    pub fn generate_binary_request(rate_hz: u32) -> String {
+        format!("$TSC,BIN,{}\r\n", rate_hz)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use core::f64::consts::PI;
+
+    fn assert_approx_eq(actual: f64, expected: f64, eps: f64) {
+        assert!(
+            (actual - expected).abs() <= eps,
+            "left: {actual}, right: {expected}"
+        );
+    }
+
+    fn expected_ang_vel(raw: i16) -> f64 {
+        let lsb_to_deg_per_sec = 200.0 / (1 << 15) as f64;
+        let deg_to_rad = PI / 180.0;
+        (raw as f64) * lsb_to_deg_per_sec * deg_to_rad
+    }
+
+    fn expected_acc(raw: i16) -> f64 {
+        let lsb_to_m_per_sec_squared = 100.0 / (1 << 15) as f64;
+        (raw as f64) * lsb_to_m_per_sec_squared
+    }
+
+    fn put_i16_be(buf: &mut [u8], value: i16) {
+        let raw = value as u16;
+        buf[0] = (raw >> 8) as u8;
+        buf[1] = (raw & 0xFF) as u8;
+    }
+
+    #[test]
+    fn parse_rejects_invalid_length() {
+        let parser = TamagawaImuParser::new("imu_link");
+        let data = [0u8; 57];
+
+        assert!(parser.parse_binary_data(&data, 1).is_none());
+    }
+
+    #[test]
+    fn parse_rejects_non_bin_header() {
+        let parser = TamagawaImuParser::new("imu_link");
+        let mut data = [0u8; 58];
+        data[0..5].copy_from_slice(b"$TSC,");
+        data[5..9].copy_from_slice(b"XIN,");
+
+        assert!(parser.parse_binary_data(&data, 1).is_none());
+    }
+
+    #[test]
+    fn parse_rejects_invalid_tsc_prefix() {
+        let parser = TamagawaImuParser::new("imu_link");
+        let mut data = [0u8; 58];
+        data[0..9].copy_from_slice(b"@TSC,BIN,");
+
+        assert!(parser.parse_binary_data(&data, 1).is_none());
+    }
+
+    #[test]
+    fn parse_extracts_fields_and_converts_units() {
+        let parser = TamagawaImuParser::new("imu_link");
+        let timestamp = 123456789u64;
+
+        let mut data = [0u8; 58];
+        data[0..5].copy_from_slice(b"$TSC,");
+        data[5..9].copy_from_slice(b"BIN,");
+
+        let gx: i16 = 1;
+        let gy: i16 = -2;
+        let gz: i16 = 1234;
+        let ax: i16 = -300;
+        let ay: i16 = 0;
+        let az: i16 = 32767;
+
+        put_i16_be(&mut data[15..17], gx);
+        put_i16_be(&mut data[17..19], gy);
+        put_i16_be(&mut data[19..21], gz);
+        put_i16_be(&mut data[21..23], ax);
+        put_i16_be(&mut data[23..25], ay);
+        put_i16_be(&mut data[25..27], az);
+
+        let imu = parser
+            .parse_binary_data(&data, timestamp)
+            .expect("should parse");
+
+        assert_eq!(imu.header.frame_id, "imu_link");
+        assert_eq!(imu.header.timestamp, timestamp);
+
+        let eps = 1e-12;
+        assert_approx_eq(imu.angular_velocity.x, expected_ang_vel(gx), eps);
+        assert_approx_eq(imu.angular_velocity.y, expected_ang_vel(gy), eps);
+        assert_approx_eq(imu.angular_velocity.z, expected_ang_vel(gz), eps);
+
+        assert_approx_eq(imu.linear_acceleration.x, expected_acc(ax), eps);
+        assert_approx_eq(imu.linear_acceleration.y, expected_acc(ay), eps);
+        assert_approx_eq(imu.linear_acceleration.z, expected_acc(az), eps);
+    }
+
+    #[test]
+    fn generate_dummy_data_roundtrip_is_close() {
+        let mut parser = TamagawaImuParser::new("imu_link");
+        let timestamp = 42u64;
+
+        let input_angular_velocity = Vector3::new(0.5, -0.3, 1.2);
+        let input_linear_acceleration = Vector3::new(8.5, 2.1, 10.2);
+
+        let data = parser.generate_dummy_binary_data(
+            timestamp,
+            input_angular_velocity.clone(),
+            input_linear_acceleration.clone(),
+        );
+        let imu = parser
+            .parse_binary_data(&data, timestamp)
+            .expect("should parse");
+
+        let ang_eps = (200.0 / (1 << 15) as f64) * (PI / 180.0);
+        let acc_eps = 100.0 / (1 << 15) as f64;
+
+        assert_approx_eq(imu.angular_velocity.x, input_angular_velocity.x, ang_eps);
+        assert_approx_eq(imu.angular_velocity.y, input_angular_velocity.y, ang_eps);
+        assert_approx_eq(imu.angular_velocity.z, input_angular_velocity.z, ang_eps);
+
+        assert_approx_eq(
+            imu.linear_acceleration.x,
+            input_linear_acceleration.x,
+            acc_eps,
+        );
+        assert_approx_eq(
+            imu.linear_acceleration.y,
+            input_linear_acceleration.y,
+            acc_eps,
+        );
+        assert_approx_eq(
+            imu.linear_acceleration.z,
+            input_linear_acceleration.z,
+            acc_eps,
+        );
+    }
+}
diff --git a/applications/tests/test_autoware/src/lib.rs b/applications/tests/test_autoware/src/lib.rs
new file mode 100644
index 000000000..438b2d083
--- /dev/null
+++ b/applications/tests/test_autoware/src/lib.rs
@@ -0,0 +1,4 @@
+#![no_std]
+extern crate alloc;
+
+pub async fn run() {}
diff --git a/applications/tests/test_autoware/vehicle_velocity_converter/Cargo.toml b/applications/tests/test_autoware/vehicle_velocity_converter/Cargo.toml
new file mode 100644
index 000000000..9d3d8b790
--- /dev/null
+++ b/applications/tests/test_autoware/vehicle_velocity_converter/Cargo.toml
@@ -0,0 +1,7 @@
+[package]
+name = "vehicle_velocity_converter"
+version = "0.1.0"
+edition = "2021"
+
+[dependencies]
+common_types = { path = "../common_types", default-features = false }
diff --git a/applications/tests/test_autoware/vehicle_velocity_converter/src/lib.rs b/applications/tests/test_autoware/vehicle_velocity_converter/src/lib.rs
new file mode 100644
index 000000000..2b523c95f
--- /dev/null
+++ b/applications/tests/test_autoware/vehicle_velocity_converter/src/lib.rs
@@ -0,0 +1,310 @@
+// 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.
+
+#![no_std]
+
+pub use common_types::{Header, Vector3};
+
+#[derive(Debug, Clone)]
+pub struct VelocityReport {
+    pub header: Header,
+    pub longitudinal_velocity: f64,
+    pub lateral_velocity: f64,
+    pub heading_rate: f64,
+}
+
+#[derive(Debug, Clone)]
+pub struct VelocityCsvRow {
+    pub timestamp: u64,
+    pub longitudinal_velocity: f64,
+    pub lateral_velocity: f64,
+    pub heading_rate: f64,
+}
+
+pub fn build_velocity_report_from_csv_row(
+    row: &VelocityCsvRow,
+    frame_id: &'static str,
+    timestamp: u64,
+) -> VelocityReport {
+    VelocityReport {
+        header: Header {
+            frame_id,
+            timestamp,
+        },
+        longitudinal_velocity: row.longitudinal_velocity,
+        lateral_velocity: row.lateral_velocity,
+        heading_rate: row.heading_rate,
+    }
+}
+
+#[derive(Debug, Clone)]
+pub struct TwistWithCovarianceStamped {
+    pub header: Header,
+    pub twist: TwistWithCovariance,
+}
+
+#[derive(Debug, Clone)]
+pub struct TwistWithCovariance {
+    pub twist: Twist,
+    pub covariance: [f64; 36],
+}
+
+#[derive(Debug, Clone)]
+pub struct Twist {
+    pub linear: Vector3,
+    pub angular: Vector3,
+}
+
+#[repr(C)]
+pub struct Odometry {
+    pub velocity: f64,
+}
+
+pub struct VehicleVelocityConverter {
+    frame_id: &'static str,
+    stddev_vx: f64,
+    stddev_wz: f64,
+    speed_scale_factor: f64,
+}
+
+impl VehicleVelocityConverter {
+    pub fn new(
+        frame_id: &'static str,
+        stddev_vx: f64,
+        stddev_wz: f64,
+        speed_scale_factor: f64,
+    ) -> Self {
+        Self {
+            frame_id,
+            stddev_vx,
+            stddev_wz,
+            speed_scale_factor,
+        }
+    }
+
+    pub fn from_params_array(
+        velocity_stddev_xx: Option<f64>,
+        angular_velocity_stddev_zz: Option<f64>,
+        speed_scale_factor: Option<f64>,
+        frame_id: &'static str,
+    ) -> Self {
+        let stddev_vx = velocity_stddev_xx.unwrap_or(0.2);
+        let stddev_wz = angular_velocity_stddev_zz.unwrap_or(0.1);
+        let speed_scale_factor = speed_scale_factor.unwrap_or(1.0);
+
+        Self::new(frame_id, stddev_vx, stddev_wz, speed_scale_factor)
+    }
+
+    pub fn default() -> Self {
+        Self::new("base_link", 0.2, 0.1, 1.0)
+    }
+
+    pub fn convert_velocity_report(&self, msg: &VelocityReport) -> TwistWithCovarianceStamped {
+        TwistWithCovarianceStamped {
+            header: msg.header.clone(),
+            twist: TwistWithCovariance {
+                twist: Twist {
+                    linear: Vector3 {
+                        x: msg.longitudinal_velocity * self.speed_scale_factor,
+                        y: msg.lateral_velocity,
+                        z: 0.0,
+                    },
+                    angular: Vector3 {
+                        x: 0.0,
+                        y: 0.0,
+                        z: msg.heading_rate,
+                    },
+                },
+                covariance: self.create_covariance_matrix(),
+            },
+        }
+    }
+
+    fn create_covariance_matrix(&self) -> [f64; 36] {
+        let mut covariance = [0.0; 36];
+        covariance[0 + 0 * 6] = self.stddev_vx * self.stddev_vx;
+        covariance[1 + 1 * 6] = 10000.0;
+        covariance[2 + 2 * 6] = 10000.0;
+        covariance[3 + 3 * 6] = 10000.0;
+        covariance[4 + 4 * 6] = 10000.0;
+        covariance[5 + 5 * 6] = self.stddev_wz * self.stddev_wz;
+
+        covariance
+    }
+
+    pub fn get_frame_id(&self) -> &'static str {
+        self.frame_id
+    }
+
+    pub fn get_stddev_vx(&self) -> f64 {
+        self.stddev_vx
+    }
+
+    pub fn get_stddev_wz(&self) -> f64 {
+        self.stddev_wz
+    }
+
+    pub fn get_speed_scale_factor(&self) -> f64 {
+        self.speed_scale_factor
+    }
+}
+
+pub mod reactor_helpers {
+    use super::*;
+
+    pub fn create_empty_twist(timestamp: u64) -> TwistWithCovarianceStamped {
+        TwistWithCovarianceStamped {
+            header: Header {
+                frame_id: "base_link",
+                timestamp,
+            },
+            twist: TwistWithCovariance {
+                twist: Twist {
+                    linear: Vector3 {
+                        x: 0.0,
+                        y: 0.0,
+                        z: 0.0,
+                    },
+                    angular: Vector3 {
+                        x: 0.0,
+                        y: 0.0,
+                        z: 0.0,
+                    },
+                },
+                covariance: [0.0; 36],
+            },
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn assert_approx_eq(actual: f64, expected: f64, eps: f64) {
+        assert!(
+            (actual - expected).abs() <= eps,
+            "left: {actual}, right: {expected}"
+        );
+    }
+
+    #[test]
+    fn node_instantiation() {
+        let converter = VehicleVelocityConverter::from_params_array(
+            Some(0.2),
+            Some(0.1),
+            Some(1.0),
+            "base_link",
+        );
+
+        assert_eq!(converter.get_frame_id(), "base_link");
+        assert_eq!(converter.get_stddev_vx(), 0.2);
+        assert_eq!(converter.get_stddev_wz(), 0.1);
+        assert_eq!(converter.get_speed_scale_factor(), 1.0);
+    }
+
+    #[test]
+    fn message_conversion() {
+        let converter = VehicleVelocityConverter::from_params_array(
+            Some(0.2),
+            Some(0.1),
+            Some(1.5),
+            "base_link",
+        );
+        let velocity_report = VelocityReport {
+            header: Header {
+                frame_id: "base_link",
+                timestamp: 1234567890,
+            },
+            longitudinal_velocity: 2.0,
+            lateral_velocity: 0.1,
+            heading_rate: 0.3,
+        };
+
+        let twist_msg = converter.convert_velocity_report(&velocity_report);
+
+        assert_eq!(twist_msg.header.frame_id, velocity_report.header.frame_id);
+        assert_eq!(
+            twist_msg.twist.twist.linear.x,
+            velocity_report.longitudinal_velocity * 1.5
+        );
+        assert_eq!(
+            twist_msg.twist.twist.linear.y,
+            velocity_report.lateral_velocity
+        );
+        assert_eq!(
+            twist_msg.twist.twist.angular.z,
+            velocity_report.heading_rate
+        );
+        assert_approx_eq(twist_msg.twist.covariance[0], 0.2 * 0.2, 1e-12);
+        assert_eq!(twist_msg.twist.covariance[7], 10000.0);
+        assert_eq!(twist_msg.twist.covariance[14], 10000.0);
+        assert_eq!(twist_msg.twist.covariance[21], 10000.0);
+        assert_eq!(twist_msg.twist.covariance[28], 10000.0);
+        assert_approx_eq(twist_msg.twist.covariance[35], 0.1 * 0.1, 1e-12);
+    }
+
+    #[test]
+    fn different_frame_id() {
+        let converter = VehicleVelocityConverter::from_params_array(
+            Some(0.2),
+            Some(0.1),
+            Some(1.0),
+            "base_link",
+        );
+
+        let velocity_report = VelocityReport {
+            header: Header {
+                frame_id: "different_frame",
+                timestamp: 1234567890,
+            },
+            longitudinal_velocity: 2.0,
+            lateral_velocity: 0.1,
+            heading_rate: 0.3,
+        };
+
+        let twist_msg = converter.convert_velocity_report(&velocity_report);
+
+        // As in the original C++ test, conversion still succeeds even with a different frame_id.
+        assert_eq!(twist_msg.header.frame_id, velocity_report.header.frame_id);
+        assert_eq!(
+            twist_msg.twist.twist.linear.x,
+            velocity_report.longitudinal_velocity
+        );
+        assert_eq!(
+            twist_msg.twist.twist.linear.y,
+            velocity_report.lateral_velocity
+        );
+        assert_eq!(
+            twist_msg.twist.twist.angular.z,
+            velocity_report.heading_rate
+        );
+    }
+
+    #[test]
+    fn from_params_array_with_defaults() {
+        let converter = VehicleVelocityConverter::from_params_array(None, None, None, "base_link");
+
+        assert_eq!(converter.get_stddev_vx(), 0.2);
+        assert_eq!(converter.get_stddev_wz(), 0.1);
+        assert_eq!(converter.get_speed_scale_factor(), 1.0);
+    }
+
+    #[test]
+    fn reactor_helpers() {
+        let empty_twist = reactor_helpers::create_empty_twist(1234567890);
+        assert_eq!(empty_twist.header.frame_id, "base_link");
+        assert_eq!(empty_twist.twist.twist.linear.x, 0.0);
+    }
+}
diff --git a/userland/Cargo.toml b/userland/Cargo.toml
index 664ec35f3..222256a19 100644
--- a/userland/Cargo.toml
+++ b/userland/Cargo.toml
@@ -66,6 +66,10 @@ 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
@@ -93,4 +97,5 @@ 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 01a66fe18..7082e1eba 100644
--- a/userland/src/lib.rs
+++ b/userland/src/lib.rs
@@ -46,6 +46,9 @@ 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