EKF Localizer
EKF Localizer
EKF Localizer is the core fusion node of the localization system. It receives low-frequency absolute poses from localization sources like NDT and high-frequency relative motion from Gyro Odometer, outputting smooth, high-frequency (50 Hz) kinematic states through Extended Kalman Filtering.
EKF Multi-sensor Fusion Diagram
State Vector & Motion Model
State vector x = [x, y, yaw, yaw_bias, v_x, w_z]ᵀ — 6-dimensional state including position, heading, heading bias, longitudinal velocity, and yaw rate.
x(k+1) = x(k) + v_x(k) * cos(yaw(k)) * dt
y(k+1) = y(k) + v_x(k) * sin(yaw(k)) * dt
yaw(k+1) = yaw(k) + w_z(k) * dt
yaw_bias(k+1) = yaw_bias(k)
v_x(k+1) = v_x(k)
w_z(k+1) = w_z(k)Algorithm Flow
EKF Localizer Predict-Update Cycle
Key Mechanisms
Mahalanobis Gating: Computes the Mahalanobis distance of the innovation vector d² = yᵀ S⁻¹ y. Measurements exceeding the threshold are rejected. This is critical for preventing pose jumps caused by NDT mismatches in repetitive scenarios.
Delay Compensation uses a fixed-lag smoothing approach. The EKF maintains a historical state queue (default 50 steps). When a measurement with a past timestamp is received, it is applied to the corresponding historical state, then the state is recomputed to the current time.
Smooth Update divides a single measurement into multiple small steps for gradual fusion, preventing state jumps from low-frequency measurements.
ROS 2 Interfaces
| Direction | Topic | Message Type | Description |
|---|---|---|---|
Sub | measured_pose_with_covariance | PoseWithCovarianceStamped | Absolute pose from NDT etc. |
Sub | measured_twist_with_covariance | TwistWithCovarianceStamped | Twist from Gyro Odometer |
Sub | initialpose | PoseWithCovarianceStamped | Initial pose |
Pub | ekf_odom | Odometry | Fused odometry |
Pub | ekf_pose | PoseStamped | Fused pose |
Pub | ekf_pose_with_covariance | PoseWithCovarianceStamped | Fused pose (with covariance) |
Pub | ekf_twist | TwistStamped | Fused velocity |
Pub | ekf_twist_with_covariance | TwistWithCovarianceStamped | Fused velocity (with covariance) |
TF | map → base_link | — | Coordinate transform |
Key Parameters
| Parameter | Default | Description |
|---|---|---|
predict_frequency | 50.0 Hz | Prediction frequency |
extend_state_step | 50 | Delay compensation history steps |
enable_yaw_bias_estimation | true | Enable heading bias estimation |
pose_smoothing_steps | 5 | Pose smoothing steps |
twist_smoothing_steps | 2 | Twist smoothing steps |
pose_gate_dist | 49.5 | Pose Mahalanobis distance gate |
twist_gate_dist | 46.1 | Twist Mahalanobis distance gate |
proc_stddev_vx_c | 10.0 | Longitudinal acceleration process noise |
proc_stddev_wz_c | 5.0 | Angular acceleration process noise |
proc_stddev_yaw_c | 0.005 | Heading process noise |