Algorithm 1. Gradient descent-based orientation filter.

$${\widehat{h}}_{q,t}^E={\widehat{q}}_{t-1}\otimes m_{q,t}^S\otimes {\widehat{q}}_{t-1}^{-1}$$ $${\widehat{m}}_{q,t}^E={\left[\begin{array}{ccc}0& 0& \sqrt{{({\widehat{h}}_{x,t}^E)}^2+{({\widehat{h}}_{y,t}^E)}^2}\end{array}{\widehat{h}}_{z,t}^E\right]}^T$$ $$F_t=\left[\begin{array}{c}{\widehat{q}}_{t-1}^{-1}\otimes a_{q,t}^E\otimes {\widehat{q}}_{t-1}-a_{q,t}^S\\ {\widehat{q}}_{t-1}^{-1}\otimes m_{q,t}^E\otimes {\widehat{q}}_{t-1}-m_{q,t}^S\end{array}\right]$$ ${\widehat{q}}_{e,t}=J_t^T{F}_t$, where $J_t$ is the Jacobian matrix of $F_t$. $${\widehat{\omega }}_{e,t}^s=2{\widehat{q}}_{t-1}\otimes {\widehat{q}}_{e,t}$$ $${\dot{\widehat{\omega }}}_{b,t}^S={\omega }_{e,t}^S$$ ${\widehat{\omega }}_t^S={\omega }_t^S-{\zeta }^S{\omega }_{b,t}$ where ${\zeta }^S$ is the integral gain $${\dot{\widehat{q}}}_t=\frac{1}{2}{\widehat{q}}_{t-1}\otimes {\widehat{\omega }}_{q,t}^S-\beta \frac{{\widehat{q}}_{e,t}}{\parallel {\widehat{q}}_{e,t}\parallel }$$ $\beta$ is the divergence rate of $q_t$ expressed as the magnitude of quaternion derivative corresponding to the gyroscope measurement error. $q_e,{\omega }_e$ are the quaternion and angular rate errors.