## 4 Simulation Results
### Simulation Setup  
The simulation program is written in C++ as it is the programming language that can be used to interface with the real vehicle. The interface to the Simulated System is the same as that of the actual vehicle, allowing for software-in-the-loop simulations of our KCL implementation. Concerning the Planning Layer, we implemented a very basic state machine to handle the sequence of actions. The vehicle model used in the simulation is the X300 AUV, which is a low-cost toredo-shaped AUV that is capable of maneuvering with 5 degrees of freedom along the surge, sway, heave, pitch, and yaw directions. The X300 hydrodynamic model is based on the general Fossen model [49; 50; 51] for an underwater vehicle. The mass and drag parameters of the experimental vehicle X300 have been identified prior to this research. Assuming the vehicle is symmetric in the \(x-z\) plane and \(y-z\) plane, and almost symmetric in the \(x-y\) plane due to relative slow speeds, the origin of vehicle frame coincides with the vehicle center of buoyancy. Various dynamic model parameter terms can thus be simplified as per Table 2. The vehicle is powered by 5 thrusters consisting of a main thruster at the aft, 2 lateral thrusters, and 2 vertical thrusters, and thus is not actuated in roll as depicted in Figure 6. The vehicle has its center of mass below its center of buoyancy, allowing the restoring forces to provide passive stability for the vehicle in roll. We also notice that the  
Figure 5: Docking approach from various sides of the docking station: (**a**) docking from the back (e.g., to dock on a stationary seafloor docking station), (**b**) docking from one side (e.g., to dock to the side of a surface vessel), and (**c**) docking from the top (e.g., to dock on a towed underwater docking station). center of mass is below the line of the thrusters, thereby generating transient roll while translating. However, it should also be noted that we limit translations as much as possible. The altitude sensors in the vehicle are arranged in the \(x-z\) plane of the vehicle as depicted in Figure 7 where they are fixed at 45 degrees forward and aft w.r.t. the vehicle body frame \(\langle b\rangle\). These two sensors allows an estimation of the slope of the seafloor by determining the seafloor vector. In the sensor frame \(\langle s\rangle\), the rays from the two sensors make contact with seafloor at points \({}^{s}A\) and \({}^{s}B\), respectively. The floor vector \(\mathbf{f}\) in \(\langle b\rangle\) is given as \({}^{b}\mathbf{f}={}^{b}\mathbf{R}_{s}({}^{s}A-{}^{s}B)\), where \({}^{b}\mathbf{R}_{s}\) denotes the rotation matrix from \(\langle s\rangle\) to \(\langle b\rangle\). The vehicle follows the seafloor terrain by aligning its body \(z\) axis in the direction of the orthogonal component \(\mathbf{n}_{b}\), calculated as a normalized cross-product of the floor vector \({}^{b}\mathbf{f}\) and the body \(y\) axis \(j_{b}\). It is obvious that the roll could point the altitude sensors far away from the vertical projection of the vehicle on the surface, thereby estimating the slope in another point than the one just below the vehicle. We assume the roll to be negligible because in the design of these kind vehicles, the center of mass and center of buoyancy are located in such a way that the restoring forces provide passive stability in roll. The roll could be further limited by adding fins to make the vehicle more horizontal in terms of roll. For estimating the vehicle altitude, we consider an absolute vertical line, passing through the vehicle body center \({}^{b}O\) and touching the floor vector \({}^{b}\mathbf{f}\) at point \({}^{b}\mathbb{C}\). The estimated altitude is the length of the vertical segment \(({}^{b}\mathbb{C}-{}^{b}O)\). Figure 6: AUV: X300 model and allocation of thrusters. Figure 7: X300: Allocation of altitude sensors w.r.t. center of vehicle body frame and estimation of vehicle altitude as length of the vertical segment.