Quaternion-based pseudo kalman filter for wearable inertial / magnetic sensor applications

This paper deals with orientation estimation using miniature inertial/magnetic sensor comprised of a tri-axial rate gyro, a tri-axial accelerometer, and a tri-axial magnetometer. Particularly, a novel quaternion-based pseudo Kalman filter (KF) is proposed by modifying an indirect KF, in order to maximize the computational efficiency and implementation simplicity. In the proposed pseudo KF, time-update process for prediction is based on the quaternion itself, while measurement-update process for correction is performed through the quaternion error. Experimental tests were conducted to verify performance of the proposed algorithm in various dynamic conditions. By designing the pseudo KF structure, matrix operations required in a typical KF are simplified. For instance, the proposed KF does not require the evaluation of the a priori and a posteriori error covariance matrices. Thus, the proposed algorithm achieves higher computational efficiency even than a typical indirect KF, without sacrificing estimation accuracy. Due to its high efficiency, the proposed algorithm can be suitable for battery-powered and low cost processor-based wearable inertial/magnetic sensor applications.


Introduction
With the advances in micro-electro-mechanical systems (MEMS) technology, miniature inertial/magnetic sensors have emerged for wearable motion capture technology [1,2].An inertial/magnetic sensor is typically comprised of a tri-axial rate gyro, a tri-axial accelerometer, and a tri-axial magnetometer, and is used to estimate accurate 3D orientation in an unconstrained environment (e.g., outdoors), associated with physical medicine and rehabilitation applications [3,4].
In terms of the orientation representation, Euler angles [5], quaternion [1,2,6,7], and direction cosine matrix [8] are common.Among them, the quaternion can arguably be considered as the most popular representation for 3D orientation estimation due to its singularity-free aspect and relatively small number of dimensions (i.e., four).
When quaternion is chosen as an orientation representation for a Kalman filter (KF) structure, there are mainly two approaches depending on how the quaternion is represented in the KF state.One approach is the direct KF which designates the quaternion itself as the state.The other approach is an indirect KF which does the quaternion error as the state [9].
With regard to the quaternion-based direct KF, Sabatini [6] proposed an extended KF by applying a first-order Taylor-Mac Laurin expansion to compute the Jacobian matrix, due to nonlinear nature of measurement model.Lee and Park [1] proposed a linear KF that is comprised of a quaternion measurement step and a KF step.The two steps have feedback relationship and enable the KF have a minimum-order structure (i.e., fourth order).
The advantage of an indirect KF over a direct KF is that the state dimension is smaller and its response is fast.Suh [10] proposed a quaternion-based indirect KF with adaptive estimation of external acceleration.Additionally, Suh et al. [9] proposed another indirect KF that can discard deteriorated pitch and roll information contained in magnetometer signals.
This paper proposes a quaternion-based pseudo Kalman filter by modifying an indirect KF, in order to maximize the computational efficiency.The proposed KF directly uses the quaternion for time update and then indirectly estimate the quaternion though quaternion error in measurement update.By doing so, the proposed algorithm achieves higher computational efficiency even than a typical indirect KF, without sacrificing estimation accuracy.

Method
Sensor Modelling and Quaternion Error.The coordinate transformation of a 3×1 vector x between the sensor frame S and the inertial frame I is x R x

FST 2016 IC
where the left superscripts S and I of x 's imply that the corresponding vectors are expressed in the sensor and inertial frame coordinates, respectively, and R S I is the rotation matrix of the inertial frame I with respect to the sensor frame S. The rotation matrix R S I (or simply R for convenience) can be expressed in terms of the orientation quaternion where 0 q is the scalar part and

R ∈ q
. Now, the algorithm uses e q ~ to denote a small error in quaternion estimate q ˆ so that e q q q ⊗ = .The violation of the quaternion constraint (i.e., unity norm) is allowed under the assumption of small error.
Sensor signals from the gyro (G), the accelerometer (A), and the magnetometer (M) are modeled, respectively, as follows [1]: where ω is the angular velocity; g is the gravity; a is the body acceleration; m is the local magnetic field vector; n's are the measurement noises which are assumed to be uncorrelated and zero-mean white Gaussian.It is natural that every sensor reading is inherently expressed with respect to the sensor reference frame S. By considering Eq. ( 1 In Eq. (4.b), the body acceleration a S is modeled as a first order low-pass filtered white noise process as in [11] i.e.
where a c is a constant to be set and t ε is the acceleration model error.Time Update Process.With regards to the state equation of quaternion error, the time derivative of error, e q , can be expressed as [10]   e e w q A q + e e ≈ where and the zero mean white noise σ is the gyro noise variance.By time-discretizing (6), we obtain the time process equation for quaternion error, i.e.,

