Always set control state on servo startup
Remove debug prints from EKF_odometry
Fix rotation of odometry velocity
Tweak path bias for better alignment