Optimal Control System for a Semi-Autonomous Underwater Vehicle