Neato Particle Filter

Accurate self-localization is one of the most fundamental challenges in robotics. To function autonomously, a robot must know where it is in its environment. This project addressed the complex problem of determining a robot's precise position and orientation using noisy sensor data and imperfect odometry measurements.

The localization challenge involved handling inherent noise and inaccuracies in sensor measurements, compensating for odometry drift that accumulates over time, and developing algorithms that can process sensor data in real-time. Additionally, the system needed to effectively estimate the robot's position and orientation (pose), implement efficient particle distribution and resampling strategies, and correctly integrate multiple coordinate frames (map, odom, baselink) for accurate spatial reasoning.

We implemented a particle filter algorithm that solved the localization problem through a probabilistic approach. The system initialized particles (potential robot positions) randomly distributed across the map, used odometry data to update particle positions as the robot moved, and assigned weights to particles based on how well they matched actual LIDAR readings. The algorithm implemented intelligent resampling to concentrate particles in likely locations, calculated the robot's estimated position by averaging weighted particle positions, and provided an interface for manual position resets when necessary.

The particle filter works through an iterative process of prediction and correction. As the robot moves, each particle's position is updated based on odometry data, while the filter simulates LIDAR readings for each particle position on the map. These simulated readings are compared with actual sensor data, and particles with readings similar to actual data receive higher weights. During resampling, particles with higher weights are more likely to be selected, and this process continues until particles converge around the most probable location.

Our particle filter implementation followed a structured approach to localization. The filter began by generating a set of randomly distributed particles across the map, with a higher concentration around the initial estimate of the robot's position. Each particle represented a potential pose (position and orientation) of the robot. As the robot moved through its environment, we used data from the odometry sensor to update the position of each particle, simulating how the particles would move if they were the actual robot while incorporating realistic motion noise to account for sensor inaccuracies.

For each particle, we simulated what the robot's LIDAR scan would look like if the robot were at that particle's position. By comparing these simulated scans with the actual LIDAR readings from the robot, we assigned weights to each particle based on how closely the simulated and actual scans matched. After assigning weights to all particles, we implemented a resampling step where particles were selected with probability proportional to their weights, meaning that particles with higher weights were more likely to be selected for the next iteration.

The final estimate of the robot's pose was calculated by taking a weighted average of all particle positions, providing a continuous, smooth estimate of the robot's location within the map. We also implemented functionality to "reset" the filter with a new initial pose estimate when necessary, using RViz to provide a new initial pose represented as a green arrow to re-initialize the particle distribution. The entire process was visualized in RViz, showing the particle cloud, laser scans, and estimated robot position in real-time, which was crucial for debugging and demonstrating the effectiveness of the algorithm.

The particle filter algorithm demonstrated successful tracking in various environments, with particles converging to the robot's actual position and effective handling of sensor noise and uncertainty. The system provided smooth pose estimation with minimal jumps and maintained real-time performance suitable for navigation tasks. The algorithm's structure involved initiating random particles in the map frame, updating particles using odometry data from the odom frame, and adjusting particle weights based on laser scan agreement in the baselink frame.

The system's frame integration managed multiple coordinate systems effectively, with the map frame serving as the global reference for the environment, the odom frame tracking the robot's traveled distance (though prone to drift), and the baselink frame representing the robot's local coordinate system. Transform management between all coordinate frames, coordinate conversions for sensor data processing, and publication of corrected transforms to ROS2 ensured accurate spatial reasoning throughout the localization process.

The particle filter implementation achieved accurate localization in predefined maps, serving as a foundation for autonomous navigation systems and providing the basis for path planning and obstacle avoidance. The system demonstrated effective recovery capability through manual pose resets when necessary, ensuring robust operation even when the filter lost track of the robot's position.

This project served as both a platform for further robotics research and a demonstration of probabilistic robotics principles. The implementation provided an educational tool for understanding localization algorithms while showcasing the practical application of advanced mathematical concepts in real-world robotics systems. The successful integration with ROS2 and visualization tools made the algorithm accessible for debugging, demonstration, and further development.