|111's my lucky number...|
Technically there are still a number of bugs and issues and you'll notice the distinct lack of object detection sensors. But, the sensor merging finally seems to be doing a little better now (or did I just jinx myself)?
At rest, it keeps the AHRS yaw in sync with the compass. When it first takes off it uses the AHRS for heading, then when vehicle speed reaches 2 mph when GPS heading information stabilizes, then it switches over to GPS, assuming there's a good fix.
If the fix (HDOP) goes above 2, which typically makes the position information highly suspect, it switches over to dead reckoning, the heading and position of which will be kept synced to GPS while a good fix is available. Hopefully the dead reckoning can pick up where the GPS left off at least for a short time.
Also the robot starts at a known position and the robot calculates the distance/bearing offset between the GPS initial readings and the known start point and uses that fixed bias to adjust future GPS position information.
Of course the approach and implementation are fraught with issues and I'm sure you can think of many without trying, including ones I've not considered.
C'est la vie. Tomorrow should be a blast.