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 A1

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 gravity and acceleration (KF_acc)

{}_{a c c}^{-}x_{k} = [\begin{matrix} {}^{-}{\hat{g}}_{k}^{b} \\ {}^{-}{\hat{a}}_{k}^{b} \end{matrix}] = [\begin{matrix} \exp (- [ω_{k - 1}^{b} \times] \cdot T_{s}) & 0_{3} \\ 0_{3} & c_{a} \cdot I_{3} \end{matrix}] [\begin{matrix} {}^{+}{\hat{g}}_{k - 1}^{b} \\ {}^{+}{\hat{a}}_{k - 1}^{b} \end{matrix}]

Process noise covariance matrix (KF_acc)

{}_{a c c}Q_{k - 1} = [\begin{matrix} [{}^{-}{\hat{g}}_{k}^{b} \times] & 0_{3} \\ 0_{3} & c_{b} \cdot I_{3} \end{matrix}] [\begin{matrix} I_{3} \cdot {({}^{g}σ T_{s})}^{2} & 0_{3} \\ 0_{3} & I_{3} \end{matrix}] {[\begin{matrix} [{}^{-}{\hat{g}}_{k}^{b} \times] & 0_{3} \\ 0_{3} & c_{b} \cdot I_{3} \end{matrix}]}^{T}

Predict Earth’s magnetic field and disturbances (KF_mag)

{}_{m a g}^{-}x_{k} = [\begin{matrix} {}^{-}{\hat{h}}_{k}^{b} \\ {}^{-}{\hat{d}}_{k}^{b} \end{matrix}] = [\begin{matrix} \exp (- [ω_{k - 1}^{b} \times] \cdot T_{s}) & 0_{3} \\ 0_{3} & c_{a} \cdot I_{3} \end{matrix}] [\begin{matrix} {}^{+}{\hat{h}}_{k - 1}^{b} \\ {}^{+}{\hat{d}}_{k - 1}^{b} \end{matrix}]

Process noise covariance matrix (KF_mag)

{}_{m a g}Q_{k - 1} = [\begin{matrix} [{}^{-}{\hat{h}}_{k}^{b} \times] & 0_{3} \\ 0_{3} & c_{b} \cdot I_{3} \end{matrix}] [\begin{matrix} I_{3} \cdot {({}^{g}σ T_{s})}^{2} & 0_{3} \\ 0_{3} & I_{3} \end{matrix}] {[\begin{matrix} [{}^{-}{\hat{h}}_{k}^{b} \times] & 0_{3} \\ 0_{3} & c_{b} \cdot I_{3} \end{matrix}]}^{T}

Update:

H = {}_{m a g}H = {}_{a c c}H = [\begin{matrix} I_{3} & I_{3} \end{matrix}]

Measurement update (KF_acc)

{}_{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

{}_{a c c}R = I_{3} \cdot {}^{a}σ^{2}

{}_{a c c}^{+}{\hat{x}}_{k} \leftarrow

KalmanUpdate (

{}_{a c c}^{-}x_{k}

{}_{a c c}Q_{k - 1}

{}_{a c c}y_{k}

H

{}_{a c c}R

)

Measurement update (KF_mag)

{}_{m a g}y_{k} \leftarrow m a g n e t o m e t e r s a m p l e

{}_{m a g}R = I_{3} \cdot {}^{m}σ^{2}

{}_{m a g}^{+}{\hat{x}}_{k} \leftarrow

KalmanUpdate (

{}_{a c c}^{-}x_{k}

{}_{m a g}Q_{k - 1}

{}_{m a g}y_{k}

H

{}_{m a g}R

)

Orientation computation (TRIAD algorithm)

Normalize

{}^{+}{\hat{g}}_{k}^{b}

and

{}^{+}{\hat{h}}_{k}^{b}

to one

M_{o b s} = [\begin{matrix} {}^{+}{\hat{g}}_{k}^{b} & {}^{+}{\hat{g}}_{k}^{b} \times {}^{+}{\hat{h}}_{k}^{b} & {}^{+}{\hat{g}}_{k}^{b} \times ({}^{+}{\hat{g}}_{k}^{b} \times {}^{+}{\hat{h}}_{k}^{b}) \end{matrix}]

M_{r e f} = [\begin{matrix} g^{n} & g^{n} \times h^{n} & g^{n} \times (g^{n} \times h^{n}) \end{matrix}]

{\hat{q}}_{k}^{b n} = m a t r i x T o Q u a t e r n i o n (M_{r e f} M_{o b s}^{T})

end for