Algorithm 1 Two-stage EKF for WiFi/MEMS integration.

Step 1: Initialize the attitude-determination EKF

${\widehat{\mathbf{D}}}_0={\left[0.5\pi 00\right]}^T$, ${\mathbf{P}}_{10}=diag\left(\left[\begin{array}{ccc}1& 1& 1\end{array}\right]\right)$, $k=1$

Step 2: Attitude-determination EKF

${\mathbf{F}}_{k-1}={\left(\partial f_{k-1}/\partial \mathbf{D}\right)|}_{{\widehat{\mathbf{D}}}_{k-1}^{+}}$, where $f_{k-1}(\mathbf{·})$ is the system model defined in Equation (12)

${\mathbf{P}}_{1k}^{-}={\mathbf{F}}_{k-1}{\mathbf{P}}_{1,k-1}^{+}{\mathbf{F}}_{k-1}^T+{\mathbf{Q}}_{1,k-1}$

${\widehat{\mathbf{D}}}_k^{-}=f_{k-1}\left({\widehat{\mathbf{D}}}_{k-1}^{+},{\mathbf{u}}_{k-1},0\right)$, where ${\mathbf{u}}_{k-1}={\left[{\omega }_{x,k-1}{\omega }_{y,k-1}{\omega }_{z,k-1}\right]}^T$

${\mathbf{H}}_k={\left(\partial {\mathbf{T}}_k^{-1}/\partial \mathbf{D}\right)|}_{{\widehat{\mathbf{D}}}_k^{-}}\times {[001]}^T$

${\mathbf{K}}_{1k}={\mathbf{P}}_{1k}^{-}{\mathbf{H}}_k^T{\left({\mathbf{H}}_k{\mathbf{P}}_{1k}^{-}{\mathbf{H}}_k^T+{\mathbf{R}}_{1k}\right)}^{-1}$

${\widehat{\mathbf{D}}}_k^{+}={\widehat{\mathbf{D}}}_k^{-}+{\mathbf{K}}_{1k}\left({\mathbf{A}}_k-{\mathbf{T}}_k^{-1}{[001]}^T\right)$

${\mathbf{P}}_{1k}^{+}=\left(\mathbf{I}-{\mathbf{K}}_{1k}{\mathbf{H}}_k\right){\mathbf{P}}_{1k}^{-}$, where $\mathbf{I}$ is the identity matrix

Step 3: $k=k+1$ and return to Step 2

Step 4: Initialize the position-tracking EKF

${\widehat{\mathbf{B}}}_0={\left[0.600.60.5\pi \right]}^T$, ${\mathbf{P}}_{20}=diag\left(\left[\begin{array}{cccc}0.01& 0.01& 0.01& 0.01\end{array}\right]\right)$, $k=1$

Step 5: Position-tracking EKF

${\mathbf{G}}_{k-1}={\left(\partial g_{k-1}/\partial \mathbf{B}\right)|}_{{\widehat{\mathbf{B}}}_{k-1}^{+}}$, where $g_{k-1}(\mathbf{·})$ is the system model defined in Equation (21)

${\mathbf{P}}_{2k}^{-}={\mathbf{G}}_{k-1}{\mathbf{P}}_{2,k-1}^{+}{\mathbf{G}}_{k-1}^T+{\mathbf{Q}}_{2,k-1}$

${\widehat{\mathbf{B}}}_k^{-}=g_{k-1}\left({\widehat{\mathbf{B}}}_{k-1}^{+},0\right)$

${\mathbf{K}}_{2k}={\mathbf{P}}_{2k}^{-}{\left({\mathbf{P}}_{2k}^{-}+{\mathbf{R}}_{2k}\right)}^{-1}$

${\widehat{\mathbf{B}}}_k^{+}={\widehat{\mathbf{B}}}_k^{-}+{\mathbf{K}}_{2k}\left({\mathbf{C}}_k-{\widehat{\mathbf{B}}}_k^{-}\right)$

${\mathbf{P}}_{2k}^{+}=\left(\mathbf{I}-{\mathbf{K}}_{2k}\right){\mathbf{P}}_{2k}^{-}$

Step 6: $k=k+1$ and return to Step 5