Algorithm 1. Visual-inertial motion tracking process.

01. Initialize ${\widehat{x}}_{0|0}$, ${\tilde{x}}_{0|0}$ and $P_{0|0}$

02. for $k=1,\cdots$ do

03. { Time update:

04.   Compute $F_d$ and $Q_d$, ${\tilde{x}}_{k|k-1}=0_{21\times 1}$, $P_{k|k-1}=F_d{P}_{k-1|k-1}{F_d}^T+Q_d$

05.   Compute ${\widehat{x}}_{k|k-1}$ with the 4th Runge Kutta integration

06.     if Pose from visual-based arrived

07.      {Measurement update:

08.       Compute the residual: $\tilde{z}=z-\widehat{z}$, Kalman gain: $K_k=P_{k|k-1}{H}^T{(H{P}_{k|k-1}{H}^T+R)}^{-1}$;

09.       Compute the correction: ${\tilde{x}}_{k|k}={\tilde{x}}_{k|k-1}+K_k\tilde{z}$, $P_{k|k}=(I_d-K_{k}H)P_{k|k-1}{(I_d-K_{k}H)}^T+K_{k}R{K_k}^T$;

10.       Use ${\tilde{x}}_{k|k}$ to correct state estimate and the obtain ${\widehat{x}}_{k|k}$}

11.     end

12. end }