Monte-Carlo-Localization-Sim-ROS2

This repository implements a modular and robust Monte Carlo Localisation (Particle Filter) system for an Autonomous Mobile Robot (AMR). This code was developed as part of a broader, integrated navigation framework for the Master of Autonomous Systems program at Hochschule Bonn-Rhein-Sieg. While the full team project included hardware deployment and full-stack path planning/exploration, this specific repository and default branch highlight my individual contributions: developing and tuning the Monte Carlo Localization (Particle Filter) stack within a ROS2 simulation environment.

Output

Particle Filter Output Simulation

Random Particle Injection Rviz Output

My Contributions (Sai Mukkundan Ramamoorthy)

I was responsible for designing and implementing the probabilistic localization system in simulation. My core deliverables included:

• Algorithm Design: Implemented a Monte Carlo Localization (Particle Filter) system from scratch.
• Motion Modeling: Developed both linear and circular motion models to accurately predict curved trajectories when angular velocity is significant.
• Sensor Modeling: Engineered a combined Likelihood Field and Ray-Casting sensor model using adaptive ray subsampling to reduce computational load.
• Spatial Optimization: Implemented optimized KNN-based distance retrieval using Ball Tree spatial indexing to identify obstacle distances rapidly within the occupancy grid.
• Numerical Stability: Utilized log-likelihood calculations to prevent numerical underflow during particle weight updates.

Joint Team Effort

While this repository focuses on my simulation-based localization work, the complete AMR project was a joint effort.

Team Members & Roles:

• Sai Mukkundan Ramamoorthy (Me): Particle Filter Localisation (Simulation), Report generation.
• Sunesh Praveen Raja Sundarasami: Path and Motion Planning, Hardware Deployment, Environment Exploration, Localisation HW Integration.

Note: Because my specific ray-casting and velocity-based motion models were heavily optimized for the simulation environment, the final hardware deployment on the physical Robile utilized a modified configuration. To allow reviewers to easily test my original simulation algorithms without hardware dependency conflicts, I have included a dedicated sim_localization.launch.py file in this branch.

Localisation Features (code explained in detail)

1. Particle Filter (Monte Carlo Localisation)

• Node Core: Implemented in particle_filter.py. Manages a cloud of particles representing the probability distribution of the robot's pose (x, y, theta).
• Initialization: Initializes the particle cloud based on RViz /initialpose commands via geometry_msgs/PoseWithCovarianceStamped.
• Motion Update: Updates particle positions dynamically using movement data and incorporates odometry variance checks.
• Resampling: Filters particles iteratively based on internal weights, eliminating low-probability states and concentrating computation on likely locations (resample_particles).

2. Sensor Models & Map Interface

