Fixes for filtered position error
Fixes for filtered position error

- include halt states, even in integral gain (caused failure to stay halted when command watchdog times out)
- include hysteresis for integral gain
- filter before hysteresis

TucoFlyer/Bot-Firmware