Add description to should_estimate/control functions.
Change input of threshold functions to speed.
Fix conversion from speed to theta_R_dot.