The aim was to simplify the structure of the filter while maintaining corrections resulting from the kinematic relationships in the model; another important element was versatility. The proposed solution does not predetermine any DOF at the connections between the segments of the model, as it is in solutions based on the Denavit–Hartenberg convention (El-Gohary and McNames 2012; Lin and Kulić 2012).
As a base of implementation, the quaternion extended Kalman filter with direct state was used. The unit quaternion \(q = (q_0, [q_1, q_2, q_3])^T\,\epsilon\,\mathbb {H}\) represents the body orientation, where \(\mathbb {H}\) is a four-dimensional non-commutative division algebra over the real numbers. The orientation quaternion is MBEQKF filter state vector \(x = q\). The angular velocity is considered to be a control input (like in Angelo 2006). The angular velocity is not part of the state vector so model of dynamic, e.g., dynamic model of human limb motion in terms of first-order Gauss–Markov stochastic process (Yun and Bachmann 2006), is not in the development of the filter equations. The state vector has a smaller dimension and it is not necessary to include first and second derivative of angular velocity in the state vector to obtain the optimal model (Sabatini 2011; Sabatini 2011; Foxlin 1996), like in filter described in Šlajpah et al. (2014) where the state vector has 18 and measurement vector has 12 elements. The non-linear measurement equations are defined by rotating the reference vectors (magnetic Earth field and gravity) using estimated orientation quaternion. Also, the Newton–Euler kinematic motion equations are used to model acceleration measurements in the kinematic chain.
The MBEQKF filter process model is kinematic Eq. (1) (Chou 1992), which describes the relation between temporal derivatives of an orientation represented by unit quaternion q and angular velocity of the body frame (\(^{B}{\omega }\)) relative to the global frame N:
$$\begin{aligned} \frac{d}{dt}q(t)=\frac{1}{2}q(t)\otimes (0, ^{B}{\omega })^T \end{aligned}$$
(1)
where \(\otimes\) stands for the quaternion multiplication. The left superscript indicates that the coordinate frames in which vectors are expressed (measured) are N for the Earth fixed coordinate system or B for the system related to the moving body.
Multiplying two unit quaternions gives a unit quaternion representing the composition of the two rotations. Hence, we can describe the orientation now and at the next moment in time, assuming a constant angular velocity:
$$\begin{aligned} q(0) = q_0,\quad q(1) = q_k \otimes q_o,\quad q(t) = (q_k)^t \otimes q_o \end{aligned}$$
(2)
By using Euler formula for quaternion we can write quaternion as \(q_k = exp(\frac{\theta }{2}n)\), where \(\theta\) is an angle, and n is the unit-vector axis of rotation. Where \(\theta n\) represent instantaneous angular velocity.
The resulting quaternion produced by the process must be normalized. We used the brute-force approach and normalise the quaternion after the measurement update stage, which lead to a suboptimal algorithm (Sabatini 2011).
By using the orientation quaternion, every vector can be translated from the global frame N to the body coordinate system B:
$$\begin{aligned} ^{B}{v}= q^{*}\otimes (0, ^{N}{v})^T\otimes q \end{aligned}$$
(3)
and from body to global
$$\begin{aligned} ^{N}{v}= q\otimes (0, ^{B}{v})^T\otimes q^{*} \end{aligned}$$
(4)
where \(q^*\) is a conjugate quaternion:
$$\begin{aligned} q^*=(q_0, [-q_1, -q_2, -q_3])^T \end{aligned}$$
In practical implementations, orientation estimation is realised on the basis of digital systems. The discrete time index is denoted by the subscript k. The discretized priori state estimation equation of the orientation kinematics process corresponding to (Eqs.1 and 2) is as follows:
$$\begin{aligned} x^{-}_{k+1}=\varPhi _{k}x_{k}+n_{k}=\exp \left[ \frac{1}{2}M_{R}(^B{\omega }_k)\varDelta t\right] x_{k}+n_{k}. \end{aligned}$$
(5)
In this equation, \(x^{-}_{k}\) is the discrete—priori estimates time state vector, \(x^{-}_{k}=q_{k}\), and \(M_{R}(^B{\omega }_k)\) denotes matrix representation of the quaternion right multiplication corresponding to the pure quaternion \((0, ^B{\omega }_k)^T\), and \(\varPhi\) is the state transition matrix. The components of the state vector are modelled as random walk, where n is the zero-mean white noise process with covariance matrix \(\sigma _{g}^{2}I\). The quaternion time-evolution is a first-order approximation of the exact process (1).
As the gyroscope data are external inputs to the filter rather than measurement, gyroscope measurement noise enters the filter as process noise through a quaternion-dependent linear transformation (Sabatini 2011). The process noise covariance matrix \(Q_{k}\) is following:
$$\begin{aligned} Q_{k}=(\varDelta t/2)^{2}\varXi _{k}(\sigma _{g}^{2}I_{4x4})\varXi _{k}^{T} \end{aligned}$$
(6)
where for \(q_k=(a, [b,c,d])\) we define as follows
$$\begin{aligned} \varXi _k=\left[ \begin{array}{ccc} a&\quad-d&\quad c\\ d&\quad a&\quad -b\\ -c&\quad b&\quad a\\ -b&\quad -c&\quad -d\\ \end{array} \right] \end{aligned}$$
The model of tracked object is built from rigid-body segments connected by joints. Sensors are attached to segments with constant offset vector from the centre of segment rotation. The defined model is a skeleton of object. In our experiments we use model of 3-segments pendulum. Each segment (with IMU) and joint has a local coordinate frame related to the coordinate frame of the sensor. Joints form a hierarchy structure with the position of a child joint given by an offset from the parent joint centre. Resulting orientations are calculated in the world coordinate frame based on two reference vectors, Earth gravity g and magnetic north mg. Quantities marked with superscript j are referring to a corresponding j segment.
The Newton–Euler equations describe the combined translational and rotational dynamics of a rigid body and can be the base of the measurement model of acceleration in a kinematic chain (skeleton model). The modelled linear acceleration of the sensor is considered the case of a rigid body rotating about a point, fixed at the origin with angular velocity \(\omega\). Every point on this body will have a radial linear acceleration:
$$\begin{aligned} l_r = (\omega \cdot o)\omega - o \left\| \omega \right\| ^2 \end{aligned}$$
(7)
where o is the offset of the point from the centre of rotation.
Also the every point on the rigid body has tangential acceleration:
$$\begin{aligned} l_t = \alpha \times o \end{aligned}$$
(8)
where \(\alpha\) is an angular acceleration calculated from angular velocity as:
$$\begin{aligned} \alpha = \frac{\omega _{k+1} - \omega _{k-1}}{2\varDelta t} \end{aligned}$$
(9)
The angular acceleration is the derivative of angular velocity and can be calculated for example by first central difference approximation based on angular velocity samples.
The whole body is in a rotating frame with a linear acceleration \(l_f\) and this is a linear acceleration from the parent segment in the skeleton model. The resulting linear acceleration of a point under that assumption is therefore: \(l = l_f + l_r + l_t\).
In the model for every segment we have a linear acceleration of the sensor \(a^{S,j}\) (10), and a linear acceleration of the joint \(a^{J,j}\) (11), which is passed to the next segment \(j+1\) as a linear acceleration of the parent. All linear accelerations that are passed between segments are in the global coordinate frame \(^{N}{a}^{J,j}_k\).
The model sensor acceleration is:
$$\begin{aligned} ^{B}{a}^{S,j}_{k}\,=\,^{B}{a}^{J, j-1}_{k} +\, ^B{\omega }_{k}\,\times\,(^B{\omega }_{k} \times ^B{o}^{S,j}_{k}) +\,^{B}{\alpha }_k\,\times\, ^B{o}^{S,j}_k +\, ^B{g}_{k} \end{aligned}$$
(10)
The model joint acceleration is:
$$\begin{aligned} ^B{a}_k^{J,j}\,=\, ^{B}{a}^{J, j-1} +\, ^B{\omega }_{k} \times (^B{\omega }_{k} \times ^B{o}^{J,j}_k) +\, ^{B}{\alpha }_k \,\times\, ^B{o}^{J,j}_{k} \end{aligned}$$
(11)
where
$$\begin{aligned} ^{B}{a}^{J, j-1}_k &= q_k^{*}\otimes (0, ^{N}{a}^{J,j-1}_k)^T\otimes q_k \end{aligned}$$
(12)
$$\begin{aligned} ^B{g}_k& = q_k^{*}\otimes (0, ^{N}{g})^T\otimes q_k \end{aligned}$$
(13)
$$\begin{aligned} ^{N}{a}^{J, j}_k& = q_k\otimes (0, ^{B}{a}^{J,j}_k)^T\otimes q_k^{*} \end{aligned}$$
(14)
Also, the distances must be transformed into the body coordinate frame:
$$\begin{aligned} ^{B}{o}_k = q_k^{*}\otimes (0, ^{N}{o}_k)^T\otimes q_k \end{aligned}$$
(15)
The MBEQKF filtering algorithm use model (5) for predicting aspects of behaviour of a system and a model of the sensor measurements (16), in order to produce the most accurate estimation of the state of system. The resulting measurement model, based on a priori estimates of the state vector, is of the form:
$$\begin{aligned} f(x^{-}_{k} = q_k)= \begin{bmatrix}^B{a}^{S,j}_k\\ q_k^{*}\otimes (0, ^{N}{mg})^T\otimes q_k \end{bmatrix}+\begin{bmatrix}n_{k}^{a}\\ n_{k}^{m} \end{bmatrix} \end{aligned}$$
(16)
where \(n_{k}^{a}\) and \(n_{k}^{m}\) are the accelerometer and magnetometer measurement noise with covariance matrices \(\sigma _{a}^{2}I\) and \(\sigma _{m}^{2}I\). The measurement noise covariance matrix V represents the level of confidence placed in the accuracy of the measurements:
$$\begin{aligned} V = \begin{bmatrix}\sigma _{a}^{2}I&\quad 0\\ 0&\quad\sigma _{m}^{2}I \end{bmatrix} \end{aligned}$$
(17)
Since the above output is non-linear, it is linearized by computing the Jacobian matrix,
$$\begin{aligned} H_{k}=\frac{d}{dx_{k}}f({x_k})\left| _{x_{k}=x^{-}_{k}}\right. . \end{aligned}$$
(18)
According to the notation introduced above, the our filter MBEQKF equations are summarised as follows:
-
the priori state estimate is:
$$\begin{aligned} x^{-}_{k+1}=\varPhi _{k}x_{k} \end{aligned}$$
(19)
-
the priori error covariance matrix is:
$$\begin{aligned} P^{-}_{k+1}=\varPhi _{k}P_{k}\varPhi _{k}^{T}+Q_{k} \end{aligned}$$
(20)
-
the Kalman gain is:
$$\begin{aligned} K_{k+1}=P^{-}_{k+1}H_{k+1}^{T}(H_{k+1}P^{-}_{k+1}H_{k+1}^{T}+V_{k+1})^{-1} \end{aligned}$$
(21)
-
the posteriori state estimate is:
$$\begin{aligned} x_{k+1}=x^{-}_{k+1}+K_{k+1}[z_{k+1}-f(x^{-}_{k+1})] \end{aligned}$$
(22)
The proposed filter is an additive filter which relaxes the quaternion normalization condition and treats the four components of the quaternion as independent parameters and uses the addition operation. Next, the resulting quaternion is normalized.
-
the posteriori error covariance matrix is:
$$\begin{aligned} P_{k+1}=P^{-}_{k+1}-K_{k+1}H_{k+1}P^{-}_{k+1}. \end{aligned}$$
(23)