Particle Filter Cluster

Probabilistic State Estimation

For autonav 2024, following the success of partially rewriting our software in C++, I decided to do the same for our localization algorithm. In this case a particle filter, where each particle is a guess at the robots position and heading. The particle filter uses encoder data to update each state estimate, and adjusts their weighted contribution to the actual reported value based on GPS data. This way, we avoid the natural drift of pure odometry while retaining its higher resolution and local accuracy.

Demonstration in our simulator

Simulator Demonstration

One great thing about SCR is the wonderful work our alumni and members have done putting together a realistic simulator. Besides helping us prepare for competition, it also allows us to create great demos of any of our projects. In the example above, I've set the model of our 2024 IGVC bot "Danger Zone" to autonomously follow a set of GPS waypoints through the course, localizing itself with the particle filter.

In the overlay on screen, you'll see three plots. One is just the GPS data received for the entire course. This is just to show where the robot actually was. Each blue dot is its own particle within the particle filter, which corresponds to an individual state estimate. The purple dot is the average of these individual particles. In reality, the weights of each particle vary quite a bit, so the plotted average isn't an exact match of the estimated state. Still, the plot communicates the essence of the particle filters behavior throughout the course, and often the weighted average and true average correspond.

If the overlay is a bit difficult to see in the video, I've included the plot by itself below. This should give a better view of what exactly is going on during the course.

Data from Simulator Run

Test Driven Development

Another one of my goals while developing this project was to get comfortable using a testing framework. I'd become interested in a testing framework as a way of reducing the impulse not to refactor when it made sense. Within larger projects I'd caused myself quite a few headaches where refactoring would have been nice but ensuring that functionality remained the same was quite difficult.

Google Test is integrated with ROS2 for C++ development, so I developed my tests there. The framework is pretty simple, and it allowed me to write tests for individual methods that sped up my development when I made refactors later. This was especially helpful near competition time, when I needed to rework the particle filter to accept information from our configuration system. With the tests I had written, all I had to do was refactor and ensure that my code still passed all my tests. The freedom to make changes like this has really paid off in my experience, so I will probably include testing in any large project I work on in the future.

Google Test Output

Just Some Fun

I made this video after accidentally plotting the output of the particle filter without fixing the axes. I think watching the algorithm move is quite beautiful, so I put it to some music and thought it was nice to look at. This lets you see what the particle filter is doing at any given moment a little more clearly, though a lot of the beauty is in how chaotic it is. The hue adjustment is just a fun touch. Music for this and the simulator demonstration are in the youtube links for those embeds.