 Research
 Open Access
 Published:
Modelbased extended quaternion Kalman filter to inertial orientation tracking of arbitrary kinematic chains
SpringerPlus volume 5, Article number: 1965 (2016)
Abstract
Inertial orientation tracking is still an area of active research, especially in the context of outdoor, realtime, human motion capture. Existing systems either propose loosely coupled tracking approaches where each segment is considered independently, taking the resulting drawbacks into account, or tightly coupled solutions that are limited to a fixed chain with few segments. Such solutions have no flexibility to change the skeleton structure, are dedicated to a specific set of joints, and have high computational complexity. This paper describes the proposal of a new modelbased extended quaternion Kalman filter that allows for estimation of orientation based on outputs from the inertial measurements unit sensors. The filter considers interdependencies resulting from the construction of the kinematic chain so that the orientation estimation is more accurate. The proposed solution is a universal filter that does not predetermine the degree of freedom at the connections between segments of the model. To validation the motion of 3segments single link pendulum captured by optical motion capture system is used. The next step in the research will be to use this method for inertial motion capture with a human skeleton model.
Background
Inertial motion capture systems are based on body sensor network, where inertial measurement unit (IMU) sensors are attached to each major segment that should be tracked (Kulbacki et al. 2015; Roetenberg et al. 2009). The model (skeleton) of tracked object is built from rigidbody segments (defined as bones) connected by joints. The mapping of IMU orientations to specific segments of the body model allows for motion capture of the subject. The orientations of sensors are typically estimated by fusing a gyroscope rate (\(\omega\)), linear acceleration (a), and magnetic field measurements (m) with respect to the global reference frame (usually aligned with Earth gravity and local magnetic north). With knowledge of all the orientations of segments over time, the overall pose can be tracked. In the literature, many methods for orientation estimation based on single IMU output signals can be found e.g., Kalman filters (Sabatini 2011; Madgwick et al. 2011), complementary filters (Mahony et al. 2008). However, this loosely coupled approach where each segment is treated independently has numerous drawbacks. Joint constraints, such as those found in the human anatomy, cannot be included easily into the tracking. The correlations between the segments are lost during estimation (Miezal et al. 2013). Furthermore, tightly coupled systems, where all parameters and measurements are considered jointly in one estimation problem, have previously been shown to provide better performance (Young 2010).
In Young et al. (2010), propagation of linear accelerations through the segment hierarchy was used to improve the identification of the gravity components under high acceleration motions. That solution is based on a very simple complementary filter (Szczęsna et al. 2016).
Next, we can find solutions based on the Kalman filter for a specific set of segments by predetermining the DOF at the connections between the segments of the model. Such solutions are based on the Denavit–Hartenberg convention and use Euler angles as their orientation representation. Examples are the extended Kalman filter for lower body parts (hip, knee, ankle) (Lin and Kulić 2012) and the unscented Kalman filter with similar process and measurement model for shoulder and elbow joint angle tracking (ElGohary and McNames 2012).
In Šlajpah et al. (2014), authors propose extended Kalman filters for each segment using 18 element state vectors. This algorithm uses quaternion representation of orientation. The solution is limited only to human walking.
A different concept is presented in Vikas and Crane (2016) where the joint angle is estimated based on more than one sensor, placed on the segment. The system is based on vestibular dynamic inclination measurements and estimates only 2 Euler angles.
Multibody systems based on IMU sensors can also estimate and track other parameters like positions, velocities, and accelerations (TorresMoreno et al. 2016).
The reported errors in the aforementioned publications (Young et al. 2010; Lin and Kulić 2012; ElGohary and McNames 2012; Šlajpah et al. 2014) are not comparative because experiments had different conditions and concerns about various movements and errors were calculated in an inconsistent way, all of the average angles errors were about 4°–7°. The referenced values were obtained from different sources: simulated, mechanical, optical systems or calculated based on depth camera.
This paper proposes a new modelbased extended quaternion Kalman filter (MBEQKF) that allows estimation of orientation on the basis of outputs from the IMU sensors. This filter reflects interdependencies from the construction of the kinematic chain so that the orientation estimation is more accurate. The proposed solution is a universal filter that does not predetermine any degree of freedom (DOF) at the connections between the segments of the model. Our aspiration for future work is to use our novel method for inertial motion capture.
Modelbased extended quaternion Kalman filter (MBEQKF)
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 (ElGohary 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 fourdimensional noncommutative 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 firstorder 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 nonlinear 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:
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:
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 unitvector axis of rotation. Where \(\theta n\) represent instantaneous angular velocity.
The resulting quaternion produced by the process must be normalized. We used the bruteforce 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:
and from body to global
where \(q^*\) is a conjugate quaternion:
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:
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 zeromean white noise process with covariance matrix \(\sigma _{g}^{2}I\). The quaternion timeevolution is a firstorder 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 quaterniondependent linear transformation (Sabatini 2011). The process noise covariance matrix \(Q_{k}\) is following:
where for \(q_k=(a, [b,c,d])\) we define as follows
The model of tracked object is built from rigidbody 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 3segments 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:
where o is the offset of the point from the centre of rotation.
Also the every point on the rigid body has tangential acceleration:
where \(\alpha\) is an angular acceleration calculated from angular velocity as:
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:
The model joint acceleration is:
where
Also, the distances must be transformed into the body coordinate frame:
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:
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:
Since the above output is nonlinear, it is linearized by computing the Jacobian matrix,
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)
Experimental setup
For a test of the proposed universal MBEQKF, a 3segment single linked pendulum was built. As reference, data from the optical system of motion capture (Vicon system) were used. Experiments demonstrate that using body model kinematic dependences in the orientation filter can improve the accuracy of the inertial motion capture system. Through simple procedural calibration and mounting sensors permanently, we remove the effect of bad calibration factors on the estimation of orientation.
The pendulum was built with three segments connected by movable single linked joints. An IMU sensor built at the Silesian University of Technology, Department of Automatic Control and Robotics was fixed to each segment. The published in Jedrasiak et al. (2013) IMU sensors signal to noise coefficients are as follows: accelerometer 43.2, magnetometer 767.9 and gyroscope 254.5. These IMU sensors have been marked as IMU1, IMU2 and IMU3 (Figs. 1, 2). On the pendulum, markers were also attached, marked as R1, R2, W1, W2, W3, W4, W5, and W6. The motion of the pendulum was mainly on one axis, but motion on other axes was also measured (shaking and swinging the pendulum from side to side). This had no effect on the results of estimations because the axes of motion are not aligned with the sensor axes.
The data were recorded by using a USB connection of sensors to the PC via application, which allowed for the capture of raw signals from IMU sensors. Next the data were processed by filters implemented in Matlab.
Recordings were captured with seven different scenarios (each scenario repeated 3 times) carried out using the Vicon system with a frequency of 100 Hz. The IMU sensors also worked with such a frequency. The recordings had a length from 9600 to 16228 samples. The recorded movement is characterized by different values of acceleration amplitudes: (4–15> \(\rm{m /s^2}\) for low acceleration dataset and (15–23> \(\rm{m /s^2}\) for high acceleration dataset. The optical system also enabled calibration of sensors and calculation of the necessary distances. The scenarios relied on forcing motion (Low or High swing) to a certain segment (S1—up, S2—middle, and S3—down) of the pendulum (Table 1). The initial extorsion angles to each segment are presented in Fig. 2. The scenario marked as Dynamic relied on repeated forcing swing and recording to the total suppression of the pendulum. The data are available in our RepoIMU repository^{Footnote 1} (Szczęsna et al. 2016).
Data synchronization and error calculation
Each experiment was recorded using Vicon Nexsus system with a sampling frequency of 100 Hz. In order to provide an informative comparison of orientation data streams with different reference frames and measured according to separate timers with the same frequency, the data must be normalized. Such a procedure can be divided into two steps: normalization in the time domain (time synchronization) and transformation of orientations to the same reference frame.
Transforming one orientation data stream from one reference frame to the other one is a simple geometric operation—rotation. Only knowledge about the relationship between two world reference frames—navigation and body—is required. As a reference body frame, the first body frame from the time domain was chosen.
Signals from the Vicon system and IMU sensors were captured at the same frequency. Knowing that, in order to synchronize the time domain we needed to find the time offset \((\varDelta {t})\) between the two signals. A time window was chosen to be \(<\varDelta t^{Max} , \varDelta t^{Max}>\), where \(\varDelta t^{Max}\) is the maximal offset we expected \((\varDelta t^{Max}< \varDelta t < \varDelta t^{Max})\). The distance between the two signals for each time offset in the window is calculated. Synchronization is performed on the \(^B\omega _{IMU}\) signal. The Vicon system does not calculate angular velocity of the body directly, so it must be calculated by the equation:
where \(q^{1}\) is the inverse of q.
Evaluation of performances of the presented filter was done on the basis of the average deviations between true and estimated orientations of the body (Gramkow 2001). Here, we used the deviation index DI corresponding to the geodesic distance between two quaternions—filter estimate \(\hat{q}\) and the true rotation q from the Vicon system, on the hypersphere \(S^3\):
All evaluations and comparisons of the performances of algorithms for orientation estimation are based on the deviation index averaged over the experiment time horizon.
Filter parameters
Filter parameters are following:

reference vectors: \(^{N}g=[0, 0, 9.81]^{T}\) and \(^{N}m=[\cos (\varphi ^{L})\sin (\varphi ^{L})]^{T},\) where \(\varphi ^{L}\) is the geographical latitude angle. For the geographical position of the laboratory, where measurements were done, we have \(\varphi ^{L}=66^{\circ}=1.1519\) rad;

parameters of noise: \(\sigma _g^2 = 0.0001\), \(\sigma _a^2 = 0.001\) and \(\sigma _m^2 = 0.000001\);

initial state \(x_0\) (starting orientation quaternion) is computed by the QUEST algorithm (Shuster and Oh 1981) based on values of acceleration and magnetic field vector of first sample;

sampling interval \(\varDelta t= 0.01\);

state covariance matrix \(P_0 = I_{4x4}\).
Results and discussion
We performed tests for the MBEQKF with kinematic dependences and the same filter but without kinematic equations in the measurement model (EQKF) (16). In each test filter MBEQKF obtained better results than EQKF (Fig. 3). The results for high dynamic motion were always worse than that for the corresponding low dynamic tests. It is well known that a factor that strongly influences the orientation measurement is the existence and magnitude of the external acceleration of the IMU sensor. One way to manage with this is using methods of levelling the influence of linear external acceleration by, for example, an adaptation mechanism (Pruszowski et al. 2015). The highest error in each test of EQKF is always for the third segment because there are the highest accelerations. Filter MBEQKF using the kinematic chain dependences can overcome that factor. It can be seen in Fig. 4, where the maximum error is similar, but most times the error of MBEQKF is near zero value and the error does not grow in time, like it does for EQKF. The bigger error is still caused by higher acceleration values but is lower than in other filters without this mechanism (see Figs. 4, 5 for this same capture and segment).
In Table 2, the average angle errors of all tests with the pendulum are presented. The average error of the MBEQKF filter is about 6°–7° which is comparable with other solutions described in the literature. In Fig. 6 is presented the result of filter MBEQKF estimation converted to Euler angles in comparison to angles computed from optical motion capture system (Vicon).
Examination of the trace of the error covariance matrix \(P_k\), which should be minimized, can measure the convergence of the Kalman filter. This condition is fulfilled in the proposed filter (Fig. 7).
Figure 8 presents a comparison of average errors to other filters with a dynamic mechanism of levelling influence of external acceleration to orientation estimation. Filter AEQKF is an extended Kalman filter with an adaptive mechanism in which the measurement noise covariance matrix is adapted at runtime to guard against the effects of body motion. The implementation is based on Angelo (2006). Filter NCF_L is implemented based on Young (2010), where a simple complementary filter was used by passing the acceleration estimation in the skeleton model (Szczęsna et al. 2016). The proposed solution (filter MBEQKF) has the lowest error. The results are similar to the NCF_L filter because they use a similar mechanism to transfer modeled acceleration in the kinematic chain. But better results are achieved by combining this with the extended Kalman filter technique. Filter AEQKF is a Kalman filter but uses, to level the influence of high acceleration, the adaptation mechanism without using the kinematic chain dependences. In the described experiment, this led to the higher average errors.
Conclusion
The article presents an evaluation of opportunities to improve the orientation estimation by using kinematic dependences in the kinematic chain. The results are shown for the two extended quaternion Kalman filters based on one segment (EQKF) and with kinematic dependences (MBEQKF). The results, based on experiments with the 3segment sigle link pendulum, show a superiority of the solution based on the estimation of acceleration in the body model (skeleton), especially for child segments. The filter is universal with the small state vector and gives comparable results for an average angle error of about 6–7 degrees with other, more complex solutions presented in the literature.
References
Chou JCK (1992) Quaternion kinematic and dynamic differential equations. IEEE Trans Robot Autom 8(1):53–64
ElGohary M, McNames J (2012) Shoulder and elbow joint angle tracking with inertial sensors. IEEE Trans Biomed Eng 59(9):2635–2641
Foxlin E (1996) Inertial headtracker sensor fusion by a complementary separatebias kalman filter. In: Proceedings of the IEEE 1996 on virtual reality annual international symposium, pp 185–194
Gramkow C (2001) On averaging rotations. J Math Imaging Vis 15(1–2):7–16
Jędrasiak K, Daniec K, Nawrat A (2013) The low cost micro inertial measurement unit. In: 8th IEEE conference on industrial electronics and applications, pp 403–408
Kulbacki M, Koteras R, Szczęsna A, Daniec K, Bieda R, Słupik J, Segen J, Nawrat A, Polański A, Wojciechowski K (2015) Scalable, wearable, unobtrusive sensor network for multimodal human monitoring with distributed control. In: 6th European conference of the international federation for medical and biological engineering. Springer International Publishing, pp 914–917
Lin JFS, Kulić D (2012) Human pose recovery using wireless inertial measurement units. Physiol Meas 33(12):2099
Madgwick Sebastian OH, Harrison Andrew JL, Vaidyanathan R (2011) Estimation of imu and marg orientation using a gradient descent algorithm. In: 2011 IEEE international conference on rehabilitation robotics, pp 1–7
Mahony R, Hamel T, Pflimlin JM (2008) Nonlinear complementary filters on the special orthogonal group. IEEE Trans Autom Control 53(5):1203–1218
Miezal M, Bleser G, Schmitz N, Stricker D (2013) A generic approach to inertial tracking of arbitrary kinematic chains. In: Proceedings of the 8th international conference on body area networks, pp 189–192. ICST (Institute for Computer Sciences, SocialInformatics and Telecommunications Engineering)
Pruszowski P, Szczęsna A, Polański A, Słupik J, Wojciechowski K (2015) Adaptation mechanism of feedback in quaternion kalman filtering for orientation estimation. In: Artificial intelligence and soft computing. Springer International Publishing, pp 739–748
Roetenberg D, Luinge H, Slycke P (2009) Xsens mvn: full 6dof human motion tracking using miniature inertial sensors. Tech. Rep, Xsens Motion Technologies BV
Sabatini AM (2006) Quaternionbased extended kalman filter for determining orientation by inertial and magnetic sensing. IEEE Trans Biomed Eng 53(7):1346–1356
Sabatini AM (2011) Estimating threedimensional orientation of human body parts by inertial/magnetic sensing. Sensors 11(2):1489–1525
Sabatini AM (2011) Kalmanfilterbased orientation determination using inertial/magnetic sensors: observability analysis and performance evaluation. Sensors 11(10):9182–9206
Shuster MD, Oh SD (1981) Threeaxis attitude determination from vector observations. J Guid Control Dyn 4(1):70–77
Šlajpah S, Kamnik R, Munih M (2014) Kinematics based sensory fusion for wearable motion assessment in human walking. Comput Methods Prog Biomed 116(2):131–144
Szczęsna A, Pruszowski P, Słupik J, Pęszor D, Polański A (2016) Evaluation of improvement in orientation estimation through the use of the linear acceleration estimation in the body model. In: Man–machine interactions, vol 4. Springer International Publishing, pp 377–387
Szczęsna A, Skurowski P, Pruszowski P, Pęszor D, Paszkuta M, Wojciechowski K (2016) Reference data set for accuracy evaluation of orientation estimation algorithms for inertial motion capture systems. In: International conference on computer vision and graphics. Springer International Publishing, pp 509–520
TorresMoreno JL, BlancoClaraco JL, GiménezFernández A, Sanjurjo E, Naya MÁ (2016) Online kinematic and dynamicstate estimation for constrained multibody systems based on imus. Sensors 16(3):333
Vikas V, Crane CD (2016) Joint angle measurement using strategically placed accelerometers and gyroscope. J Mech Robot 8(2):021003
Young AD, Ling Martin J, Arvind DK (2010) Distributed estimation of linear acceleration for improved accuracy in wireless inertial motion capture. In: Proceedings of the 9th ACM/IEEE international conference on information processing in sensor networks, pp 256–267
Young AD (2010) Use of body model constraints to improve accuracy of inertial motion capture. In: IEEE 2010 international conference on body sensor networks, pp 180–186
Yun X, Bachmann ER (2006) Design, implementation, and experimental results of a quaternionbased kalman filter for human body motion tracking. IEEE Trans Robot 22(6):1216–1227
Authors' contributions
AS conceived and designed the study and wrote the article. PP performed the implementation in Matlab and reviewed the paper. Both authors read and approved the final manuscript
Acknowledgements
This work was supported by a statute project of the Silesian University of Technology, Institute of Informatics (BK263/RAU2/2015). This work was partly performed using the infrastructure supported by POIG.02.03.0124099/13 Grant: “GCONiIUpperSilesian Center for Scientific Computation”. Data were captured in the Human Motion Laboratory of PolishJapanese Academy of Information Technology (http://bytom.pja.edu.pl/, http://hm.pjwstk.edu.pl/en/).
Competing interests
The authors declare that they have no competing interests.
Author information
Rights and permissions
Open Access This article is distributed under the terms of the Creative Commons Attribution 4.0 International License (http://creativecommons.org/licenses/by/4.0/), which permits unrestricted use, distribution, and reproduction in any medium, provided you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons license, and indicate if changes were made.
About this article
Received
Accepted
Published
DOI
Keywords
 Inertial motion capture
 Orientation estimation
 Kalman filter
 Quaternion
 Kinematic chain