Adapted flow estimator, position and velocity control to new state machine
Conditions where set wrongly in the first 2s after boot