Robust Inertial Measurement Unit-Based Attitude Determination Kalman Filter for Kinematically Constrained Links

The external acceleration of a fast-moving body induces uncertainty in attitude determination based on inertial measurement unit (IMU) signals and thus, frequently degrades the determination accuracy. Although previous works adopt acceleration-compensating mechanisms to deal with this problem, they cannot completely eliminate the uncertainty as they are, inherently, approaches to an underdetermined problem. This paper presents a novel constraint-augmented Kalman filter (KF) that eliminates the acceleration-induced uncertainty for a robust IMU-based attitude determination when IMU is attached to a constrained link. Particularly, this research deals with an acceleration-level kinematic constraint derived on the basis of a ball joint. Experimental results demonstrate the superiority of the proposed constrained KF over the conventional unconstrained KF: The average accuracy improved by 1.88° with a maximum of 4.18°. More importantly, whereas the accuracy of conventional KF is dependent to some extent on test acceleration conditions, that of the proposed KF is independent of these conditions. Due to the robustness of the proposed KF, it may be applied when accurate attitude estimation is needed regardless of dynamic conditions.


Introduction
Attitude determination based on the inertial measurement unit (IMU) is widely used in various fields, such as human motion tracking, sports science, and robotics [1][2][3][4][5][6][7][8]. For example, the attitude or the orientation, instead of position information, can be effectively used to track limb movements using IMU alone [9,10] or by the fusing of IMU and aiding sensors [11,12]. A typical IMU consists of a triaxial accelerometer and a triaxial gyroscope, and Kalman filtering is the most popular framework for the attitude determination [13,14]. Despite the variety of detailed approaches to Kalman filters (KF), the basic concepts of IMU-based attitude KFs are all the same: The attitude is predicted by time-integrating the gyroscope signals, but the unbounded drift is caused by the error accumulation in the course of integration. For the correction, the accelerometer provides a drift-free vertical reference by sensing gravity [14,15].
However, the case where the accelerometer signal is dominated by the gravitational acceleration is limited to the static condition, as the accelerometer signal in dynamic conditions is the sum of the gravitational acceleration and the external acceleration [15]. In order to distinguish between the two, attitude information is needed. It should be remembered that the attitude is, nonetheless, what we want to determine. Therefore, IMU-based attitude determination under dynamic conditions is a type of underdetermined problem because the accelerometer signal used for the correction has two unknowns: The attitude and the external acceleration. Accordingly, the external acceleration of performance of the proposed constrained KF in comparison with conventional unconstrained KFs under various accelerated conditions.

Problem Definition and Sensor Modeling
Let us assume that an IMU, to which the sensor-fixed frame S is attached, rotates with respect to a fixed inertial reference frame, I (see Figure 1). The rotation matrix S I R of {I}, with respect to {S}, can be interpreted as a set of three-unit axis column vectors of {I}, written in {S}. That is, Note that the last column, S Z I , represents the attitude, as it can be used to calculate roll and pitch, i.e., γ = atan2( S Z 2 , S Z 3 ) and β = atan2(− S Z 1 , S Z 2 /sin γ), respectively, when Hence, the purpose of the proposed KF is to determine S Z I . (henceforth, written simply as S Z, for convenience). In this paper, the axis vectors are represented by capital letters to distinguish them from other variables.

Problem Definition and Sensor Modeling
Let us assume that an IMU, to which the sensor-fixed frame S is attached, rotates with respect to a fixed inertial reference frame, I (see Figure 1). The rotation matrix R Hence, the purpose of the proposed KF is to determine Z S I (henceforth, written simply as Z S , for convenience). In this paper, the axis vectors are represented by capital letters to distinguish them from other variables. Sensor signals from the accelerometer (A) and gyroscope (G) are modeled, respectively, as follows: where g S is the gravity vector; ω is the angular velocity; a is the external acceleration; and the n 's are the measurement noises that are assumed to be zero-mean white Gaussian and also uncorrelated each other [17]. It is natural that vectors in sensor signals are observed in the sensor-frame coordinates, as indicated by the left superscript, S. When the Z-axis of the inertial frame points vertically upward (which is the most common setup), g S in Equation (2)

