Check your wheel pulses - are they working? this is most likely the problem.
Observe them in the GPS service mode. Wheel pulses should be positive and increase with speed when going forwards, negative when backwards.
Check the direction of travel, Backwards when in reverse, forwards in all other gears.
Make sure gyro sites at approx 2500mV, going up to 3000 when turning right, down to 2000 when going left.