A setback

The Kalman filter sensor fusion failed. At the part where it does the measurement update for the covariance, it wasn't producing a symmetric covariance. My next idea was to fake it with a scalar weighting. That didn't work either.

The GPS heading is really good, particularly when the vehicle is going straight. So, we will watch for the gyro reading to be large. If it is, we set a counter to 400 (gyro readings). If it is small, we decrement the counter. When the counter is below zero, we just use the angle from the last GPS reading to the next as the heading, and reset the heading state to exactly that. We keep a heading offset and a separate free-roaming gyro heading. In between GPS resets of the offset, we add that offset to the free gyro heading to keep the actual heading up-to-date.