Algorithm 3. Five-step turn detection algorithm.
Input:	Angle value sequence $\mathsf{\theta }$ = [${\mathsf{\theta }}_1,{\mathsf{\theta }}_2,\dots {\mathsf{\theta }}_{\mathrm{n}}]$
Output:	Direction change list $\mathrm{U}$.
1:	${\mathsf{\theta }}_{\max }\leftarrow$Findpeaks($\mathsf{\theta }$)/* Find the local maximum sequence
2:	for ${\mathsf{\theta }}_{\mathrm{i}}$ in ${\mathsf{\theta }}_{\max }$ do
3:	if (${\mathsf{\theta }}_{\mathrm{i}}>15$ or ${\mathsf{\theta }}_{\mathrm{i}}<-15$) and ($i$>2) then
4:	${\mathsf{\theta }}_{\mathrm{sum}}$ = $\mathrm{sum}\left[{\mathsf{\theta }}_{\mathrm{i}-2}:{\mathsf{\theta }}_{\mathrm{i}+2}\right]$
5:	if (${\mathsf{\theta }}_{\mathrm{sum}}>30 and {\mathsf{\theta }}_{\mathrm{sum}}\le 60$) then
6:	$\mathrm{U}.\mathrm{add}\left(\text{“}\mathrm{Go}\mathrm{left}\text{”}\right)$
7:	else if (${\mathsf{\theta }}_{\mathrm{sum}}>60 and {\mathsf{\theta }}_{\mathrm{sum}}\le 120$) then
8:	$\mathrm{U}.\mathrm{add}\left(\text{“}\mathrm{Turn}\mathrm{left}\text{”}\right)$
9:	else if (${\mathsf{\theta }}_{\mathrm{sum}}>-60 and {\mathsf{\theta }}_{\mathrm{sum}}\le -30$) then
10:	$\mathrm{U}.\mathrm{add}\left(\text{“}\mathrm{Go}\mathrm{right}\text{”}\right)$
11:	else if (${\mathsf{\theta }}_{\mathrm{sum}}>-120 and {\mathsf{\theta }}_{\mathrm{sum}}\le -60$) then
12:	$\mathrm{U}.\mathrm{add}\left(\text{“}\mathrm{Turn}\mathrm{right}\text{”}\right)$
13:	else if (${\mathsf{\theta }}_{\mathrm{sum}}>120 or {\mathsf{\theta }}_{\mathrm{sum}}\le -120$) then
14	$\mathrm{U}.\mathrm{add}\left(\text{“}\mathrm{Turn}\mathrm{around}\text{”}\right)$
15:	end for
16:	return $\mathrm{U}$