This is what consumed the last three weeks of my life.
Green line: The motion if there was no noise - sort of like in orienteering, if your pace and bearing were correct.
Blue line: The actual path (movement is noisy). Very much like orienteering!
Cyan line: The true bearing to the marker/beacon
Red line: What my little guy gets as a measurement to the beacon (ie his measurement is noisy) - again a lot like orienteering
The red and blue circles around the little guy are tolerance bands on the estimate of the true position.
You can see this particular style makes a mistake on the second pass by #5 when it's really close. This is the correct behavior for this implementation (ie I didn't screw it up, the algorithm from the book has room for improvement).
Then after that one got working, I had to do two more different ones. Then answer a series of questions and prepare innumerable plots.
If you have one misplaced apostrophe, or mix up sine and cosine, it doesn't work, and you spend forever scanning your code for it. The most common problem we had was something like:
What's the average of a 360° angle and a 0° angle? Well, those both point to the same place, which is, for example, north, but if you add the numbers and divide by 2, you get 180 and report back "south", which is wrong if you are talking position orientation. So you have to minimize the angle (lop off any extra 360s), or break it up into sine and cosine and then arc tangent it back. Good times.
Next up, we don't get predefined markers. We'll encounter new markers, have to add them to our world, and then we won't know which marker belongs with each measurement.
One of the videos I submitted on Wednesday: