added odometry handler, fixed KC in control algorithm