Half a year after our terrible performance at SparkFun's AVC competition, I've analyzed the data collected during one of the runs and have figured out the mistakes I made in the navigation software. I had previously tried looking at the data, but needed to be able to visualize it to make any sense of it all. My first step was to generate a KML file from the data collected during the run, showing the planned waypoints as well as the recorded GPS locations. I then imported the KML file into Google maps so I could see the route taken by the robot.
The blue stars indicate the planned waypoints. The red pins show the actual route of the vehicle. This seems accurate and shows that the vehicle took a sharp right turn after the first corner and seemed to be heading for the wrong waypoint. The placemarks on the map are annotated with notes including the compass bearing and target bearing and shows that the navigation software had calculated a bearing of 49 degrees. The robot wasn't even accurately keeping to that bearing, but that's another story.
Next, I found an online calculator for calculating the bearing between latitude and longitude locations and entered one of the GPS co-ordinates and the target waypoint. This calculated an angle of 41 degrees instead and mapped out the route on Google maps too, which confirmed that this was correct route.
Clearly, I had an error in my math. Because I was running on an Arduino microcontroller with limited computing power, I had chosen to implement a simple algorithm based on trigonometry, using the delta of latitude and longitude between the two points to form a right-angled triangle and then used the atan() function to calculate the angle. This was a huge mistake and didn't take into account that at a latitude of 40 degrees, one degree of latitude is 68.99 miles, whereas one degree of longitude is 53.06 miles. After modifying the simple trig math to scale the latitude and longitude deltas accordingly, the calculations become correct, within a reasonable tolerance.
UPDATE: I have now run simulations of one million random pairs of latitude/longitude co-ordinates within the bounds of SparkFun's parking lot to compare the results of the modified simple algorithm versus the accurate and more expensive algorithm and the difference between the two algorithms never exceeds 0.1 degrees.
This wasn't the only issue affecting our performance, but was one of the main factors. The compass wasn't calibrated either and wasn't giving accurate readings, but I am hoping that this is now resolved. I'll be taking G-Force out for some new test runs soon to see if these changes help.