Added odometry. Velocity filter code also included, but not active. sensors->torsoVelocity