# Multi-Modal Sonar Mapping of Offshore Cable Lines with an Autonomous Surface Vehicle
## 3 Consistent Mapping
### Motion Estimation  
In an offshore ocean environment, the ASV suffers from large disturbances due to wind and wave currents. This makes precise position and attitude estimations crucial for consistent mapping; otherwise, each captured scene from the mapping sensors will not overlap correctly, thereby making it difficult to recognize the target objects in the resulting map. For this purpose, we first define the coordinate systems to describe the vehicle motion. We assume that the effect of the Earth's rotation on the vehicle motion can be neglected, and thus any Earth-fixed frame such as Earth-centered Earth-fixed (ECEF) or East, North, up (ENU) can be employed as an inertial reference frame [I]. In this study, we considered the ENU as an inertial reference frame. We also define the vehicle's body-fixed frame {B} as the center of the inertial measurement unit (IMU), where the specific forces and angular velocities are measured. Subsequently, we can define the vehicle's navigation states, including the position \(\mathbf{r}^{1}\in\mathbb{R}^{3}\), linear velocity \(\mathbf{v}^{1}\in\mathbb{R}^{3}\), and orientation \(\mathbf{R}^{1}_{\text{B}}\in\mathbf{SO}(3)\). Figure 3: Illustration of waypoint tracking performance indices. Then, we must define a vertical datum representing the height of the target objects. The most typically used datum is geoid, and the height from the geoid is the orthometric height \(H\). The relation between \(H\) and the other height information is described as follows:  
\[H\approx h-N. \tag{2}\]  
where \(N\) and \(h\) are the geoid and ellipsoid height, respectively, and both can be obtained from the NMEA 0183 packet data received by the GPS antenna. This relationship is depicted in Figure 4. Note that the vertical deflection (VD) corresponds to the discrepancy between the true vertical and ellipsoid normal. The VD over the field test area was less than 5\({}^{\prime\prime}\), implying that the horizontal deviation due to the VD is at most 2.5 mm for the 100 m range measurement. Therefore, we can neglect the VD for the local survey and let \(H=h-N\). Based on these representations, the overall procedure for the vehicle state estimation can be represented as shown in Figure 5. The vehicle state estimation combines measurements from GPS and the absolute heading reference system (AHRS). In particular, the GPS provides position information \(\mathbf{r}^{\text{GPS}}=[\lambda,\varphi,h]^{\top}\), where \(\lambda\), \(\varphi\), and \(h\) are the geodetic coordinates represented in a reference ellipsoid (e.g., WGS84). Owing to the geometric configuration of the satellite constellation, GPS usually has a higher dilution of precision (DOP) in the vertical direction than in the horizontal direction. Thus, it has larger uncertainty in the vertical estimation. Typical GPS position accuracy running in satellite-based augmentation system (SBAS) mode shows 0.3 m and 0.6 m in horizontal and vertical axes, respectively. Therefore, having additional altitude information from a more accurate sensor is required. In this regard, we employed a survey-grade AHRS for both heave \(z^{\text{local}}\) and orientation \(\mathbf{q}^{\text{I}}\) estimation. Moreover, the position \(\mathbf{r}^{\text{GPS}}\) expressed in a geodetic coordinate system is transformed into an ENU frame. Assuming that the reference position is not far from the survey area  
Figure 4: Height definition. Figure 5: Pose estimation flow chart. (e.g., located at the starting point of the vehicle), the transformation equations using the Taylor series expansion can be approximated as follows:  
\[ \begin{bmatrix}x^{\text{ENU}}\\ y^{\text{ENU}}\\ z^{\text{ENU}}\end{bmatrix}  = \begin{bmatrix}(R_{E}(\varphi)+h)\cos\varphi&0&0\\ 0&R_{N}(\varphi)+h&0\\ 0&0&-1\end{bmatrix}\begin{bmatrix}\lambda-\lambda_{r}\\ \varphi-\varphi_{r}\\ h-h_{r}\end{bmatrix}