Refactored controller state and output update.

Implemented linear interpolation for controller state update and
output update.

Refactored state and output update and to be outside of
YawRateController class so that it can be used more easily
outside of firmware