Algorithm 1. GDA algorithm
Input: acc → measured acceleration, ω → measured angular velocity, mag → measured magnetometer,$G_{acc}$ → earth’s gravity, $G_{mag}$ → magnetic field vectors
Output: ${\hat{\mathrm{q}}}_{\mathrm{t}}$→ updated quaternions
1.	${\mathrm{S}}_{\mathsf{\omega }}=\left[0{ \mathsf{\omega }}_{\mathrm{x}}{ \mathsf{\omega }}_{\mathrm{y}}{ \mathsf{\omega }}_{\mathrm{z}}\right]$
2.	If ( | ||${\mathrm{mag}}^{\mathrm{s}}$|| -${\mathrm{mag}}^{\mathrm{Earth}}$ | < mag_ stability_thereshold ) then
3.	stability = 1
4.	else then
5.	stability = 0
6.	end if
7.	F(${\hat{\mathrm{q}}}_{\mathrm{t}-1} . \mathrm{acc}$) = $({\hat{\mathrm{q}}}_{\mathrm{t}-1}^{\ast }\otimes$ ${\mathrm{G}}_{\mathrm{acc}}) \otimes {\hat{\mathrm{q}}}_{\mathrm{t}-1}$- acc
8.	If (stability) then
9.	F(${\hat{\mathrm{q}}}_{\mathrm{t}-1} . \mathrm{mag}$) = ${\hat{\mathrm{q}}}_{\mathrm{t}-1}^{\ast }\otimes$ ${\mathrm{G}}_{\mathrm{mag}} \otimes {\hat{\mathrm{q}}}_{\mathrm{t}-1}$ − $\mathrm{mag}$
10.	$\nabla \mathrm{F}\left(\mathrm{q}\right)$ = ${\left[\begin{array}{c}\mathrm{J}\left({\hat{\mathrm{q}}}_{\mathrm{t}-1 }.{\mathrm{acc}}_{\mathrm{t}}\right)\\ \mathrm{J}\left({\hat{\mathrm{q}}}_{\mathrm{t}-1} .\mathrm{mag}\right)\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}\mathrm{F}\left({\hat{\mathrm{q}}}_{\mathrm{t}-1} . {\mathrm{acc}}_{\mathrm{t}}\right)\\ \mathrm{F}\left({\hat{\mathrm{q}}}_{\mathrm{t}-1} . \mathrm{mag}\right)\end{array}\right]$
11.	else then
12.	$\nabla \mathrm{F}\left(\mathrm{q}\right)={\mathrm{J}}^{\mathrm{T}}\left({\hat{\mathrm{q}}}_{\mathrm{t}-1 }.{\mathrm{acc}}_{\mathrm{t}} \right)\mathrm{F}({\hat{\mathrm{q}}}_{\mathrm{t}-1} .{ \mathrm{acc}}_{\mathrm{t}}$)
13.	end if
14.	${\dot{\mathrm{q}}}_{\mathrm{t}}=\frac{1}{2}{\hat{\mathrm{q}}}_{\mathrm{t}-1 }\otimes {\mathrm{S}}_{\mathsf{\omega }}-\mathsf{\beta }\frac{\nabla \mathrm{F}\left(\mathrm{q}\right)}{\left|\nabla \mathrm{F}\left(\mathrm{q}\right)\right|}$
15.	${\mathrm{q}}_{\mathrm{t}}={\hat{\mathrm{q}}}_{\mathrm{t}-1}+{\dot{\mathrm{q}}}_{\mathrm{t}} \Delta \mathrm{t}$
16.	${\hat{\mathrm{q}}}_{\mathrm{t}}$ = $\frac{{\mathrm{q}}_{\mathrm{t}}}{\left|{\mathrm{q}}_{\mathrm{t}}\right|}$
17.	Return ${\hat{\mathrm{q}}}_{\mathrm{t}}$