Position and orientation of robot and sensors now tracked.

Position and orientation calculations and tracking are done with
transformation matrices.
Class structure of project modified slightly.

Added class for individual sensors. Robot now contains a sensor array.