Added EKF beadband, improved anti-backlash code and comments