Particle Filter Localization

A C++ ROS 2 package that estimates a mobile robot's planar pose (x, y, yaw) in a known occupancy grid map. It combines wheel odometry with laser range measurements using a particle filter, publishes the estimated pose and particle cloud, and broadcasts the map-to-odometry transform.

The package contains the localization node and filter implementation. A robot or simulator, map publisher, sensor drivers, and their TF transforms must be supplied separately; no launch files, maps, or simulation worlds are included.

How it works

1. Wait for an occupancy grid, odometry, and laser scan, then look up the laser's pose relative to the robot base.
2. Initialize particles around (0, 0, 0) in the global frame, with standard deviations of 0.1 m in x/y and π/4 rad in yaw.
3. Apply an odometry motion model with configurable translation and rotation noise.
4. Sample beams from the scan and ray-cast through the map to calculate expected ranges. Score particles using hit, short-return, maximum-range, and random-return terms.
5. Normalize particle weights and perform systematic resampling when the effective particle count is below the configured particle count.
6. Compute the weighted mean state and publish the pose, particle markers, and TF transform.

The node runs a fixed 30 Hz loop using the latest received messages. Updates are not synchronized to scan timestamps and can reuse the same scan across loop iterations.

Dependencies and build

Use a ROS 2 environment with ament_cmake, colcon, a C++ compiler, and CMake 3.8 or newer. The build requires:

• rclcpp, std_msgs, sensor_msgs, nav_msgs, geometry_msgs, and visualization_msgs
• tf2, tf2_ros, and tf2_geometry_msgs
• Eigen3
• ament_lint_auto and ament_lint_common when building with testing enabled

No specific ROS 2 distribution is declared or verified in this repository. Source your ROS 2 installation before building. Place this repository at ~/ros2_ws/src/pf_localization, then run:

cd ~/ros2_ws
rosdep install --from-paths src --ignore-src -r -y
colcon build --packages-select pf_localization
source install/setup.bash

The current package.xml lists tf2_ros2, while CMakeLists.txt requires tf2_ros, and does not declare Eigen3. If dependency resolution fails, correct these manifest entries or ensure tf2_ros and Eigen3 are installed separately before building. On Ubuntu, Eigen3 is provided by libeigen3-dev.

Run

Start your robot or simulator, map publisher, and TF publishers, then launch the node from a terminal with the workspace sourced:

ros2 run pf_localization pf_localization

The executable is pf_localization; its ROS node name is pf_localization_node. It begins initializing only after all three input topics have delivered a message. A successful initialization logs pf created!.

For a robot using different topic and frame names:

ros2 run pf_localization pf_localization --ros-args \
  -r diff_drive/odometry:=/odom \
  -r diff_drive/scan:=/scan \
  -p base_frame_id:=base_link \
  -p odom_frame_id:=odom \
  -p global_frame_id:=map \
  -p num_particles:=500 \
  -p laser_beams:=16

For simulation or bag playback using /clock, add -p use_sim_time:=true and ensure a clock publisher is running.

Parameter file

Save the following as pf_params.yaml in your working directory:

pf_localization_node:
  ros__parameters:
    base_frame_id: diff_drive/base_link
    odom_frame_id: diff_drive/odom
    global_frame_id: map
    num_particles: 100
    laser_beams: 8
    laser_sigma_hit: 0.2
    laser_z_hit: 0.95
    laser_z_rand: 0.05
    laser_z_short: 0.1
    laser_z_max: 0.05
    laser_lambda_short: 0.1
    odom_alpha1: 0.2
    odom_alpha2: 0.2
    odom_alpha3: 0.2
    odom_alpha4: 0.2

Load it with:

ros2 run pf_localization pf_localization --ros-args --params-file pf_params.yaml

Parameters are read at startup; changing them while the node runs does not reconfigure the filter.

ROS interfaces

Topic names below are relative and resolve under the node's namespace. All subscriptions and publishers use a queue depth of 10 with the default ROS 2 QoS settings.

Subscribed topics

Topic	Message type	Purpose
map	nav_msgs/msg/OccupancyGrid	Known map for predicting laser ranges.
diff_drive/odometry	nav_msgs/msg/Odometry	Robot x/y position and yaw for the motion model.
diff_drive/scan	sensor_msgs/msg/LaserScan	Laser ranges; the first scan also supplies scan geometry and the sensor frame.

Published topics

Topic	Message type	Purpose
pose	geometry_msgs/msg/PoseStamped	Estimated robot pose in global_frame_id, stamped with the node's current time.
particles	visualization_msgs/msg/MarkerArray	Green arrows showing particle positions and headings in global_frame_id.

The pose output does not include covariance.

TF frames

The expected frame tree with default parameters is:

map                              global_frame_id
└── diff_drive/odom               odom_frame_id
    └── diff_drive/base_link      base_frame_id
        └── <laser frame>         LaserScan.header.frame_id

This node broadcasts global_frame_id → odom_frame_id (map → diff_drive/odom by default). Your robot must provide the odometry-to-base and base-to-laser transforms. The node looks up the latest available transforms.

The map-to-odometry transform is calculated as:

T_map_odom = T_map_base × inverse(T_odom_base)

Use only one publisher for this map-to-odometry transform.

Parameters

Distances are in meters and angles are in radians.

Parameter	Default	Meaning
base_frame_id	diff_drive/base_link	Robot base frame used in TF lookups.
odom_frame_id	diff_drive/odom	Odometry frame and child frame of the published TF.
global_frame_id	map	Global frame for the estimated pose, particles, and TF parent.
num_particles	100	Number of pose hypotheses. More particles increase computation.
laser_beams	8	Controls beam subsampling using an index stride of ranges.size() / laser_beams; the actual number of beams evaluated can exceed this value.
laser_sigma_hit	0.2	Width of the Gaussian score for measured versus predicted range.
laser_z_hit	0.95	Weight of the expected-obstacle hit term.
laser_z_rand	0.05	Weight of the uniform random-return term.
laser_z_short	0.1	Weight of unexpected returns shorter than the predicted range.
laser_z_max	0.05	Weight of maximum-range returns.
laser_lambda_short	0.1	Exponential decay rate for short returns.
odom_alpha1	0.2	Rotation noise caused by rotation.
odom_alpha2	0.2	Rotation noise caused by translation.
odom_alpha3	0.2	Translation noise caused by translation.
odom_alpha4	0.2	Translation noise caused by rotation.

The laser weights are used directly as scoring coefficients and are not normalized by the implementation. Keep num_particles > 0, laser_sigma_hit > 0, and odometry noise coefficients nonnegative. Scans need at least two range samples, and laser_beams must satisfy 1 <= laser_beams <= ranges.size() to avoid a zero sampling stride or division by zero. These constraints are not validated by the node.

Visualization and diagnostics

In RViz2, set the fixed frame to global_frame_id (default map) and add:

• Map for /map.
• Pose for /pose.
• MarkerArray for /particles.
• TF to inspect the frame tree.

Adjust topic names if running under a namespace or using remappings. You can inspect the inputs and output from the command line:

ros2 node info /pf_localization_node
ros2 topic echo /pose --once
ros2 topic info /map --verbose
ros2 topic info /diff_drive/scan --verbose
ros2 run tf2_ros tf2_echo diff_drive/odom diff_drive/base_link

If no pose appears, confirm that all input topics have delivered messages and that base-to-laser TF is available. Repeated Could not transform warnings during initialization indicate a missing or mismatched laser transform. The odometry-to-base lookup during pose publication is not caught; a missing transform at that stage can terminate the node.

Check QoS compatibility if topics exist but messages do not arrive. The default reliable subscriptions may not connect to best-effort sensor publishers. The map subscription is volatile, so it does not request a retained map from a transient-local publisher; ensure a map is published after the node starts or adapt the subscription QoS.

Current assumptions and limitations

• Initialization is local around the global origin. There is no /initialpose subscriber, configurable initial pose, or global particle initialization. The previous odometry state also starts at zero, so the first odometry reading is treated as motion from that origin.
• The occupancy grid is converted and cached on the first localization update; later map changes are not incorporated into the filter. Only cells with occupancy value 0 are considered free; unknown and nonzero cells block ray casting.
• Map coordinates use the map origin's translation and resolution, but ignore its orientation. Use an axis-aligned map whose coordinates agree with global_frame_id.
• Scan angles and maximum range are captured at filter creation. The filter assumes evenly spaced scan angles and does not explicitly filter invalid readings or use range_min.
• Pose yaw is calculated with an arithmetic weighted mean, which can give an incorrect result when particle headings straddle -π and π.
• The repository provides lint integration through ament_lint_auto, but no dedicated algorithm tests or scripts to reproduce the plots below.

Repository structure

pf_localization/
├── CMakeLists.txt                 Build and installation rules
├── package.xml                    ROS 2 package metadata
├── include/pf_localization/       Public node, filter, and utility headers
├── src/
│   ├── main.cpp                   ROS initialization and node entry point
│   ├── pf_localization.cpp        Topics, parameters, TF, and visualization
│   ├── particle_filter.cpp        Motion model, laser scoring, and resampling
│   └── utils.cpp                  Pose conversions, grid conversion, and ray casting
└── images/                        Example trajectory, particle cloud, and error plots

The build produces the pf_localization executable and the shared libraries pf_localization_core, particle_filter, and pf_utils.

Example plots

These images are included in the repository as example results; the dataset and commands used to generate them are not included.

Trajectory

Particle clouds

Localization error