Quaternion-Based Unscented Kalman Filter for Accurate Indoor Heading Estimation Using Wearable Multi-Sensor System

Inertial navigation based on micro-electromechanical system (MEMS) inertial measurement units (IMUs) has attracted numerous researchers due to its high reliability and independence. The heading estimation, as one of the most important parts of inertial navigation, has been a research focus in this field. Heading estimation using magnetometers is perturbed by magnetic disturbances, such as indoor concrete structures and electronic equipment. The MEMS gyroscope is also used for heading estimation. However, the accuracy of gyroscope is unreliable with time. In this paper, a wearable multi-sensor system has been designed to obtain the high-accuracy indoor heading estimation, according to a quaternion-based unscented Kalman filter (UKF) algorithm. The proposed multi-sensor system including one three-axis accelerometer, three single-axis gyroscopes, one three-axis magnetometer and one microprocessor minimizes the size and cost. The wearable multi-sensor system was fixed on waist of pedestrian and the quadrotor unmanned aerial vehicle (UAV) for heading estimation experiments in our college building. The results show that the mean heading estimation errors are less 10° and 5° to multi-sensor system fixed on waist of pedestrian and the quadrotor UAV, respectively, compared to the reference path.

Although these studies could offer effective heading estimations for indoor navigation, the linearization steps of EKF with the Jacobean matrix in attitude determination were not stable and accurate enough. In order to accommodate highly non-linear kinematics models related to the attitude estimation, the unscented Kalman filter (UKF) has been proved to be better solution than EKF, owing to the sigma-point approximation used in UKF [33,34]. The UKF in space applications had more robustness and accuracy than the EKF as shown in [35]. Although computational cost was a little higher than with EKF, there were new sigma-points algorithms aiming to reduce this cost in attitude determination [36]. However, those studies of UKF for attitude determination mainly focused on outdoor applications, and indoor context is equally important and more complicated. The quaternion-based Kalman filter was designed in [37] to for human body motion tracking, and the feasibility of real time human body motion tracking was validated. A novel quaternion Kalman filter was presented in [38], and the proposed filter succeeded in the particular case of high initial estimation errors whereas EKF fails to converge. A quaternion based EKF was developed for determining the orientation of a rigid body [39] and a quaternion-based indirect Kalman filter structure was used to obtain adaptive estimation of external acceleration [40]. However, the nonlinear estimations for attitude solution of the above works are weaker than UKF, which was demonstrated to be a powerful nonlinear estimation technique and had been shown to be a superior alternative to the EKF in a variety of applications [41].
To meet the requirements of accurate indoor heading estimation using a low-cost MEMS strapdown inertial navigation system, an effective method of quaternion-based UKF algorithm has been proposed. The quaternion-based UKF algorithm has the following advantages over the general Kalman filter and EKF [27][28][29][30][31][32][33][34][35][36][42][43][44][45][46]: (1) The quaternion-based orientation of an object has no singularities when the pitch angle passes through ± /2 than Euler angles or Direction Cosine Matrix (DCM); (2) Quaternion-based matrix transformation has higher computational efficiency than Euler angles and DCM, and it is more suitable for our low cost MEMS multi-sensor system; (3) EKF models are linearized through a first order Taylor series expansion of the process/measurement models around the current state estimate, meanwhile, Jacobian matrix computation is quite complex; but UKF is second order approximation, which has capability of dealing with large and small attitude errors.
In this paper, a miniature wearable multi-sensor system is designed for accurate indoor heading estimation applied in mobile robots, indoor UAV and pedestrian navigation. The multi-sensor system is composed of low-cost MEMS devices, including one three-axis accelerometer, three single-axis gyroscopes, one three-axis magnetometer, and one microprocessor. The proposed quaternion-based UKF algorithm can make use of the complementary features of gyroscopes not affected by magnetic disturbance and magnetometers without long-term drift for heading estimation. The experiments are carried out with a multi-sensor system fixed on the waist of pedestrians and a quadrotor UAV. This paper is organized as follows: in Section 2, the wearable multi-sensor system is described. The heading estimation multi-sensor system is discussed in Section 3. In Section 4 the proposed method comprising quaternion-based UKF algorithm is described in detail. Analysis of both experiments and results is given in Section 5. Finally, the conclusions and future work are summarized in Section 6.

