The thesis research focuses on navigation, guidance, control infrastructures and methodologies required for cooperative control of underwater vehicles. A toolset is proposed for navigation, guidance, and control of marine vessels, adapting to different vessels and sensor suites, enabling integration of delayed and asynchronous measurements from local and remote sensor sources. The toolset is applied to remote sonar based localization of small unmanned underwater vehicles. This concept is further combined with path following and target tracking algorithms, resulting in a diver tracking and localization aiding system of systems. A modified path stabilization controller is presented, capable of safe movement around and with the diver. It implements diver monitoring and tracking functionalities, and, combined with absolute positioning, a diver guidance feature is available. The thesis proposes an incremental experiment design methodology aimed at reducing development time and improving reproducibility. This methodology is used for design of final benchmark experiments for the proposed diver tracking and guidance systems. Results are presented from simulation incremental up to field results with real divers. Developed algorithms and methodologies define the three thesis contributions as: 1. System for navigation, guidance and control of marine vessels enabling integration of delayed and asynchronous measurements from stationary or mobile sensor sources, 2. Algorithm for cooperative control of underwater agents with seamless switching between diver guidance and tracking tasks, 3. Methodology for incremental and reproducible development, integration, validation and uncertainty evaluation for systems of systems.