Algorithm 2 Elevation angle MLE.
Input: Trained surface GP f, a horizontal sonar measure $({\psi }_k^h,{\rho }_k^h)$, robot trajectory H and the corresponding curvilinear abscissa $\mathbf{s}$, Rotation axis of vertical scan expressed in the vertical sonar frame ${\mathbf{n}}_{|\{S^V\}}^v$, Number of $\theta$ samples $n_q$
Output: Estimated local maximum elevation angles $\widehat{\Theta }$
1:	$\widehat{\mathbf{\Theta }}\leftarrow \varnothing$, ${\Delta }_{\theta }\leftarrow \frac{{\theta }_{max}-{\theta }_{min}}{n_q}$
2:	for q do
3:	${\theta }_q\leftarrow {\theta }_{q-1}+{\Delta }_{\theta }$
4:	$\mathbf{x}\leftarrow g^h\left(\left[\begin{array}{ccc}{\rho }_k^h& {\theta }_q& {\psi }_k^h\end{array}\right],k\right)$	▹ Equation (20)
5:	$I\leftarrow \varnothing$
6:	for $i\in [1,n^r-1]$ do
7:	${\mathbf{n}}_{|\{0\},i}^v,{\mathbf{n}}_{|\{0\},i+1}^v\leftarrow R\left({\mathbf{q}}_i\right)R\left({\mathbf{q}}_V\right){\mathbf{n}}_{|\{S^V\}}^v,R\left({\mathbf{q}}_{i+1}\right)R\left({\mathbf{q}}_V\right){\mathbf{n}}_{|\{S^V\}}^v$	▹ Equation (21)
8:	if $〈{\mathbf{n}}_{|\{0\},i}^v,\mathbf{x}-{\mathit{\tau }}_i〉〈{\mathbf{n}}_{|\{0\},i+1}^v,\mathbf{x}-{\mathit{\tau }}_{i+1}〉<0$then	▹ Equation (22)
9:	$I\leftarrow i$
10:	end if
11:	end for
12:	${\widehat{\theta }}_q\leftarrow 0$
13:	$l_q^{max}\leftarrow 0$	▹ Maximum likelihood value
14:	for $i\in I$ do
15:	${\mathbf{n}}_{|\{0\},[i,i+1]}^v(t)\leftarrow linearInterp\left({\mathbf{n}}_{|\{0\},i}^v,{\mathbf{n}}_{|\{0\},i+1}^v\right)$
16:	${\mathit{\tau }}_{[i,i+1]}(t)\leftarrow linearInterp\left({\mathit{\tau }}_i,{\mathit{\tau }}_{i+1}\right)$
17:	${\mathit{\eta }}_{[i,i+1]}(t)\leftarrow linearInterp\left({\mathit{\eta }}_i,{\mathit{\eta }}_{i+1}\right)$
18:	$\widehat{t}\leftarrow \mathrm{root}\left(〈{\mathbf{n}}_{|\{0\},[i,i+1]}^v,\mathbf{x}-{\mathit{\tau }}_{[i,i+1]}〉(t)\right)$	▹ Equations (28) and (30)
19:	${\widehat{\mathit{\eta }}}_q\leftarrow {\mathit{\eta }}_{[i,i+1]}\left(\widehat{t}\right)$
20:	${\widehat{\mathbf{x}}}_{|\{S^V\}}={\left[{\widehat{x}}_i^v{\widehat{y}}_i^{v}0\right]}^T\leftarrow \mathbf{x}\ominus \left({\widehat{\mathit{\eta }}}_q\oplus {\mathbf{p}}^V\right)$	▹ Equation (32)
21:	${\widehat{s}}_q\leftarrow s_i+L_{{\mathit{\eta }}_i}^{{\widehat{\mathit{\eta }}}_q}$	▹ Equation (31)
22:	${\widehat{\psi }}_q^v\leftarrow atan2({\widehat{y}}^v,{\widehat{x}}^v)$	▹ Equation (33)
23:	${\widehat{\rho }}_q^v\leftarrow \left|\right|{\widehat{\mathbf{x}}}_{|\{S^V\}}{\left|\right|}_2$	▹ Equation (34)
24:	$l_q\leftarrow ln(p({\widehat{\rho }}_i^v|f,{\widehat{s}}_i,{\widehat{\psi }}_i,{\theta }_q))$	▹ Equation (36)
25:	if $l_q>l_q^{max}$ then
26:	$l_q^{max}\leftarrow l_q$
27:	${\widehat{\theta }}_q\leftarrow {\theta }_q$
28:	end if
29:	end for
30:	if $(l_q^{max}<l_{q-1}^{max})\wedge (l_{q-1}^{max}>l_{q-2}^{max})$ then	▹ $\frac{{\partial }^{2}J}{\partial {\theta }^2}<0$, Equation (19)
31:	$\widehat{\mathbf{\Theta }}\leftarrow {\widehat{\theta }}_q$
32:	end if
33:	end for