Autoware Pose Covariance Modifier Node

Purpose

This package makes it possible to use GNSS and NDT poses together in real time localization.

Function

This package takes in GNSS (Global Navigation Satellite System) and NDT (Normal Distribution Transform) poses with covariances.

It outputs a single pose with covariance:

• Directly the GNSS pose and its covariance.
• Directly the NDT pose and its covariance.
• Both GNSS and NDT poses with modified covariances.

• This package doesn’t modify the pose information it receives.
• It only modifies NDT covariance values under certain conditions.

Assumptions

• The NDT matcher provides a pose with a fixed covariance.
• The NDT matcher is unable to provide a dynamic, reliable covariance value.

Requirements

• The GNSS/INS module must provide standard deviation values (its error / RMSE) for the position and orientation.
• It probably needs RTK support to provide accurate position and orientation information.
• You need to have a geo-referenced map.
• GNSS/INS module and the base_link frame must be calibrated well enough.
• In an environment where GNSS/INS and NDT systems work well, the base_link poses from both systems should be close to each other.

Description

GNSS and NDT nodes provide the pose with covariance data utilized in an Extended Kalman Filter (EKF).

Accurate covariance values are crucial for the effectiveness of the EKF in estimating the state.

The GNSS system generates reliable standard deviation values, which can be transformed into covariance measures.

But we currently don’t have a reliable way to determine the covariance values for the NDT poses. And the NDT matching system in Autoware outputs poses with preset covariance values.

For this reason, this package is designed to manage the selection of the pose source, based on the standard deviation values provided by the GNSS system.

It also tunes the covariance values of the NDT poses, based on the GNSS standard deviation values.

Flowcharts

Without this package

Only NDT pose is used in localization. GNSS pose is only used for initialization.

graph TD
    ndt_scan_matcher["ndt_scan_matcher"] --> |"/localization/pose_estimator/pose_with_covariance"| ekf_localizer["ekf_localizer"]

classDef cl_node fill:#FFF2CC,stroke-width:3px,stroke:#D6B656;

class ndt_scan_matcher cl_node;
class ekf_localizer cl_node;

With this package

Both NDT and GNSS poses are used in localization, depending on the standard deviation values coming from the GNSS system.

Here is a flowchart depicting the process and the predefined thresholds:

```mermaid graph TD gnss_poser[“gnss_poser”] –> |”/sensing/gnss/
pose_with_covariance”| pose_covariance_modifier_node ndt_scan_matcher[“ndt_scan_matcher”] –> |”/localization/pose_estimator/ndt_scan_matcher/
pose_with_covariance”| pose_covariance_modifier_node

subgraph pose_covariance_modifier_node ["Pose Covariance Modifier Node"]
    pc1{{"gnss_pose_yaw<br/>stddev"}}
    pc1 -->|"<= 0.3 rad"| pc2{{"gnss_pose_z<br/>stddev"}}
    pc2 -->|"<= 0.1 m"| pc3{{"gnss_pose_xy<br/>stddev"}}
    pc2 -->|"&gt; 0.1 m"| ndt_pose("NDT Pose")
    pc3 -->|"<= 0.1 m"| gnss_pose("GNSS Pose")
    pc3 -->|"0.1 m < x <= 0.2 m"| gnss_ndt_pose("`Both GNSS and NDT Pose
    (_with modified covariance_)`")
    pc3 -->|"&gt; 0.2 m"| ndt_pose
    pc1 -->|"&gt; 0.3 rad"| ndt_pose
end

pose_covariance_modifier_node -->|"/localization/pose_estimator/pose_with_covariance"| ekf_localizer["ekf_localizer"]

classDef cl_node fill:#FFF2CC,stroke-width:3px,stroke:#D6B656; classDef cl_conditional fill:#FFE6CC,stroke-width:3px,stroke:#D79B00; classDef cl_output fill:#D5E8D4,stroke-width:3px,stroke:#82B366;

class gnss_poser cl_node; class ndt_scan_matcher cl_node; class ekf_localizer cl_node; class pose_covariance_modifier_node cl_node;

File truncated at 100 lines see the full file

Changelog for package autoware_pose_covariance_modifier

0.46.0 (2025-06-20)

• Merge remote-tracking branch 'upstream/main' into tmp/TaikiYamada/bump_version_base
• feat(autoware_ndt_scan_matcher)!: port the package to Autoware Core (#10688)
• Contributors: Ryohsuke Mitsudome, TaikiYamada4

0.45.0 (2025-05-22)

0.44.2 (2025-06-10)

0.44.1 (2025-05-01)

0.44.0 (2025-04-18)

0.43.0 (2025-03-21)

• Merge remote-tracking branch 'origin/main' into chore/bump-version-0.43
• chore: rename from [autoware.universe]{.title-ref} to [autoware_universe]{.title-ref} (#10306)
• Contributors: Hayato Mizushima, Yutaka Kondo

0.42.0 (2025-03-03)

• Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base
• feat(autoware_ekf_localizer)!: porting from universe to core 2nd (#10067) Co-authored-by: Ryohsuke Mitsudome <<43976834+mitsudome-r@users.noreply.github.com>>
• Contributors: Fumiya Watanabe, Motz

0.41.2 (2025-02-19)

• chore: bump version to 0.41.1 (#10088)
• Contributors: Ryohsuke Mitsudome

0.41.1 (2025-02-10)

0.41.0 (2025-01-29)

• revert: revert "feat(autoware_ekf_localizer)!: porting from universe to core (#9978)" (#10004) This reverts commit 037c315fbee69bb5923ec10bb8e8e70f890725ea.

• feat(autoware_ekf_localizer)!: porting from universe to core (#9978)

  • feat: delete ekf_localizer files
  • doc: Modify ekf_localizer directory links

  * ci: remove ekf_localizer from the codecov target list ---------

• Contributors: Motz, Ryohsuke Mitsudome

0.40.0 (2024-12-12)

• Merge branch 'main' into release-0.40.0

• Revert "chore(package.xml): bump version to 0.39.0 (#9587)" This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5.

• fix: fix ticket links in CHANGELOG.rst (#9588)

• chore(package.xml): bump version to 0.39.0 (#9587)

  • chore(package.xml): bump version to 0.39.0
  • fix: fix ticket links in CHANGELOG.rst

  * fix: remove unnecessary diff ---------Co-authored-by: Yutaka Kondo <<yutaka.kondo@youtalk.jp>>

• fix: fix ticket links in CHANGELOG.rst (#9588)

• fix(cpplint): include what you use - localization (#9567)

• 0.39.0

• update changelog

• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)

• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe

File truncated at 100 lines see the full file