Overview

The Extend Kalman Filter Localizer estimates robust and less noisy robot pose and twist by integrating the 2D vehicle dynamics model with input ego-pose and ego-twist messages. The algorithm is designed especially for fast-moving robots such as autonomous driving systems.

Flowchart

The overall flowchart of the autoware_ekf_localizer is described below.

Features

This package includes the following features:

• Time delay compensation for input messages, which enables proper integration of input information with varying time delays. This is important especially for high-speed moving robots, such as autonomous driving vehicles. (see the following figure).
• Automatic estimation of yaw bias prevents modeling errors caused by sensor mounting angle errors, which can improve estimation accuracy.
• Mahalanobis distance gate enables probabilistic outlier detection to determine which inputs should be used or ignored.
• Smooth update, the Kalman Filter measurement update is typically performed when a measurement is obtained, but it can cause large changes in the estimated value, especially for low-frequency measurements. Since the algorithm can consider the measurement time, the measurement data can be divided into multiple pieces and integrated smoothly while maintaining consistency (see the following figure).
• Calculation of vertical correction amount from pitch mitigates localization instability on slopes. For example, when going uphill, it behaves as if it is buried in the ground (see the left side of the “Calculate delta from pitch” figure) because EKF only considers 3DoF(x,y,yaw). Therefore, EKF corrects the z-coordinate according to the formula (see the right side of the “Calculate delta from pitch” figure).

Node

Subscribed Topics

Published Topics

Published TF

• base_link TF from map coordinate to estimated pose.

Functions

Predict

The current robot state is predicted from previously estimated data using a given prediction model. This calculation is called at a constant interval (predict_frequency [Hz]). The prediction equation is described at the end of this page.

Measurement Update

Before the update, the Mahalanobis distance is calculated between the measured input and the predicted state, the measurement update is not performed for inputs where the Mahalanobis distance exceeds the given threshold.

The predicted state is updated with the latest measured inputs, measured_pose, and measured_twist. The updates are performed with the same frequency as prediction, usually at a high frequency, in order to enable smooth state estimation.

Parameter description

The parameters are set in launch/ekf_localizer.launch .

For Node

{{ json_to_markdown(“localization/autoware_ekf_localizer/schema/sub/node.sub_schema.json”) }}

For pose measurement

{{ json_to_markdown(“localization/autoware_ekf_localizer/schema/sub/pose_measurement.sub_schema.json”) }}

For twist measurement

{{ json_to_markdown(“localization/autoware_ekf_localizer/schema/sub/twist_measurement.sub_schema.json”) }}

For process noise

{{ json_to_markdown(“localization/autoware_ekf_localizer/schema/sub/process_noise.sub_schema.json”) }}

note: process noise for positions x & y are calculated automatically from nonlinear dynamics.

Simple 1D Filter Parameters

{{ json_to_markdown(“localization/autoware_ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json”) }}

File truncated at 100 lines see the full file

Changelog for package autoware_ekf_localizer

1.1.0 (2025-05-01)

1.0.0 (2025-03-31)

0.3.0 (2025-03-21)

• chore: fix versions in package.xml
• chore(ekf_localizer): increase z_filter_proc_dev for large gradient road (#211) increase z_filter_proc_dev Co-authored-by: SakodaShintaro <<shintaro.sakoda@tier4.jp>>
• feat(autoware_ekf_localizer)!: porting from universe to core 2nd (#180)
• Contributors: Kento Yabuuchi, Motz, mitsudome-r