Acceleration-Level Ball Joint Constraint
This study deals with a ball-and-socket joint (or simply, a ball joint) constraint, which is defined by the condition that the center of the ball coincides with the center of the socket. This condition only restricts three translational degrees of freedom of a point in the ball-side link, relative to the socket-side link, but allows three rotational degrees of freedom. Then, we assumed that an IMU is Sensor signals from the accelerometer (A) and gyroscope (G) are modeled, respectively, as follows: where S g is the gravity vector; ω is the angular velocity; a is the external acceleration; and the n's are the measurement noises that are assumed to be zero-mean white Gaussian and also uncorrelated each other [17]. It is natural that vectors in sensor signals are observed in the sensor-frame coordinates, as indicated by the left superscript, S. When the Z-axis of the inertial frame points vertically upward (which is the most common setup), S g in Equation (2) can be expressed in terms of S Z, i.e., S g = g S Z, where g = 9.8 m/s 2 . It should be noted that Equation (2) has two unknowns that are interdependent: S Z and S a.

Acceleration-Level Ball Joint Constraint
This study deals with a ball-and-socket joint (or simply, a ball joint) constraint, which is defined by the condition that the center of the ball coincides with the center of the socket. This condition only restricts three translational degrees of freedom of a point in the ball-side link, relative to the socket-side link, but allows three rotational degrees of freedom. Then, we assumed that an IMU is mounted on a link, which was connected to another link via a ball joint. When observed in the sensor frame, the vector from the joint center to the origin of the sensor frame, S d, was constant during motion and could be predetermined during the initial calibration procedure. Moreover, the acceleration of the sensor could be considered as the sum of the acceleration of the joint center and the acceleration caused by the rotation of that sensor around the joint center. As a simplified system, the socket-side link was firmly fixed to the ground in this study (see Figure 1). Accordingly, the joint center's acceleration was zero, and the acceleration of the sensor was as follows: which is a function of the angular velocity and acceleration, and the predetermined constant position vector. Here, .
ω (having the overhead dot) represents the first-time derivative of ω, and [ω×] is a 3 × 3 skew symmetric matrix to represent the standard vector cross product. Because the ideal angular velocity S ω, in Equation (4) was unavailable in practice because of the measurement noise, it was necessary to express Equation (4) using the actual gyroscope output, y G . Therefore, if we replace S ω with y G − n G , according to Equation (3), and use [( where the error of the acceleration equation, S a ε , is derived as In Equation (5), . y G is obtained by numerical differentiation, i.e., . y G,t ≈ (y G,t − y G,t−1 )/∆t, where ∆t is the time step size. Furthermore, for the derivation of Equation (5) Finally, by using the relationship [a×]b = −[b×]a, the gyroscope noise n G and its first-time derivative . n G are set at the right side of the corresponding terms to enable the formulation of the noise covariance matrix.

Constraint-Augmented Kalman Filter
The proposed algorithm was based on our previous work [17], which was an attitude-determination algorithm, using a linear KF. The basic structure of the proposed KF can be defined by the following process and measurement models: and where is the state vector, z is measurement vector, Φ is the state transition matrix, H is the observation matrix, and w and v are the white Gaussian process and measurement noises, respectively. Because the purpose of our KF is to determine S Z, the state vector is simply defined as x = S Z.
First, the process model based on the strapdown integration of the gyroscope measurement is From Equation (7) and Equation (9), the transition matrix Φ t−1 , and process noises w t−1 , are I − ∆t[y G,t−1 ×] and ∆t(−[ S Z t−1 ×])n G , respectively. Moreover, the process noise covariance matrix is the gyroscope noise variance. One may refer to Reference [17] for details of the process model. Second, the measurement model was based on the accelerometer signal, Equation (2), combined with the acceleration-level kinematic constraint, Equation (5), derived in Section 2.2. By substituting Equation (5) into Equation (2), the constraint-augmented measurement equation becomes From Equation (8) and Equation (10), the measurement vector Z t , observation matrix H t , and measurement noises v t are and As n A and S a ε are uncorrelated, the measurement noise covariance matrix, , can be divided into two terms, as follows: where Σ A is the covariance matrix of the accelerometer's measurement noise and is set to σ 2 A I, using the noise variance of the accelerometer σ 2 A . Additionally, Σ ε,t is the covariance matrix of the constraint equation error and is defined as E[ S a ε,t S a T ε,t ]. Furthermore, under the assumption that terms [ S d×] . n G and are uncorrelated, Σ ε can be written as where Σ . n G is the covariance matrix of the time derivative of the gyroscope noise and is defined as n G is the variance of the derivative of the gyroscope noise. The overall structure of the proposed KF is illustrated in Figure 2. Second, the measurement model was based on the accelerometer signal, Equation (2), combined with the acceleration-level kinematic constraint, Equation (5), derived in Section 2.2. By substituting Equation (5) into Equation (2), the constraint-augmented measurement equation becomes , , , From Equation (8) and Equation (10), the measurement vector z t , observation matrix H t , and As n A and a S ε are uncorrelated, the measurement noise covariance matrix,  , can be divided into two terms, as follows: where Σ A is the covariance matrix of the accelerometer's measurement noise and is set to 2 I where n Σ G  is the covariance matrix of the time derivative of the gyroscope noise and is defined as σ  is the variance of the derivative of the gyroscope noise. The overall structure of the proposed KF is illustrated in Figure 2.

Experimental Setup and Test Conditions
For the verification of the proposed KF, which was implemented using MATLAB ® programming, an MPU6050 IMU sensor (from InvenSense, San Jose, CA, USA), that included a

Experimental Setup and Test Conditions
For the verification of the proposed KF, which was implemented using MATLAB ® programming, an MPU6050 IMU sensor (from InvenSense, San Jose, CA, USA), that included a triaxial gyroscope and a triaxial accelerometer was used. The MPU6050 signals were communicated to the PC via the Arduino UNO board and entered into the proposed algorithm at a 100-Hz sampling rate (i.e., ∆t = 0.01 s).
Moreover, in order to obtain the true reference attitude, an OptiTrack Flex13 3D optical tracking system (from NaturalPoint, Inc., Corvallis, OR, USA) was used. The MPU6050 was mounted on top of a plastic right triangle ruler, and three light-emitting diode markers from the Flex 13 system were also attached to each vertex of the ruler. These markers provided a three-dimensional orientation from which the reference attitude vector S Z re f could be extracted for accuracy evaluation of methods. Thereafter, the ruler was firmly fixed on a link whose end was located at the ground via a ball joint. Therefore, although the link could be rotated in any direction, its translational motion was constrained by the ball joint. The link-fixed vector from the origin of the sensor frame to the joint center was  to the PC via the Arduino UNO board and entered into the proposed algorithm at a 100-Hz sampling rate (i.e., Δt = 0.01 s). Moreover, in order to obtain the true reference attitude, an OptiTrack Flex13 3D optical tracking system (from NaturalPoint, Inc., Corvallis, OR, USA) was used. The MPU6050 was mounted on top of a plastic right triangle ruler, and three light-emitting diode markers from the Flex 13 system were also attached to each vertex of the ruler. These markers provided a three-dimensional orientation from which the reference attitude vector Z S ref could be extracted for accuracy evaluation of methods. Thereafter, the ruler was firmly fixed on a link whose end was located at the ground via a ball joint. Therefore, although the link could be rotated in any direction, its translational motion was constrained by the ball joint.  Note that all of the three tests above were performed under highly dynamic conditions to observe the effect of constraint augmentation on the determination performance under accelerated conditions. Tests 2 and 3 had higher acceleration magnitudes than test 1. The difference between tests 2 and 3 is the test duration, i.e., test 3 had a considerably longer test duration than test 2, and accordingly, IMU was exposed to the severe condition longer.
For each of the aforementioned tests, results of the proposed KF (method A) were compared with those of the other four methods. Method B was the unconstrained KF introduced in Reference [17] and was also the platform of method A. In method B, a c , the external acceleration model parameter that was dimensionless, was set to 0.01, which was experimentally chosen to produce satisfactory results for the three tests. Method C was another constrained KF, which adopted the estimate projection introduced by Simon et al. [30], but used the same acceleration-level constraint equation in Equation (6) formulated for the proposed method, method A. Therefore, the only difference between methods A and C was the type of constraint augmentation used. Method D was a quaternion-based unconstrained KF, presented in Reference [10] by Madgwick et al. The dimensionless-filter gain associated with gyroscope-measurement errors used in Reference [10] was set to 0.026, which was Note that all of the three tests above were performed under highly dynamic conditions to observe the effect of constraint augmentation on the determination performance under accelerated conditions. Tests 2 and 3 had higher acceleration magnitudes than test 1. The difference between tests 2 and 3 is the test duration, i.e., test 3 had a considerably longer test duration than test 2, and accordingly, IMU was exposed to the severe condition longer.
For each of the aforementioned tests, results of the proposed KF (method A) were compared with those of the other four methods. Method B was the unconstrained KF introduced in Reference [17] and was also the platform of method A. In method B, c a , the external acceleration model parameter that was dimensionless, was set to 0.01, which was experimentally chosen to produce satisfactory results for the three tests. Method C was another constrained KF, which adopted the estimate projection introduced by Simon et al. [30], but used the same acceleration-level constraint equation in Equation (6) formulated for the proposed method, method A. Therefore, the only difference between methods A and C was the type of constraint augmentation used. Method D was a quaternion-based unconstrained KF, presented in Reference [10] by Madgwick et al. The dimensionless-filter gain associated with gyroscope-measurement errors used in Reference [10] was set to 0.026, which was also experimentally chosen to produce satisfactory results. Method E was a direct method from measurements without using a KF, i.e., the attitude was simply determined from the equation S Z=(y A − S a)/g, where the external acceleration in Equation (6) was used after the low-pass filtering with a cutoff frequency of 10 Hz. The determination accuracy was evaluated in terms of the root-mean square-error (RMSE) of Euler angles (i.e., roll and pitch). Table 1 lists the roll and pitch RMSEs estimated from five different algorithms for the three tests.

Results
Regarding test 1, in the cases of methods B and D, which were unconstrained KFs, method B performed well in spite of the accelerated condition, but method D was affected by the condition, i.e., the average RMSE of the roll and pitch from method B was 2.44 • , whereas that from method D was 6.50 • . Both methods A and C, embedding the kinematic constraint on method B, exhibited slight improvements in comparison with method B, i.e., 1.93 • from method A and 2.15 • from method C. Method E, which was the direct method that used the IMU signal and constraint, yielded a higher error (3.86 • ) than methods A, C, and B, because of the severe noisiness of the data.
Because of the considerably fast-motion condition of test 2, methods B and D produced high values of averaged RMSEs, i.e., 5.46 • from method B and 7.82 • from method D. In contrast, methods A and C produced a similar level of RMSEs with test 1, in spite of the more severe condition of test 2, i.e., 1.85 • from method A and 2.13 • from method C. Figure 4 shows determination errors from methods A and B for 40 s (10-50 s) out of the total duration of 125 s in test 2. Because method A used the kinematic constraint, it had a relatively uniform error of less than 7 • in the entire section, regardless of experimental conditions in terms of external acceleration. On the contrary, the determination error from method B highly fluctuated. Particularly, the roll determination error reached a maximum of 18.5 • in 18 s. This difference can be explained by answering the question of how to deal with the external acceleration. The proposed method (method A) used the kinematic constraint equation, which was independent from the acceleration condition, whereas the unconstrained method (method B) used the stochastic Markov chain-based acceleration model, which could not cope with the rapidly changing acceleration [13].
In test 3, methods A and C were superior to the other methods, as they also were in the other tests. Because of the prolonged exposure to accelerated conditions, method D produced the largest determination error (9.11 • ) among the five methods. Method E performed better than method D despite of its simplicity without a KF. It should be noted that method E was free from drift because of the use of the kinematic constraint.

Discussion and Conclusion
This paper presented a novel KF, which eliminated the acceleration-related uncertainty for robust IMU-based attitude determination when IMU was attached to a constrained link. Particularly, this research dealt with an acceleration-level kinematic constraint, derived by a ball-and-socket joint. The constraint replaced the stochastic Markov chain-based acceleration model in the measurement model of the attitude determination KF.
Our experimental results showed the superiority of the proposed KF (method A) over unconstrained KFs (methods B and D); in method A, accuracy improved by an average of 1.88° and a maximum of 4.18° in comparison with method B, and by an average of 5.85° and a maximum of 7.70°, in comparison with method D. More importantly, whereas the accuracies of conventional KFs were dependent on test acceleration conditions to some extent (that is, acceleration-related uncertainty), that of the proposed KF was completely independent of test conditions. The maximum RMSE from the proposed KF was just 2.34° of the pitch in test 3. This robustness of the attitude determination comes from the accurate estimation of the external acceleration by using the kinematic constraint (see the summary of RMSEs of the external acceleration in Table 2). There was no significant difference between methods A and C in terms of the determination accuracy, as both used the same constraint and KF platform. However, whereas method A used the constraint instead of the Markov-chain acceleration model, method C used the constraint after the acceleration model. In other words, method A only had one correction step using the constraint, but method C had two correction steps using the acceleration model followed by the constraint. This resulted in the difference in calculation cost, i.e., the ratio of the cost of method A to that of method C is 1.25.
Even in the proposed constrained KF, an error of approximately 2° uniformly occurred in all tests. This error level (i.e., <2.5°) was sufficient in general for the dynamic tracking of human motions

Discussion and Conclusions
This paper presented a novel KF, which eliminated the acceleration-related uncertainty for robust IMU-based attitude determination when IMU was attached to a constrained link. Particularly, this research dealt with an acceleration-level kinematic constraint, derived by a ball-and-socket joint. The constraint replaced the stochastic Markov chain-based acceleration model in the measurement model of the attitude determination KF.
Our experimental results showed the superiority of the proposed KF (method A) over unconstrained KFs (methods B and D); in method A, accuracy improved by an average of 1.88 • and a maximum of 4.18 • in comparison with method B, and by an average of 5.85 • and a maximum of 7.70 • , in comparison with method D. More importantly, whereas the accuracies of conventional KFs were dependent on test acceleration conditions to some extent (that is, acceleration-related uncertainty), that of the proposed KF was completely independent of test conditions. The maximum RMSE from the proposed KF was just 2.34 • of the pitch in test 3. This robustness of the attitude determination comes from the accurate estimation of the external acceleration by using the kinematic constraint (see the summary of RMSEs of the external acceleration in Table 2). There was no significant difference between methods A and C in terms of the determination accuracy, as both used the same constraint and KF platform. However, whereas method A used the constraint instead of the Markov-chain acceleration model, method C used the constraint after the acceleration model. In other words, method A only had one correction step using the constraint, but method C had two correction steps using the acceleration model followed by the constraint. This resulted in the difference in calculation cost, i.e., the ratio of the cost of method A to that of method C is 1.25.
Even in the proposed constrained KF, an error of approximately 2 • uniformly occurred in all tests. This error level (i.e., <2.5 • ) was sufficient in general for the dynamic tracking of human motions and robotic systems. For example, marginal RMSE for the manual routine task was 3.65 • in Reference [16]. The violation of the sensor-to-joint center position vector was expected because of the link's bending, the fixation instability of the socket attached to the ground, and the low-cost IMU performance degradation, which were causes of error. If these causes of error are removed, the determination performance of the proposed method may be further improved. It is noticeable that the accuracies of conventional KF were dependent to some extent on acceleration conditions, whereas those of the proposed KF were independent of these conditions. Therefore, if the IMU-based attitude determination is applied for a case where the motion is restricted by a joint constraint, the accuracy and robustness of the determination may be improved by using the constraint instead of approaching the case as a general-purpose underdetermined problem.
In this study, the socket-side link was fixed to the ground in order to simplify the problem. The proposed method can be effectively applied to the attitude determination of shanks during stance phases for pedestrian navigation systems. In addition, the method can be useful for manipulator kinematics for a link rotating around a static point. Note that the attitude provided from the proposed method is an absolute attitude with respect to the gravity and is different from the angles through joint-attached encoders. Furthermore, as a byproduct, the proposed method can determine the vertical position of the point in the segment (or the link), of which the attitude was considered in this study, for example, by calculating ( S Z) T S d.
Although the direct application of the proposed method is currently limited, this paper provides a proof-of-concept for the first time, in that the uncertainty induced by the external acceleration can be removed by the augmentation of acceleration-level kinematic constraints and thus the robust attitude estimation may be possible even under highly severe operation conditions. Due to the robustness of the proposed KF, it may be applied when an accurate attitude estimation is needed regardless of dynamic conditions.