Autonomous underwater vehicles (AUV) are robots, which usually estimate their position by localization the help of internal or external sensors. In this thesis, small swarm robots from the CoCoRo are used as experimental platform. It is often useful, to know the exact position inside the testing area to evaluate swarm algorithms. Controlling the position of the robot should be possible as well. A software is developed, which is able to track a robot inside an aquarium. Two cameras are install at each side of this aquarium for determining the 3D position which includes the diving depth. Perspective distortions, which come from viewing angle, are compensated with the help of image transformation. With the corrected image, the template matching algorithm with normalized cross-correlation is used to track the robot in the camera image. A wireless connection is established between the computer and the robot to read out sensor data and to control the motors. Waypoints can be set by the user which the robot follows. The computer uses two independent controllers for rotational and for distance control.