easynav_mhamcl_localizer

Description

Multi-Hypothesis AMCL (MH-AMCL) localizer over a Costmap2D map. It is the EasyNav port of mh_amcl, described in Portable Multi-Hypothesis Monte Carlo Localization for Mobile Robots (A. García, F. Martín, J. M. Guerrero, F. J. Rodríguez and V. Matellán, ICRA 2023).

Instead of a single particle filter, it keeps a set of them (hypotheses) about the pose of the robot:

• Start: the first hypothesis starts at initial_pose (or the pose sent to initialpose).
• Creation: every 1 / hypotheses_freq seconds, a cascade map matching looks in the whole map for the poses from which the last perception could have been obtained. The map is stored in a pyramid of resolutions (each level halves the previous one). The coarsest level is scanned entirely with a fixed angular step, and only the promising cells are refined in the finer levels. A new hypothesis starts at every candidate that is far enough from the existing hypotheses.
• Destruction: a hypothesis is removed if it is out of the free space of the map or its quality is too low.
• Merge: hypotheses that converge to the same pose are merged.
• Output: the pose (and covariance) of the hypothesis with the best quality. Another hypothesis takes over only if it is clearly better than the current one.

The quality of a hypothesis is the best fraction of the last perception that falls on an obstacle of the map from any of its particles. It describes how well a hypothesis explains what the robot sees much better than the covariance does.

This allows to localize the robot without knowing where it is and to recover from kidnapping or wrong estimates.

Every hypothesis is a regular particle filter with the phases run independently:

Reseed also adapts the number of particles of each hypothesis in [min_particles, max_particles]: it grows when the quality is low and shrinks when it is high.

Differences with the Nav2 version

• The observation are the fused PointPerceptions of NavState (like the other EasyNav localizers) instead of a LaserScan. Every point is seen along the ray from the robot to it.
• The map is the map.base Costmap2D of NavState, not an OccupancyGrid topic.
• The odometry is not read from its own subscription: it is the odometry perception of easynav_sensors (OdometryPerceptionHandler), falling back to odom -> base_footprint in the RTTFBuffer. TF and the initial pose are handled like easynav_costmap_localizer.
• The map matching refines the candidates down to the original resolution and runs in a background thread. The candidates are moved with the odometry received meanwhile.
• Fixes with respect to the original implementation: parents in reseed are really selected among the winners, reseed noise is a standard deviation and not a variance, hypotheses are removed and merged safely, and new hypotheses start with the quality of their candidate.

Authors and Maintainers

• Authors: Intelligent Robotics Lab
• Maintainers: Francisco Martín Rico fmrico@gmail.com

Supported ROS 2 Distributions

Distribution	Status
humble
jazzy
kilted
lyrical
rolling

Plugin (pluginlib)

• Plugin Name: easynav_mhamcl_localizer/MHAMCLLocalizer
• Type: easynav::mhamcl::MHAMCLLocalizer
• Base Class: easynav::LocalizerMethodBase
• Library: easynav_mhamcl_localizer
• Description: Multi-Hypothesis AMCL localizer over a Costmap2D map.

See config/example_params.yaml. To feed the odometry as a perception, add a sensor to sensors_node:

sensors_node:
  ros__parameters:
    sensors: [laser1, odom]
    odom:
      topic: odom
      type: nav_msgs/msg/Odometry

Parameters

All parameters are declared under the plugin namespace, i.e., /<node_fqn>/easynav_mhamcl_localizer/MHAMCLLocalizer/....

Initial pose

File truncated at 100 lines see the full file

Changelog for package easynav_mhamcl_localizer

0.5.0 (2026-10-08)

• First release: Multi-Hypothesis AMCL
• Configurable min_height (was a fixed 0.1 m)
• Builds on Humble, Jazzy, Kilted, Lyrical and Rolling
• Contributors: Francisco Martín Rico