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.10.0 (2026-09-28)

• Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base

• test(autoware_ekf_localizer): organize and remove test codes (#1414)

• test(autoware_ekf_localizer): add more unit tests for [update_step()]{.title-ref} (#1412)

• feat(localization): add node designs for the pose/twist estimation nodes (#1409)

  * feat(localization): add node designs for the pose/twist estimation nodes Declare NdtScanMatcher, EkfLocalizer, GyroOdometer, StopFilter and Twist2Accel node designs, with remap_target set to each node's hardcoded topic and service names, and extend PoseInitializer with the map, GNSS, stop-check inputs and the align/trigger/partial-map-load clients that pose_initializer.launch.xml remaps. These let a system designer module compose the localization stack directly from nodes.

  * feat(localization): describe node designs and bump to format 0.4.0 ---------

• fix(autoware_ekf_localizer): fix the conditional check in [push_pose()]{.title-ref} (#1402)

• refactor(autoware_ekf_localizer): core logic isolation - PART 3 - The ADDITIONAL refactoring (#1397)

  • updated EKFUpdateResult struct
  • added new internal state handlers in core header
  • update initialize to flip the flag
  • briefly moved push_pose logic from node to core
  • briefly moved push_twist logic from node to core
  • revamped reset() to activate() with bool flag param
  • update update_step() top package mega struct, including draining queues
  • finalize the wrapping of megastruct inside core
  • purge node wrapper private state variables
  • update publish_estimate_result
  • update node wrapper with reset/activate func to be cleaner
  • update timeer_callback func in node
  • update publish+_estimate_result
  • final fixes - build test all good sucecssfully with good codecov
  • style(pre-commit): autofix
  • attempted to fix the build test agnocast stuff by properly typecasting the msg
  • style(pre-commit): autofix
  • rearrange the order of queue draining blocks in update_step()
  • align yaw bias and odom to other standard passthroughs
  • style(pre-commit): autofix
  • updated update_step() interface to ingest rclcpp::Time current time, then use double time for math stuffs

  * style(pre-commit): autofix ---------Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

• refactor(autoware_ekf_localizer): core logic isolation - PART 2 - The REAL refactoring (#1352)

  • revamped ekf_localizer.hpp header - removed rclcpp timestamps, new corewarnings
  • revamped ekf_localizer.hpp header - removed rclcpp timestamps, new corewarnings
  • revamped ekf_localizer.cpp, get rid of rclcpp time dependency, and adapt new warnings
  • removed the warning dependency inside header of node
  • revamped source of node wrapper, including ROS warnings and time assignment
  • removed friend class declaration of EKFLocalizerDiagnosticsTest for test code (we wont gonna do that here)
  • removed warning mock
  • module test - updated get current pose and twist by excluding time involvement
  • update tests - measurement_update_pose and twist are now comply to new structure
  • added 1 more assertion of no-warning stuffs on each rejection tests
  • removed redundant test_diagnostic.cpp and test_diagnostics_topic.cpp cuz we did all of em in test_ekf_localizer_integration.cpp
  • removed NoOpWhenConstructedWithNullptr module test cuz this concept is dead
  • fix various typo and misfits, now build good and test good, all good, raedy for PR
  • added new struct EKFUpdateResult to header of core logic, containing agg diags and warnings vector

File truncated at 100 lines see the full file