Robot bearing, position implementation

Implemented position and bearing of robot, including angular velocity.