Skip to content

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:

graph TD
    gnss_poser["gnss_poser"] --> |"/sensing/gnss/<br/>pose_with_covariance"| pose_covariance_modifier_node
    ndt_scan_matcher["ndt_scan_matcher"] --> |"/localization/pose_estimator/ndt_scan_matcher/<br/>pose_with_covariance"| pose_covariance_modifier_node

    subgraph pose_covariance_modifier_node ["Pose Covariance Modifier Node"]
        pc1gnss_pose_yaw<br/>stddev
        pc1 -->|"<= 0.3 rad"| pc2gnss_pose_z<br/>stddev
        pc2 -->|"<= 0.1 m"| pc3gnss_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;

class pc1 cl_conditional;
class pc2 cl_conditional;
class pc3 cl_conditional;

class ndt_pose cl_output;
class gnss_pose cl_output;
class gnss_ndt_pose cl_output;

How to use this package#

This package is disabled by default in Autoware, you need to manually enable it.

To enable this package, you need to change the use_autoware_pose_covariance_modifier parameter to true within the pose_twist_estimator.launch.xml.

Without this condition (default)#

  • The output of the ndt_scan_matcher is directly sent to ekf_localizer.
    • It has a preset covariance value.
    • topic name: /localization/pose_estimator/pose_with_covariance
  • The GNSS pose does not enter the ekf_localizer.
  • This node does not launch.

With this condition#

  • The output of the ndt_scan_matcher is renamed
    • from: /localization/pose_estimator/pose_with_covariance.
    • to: /localization/pose_estimator/ndt_scan_matcher/pose_with_covariance.
  • The ndt_scan_matcher output enters the autoware_pose_covariance_modifier.
  • The output of this package goes to ekf_localizer with:
    • topic name: /localization/pose_estimator/pose_with_covariance.

Node#

Subscribed topics#

Name Type Description
input_gnss_pose_with_cov_topic geometry_msgs::msg::PoseWithCovarianceStamped Input GNSS pose topic.
input_ndt_pose_with_cov_topic geometry_msgs::msg::PoseWithCovarianceStamped Input NDT pose topic.

Published topics#

Name Type Description
output_pose_with_covariance_topic geometry_msgs::msg::PoseWithCovarianceStamped Output pose topic. This topic is used by the ekf_localizer package.
selected_pose_type std_msgs::msg::String Declares which pose sources are used in the output of this package
output/ndt_position_stddev std_msgs::msg::Float64 Output pose ndt average standard deviation in position xy. It is published only when the enable_debug_topics is true.
output/gnss_position_stddev std_msgs::msg::Float64 Output pose gnss average standard deviation in position xy. It is published only when the enable_debug_topics is true.

Parameters#

The parameters are set in config/pose_covariance_modifier.param.yaml .

Name Type Description Default Range
threshold_gnss_stddev_yaw_deg_max float If GNSS yaw standard deviation values are larger than this, trust only NDT 0.3 N/A
threshold_gnss_stddev_z_max float If GNSS position Z standard deviation values are larger than this, trust only NDT 0.1 N/A
threshold_gnss_stddev_xy_bound_lower float If GNSS position XY standard deviation values are lower than this, trust only GNSS 0.1 N/A
threshold_gnss_stddev_xy_bound_upper float If GNSS position XY standard deviation values are higher than this, trust only NDT 0.25 N/A
ndt_std_dev_bound_lower float Lower bound value for standard deviation of NDT positions (x, y, z) when used with GNSS 0.15 N/A
ndt_std_dev_bound_upper float Upper bound value for standard deviation of NDT positions (x, y, z) when used with GNSS 0.3 N/A
gnss_pose_timeout_sec float If GNSS data is not received for this duration, trust only NDT 1 N/A
enable_debug_topics boolean Publish additional debug topics 1 N/A

FAQ#

How are varying frequency rates handled?#

The GNSS and NDT pose topics may have different frequencies. The GNSS pose topic may have a higher frequency than the NDT.

Let's assume that the inputs have the following frequencies:

Source Frequency
GNSS 200 Hz
NDT 10 Hz

This package publishes the output poses as they come in, depending on the mode.

End result:

Mode Output Freq
GNSS Only 200 Hz
GNSS + NDT 210 Hz
NDT Only 10 Hz

How and when are the NDT covariance values overwritten?#

Mode Outputs, Covariance
GNSS Only GNSS, Unmodified
GNSS + NDT GNSS: Unmodified, NDT: Interpolated
NDT Only NDT, Unmodified

NDT covariance values overwritten only for the GNSS + NDT mode.

This enables a smooth transition between GNSS Only and NDT Only modes.

In this mode, both NDT and GNSS poses are published from this node.

NDT covariance calculation#

As the gnss_std_dev increases within its bounds, ndt_std_dev should proportionally decrease within its own bounds.

To achieve this, we first linearly interpolate:

  • Base value: gnss_std_dev
  • Base range: [threshold_gnss_stddev_xy_bound_lower, threshold_gnss_stddev_xy_bound_upper]
  • Target range: [ndt_std_dev_bound_lower, ndt_std_dev_bound_upper]
  • Target value: ndt_std_dev_target
  • Final value = ndt_std_dev_bound_lower + ndt_std_dev_bound_upper - ndt_std_dev_target (to get the inverse)

range to range lerp animation