Some more testing. I have heading/yaw working now. However the tractor in AOG does not change heading based on the IMU even when stopped. Is it supposed to? I think this is leading to my error that I am seeing in tilt. Since the tractor CL is not parallel to the AB line I am getting error in the tilt/roll correction.
Also I am not getting speed from the GPS. Is this a setting in u-center that I need to change? I am using the rover config from the ardusimple webpage.