{\bf{a}} \otimes {\bf{b}} =[a_1, a_2, a_3, a_4]\otimes [b_1,b_2,b_3,b_4]\\
=\begin{bmatrix}
a_1 b_1 - a_2 b_2 - a_3 b_3 -a_4 b_4\\\
a_1 b_2 - a_2 b_1 + a_2 b_1 + a_3 b_4 - a_4 b_3\\\
a_1 b_3 -  a_2 b_4 + a_3 b_1 + a_4 b_2 \\\
a_1 b_4 +a_2 b_3 -a_3 b_2 +a_4 b_1]
\end{bmatrix}
\cdots(4)

^B{\bf{v}} = ^A_B\hat{q} \otimes ^A{\bf{v}} \otimes ^A_B\hat{q}^* \cdots(5)

^S\omega=[0 ,\omega_x,\omega_y,\omega_z] [rad/s]\cdots(10)

^S_E\dot{q}=\frac{1}{2}^S_E\hat{q}\otimes^S\omega\cdots(11)

^S_E\dot{q}_{\omega,t}=\frac{1}{2}^S_E\hat{q}_{est,t-1}\otimes^S\omega_t\cdots(12)

^S_Eq_{\omega,t}=^S_E\hat{q}_{\omega,t-1} + ^S_E\dot{q}_{\omega,t}\Delta t\cdots(13)

min(^S_E\hat{q}\in R^4) {\bf{f}}(^S_E\hat{q},^E\hat{d},^S\hat{s})\cdots(14)\\\
{\bf{f}}(^S_E\hat{q},^E\hat{d},^S\hat{s}) = ^S_E\hat{q}^*\otimes ^E\hat{d} \otimes ^S_E\hat{q} - ^S\hat{s}\cdots(15)\\\
^S_E\hat{q}=[q_1,q_2,q_3,q_4]\cdots(16)\\\
^E\hat{d}=[0, d_x,d_y,d_z]\cdots(17)\\\
^S\hat{s}=[0,s_x,s_y,s_z]\cdots(18)

^S_E{q}_{k+1}=^S_E\hat{q}_k -\mu\frac{\nabla{\bf{f}}(^S_E\hat{q}_k,^E\hat{d},^S\hat{s})}{||\nabla{\bf{f}}(^S_E\hat{q}_k,^E\hat{d},^S\hat{s})||},k=0,1,2\dots n \cdots(19)\\\

\nabla{\bf{f}}(^S_E\hat{q}_k,^E\hat{d},^S\hat{s})={\bf{J}}^T(^S_E\hat{q}_k,^E\hat{d}) {\bf{f}}(^S_E\hat{q}_k,^E\hat{d},^S\hat{s}) \cdots(20)\\\

{\bf{f}}(^S_E\hat{q}_k,^E\hat{d},^S\hat{s})=
\begin{bmatrix}
2d_x(\frac{1}{2}-q_3^2-q_4^2)+2d_y(q_1q_4+q_2q_3)+2d_z(q_2q_4-q_1q_3)-s_x\\\
2d_x(q_2q_3-q_1q_4)+2d_y(\frac{1}{2}-q_2^2-q_4^2)+2d_z(q_1q_2+q_3q_4)-s_y\\\
2d_x(q_1q_3+q_2q_4)+2d_y(q_3q_4-q_1q_2)+2d_z(\frac{1}{2}-q_2^2-q_3^2)-a_z
\end{bmatrix} \cdots(21)\\\

{\bf{J}}^T(^S_E\hat{q}_k,^E\hat{d})=
\begin{bmatrix}
2d_yq_4-2d_zq_3 & 2d_yq_3+2d_zq_4 & -4d_xq_32d_yq_2-2d_zq_1 & -4d_xq_4+2d_yq_1+2d_zq_2\\\
-2d_xq_4+2d_zq_2 & 2d_xq_3-4d_yq_2+2d_zq_1 & 2d_xq_2+2d_zq_4 & -2d_xq_1-4d_yq_4+2d_zq_3\\\
2d_xq_3-2d_yq_2 & 2d_xq_4-2d_yq_1-4d_xq_2 & 2d_xq_1+2d_yq_4-4d_zq_3 & 2d_xq_2+2d_yq_3
\end{bmatrix} \cdots(22)

^E\hat{g}=[0, 0, 0, 1]\cdots(23)\\\
^S\hat{a}=[0,a_x, a_y ,a_z]\cdots(24)

{\bf{f}}_g(^S_E\hat{q},^S\hat{a})=
\begin{bmatrix}
2(q_2q_4-q_1q_3)-a_x\\\
2(q_1q_2+q_3q_4)-a_y\\\
2(\frac{1}{2}-q_2^2 -q_3^2)-a_z
\end{bmatrix}
\cdots(25)\\\

{\bf{J}}_g(^S_E\hat{q})=
\begin{bmatrix}
-2q_3 & 2q_4 & -2q_1 & 2q_2 \\\ 
2q_2 & 2q_1 & 2q_4 & 2q_3 \\\
0 & -4q_2 & -4q_3 &0
\end{bmatrix} \cdots(26)

^S_Eq_{\nabla,t}=^S_Eq_{est,t-1}-\mu_t \frac{\nabla{\bf{f}}}{||\nabla{\bf{f}}||} \cdots(33)

\mu_t = \alpha || ^S_E\dot{q}_{\omega ,t}|| \Delta t , \alpha > 1\cdots(35)

^S_E{q}_{est,t} = {\gamma_t} ^S_E{q}_{\nabla,t} + (1-\gamma_t)^S_E{q}_{\omega,t} , 0\leq \gamma_t \leq 1\cdots(36)

(1-\gamma_t)\beta = \gamma_t \frac{\mu_t}{\Delta t}\cdots(37)

\gamma_t=\frac{\beta}{\frac{\mu_t}{\Delta t}+\beta}\cdots(38)

^S_E{q}_{\nabla,t}\approx -\mu_t \frac{\nabla {\bf{f}}}{||{\bf{f}}||}\cdots(39)

\gamma_t \approx \frac{\beta \Delta t}{\mu_t}\cdots(40)

^S_Eq_{est,t}=\frac{\beta \Delta t}{\mu_t}(-\mu_t \frac{\nabla {\bf{f}}}{||\nabla {\bf{f}}||})+(1-0)(^S_E\hat{q}_{est,t-1}+^S_E\dot{q}_{\omega,t}\Delta t) \cdots(41)

^S_Eq_{est,t} = ^S_E\hat{q}_{est,t-1}+^S_E\dot{q}_{est,t}\Delta t \cdots(42)\\\
^S_E\dot{q}_{est,t} = ^S_E\dot{q}_{\omega,t}-\beta ^S_E\dot{\hat{q}}_{\epsilon,t} \cdots(43)\\\
^S_E\dot{\hat{q}}_{\epsilon,t}= \frac{\nabla {\bf{f}}}{||{\bf{f}}||} \cdots(44)