Algorithm 1: Tightly coupled motion-capture algorithm (TCMA)

Input: Sensor measurements ${\mathit{y}}_t$, an initial state estimate ${\widehat{\mathit{x}}}_0$ with covariance ${\widehat{\mathit{P}}}_0$, process and measurement noise covariance matrices $\mathit{Q}$ and $\mathit{R}$; see (4), (13) and (14).

Output: Estimates of the system states ${\widehat{\mathit{x}}}_t$ and the respective covariance ${\widehat{\mathit{P}}}_t$ for $t=1,\dots T$.

1: for $t=1,\dots ,T$ do

2: Time update  Compute the state prediction by numerically integrating the noise-free dynamics in (4) from the previous estimate ${\widehat{\mathit{x}}}_{t-1}$ over one time step $\Delta t$, $$\begin{array}{cc}\hfill {\check{\mathit{x}}}_t& =\mathit{f}({\widehat{\mathit{x}}}_{t-1},\Delta t).\hfill \end{array}$$ (15)

Compute the predicted state covariance as $$\begin{array}{cc}\hfill {\check{\mathit{P}}}_t& ={\mathit{F}}_{t-1}{\widehat{\mathit{P}}}_{t-1}{\mathit{F}}_{t-1}^{\mathsf{T}}+\mathit{Q},\hfill \end{array}$$ (16)

where the discrete-time state transition Jacobian is approximated by discretizing the continuous-time Jacobian as ${\mathit{F}}_{t-1}\approx \exp \left({\mathit{G}}_{t-1}\Delta t\right)$, with ${\mathit{G}}_{t-1}={\left.{\displaystyle \frac{\partial \mathit{g}\left(\mathit{x}\right)}{\partial \mathit{x}}}\right|}_{\mathit{x}={\widehat{\mathit{x}}}_{t-1}}$ computed numerically.

3: Measurement update

4:   for $k=1,\dots ,ϵ$ until ${\overline{\mathit{x}}}_t^{k+1}\approx {\overline{\mathit{x}}}_t^k$ do

5:    Update the state with measurements ${\mathit{y}}_t$ as $$\begin{array}{cc}\hfill {\overline{\mathit{x}}}_t^{k+1}& ={\check{\mathit{x}}}_t+{\mathit{K}}_t^k\left({\mathit{y}}_t-{\mathit{h}}_t\left({\overline{\mathit{x}}}_t^k\right)-{\mathit{H}}_t^k({\check{\mathit{x}}}_t-{\overline{\mathit{x}}}_t^k)\right),\hfill \end{array}$$ (17)

where ${\mathit{K}}_t^k={\check{\mathit{P}}}_t{\left({\mathit{H}}_t^k\right)}^{\mathsf{T}}{\left({\mathit{H}}_t^k{\check{\mathit{P}}}_t{\left({\mathit{H}}_t^k\right)}^{\mathsf{T}}+\mathit{R}\right)}^{-1}\mathrm{and}$ ${\mathit{H}}_t^k={\left.{\displaystyle \frac{\partial {\mathit{h}}_t\left({\mathit{x}}_t\right)}{\partial {\mathit{x}}_t}}\right|}_{{\mathit{x}}_t={\overline{\mathit{x}}}_t^k}$.

6:   Compute the filtered state and covariance $$\begin{array}{c}\hfill {\widehat{\mathit{x}}}_t={\overline{\mathit{x}}}_t^{k+1},{\widehat{\mathit{P}}}_t=\left(\mathit{I}-{\mathit{K}}_t^k{\mathit{H}}_t^k\right){\check{\mathit{P}}}_t.\end{array}$$ (18)