Lightweight Extended Kalman Filter for MARG Sensors Attitude Estimation

Low-power consumption of orientation estimation using low-cost inertial sensors are crucial for all the applications which are resource constrained critically. This paper presents a novel Lightweight quaternion-based Extended Kalman Filter (LEKF) for orientation estimation for magnetic, angular rate and gravity (MARG) sensors. In this filter, with employing the quaternion kinematic equation as the process model, we derived a simplified measurement model to create the lightweight system model for Kalman filtering, where the measurement model works efficiently and the involved computation of measurement model is reduced. It’s later proved that the proposed filter saves time consumption. Further, due to that no linearization is involved for the proposed measurement model, the good performance would be guaranteed in theory. For the experiments, a commercial sensor for data collection and an optical system to provide reference measurements of orientation, namely Vicon, are utilized to investigate the performance of the proposed filter. Evaluation for different application scenarios are considered, which primarily includes human motion capture and the drone application. Results indicate that the proposed filter provides reliable performance for both applications. What’s more, the comparison experiment shows that the proposed filter provides better performance in terms of either attitude estimation accuracy or computational time.


I. INTRODUCTION
Magnetic, angular rate and gravity (MARG) sensors have been widely applied to our surrounding devices. These applications include the Unmanned Aerial Vehicle (UAV) [1], [2], human motion capture [3], wearable devices [4] and so on. The MARG sensors are employed to compute the orientation of a rigid body with respect to an inertial frame so as to precisely control the actuators or extract motion features. Normally, orientation parameters can be acquired from two approaches. The first approach, related to the inertial navigation system (INS), is to do integral of angular rates from gyroscope over time if initial conditions are known. The second approach is summarized as Wahba's problem [5], [6] formed by at least two vector observations. As is known, in the case of low-cost MARG sensors, gyroscope measures the angular velocity of a moving object. Accelerometer and magnetometer measure the earth's gravitational and magnetic fields respectively to provide an absolute reference of orientation with the assumption that the measurements are not disturbed by any external noises. Their performance are always subject to their inherent drawbacks, for example, drift error, external disturbance and so on, that means we can not just rely on any one to obtain accurate orientation estimation. For instance, the angular-rate integral will diverge very fast as time goes and accelerometer's output will be disturbed by non-gravitational acceleration when the motion isn't flat. There are too many uncertainties inside almost every process of their sensing. Hence, a reliable attitude estimation solution is to fuse them together using sensor fusion techniques.
Looking back on the past decades, amounts of approaches have been proposed for sensor fusion. Roughly, we can classify them into two categories: complementary filter [7], [8], [9] and Kalman filter [10], [11], [12] based methods. Complementary filters obtain a more accurate orientation estimation by compensating for each sensor in the frequency domain while Kalman filters adopt a probabilistic determination of the state modeled as a Gaussian distribution given the system model. Early thoughts of complementary filter based solutions are raised around 1990 by Foxlin who first applied MARG sensors to monitor the attitude motion of human's head [13], the strategy of which is that combines the accelerometer and magnetometer as the electronic compass (eCompass) [14], the problem of which can be effectively solved by optimization algorithms like Gradient Descent Algorithm (GDA) [7], Gauss Newton Algorithm (GNA) [15] and Levenberg-Marquadt Algorithm (LMA) [16] or solutions to Wahba's problem for example TRIAD [17], [18], FOMA [19], QUEST [20] and ESOQ [21]. Based on this, Mahony in 2008 studied the attitude representation on spectial orthogonal groups by different way [22]. Madgwick computed the attitude using the Gradient Descent Algorithm (GDA) via quaternion representation [7]. Especially fast complementary filter (FCF) [8] proposed by Wu was proved to be faster and more robust than previous. Although all the above solutions are the constant gain complementary filters, the typical drawback of which is that the gain is always empirically given for one time and the inability to adapt the weight for another quite different scenario when their accuracy is affected, it has reflected the development trend where lightweight solutions with maintained accuracy are preferred when seeking the balance between the performance and the computation complexity.
The Kalman filter is by far the most common one and statistically the optimal estimator for linear system with independent white Gaussian noises, generally one iteration is sufficient for convergence. As a result of the excellent performance, considering that the systems are normally nonlinear and to apply Kalman filter to nonlinear systems, adaption and implementation of Kalman filter for nonlinear systems are significantly investigated in the past decades. Adapted methods basically contain extended Kalman filter (EKF) [23], unscented Kalman filter (UKF) [24], cental difference Kalman filter(CDKF) [25], [26], and cubature Kalman filter (CKF) [27], where EKF is the most popular solution, and the later three are the so called sigma-point Kalman filter with higher computations and advanced performance. Same with complementary filters, Kalman filter is also often considered for attitude estimation. Sabatini presented his work to obtain accurate orientation estimation using EKF in [11]. To reveal higher accuracy, Marina proposed the attitude estimation algorithm based on UKF [1]. Although performance is guaranteed, these methods are heavy on computations and resource constrained devices could not afford for it. Similar with the development trend of complementary filters, the problem of attitude estimation using Kalman filter are revisited recently. Valenti presented a linear Kalman filter (LKF) [12] using the algebraic quaternion algorithm. As well, Guo proposed the fast Kalman filter (Guo's FKF) [28] with their previous contributions, which has been proved to be better than LKF. These attempts exactly made a major breakthrough on reducing computational cost. However, existing solutions are still not perfect. For example, too many linear approximations could lower the accuracy, and there may be alternative methods to further reduce the computational cost and maintain the accuracy.
Therefore, what motivates us is to develop a novel lightweight Kalman estimator to reduce computational cost and maintain the accuracy so as to further reduce power dissipation for MARG sensors attitude estimation. In this paper, a simplified measurement model is systematically derived to establish the Lightweight quaternion-based Extended Kalman Filter (LEKF) with employing the quaternion kinematic equation as the process model. This proposed measurement model simplifies the involved computations and is proved to make a contribution on reducing computation cost. During the experiments, a commercial sensor for data collection and an optical system to provide reference measurements of orientation, namely Vicon, are utilized to investigate the performance of the established Kalman filter. Evaluation for different application scenarios is considered, which primarily include human motion capture and the drone applications. Results indicate that the proposed filter provides reliable performance for both applications. Moreover, the comparison experiment shows that the proposed filter provides better performance in terms of either attitude estimation accuracy or computational time.
This paper is organized as follows. Section II involves the attitude determination from the accelerometer and magnetometer. Section III describes the design and implementation of the proposed filter. Experimental environment, experiments and results are presented in Section IV. Section V gives the concluding remarks.

II. ORIENTATION FROM OBSERVATION VECTORS
This section deals with the orientation determination from the observers with the assumption that the observers are not influenced by any external disturbance. Given normalized observation vectors A b = (a x , a y , a z ) T , M b = (m x , m y , m z ) T obtained from accelerometer and magnetometer respectively in the body frame, the orientation determination in the North-East-Down (NED) frame can be modeled by where C is the Direction Cosine Matrix (DCM) and A r = (0, 0, 1) T , M r = (m N , 0, m D ) T denote the reference vectors of earth gravitational and magnetic fields respectively, while m D and m N in [8] are computed by As was discussed earlier, orientation determination from a set of vector observations is called Wahba's problem, there are amounts of solutions to this problem. Although methods such as QUEST, FOMA and ESOQ guarantee the performance, these methods greatly increase the computational cost and are commonly implemented for more than two observation vectors' systems. In our case, the sampling rate generally is synchronized with the gyroscope where only two vectors are involved, the earliest and simplest solution, TRIAD method, is sufficient and taken into consideration, and we review the formula: where the observation vectors o i and the reference vectors r i are computed by Directly, we assign (W 1 , V 1 ) to the pair A b , A r and (W 2 , V 2 ) to the pair M b , M r . Through derivation, the final result C is given by It should be noted that the orientation parameters, namely the frequently used Euler angles, consist of the roll, pitch, and yaw, in this way, only four parameters of the DCM are required to extract them, thereinto C 1,3 and C 2,3 are for determining the pitch and roll angles, and C 1,1 and C 1,2 are responsible for the yaw angle. Consequently, we can choose the four parameters to formulate the measurement model: Conspicuously, the derived measurement model involves a few computations, on the one hand, it is a low dimensional system and easy to obtain the results given the measurements from the accelerometer and magnetometer, on the other hand, it is later proved that other involved computations are greatly reduced, particularly the computation of the Jacobian matrix of this model. What's more, due to without involving any linearization, the good performance would be guaranteed in theory.
Combining with the transformation relation between the DCM and quaternion, the observation model can also be rewritten in quaternion as III. FILTER DESIGN This section investigates the establishment of the proposed LEKF, and we firstly assume that the bias error of gyroscope are constant and has been cancelled before used. In order to formulate the system model, we employ the discretized quaternion kinematic equation as the process mode, which is written as where q k = q 0,k q 1,k q 2,k q 3,k T defines the process state in quaternion form at time k, I 4×4 is the 4 × 4 identity matrix, and ∆T denotes the sampling interval between epoch k and k − 1, while [Ω×] is determined with the gyroscope output ω = (ω x , ω y , ω z ) T by By integrating with the derived measurement model, the Kalman filter model is formulated by where w k and υ k denote the progress noise and observation noise respectively, while H k is the observation matrice, we write down the derivation by As is known, the process model and measurement model can not be applied to the covariance directly. Instead, the matrices of the Jacobian are computed, which are actually the partial derivatives for both models. For the process model, the Jacobian Ξ k−1 is given by while the Jacobian J for the measurement model is derived The detailed results are listed in the Appendix A. Henceforth, we can easily handle the covariance Σ w k for the process noise and Σ υ k for the observation noise, which are respectively computed by and where Σ gyro = σ 2 g · I 3×3 is the covariance of gyroscope, Σ acc = σ 2 a · I 3×3 for accelerometer, and Σ mag = σ 2 m · I 3×3 for magnetometer, in general they can be obtained from the device specification directly.
At this point, we have completely described the derivation of the proposed LEKF, and the workflow is summarized in Fig. 1. Considering the constraints of the quaternion representing the orientation and to ensure the filer working correctly, another thing that should be noted, is that the last resulting quaternion must be normalized in every iteration of Kalman filtering. Intuitively, the LEKF is a lightweight Kalman filter, due to the simplified measurement model, and that its involved computation is reduced, particularly up-to-half elements of the Jacobian J are constant, and other parameters have plenty of common factors.

IV. EXPERIMENTS AND RESULTS
In this section, we conduct several experiments to investigate the performance of the proposed LEKF. The detailed description about experimental setup and data collection is given in the first subsection.

A. Experimental Environment and Data Collection
The experimental data for testing the filters is collected with a commercial sensor, namely WitMotion HWT905. HWT905 is a 10-axis orientation sensor with temperature compensation which contains 16 bit 3-axis gyroscopes, accelerometers and magnetometers, and interfaced with a compact computer called NanoPi Duo2. The hardware is shown in Fig. 2. In the following experiments, the raw data is collected via the UART to USB data wire at 200Hz. Besides, bias errors of HWT905 are cancelled by keeping the sensor stationary, and the ellipsoid fitting algorithm is applied to calibrate the magnetometer [29]. For the filter setup, the sensor noise characteristics are obtained directly from the official datasheet of HWT905, that is, Σ gyro = 0.0025·I 3×3 ( • /s) 2 , Σ acc = 0.0001 ·I 3×3 g 2 , and Σ mag = 0.0004 · I 3×3 uT 2 . In order to provide the reference measurements, an optical motion capture system called Vicon is utilized, which consists of 8 infrared cameras, and is able to track the position of optical markers continually. To successfully extract the orientation parameters, three optical markers are mounted on the top of the drone, while the experimental devices are fixed on the bottom the drone, as shown in Fig. 3. The position of the attached markers is measured with sampling rate of 200 Hz, and then we extract the reference orientation parameters by post-processing. However, due to that both systems for data collection have different coordinate systems and clock resources, we have to take two steps to align the coordinate systems and clocks of the two different systems. Firstly, we keep the drone stationary, obtain the corresponding orientation using an advanced Kalman filter, cubature Kalman filter [27], and then figure out the offset between the two coordinate systems. Secondly, for each experimental data, we perform a cross-correlation of both angular rates of the two systems to figure out the time drift, as indicated in Fig. 4, where both angular rate have been aligned to the same coordinate system. In theory, the error of time synchronization is less than one sampling interval. For the experiments, evaluation for different application scenarios is considered, which primarily includes human motion capture and the drone application. In the case of human motion capture, the data is acquired from the motion randomly generated by moving the drone manually. For the drone application, the data is recorded during the real flight. All the experimental data are post-processed using python 3.7 on an Ubuntu 18.04 server with a CPU of 4-core i7-7700K. q 0 = 1 0 0 0 T and P 0 = 0.001 · I 4×4 are set as the initial conditions for the proposed filter.

B. Accuracy for Human Motion Capture
In this experiment, the absolute accuracy of the proposed filter for human motion capture is assessed. Fig. 5 shows the recorded raw data. Processing with our proposed filter, the results in Euler angles are estimated and plotted in Fig.  6 where the black line denotes the reference measurements. As the Vicon is assembled in the iron house, the magnetic distortion can't be overcome completely by the calibration algorithm, so that the divergence occurs in the yaw angle. We can see that the estimated results using LEKF coincide with the reference measurements very well. The root mean  Table. I. It's easy to conclude that the LEKF provides reliable performance of the roll and pitch angles for such applications, while the yaw angle suffers from the magnetic distortion.

C. Accuracy During the Flight on an UAV
The absolute accuracy of the LEKF for drone applications is investigated in this experiment. The raw data is recorded during the flight of the drone, as shown in Fig. 7, where the spike in the tail is caused by the sudden landing of the drone. Obviously, the raw data presents the high-frequency vibration, particular on the axis-z of the measurements of the accelerometer. Moreover, the magnetometer is significantly disturbed due to the drone motors always work with powerful pulse current. Fig. 8 depicts the estimated results using the LEKF. Different with the application of human motion capture, it's distinct that the magnetic distortion from the running motors causes the divergence greatly in the yaw angle, and the unstable current results in unstable performance in the yaw angle. Table. II  This experiment proves at least two important things. The first is that the LEKF provides good robustness especially when high frequency vibration happens, in other words, it has a good anti-vibration ability, and the second is that the estimated roll and pitch angles are immune to the magnetic disturbance.

D. Comparison with Representative Kalman Filters
In this subsection, we do a detailed comparison between the LEKF and representative Kalman filters from two aspects: accuracy and computational cost. Representative Kalman filters primarily include Indirect Kalman Filter (IKF) [10] and Guo's FKF [28], where the former is the famous one and the later is the recent one, experimental conditions of which are the exact same as the LEKF.
Through reprocessing the data of the previous experiments, Table. III records the overall assessment results. For human motion capture, it is obvious that the proposed LEKF provides the best performance among them. In the case of the drone application, the computational time of LEKF is still the lowest one, while their accuracy are all influenced by the magnetic disturbance caused by the running motors, particularly IKF is affected significantly. In contrast, LEKF and Guo's FKF provide comparable performance in terms of accuracy. To understand fully the evaluation results, Fig. 9 and Fig. 10 shows the details of the comparison, where Fig. 9 only depicts the comparable results. Henceforth, we can conclude that, the proposed LEKF is with the lowest computational cost, while the accuracy of LEKF is improved for human motion capture and maintained for the drone application.
In theory, the conclusions also can be checked. For IKF, the process model and measurement model are both with higher dimensional variables, and the computations of roll, pitch and yaw are mixed, this is why the computation time is larger, and the magnetic disturbance has a significant impact on all the Euler angles. For Guo's FKF, although its structure is same with the proposed LEKF, the measurement model is formulated by using linearization technique, and the computation of  the process mode, we derive a simplified measurement model to establish the proposed LEKF. Our analysis prove that the proposed measurement model is conductive to reduce computation effort, this measurement model works efficiently and simplifies the involved computation, especially the computation of its Jacobian where up-to-half elements are constant, and others have plenty of common factors. Performance of the proposed LEKF are assessed using a commercial sensor for collecting raw MARG sensors' data and an optical system called Vicon to provide reference measurements. Evaluation for different application scenarios is considered, which primarily includes human motion capture and the drone application. Results indicate that the proposed LEKF provides reliable performance for both applications, and has a good robustness when high frequency vibration happens. Further more, we do a detailed comparison between the LEKF and representative Kalman filters from two aspects: accuracy and computation cost, experimental results show that the LEKF is the fastest one and improves the accuracy in some way. where ACKNOWLEDGMENT This work was supported by JSPS KAKENHI Grant Number 18K11400.