Heading Reference

The hardest thing is getting an accurate heading reference. There are at least 4 possible sources:

1. Magnetic compass - We are carrying one, but it is close to the motor. I'll have to check some of the data to see if the motors influence it a lot.
2. Gyroscope - Subject to random walk drift and needs an accurate starting heading
3. GPS heading - The GPS calculates a heading itself and reports it in RMC
4. GPS delta-position - Use trigonometry and the position 1 second ago

 A proper solution would use a Kalman filter. I haven't gotten that to work on the robot yet. So, I am using primarily 2, with 3 to initialize and 4 to correct when in motion. It goes like this:

• We initialize the gyro using "Average-G". This is the average of the last 50 gyro readings on each axis. This average is subtracted from the in-motion readings to produce a zero-biased rotation rate
• We use the first RMC sentence to get our initial heading
• Once in motion, we use the gyro as the primary heading reference. We set up an inertial frame parallel to the initial orientation of the robot, and an initial identity quaternion. We use normal quaternion integration to integrate the gyro readings and get a new gyro-based orientation. We use that orientation to transform the vector pointing forward (the nose vector) from the body to the reference frame. The gyro heading is then the angle between the initial nose vector (same as the body nose vector) and the current nose vector. We use the initial heading to calculate the offset between the gyro heading and true north, and add this offset to the gyro heading to get the instantaneous heading relative to true north
• Whenever a large maneuver is executed, IE a large yaw rate is sensed, a countdown is set to 400 readings (4 seconds). When this countdown expires, we use method 4 to get the heading if the speed of the vehicle is high enough. We use this heading to calculate a new heading offset.

Improvements are as follows:

• We know the initial heading from the first two waypoints. Provided the robot is accurately aimed toward the waypoint, it will be plenty good enough.
• We are going to add a green button. Before the button is pushed, the robot will continually be recording the gyro readings, but will remember the last 50. Each reading, we will pop one reading out of the queue, and average it with the running total. The weighting will be 1 point for the new reading and 49 for the old average. In effect this takes the average of all the measurements from robot reset, but each old measurement has an exponentially decaying weight. We include some code to wait for 49 measurements before using a weighting of 49. We remember 50 measurements because the very act of pushing the green button will cause the robot to shake and will ruin the current measurements.

With these improvements,  I expect the initial heading to be much better and I expect to be able to run almost entirely on gyro readings.