• Occupancy Field Model (occupancy_field.py): Fetches the grid map from the continuous ROS map_server and processes obstacles into a fast Nearest Neighbors algorithm (sklearn's BallTree). This guarantees an extremely fast $O(\log n)$ query time for fetching the distance to the closest obstacle.
• Likelihood Field System: Extracts LaserScan endpoints, transforms them into map space for a given particle, and evaluates weights by querying the OccupancyField for matching walls/obstacles.
• Ray-Casting Evaluator: Contains a secondary ray-casting evaluator (evaluate_particle_with_ray_casting_optimized) for dense checking of expected ranges.

3. Coordinate Tracking & TF Integration

• TFHelper (helper_functions.py): Seamlessly manages ROS 2 tf2 functionality. Safely translates LaserScan polar matrices into base_link Cartesian shapes, handling angle normalization globally.
• Map->Odom Broadcasting: Dynamically closes the tree by generating and broadcasting the map to odom corrective translation based on the Particle Filter's best pose estimate.

Technologies Used

• Platform: Robile robot
• Middleware: ROS2 (Robot Operating System) and Gazebo Simulator
• Libraries:
  • NetworkX (for graph-based path planning)
  • NumPy (for efficient numerical operations)
  • scikit-learn (for DBSCAN clustering)
  • Ball Tree (for spatial indexing)

Repository Structure

Monte-Carlo-Localization-Sim-ROS2/
│
├── clearance_maps/                 # Pre-generated safety maps and clearance data
│
├── environment_exploration/        # Autonomous exploration and frontier detection
│   ├── frontier_explorer.py        # Main frontier exploration node
│   └── utils/                      # Utilities for detection, clustering, and goal selection
│
├── launch/                         # ROS2 launch files
│   ├── motionandpathplanner.launch.py
│   └── sim_localization.launch.py
│
├── localisation/                   # ⭐ Sai's implementation of particle filter
│   ├── particle_filter.py          # Monte Carlo localization implementation with linear & circular
│   │                               # motion models, adaptive likelihood, and raycasting sensor models
│   ├── occupancy_field.py          # Optimised KNN based distance retrieval to identify obstacle distance
│   └── helper_functions.py         # Code to perform ROS2 TF transformations
│
├── motion_and_path_planning/       # Motion control and global/local path planning functionalities
│   ├── a_star_path_planner.py      # A* algorithm for global planning
│   ├── potential_field_implementation.py # Potential field for local obstacle avoidance
│   ├── pose_execuetor.py           # Executes the poses
│   ├── path_visualizer.py          # Renders planned paths in RViz
│   └── voronoi_path_planner.py     # Voronoi based planning
│
├── my_map/                         # SLAM generated maps (metadata & db3)
│
├── rviz_config/                    # RViz visualization configurations
│   └── robile_nav.rviz
│
├── srv/                            # ROS2 Custom Service definitions
│   └── GetStatus.srv
│
├── particle_filter_output_simulation.png # Localization simulation output visualization
├── random particle injection rviz output.png # Random particle injection visualization
├── AMR_Autonomous_navigation_exploration_localisation.pdf # Project report documentation
├── package.xml                     # ROS2 package configuration
├── setup.py                        # Python package setup
└── README.md                       # Project documentation

Branch Strategy

To evaluate the most effective localization strategy for the Robile platform in simulation, I developed and compared two distinct approaches. Both are available in this repository:

• velocity_based_particle_filter (Default Branch): My most optimized implementation. Uses a velocity-based motion model and a hybrid sensor model combining likelihood fields with adaptive raycasting.
• odom_based_particle_filter: An alternative implementation utilizing an odometry-based motion model and a pure likelihood field sensor model.

Note: The main branch contains the finalized team integration for the physical hardware deployment (path planning, exploration, etc.) for full context.

Getting Started

To get started with the project, follow these steps:

1. Clone the repository (Default branch: velocity_based_particle_filter):

   git clone -b velocity_based_particle_filter https://github.com/saiga006/Monte-Carlo-Localization-Sim-ROS2.git
   cd Monte-Carlo-Localization-Sim-ROS2

2. Install ROS2 and dependencies: Ensure you have ROS2 installed. Then, install the dependencies:

   rosdep install --from-paths src --ignore-src -r -y

3. Build the workspace:

   colcon build
   source install/setup.bash

4. Launch the localization sub-system within the robot's simulation stack:

ros2 launch amr_project_amr_t04 sim_localization.launch.py

Technical Implementation Details (Localization)

State-Space and Motion Models

The particle filter estimates the robot's pose by maintaining a set of weighted particles. I implemented dual motion models to handle different movement patterns:

Linear Motion Model: $$x_t = x_{t-1} + v_x \cdot \cos(\theta_{t-1}) \cdot \Delta t - v_y \cdot \sin(\theta_{t-1}) \cdot \Delta t + \epsilon_x$$ $$y_t = y_{t-1} + v_x \cdot \sin(\theta_{t-1}) \cdot \Delta t + v_y \cdot \cos(\theta_{t-1}) \cdot \Delta t + \epsilon_y$$ $$\theta_t = \theta_{t-1} + \omega \cdot \Delta t + \epsilon_\theta$$

Circular Motion Model: Utilized when angular velocity is significant ($\omega \neq 0$) to provide accurate curved trajectory predictions.

Sensor Models & Optimization

To evaluate particle weights against incoming laser scan data, I utilized a Likelihood Field Model:

$$P(z_t \mid x_t) = \prod_{k=1}^K [z_{hit} \cdot p_{hit}(z_t^k \mid x_t) + z_{rand} \cdot p_{rand}(z_t^k \mid x_t)]$$

To achieve real-time performance in simulation, I optimized the sensor updates using:

1. Adaptive Ray Subsampling: Finer sampling near the robot and coarser sampling at longer ranges.
2. Efficient Distance Queries: Utilizing the Ball Tree algorithm for $O(\log n)$ complexity during nearest-neighbor lookups on the occupancy field.

References

1. Understanding the Particle Filter | Autonomous Navigation, Part 2 of MATLAB series
2. Particle Filter Explained without Equations from Uppsala University
3. D. Fox et al., "Monte carlo localization: Efficient position estimation for mobile robots," Proceedings of the National Conference on Artificial Intelligence, pp. 343–349, 1999.
4. F. Gustafsson, ‘Particle Filter Theory and Practice with Positioning Applications’, IEEE Aerospace and Electronic Systems Magazine, vol. 25, no. 7, pp. 53–81, 2010.

Acknowledgments

The localisation code implementation is specifically adapted based on the work of Amy Phung and the Olin College Computational Robotics module assignment. We are highly appreciative of their open-source educational materials documenting the framework:

• Amy Phung's Particle Filter Documentation
• Olin College CompRobo: Robot Localization Assignment

Additionally, we acknowledge the support of teaching assistant Anudeep Sai Akula in setting up the Robile platform and guiding us in case of any issues.