Modelbased extended quaternion Kalman filter to inertial orientation tracking of arbitrary kinematic chains
 Agnieszka Szczęsna^{1}Email author and
 Przemysław Pruszowski^{1}
Received: 24 April 2016
Accepted: 4 November 2016
Published: 14 November 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.
Keywords
Inertial motion capture Orientation estimation Kalman filter Quaternion Kinematic chainBackground
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 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).
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 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: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.$$\begin{aligned} x_{k+1}=x^{}_{k+1}+K_{k+1}[z_{k+1}f(x^{}_{k+1})] \end{aligned}$$(22)

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 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.
Description of experiments data
Name  Samples number  Initial extorsion (°)  Max. \(\omega\) (rad /s)  Max. amplitude a (\(\rm{m/s^2}\)) 

High accelerations  
Middle_Hight  14805  90 (to S2)  9  15.4 
Up_Hight  9769  90 (to S1)  10.5  22 
Dynamic  16228  9 times repeated 90 (to S1)  11  23 
Low accelerations  
Down_Low  10051  45 (to S3)  2.7  4 
Middle_Low  9621  45 (to S2)  4.6  4.2 
Up_Low  9921  45 (to S1)  10.3  13.4 
Down_Hight  12799  90 (to S3)  6.6  11 
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.
Filter parameters

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
Average error angle (rad) in each segment of pendulum
Segment  EQKF  MBEQKF 

S1  0.227015997  0.084566179 
S2  0.202370805  0.130596741 
S3  0.300123365  0.156497101 
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.
Declarations
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.
Open AccessThis 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.
Authors’ Affiliations
References
 Chou JCK (1992) Quaternion kinematic and dynamic differential equations. IEEE Trans Robot Autom 8(1):53–64View ArticleGoogle Scholar
 ElGohary M, McNames J (2012) Shoulder and elbow joint angle tracking with inertial sensors. IEEE Trans Biomed Eng 59(9):2635–2641View ArticlePubMedGoogle Scholar
 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–194Google Scholar
 Gramkow C (2001) On averaging rotations. J Math Imaging Vis 15(1–2):7–16MathSciNetView ArticleMATHGoogle Scholar
 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–408Google Scholar
 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–917Google Scholar
 Lin JFS, Kulić D (2012) Human pose recovery using wireless inertial measurement units. Physiol Meas 33(12):2099View ArticlePubMedGoogle Scholar
 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–7Google Scholar
 Mahony R, Hamel T, Pflimlin JM (2008) Nonlinear complementary filters on the special orthogonal group. IEEE Trans Autom Control 53(5):1203–1218MathSciNetView ArticleGoogle Scholar
 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)Google Scholar
 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–748Google Scholar
 Roetenberg D, Luinge H, Slycke P (2009) Xsens mvn: full 6dof human motion tracking using miniature inertial sensors. Tech. Rep, Xsens Motion Technologies BVGoogle Scholar
 Sabatini AM (2006) Quaternionbased extended kalman filter for determining orientation by inertial and magnetic sensing. IEEE Trans Biomed Eng 53(7):1346–1356View ArticlePubMedGoogle Scholar
 Sabatini AM (2011) Estimating threedimensional orientation of human body parts by inertial/magnetic sensing. Sensors 11(2):1489–1525View ArticlePubMedPubMed CentralGoogle Scholar
 Sabatini AM (2011) Kalmanfilterbased orientation determination using inertial/magnetic sensors: observability analysis and performance evaluation. Sensors 11(10):9182–9206View ArticlePubMedPubMed CentralGoogle Scholar
 Shuster MD, Oh SD (1981) Threeaxis attitude determination from vector observations. J Guid Control Dyn 4(1):70–77ADSView ArticleMATHGoogle Scholar
 Š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–144View ArticleGoogle Scholar
 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–387Google Scholar
 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–520Google Scholar
 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):333View ArticlePubMed CentralGoogle Scholar
 Vikas V, Crane CD (2016) Joint angle measurement using strategically placed accelerometers and gyroscope. J Mech Robot 8(2):021003View ArticleGoogle Scholar
 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–267Google Scholar
 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–186Google Scholar
 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–1227View ArticleGoogle Scholar