Assessing the Performance of Sensor Fusion Methods: Application to Magnetic-Inertial-Based Human Body Tracking

. 2016 Jan 26;16(2):153. doi: 10.3390/s16020153

Algorithm A2

for each measured sample from the sensors do

ω_{k}^{b} \leftarrow g y r o s c o p e s a m p l e

Prediction

Predict the quaternion and the magnetic disturbance

{}^{-}{\hat{x}}_{k} = [\begin{matrix} {}^{-}{\hat{q}}_{k}^{b n} \\ {}_{m}^{-}{\hat{b}}_{k}^{n} \end{matrix}] = [\begin{matrix} \exp (Ω (ω_{k - 1}^{b}) T_{s}) & 0_{3} \\ 0_{3} & I_{3} \end{matrix}] [\begin{matrix} {}^{+}{\hat{q}}_{k - 1}^{b n} \\ {}_{m}^{+}{\hat{b}}_{k - 1}^{n} \end{matrix}]

Process noise covariance matrix

Q_{k - 1} = [\begin{matrix} {(\frac{{}^{g}σ T_{s}}{2})}^{2} Ξ ({}^{+}{\hat{q}}_{k - 1}^{b n}) I_{3} Ξ {({}^{+}{\hat{q}}_{k - 1}^{b n})}^{T} & 0_{4 \times 3} \\ 0_{3 \times 4} & {}^{b}σ^{2} \cdot I_{3} \end{matrix}]

Update:

compute

{}^{-}{\hat{R}}^{b n} = q u a t e r n i o n T o M a t r i x ({}^{-}{\hat{q}}^{b n})

y_{k} = [\begin{matrix} {}_{m a g}y_{k} \\ {}_{a c c}y_{k} \end{matrix}], \begin{matrix} {}_{m a g}y_{k} \leftarrow m a g n e t o m e t e r s a m p l e \\ {}_{a c c}y_{k} \leftarrow a c c e l e r o m e t e r s a m p l e \end{matrix}

Sensor data validation

{}_{a c c}R = {\begin{matrix} I_{3} \cdot {}^{a}σ^{2}, i f | {}_{a c c}y - {}^{-}{\hat{R}}^{b n} g^{n} | < {}_{a c c}ε \\ \infty, o t h e r w i s e \end{matrix}

{}_{m a g}R = {\begin{matrix} I_{3} \cdot {}^{m}σ^{2}, i f | {}_{m a g}y - {}^{-}{\hat{R}}^{b n} (h^{n} - {}_{m}^{-}{\hat{b}}_{k}^{n}) | < {}_{m a g}ε \\ \infty, o t h e r w i s e \end{matrix}

R = [\begin{matrix} {}_{m a g}R \\ {}_{a c c}R \end{matrix}]

Jacobian calculation

H = [\begin{matrix} Ψ ({}^{-}{\hat{q}}_{k}^{b n}, h^{n} + {}_{m}^{-}{\hat{b}}_{k}^{n}) & R^{b n} \\ Ψ ({}^{-}{\hat{q}}_{k}^{b n}, g^{n}) & 0_{3} \end{matrix}]

{}^{+}{\hat{x}} \leftarrow

KalmanUpdate (

{}^{-}{\hat{x}}

Q_{k - 1}

y_{k}

H

R

)

end for