Wearable Multi-Sensor System
The wearable multi-sensor system is a miniature strapdown inertial navigation system designed in our lab to satisfy the needs of indoor navigation for mobile robots, UAVs and pedestrians. The multi-sensor system is composed of one three-axis accelerometer, three single-axis gyroscopes and one magnetometer and one microprocessor, as shown in Figure 1, and its small size is 32 × 32 × 15 mm. In fact, our multi-sensor system can be regarded as a combination of an IMU and magnetometer. The accelerometer uses an ADXL312 made by Analog Devices Inc. (Norwood, MA, USA), and has a measurement range of ±12 g and high resolution of 2.9 mg/LSB with I 2 C/SPI bus for digital output. The ADXRS453 made by Analog Devices Inc. is selected for the three single-axis gyroscopes, and it is intended for industrial applications with its features of ±300°/s angular rate sensing and 16°/h bias instability. The three-axis magnetometer uses the HMC1053 from Honeywell Corp. (Morristown, NJ, USA), with a measurement range of ±6 gauss and resolution of 0.12 milligauss. We use the STM32F407 as our microprocessor, and it is based on the high-performance ARM Corte-M4 32-bit RISC core operating at a frequency of 168 MHz and 1 Mbyte of flash memory. It provides a good data calculation performance because of its ability to work with the large number of floating calculation points of the multi-sensor system.

Coordinate Systems
The main task of a wearable multi-sensor system is to determine attitude angles which are the main navigation parameters of a body segment. The designed multi-sensor system is based on the geographical coordinate system and body coordinate system shown in Figure 2. The geographical coordinate system determines the north, east and vertical directions of the position of the body. This coordinate system is linked to the local Earth referential [47]. The orthogonal axes of N, E and D represent the axis of north, east, and upright respectively. The body coordinate system is attached to the multi-sensor system, and Xb, Yb and Zb represent the directions of its head, starboard and bottom respectively. The body coordinate system (frame b) can be obtained from NED coordinate system (frame n) through three sequential rotation angles heading (ψ), pitch (θ) and roll (φ), which present the body attitude. The transformation matrix from ONED to OXbYbZb is written as: cos cos sin cos sin sin cos cos sin sin cos cos sin sin sin cos sin sin sin cos sin cos cos sin sin sin cos cos cos

Heading Estimation
The magnetometer is easily subject to interferences from external magnetic sources such as the steel of buildings and electronic equipment. These kinds of disturbance lead to unpredictable performance of the magnetometer, which is a major drawback of using magnetic sensors. Figure 3 is an example of a test of the magnetometer of the multi-sensor system in a corridor of our college building. There are elevators and other metallic structures in the corridor which can produce magnetic sources to perturb the magnetometer, as illustrated by the red line in Figure 3. Therefore, magnetometer-based heading estimation is not reliable. However, the gyroscope is not perturbed by indoor magnetic disturbances. In addition, the magnetometer-based heading is relatively stable in tests over longer hours. A new heading estimation method is proposed to make use of complementary features of gyroscopes and magnetometers, meanwhile, the quaternion-based UKF to eliminate the errors derived from the gyroscope and magnetometer is adopted. The flow chart of accurate heading estimation using quaternion-based UKF is shown in Figure 4.

Heading Estimation Using a Magnetometer
The pitch angle and roll angle can be computed from acceleration signals and the yaw angle is determining by computing the magnetometer output [48,49]. The relationship between gravitational acceleration components ( , , ) x y z a a a in the frame b and the gravity vector in the frame n, when regardless of its acceleration of multi-senor system, can be written as: 0 s i n 0 sin cos cos cos With the acceleration outputs the pitch angle and roll angle can be obtained as: 2 2 arctan( ) However, the outputs of accelerometer are total accelerations, including the acceleration of the multi-senor system and the acceleration of gravity. In consideration of the multi-sensor system in indoor environments, GPS cannot be used in the acceleration measurement of the body coordinate system, meanwhile, the indoor body coordinate system is in a situation of low dynamics and motion at constant velocity. In this paper, the multi-sensor system fixed on pedestrians and quadrotors moves at a constant speed, and there is no sharp acceleration and deceleration in tests.
For heading angle computation with a magnetometer, the rotation relationship between the geomagnetic field intensity ( , , ) Considering the local magnetic declination, the real local heading estimation based on magnetometer is: 2 1 cos sin arctan arctan cos sin sin cos sin where D is the local declination angle.

