Particle Drift
For my grade 12 final, feeling ambitious, my friend and I decided to build a robot that maps rooms from scratch. Like a Roomba, but just the mapping part, not the cleaning. He knew hardware, so he built the car. I wrote the SLAM algorithm in Java, and we put the two together.
Here's how it works. The map is an occupancy grid. Each cell has a probability of containing something: a wall, furniture, whatever. Say the LiDAR reads 100 cm at some angle. The cells along that beam become less likely to be occupied, and the cell at 100 cm becomes more likely. You know, because there's something there.
I stored those probabilities as log odds, so each observation adds to the evidence already in the map. Using logarithms to represent probability is pretty clever. This is where I learned about that.
But where is the car in that map? Encoder readings aren't perfect, so I used a particle filter to keep several possible positions and headings. Each movement updates those guesses with some Gaussian noise. I ray-cast from each guessed position into the map, compare the predicted distances with the actual LiDAR readings, and give better matches more weight. Weighted resampling carries the better explanations into the next update.
We built a simulation to test it, then tried it on the hardware as well. I learned a lot about microcontrollers, networking and C++ hardware programming along the way.