Add description to should_estimate/control functions.
Change input of threshold functions to speed.
Fix conversion from speed to theta_R_dot.
Merge remote-tracking branch 'oliverlee/gainschd' into gainschd
Minor optimization of negation operation.