Algorithm 1 Scan merging.
Input: Sonar measurements ${\mathcal{M}}^h$, ${\mathcal{M}}^v$, Robot trajectory H
Output: Estimated horizontal measurements $p(\widehat{\theta })$
1: $\mathbf{s}={\left[\begin{array}{ccc}{s}_1& \cdots & s_{n_r}\end{array}\right]}^T\leftarrow s(H)$	▹ Curvilinear abscissa, Equation (12)
2: $f_p\leftarrow fitEllipticCylinder({\mathcal{M}}^v)$	▹ Surface prior
3: $f\leftarrow \mathcal{GP}(\mathbf{s},{\mathcal{M}}^v,f_p)$	▹Surface Estimation by Gaussian Process
4: for each ${\psi }_i^h,{\rho }_{ij}^h$ do	▹ Estimation
5:    $\widehat{\mathbf{\Theta }}=\mathrm{ElevationAngleMLE}\left(f,\mathbf{s},{\psi }_i^h,{\rho }_{ij}^h\right)$	▹ See Algorithm 2
6:    for $\widehat{\theta }\in \widehat{\mathbf{\Theta }}$ do
7:        $p(\widehat{\theta })\leftarrow \mathbf{UncertaintyEstimate}\left(p({\rho }^v|\theta ),\widehat{\theta }\right)$
8:    end for
9: end for