A A − =
and discarding high order terms, 9) is approximated as In comparison to the quaternion error process in (8), quaternion process equation is derived from quaternion strapdown integration equation, i.e., The time-derivative of the quaternion t q is [2]: Since ω is unavailable, Eq. ( 11) is rewritten using the . Then, this leads to the time process equation for quaternion: ) ( e e × − q I q R [9].In order to insert the body acceleration model Eq. ( 5) into the accelerometer model Eq.(4.b), we need to define the error of the predicted acceleration as where the minus superscript denotes the a priori (or predicted) estimate, while the plus superscript in Eq. ( 18) below denotes the a posteriori (or corrected) estimate [11].In Eq. ( 18), the predicted body acceleration of the current time step , respectively.Then, the measurement noise covariance matrix a is uncorrelated to A n , Eq. ( 24) can be divided into where acc Σ is the covariance matrix of the body acceleration vector and is set as  A σ is the accelerometer noise variance [11].

Summary of Pseudo Kalman
Filter.The procedure of the proposed pseudo KF can be summarized as follows.
1) Compute the a priori quaternion estimate: 2) Compute the a posteriori quaternion error estimate: 3) Compute the a posteriori quaternion estimate: Note that, in the proposed pseudo KF, time-update process for prediction is based on the quaternion itself, while measurement-update process for correction is performed through the quaternion error.Thus, Eq. (26) of Step 1 is from Eq. ( 11) based on the quaternion process while Eq. ( 27) of Step 2 is from Eq. ( 10) associated with the quaternion error.
In Step 2, the observation matrix H and the measurement vector t z are functions of the a priori quaternion estimate − t q ˆ.In our method, the a priori error covariance matrix (usually denoted by − t P ) is set to the process noise covariance matrix 27) is no other than the Kalman gain (usually denoted by t K ) and the proposed KF does not require the evaluation of the a posteriori error covariance matrix (usually denoted by + t P ).Also, it is notable that the measurement vector t z plays a role as residual (also known as measurement innovation).The overall structure of the proposed algorithm is illustrated in Fig. 1.For each of the above tests, the orientation was estimated using three different estimation methods.Method 1 is the extended quaternion-based direct KF proposed by Sabatini (2006) [6].Method 2 is the quaternion-based indirect KF proposed by Suh (2010) [10].Method 3 is the proposed pseudo KF algorithm which is a sort of modified indirect KF.The estimation accuracy and computational cost were investigated.
Results of Estimation Accuracy and Computational Efficiency.In Test A, all of the three methods produced accurate results (i.e., <2°) since the test condition was desirable for inertial sensing-based orientation estimation (see Table 1).
In Test B, the estimation accuracies were decreased due to the higher body accelerations.However, all of the three methods have body acceleration compensating mechanism.This protects the algrotihms from severe deterioration of estimation accuracy (see Table 1 and Fig. 3).Particularly, Method 1 outperformed Methods 2 and 3.In Test C, which involves the most severe condition (see Fig. 4.a for the magnitude of the applied body acceleration), all methods could not produce less than 4° accuracy in average.It was expectable due to the prolonged exposure to the accelerated conditions as well as the acceleration changes that were repetitively applied.However, Method 3 (the proposed method) performed the best among the three methods, which may imply the excellence of the acceleration model of the proposed method (see Table 1 and Fig. 4.b).With regard to the accuracy of acceleration estimation that is through the orientation estimation, the proposed method showed fairly accurate estimation, i.e., average root mean square error (RMSE) of 1.70 m/s 2 (see Fig. 4.a).In overall (based on the three tests), it can be said that the three methods have a similar degree of accuracy in orientation estimation.
In terms of the computational efficiency, the computation times for each time step for the conventional methods (Methods 1 and 2) and the proposed method (Method 3) on a 3.6 GHz personal computer (Intel ® Core TM i7 processor) are given in Table 2.The proposed method was about two times faster than the conventional methods, due to the simplified pseudo KF procedures.

Discussions and Conclusions
It is important to note that the proposed pseudo Kalman filter is realized only because the Kalman state is 'small' quaternion error.In measurement update procedure of a typical KF, a posteriori state estimate is determined by optimally weighting a priori state estimate and measurement vector depending on their reliabilities (represented by their covariance matrices).In the proposed method, the a priori state estimate is not used.This modification is possible because measurement vector in the proposed method is small and thus can play a role as residual vector.As a result, the simplified KF structure is achieved.
With regard to body acceleration compensating mechanism, concepts of acceleration compensation of Methods 1 and 2 are different from that of Method 3. Methods 1 and 2 switch the measurement noise covariance matrix of the accelerometer depending on the dynamic condition so that weight of accelerometer signals are adjusted compared to weight of gyro signals.This paper does not make claims that the compensation mechanism in the proposed method is superior to those of Methods 1 and 2, based on the result of Test C.However, the orientation estimation algorithm should have a proper acceleration compensation scheme to maintain an acceptable accuracy level in dynamic conditions.This paper proposed a novel quaternion-based pseudo Kalman filter by modifying an indirect KF, in order to maximize the computational efficiency and implementation simplicity.The proposed algorithm achieves higher computational efficiency than a typical indirect KF, without sacrificing estimation accuracy.Due to its high efficiency, the proposed algorithm can be suitable for battery-powered and low cost processor-based wearable inertial/magnetic sensor applications.
-symmetric matrix function of a , the so-called cross-product matrix.The proposed algorithm estimates the quaternion error 3 e

I
are known vectors.The proposed algorithm does not take sensor biases into consideration.

Σ
is the covariance matrix of the accelerometer's measurement noise and is set as

Figure 1 .Figure 2 .
Figure 1.Flowchart of the proposed pseudo Kalman filter

Table 2 .
Computation time per one time step.