Heading Estimation Using a Gyroscope
Quaternion has the advantage of no singularity when the pitch is approaching 90° and higher computation efficiency than Euler angles. Quaternion is a four-dimension vector and subject to normalization, which is defined as follows: Quaternion-based state update model with gyroscope measurement is shown as follows: where , , x y z ω ω ω are the measurements of gyroscope.
We could provide the analytical solution of Equation (7), and the discrete form is: where k = 1, 2,…n, , and dt is sampling time.
The quaternion-based traditional matrix from NED coordinate system to body coordinate system in DCM form is written as: 2  2  2  2  0  1  2  3  1 2  0 3  1 3  0 2  11  12  13  2  2  2  2  1 2  0 3  0  1  2  3  2 3  0 1  21  22  23  2  2  2  2  1 3  0 2  2 3  0 1  0  1  2  3  31  32  33 2( ) 2( ) q q q q q q q q q q q q C C C C qq qq q q q q qq qq C C C q q q q q q q q q q q q C C C The reverse transformation matrix n b C can be simply obtained as the transpose of n b C . Therefore, the attitude angles based on quaternion and gyroscope in form of Euler angles is expressed as:

Quaternion-Based UKF
The Kalman filter is a model-based estimation technique. In this paper, the unscented Kalman filter is implemented to integrate the attitude determination from the gyroscope and magnetometer outputs through the quaternion method.

Kalman Filter Design
The state model X and measurement model Y are built respectively as follows: The discrete time representation of Kalman filter is shown as: where the system matrix F and output function H[X(n)] are: ) arctan q q q q q q q q H X n q q q q q q q q q q q q W and v are the process noise and measurement noise, respectively. They are simplified as independent Gaussian white noise. The process noise covariance matrix and measurement noise covariance matrix are Q and R respectively.

Covariance of Process Noise and Measurement Noise
In the Kalman filter design, we consider the process noise and measurement noise as design parameters to achieve minimum for covariance of error estimation. Therefore, it is essential to determine the covariance of process noise and measurement noise. For heading estimation in this paper, the process noise derives from outputs ( x ω , y ω , z ω ) of MEMS gyroscopes, and we assume that ω  , y ω  and z ω  are deviations of x ω , y ω and z ω . The Equation (7) can be rewritten as [50]: The right term of Equation (15) can be regarded as process noise. For the discrete system, the covariance of process noise Q is: where 2 x ω σ , 2 y ω σ and 2 z ω σ are variance of gyroscope outputs x ω , y ω and z ω .
The covariance of measurement noise is decided by the measurement process. In this paper, the pitch (θ), roll (φ) and heading (ψ) angles are calculated from accelerometer and magnetometer outputs. We assume that, and x a  , y a  and z a  are deviation of accelerometer outputs x a , y a and z a . Through the Equation (2) With the 2 x a σ , 2 y a σ and 2 z a σ are the variance of accelerometer x a , y a and z a , there is: The covariance of pitch and roll is as follows: Similarly, the deviation of magnetometer based heading can be gotten from Equations (3) and (5) The variance of magnetometer based heading ( mag ψ ) is expressed as: Finally, we can determine the covariance of measurement noise matrix R as: where 2 C σ is variance of quaternion normalization equation and its value is zero, however, in order to avoid the mathematical problem in calculation, and 2 C σ is given a small value.

Unscented Transformation
The unscented transformation (UT) is an effective way to approximate how the mean and covariance of a random variable change when it undergoes a nonlinear transformation. Consider propagating random variable x (dimension L) through a nonlinear function y = h(x). Assume X has mean x and covariance Px. To calculate the statistics of y, we form a matrix χ of 2L + 1 sigma λ α κ = + − is a scaling factor. Usually, α is set to a small positive value (e.g., 1e−3), κ is set to 0 and β is set for 2 for Gaussian distribution.

UKF Algorithm Equations
The UKF is more robust and accurate than EKF under realistic initial attitude-error conditions and large attitude change when dealing with highly nonlinear problem. The UKF algorithm details are presented as follows. First, we initialize with inertial quaternion value as follows: Secondly, sigma points are computed by: Thirdly, the state update equations are expressed as: where F and H are the highly nonlinear equations of above sections, and Q is the covariance of process noise. Finally, the measurement update equations are given as follows: where R is the covariance of measurement noise, and K is the Kalman gain.

Two-Axis Turntable Test of Multi-Sensor System
To evaluate the performance and measurement precision of our designed multi-sensor system, a two-axis turntable is used for static and dynamic test experiments. The two-axis turntable test platform is shown in Figure 5. The two-axis turntable is used not only for static and dynamic tests, but also for calibration tests of the multi-sensor system. Through test experiments including static, dynamic and heading determinations, the results show that our multi-sensor system with the proposed algorithm has a static accuracy better than 0.5°, dynamic accuracy better than 1° and heading accuracy better than 2°. Although the accuracy of our multi-sensor system is a little lower than the MTi-300 from Xsens [51], however, our multi-sensor system smaller and much cheaper than the MTi-300 so it is more suitable for wearable applications like indoor robots, UAVs, pedestrians and so on.

