Fix minor comparison bug.
Fix minor bugs in Reset() and estimtor state saving.

Set point of rear wheel rate and yaw rate were being set to zero on the call to
Reset(), which is called whenever TurnOn() or TurnOff() is called, so it had
the effect of always having a set point of zero.