Indoor Heading Experiments
In order to verify that our wearable multi-sensor system using the quaternion-based UKF algorithm can meet the accurate heading estimation needs of robots, pedestrians and UAVs in indoor environments, we carried out two experiments in our college building corridor. One experiment is implemented with our multi-sensor system fixed on the waist of a pedestrian and the other one is carried out with our multi-sensor system fixed on a quadrotor UAV, as shown in Figure 6.

Result Analysis
The results of experiments using the multi-sensor system on the waist of a pedestrian and on a quadrotor UAV for indoor heading estimation using our proposed quaternion-based UKF algorithm are shown in Figures 8 and 9, respectively, in which Figures 8 and 9a-c are three attitude angles of the pedestrian and the quadrotor. In Figures 8 and 9c, the red line is the heading estimation, and the four straight lines in pink, blue, black and green are reference path directions in the corridor of our college building. Figures 8 and 9d are the error between the heading estimation using our proposed algorithm and the reference path directions. Figure 10 is the moving average of the yaw error analysis, which is a kind of technical analysis tool commonly used with time series data to smooth out short-term fluctuations and highlight longer-term trends. In this paper, we define the moving points as 50. By comparison with the evolution of the error and moving error average, it is found that the moving average method can be more informative than the evolution of the error.  It is easy to obtain the mean heading estimation error of the multi-sensor system on a pedestrian's waist as about 10° by contrast with the reference path. The mean heading estimation error of the multi-sensor system on a quadrotor is about 5°. Because the heading range is from −180° to 180°, the rates of mean heading errors are 5.56% (10°/180°) and 2.78% (5°/180°) Due to the oscillation in all tests, an error rate ≤5% can be identified as an accurate indoor heading estimation. Through comparison of Figures 8 and 9, we can obviously find out that the roll angle and pitch angle of the multi-sensor system on pedestrian's waist have a larger variation range than that of the multi-sensor system on the quadrotor, which indicates the maneuvering conditions of the quadrotor are more stable than those of pedestrian. Nonetheless, the heading estimation error of the quadrotor has more dynamic changes than that of the pedestrian, which means there is external perturbation affecting the heading estimation. From an analysis of the indoor disturbance sources and a comparison between the pedestrian and quadrotor data mentioned above, it is found that the four brushless electric machines of the quadrotor can produce a certain external magnetic disturbance to have an influence on the heading estimation, which however is difficult to eliminate.
Although the experiments have verified that heading estimation with our wearable multi-sensor system using quaternion-based unscented Kalman filter has high accuracy, however, in the experiments the pedestrian and quadrotor are set into motion at a constant velocity, which can guarantee an accurate heading estimation. In practice however, the indoor movement conditions are complex, especially when emergency events occur. Therefore, experiments under complicated conditions should be carried out along to improve the heading estimation algorithm.

Conclusions
In this paper, a wearable multi-sensor system based on nine degrees of freedom low-cost MEMS sensors and STM32F407 has small size and good cost performance. The proposed quaternion-based unscented Kalman filter can use the complementary features of gyroscopes and magnetometers to get accurate heading estimations. It was tested through indoor experiments with the multi-sensor system fixed on the waist of a pedestrian and a quadrotor UAV, and the results show that the mean heading estimation errors are about 10° and 5°, respectively, in comparison to the reference path. However, in the future, we will carry out experiments in more complicated conditions such as sharp acceleration and deceleration, and in the next step we will combine the accurate heading estimation with distance estimation, the other element of indoor dead-reckoning, for accurately navigating robots, pedestrians and UAVs. The WiFi technique and camera technique are other promising indoor navigation techniques. We will combine WiFi or a camera with the IMU, and carry out feature recognition studies of buildings to calibrate the multi-sensor system in order to improve the location accuracy.
with contract number 2013AA041105 and the Natural Science Foundation of China (NSFC) with Grant No. 61405148.

Author Contributions
Xubing Yuan conceived the idea and wrote the paper. Shuai Yu performed the experiments; Shengzhi Zhang analyzed the partial data; the multi-sensor system was designed by our lab; Sheng Liu coordinated the research.