A Quaternion Scaled Unscented Kalman Estimator for Inertial Navigation States Determination Using INS / GPS / Magnetometer Fusion

This Inertial Navigation System (INS), Global Positioning System (GPS) and fluxgate magnetometer technologies have been widely used in a variety of positioning and navigation applications. In this paper, a low cost solid state INS/GPS/Magnetometer integrated navigation system has been developed that incorporates measurements from an Inertial Navigation System (INS), Global Positioning System (GPS) and fluxgate magnetometer (Mag.) to provide a reliable complete navigation solution at a high output rate. The body attitude estimates, especially the heading angle, are fundamental challenges in a navigation system. Therefore targeting accurate attitude estimation is considered a significant contribution to the overall navigation error. A better estimation of the body attitude estimates leads to more accurate position and velocity estimation. For that end, the aim of this research is to exploit the magnetometer and accelerometer data in the attitude estimation technique. In this paper, a Scaled Unscented Kalman Filter (SUKF) based on the quaternion concept is designed for the INS/GPS/Mag integrated navigation system under large attitude error conditions. Simulation and experimental results indicate a satisfactory performance of the newly developed model.


Introduction
The principal software functions executed in the Inertial Navigation System (INS) computer are the angular rate and linear acceleration signals.The angular rate signals is integrated to compute the attitude data (denoted as attitude algorithm).The attitude information is used to transform the linear acceleration signal into a suitable navigation coordinate frame where it is double integrated in order to obtain the velocity and position states (denoted as velocity and position algorithms).Thus three integration functions (INS algorithm) are involved: attitude, velocity, and position.Each of which requires high accuracy to assure negligible error compared to inertial sensors' accuracy requirements.Since acceleration from the accelerometers and angular rate from the rate gyros are generally susceptible to various measurement noise sources, the accuracy of position, velocity and attitude information degrades with time [1]- [4].Recent research has shown that the growth of numerical errors in Inertial Measurement Units (IMUs) navigation with time can be prevented by using external aiding sensors such as the Global Positioning System (GPS) [5]- [7], and the magnetometer [8] [9].The magnetometers are susceptible to magnetic disturbances, which corrupt their measurements of the earth magnetic field.These errors typically known as Hard-iron and Soft-iron effects are calibrated out once the system is installed in its final mounting position.The benefits of this integration are well known.The supporting sensors (GPS and magnetometers) provide the earth-fixed position, velocity, and earth magnetic field measurements which the filter uses to calculate corrections to the trajectory state and estimate inertial sensor errors.This fusion of multiple sensors allows for a wide variety of sensor characterizations including bias, scale factor, and unit mounting misalignment.The largest error in attitude and heading propagation is associated with the gyro bias terms.Without a filter structure and separate independent measurements, the attitude information would diverge from the true trajectory.Since in INS algorithm, the accuracy of attitude information governs the accuracy of velocity and position estimation, the most researched topic for the INS integration functions has been primarily focused on the design of algorithms for the attitude integration function and initial alignment.Several parameterizations have been used to represent the attitude, such as Euler angles, Direct Cosine Matrix, Quatemions, Rotation vector, Rodrigues parameters, etc.The detailed discussion on each par metrization for attitude estimation is found in [7] [10] [11].The Extended Kalman Filter (EKF) is widely used nonlinear filtering method for attitude estimation [12].The Unscented Kalman Filter (UKF) is an extension of the classical Kalman filter to nonlinear process and measurement models and has been proposed for attitude estimation, which is more robust than the EKF for large initial attitude-error conditions [6] [7] [10] [11].The quatemion's method is advantageous since it requires less computation, gives better accuracy, avoids singularity and the kinematics equation is bilinear [13] [14].However, the quatemion must obey a normalization constraint, which can be violated in many nonlinear quatemion filtering approaches.In fact, if the state vector is quatemion, the predicted quaternion mean should be calculated in the rotational space to preserve the nonlinear nature of unit quatemion.To directly utilize UKF formulation built in vector space and to avoid a mean computation problem of quatemions, a modified method employing the combination of the quaternion with the generalized Rodrigues parameters is used in [7] [10] [11].This method converts a quatemion to a Rodrigues parameter and converted Rodrigues parameters are used to compute the predicted mean, the covariance and cross-relation matrix since Rodrigues parameters are unconstrained.Rodrigues parameters are converted back to quaternion be-ause this method uses the inertial quaternion for state propagation.An UKF in a unit quaternion space is possible when a weighted mean of quaternions produces a unit quaternion estimate, and the predicted covariance computation and quaternion guarantees that quaternion in filter lies on unit sphere.As an additional development, an alternative formulation for the square root version of the UKF is developed and presented in this paper for the integrated INS navigation, GPS and magnetometer.This version is based on a generalized unconstrained three component attitude-error vector to represent the quaternion error vector, where no parameter conversion is required, such as transformation between quaternions and Rodrigues parameters.A multiplicative quaternion-error is used for predicted covariance computation of the quaternion because it represents the distance from the predicted mean quaternion, and the unit quaternion is not closed for subtraction, guaranteeing that the quaternion in filter lies in unit quaternion space.The quaternion multiplication is used to perform the updates.Since quaternion process noise finally results in an increase of the uncertainty in attitude orientation, we treat then the quaternion error vector as a rotation vector.No small angle assumption is made in the model development.

Mathematical Model of Integrated INS/GPS/Mag System
In an INS, specific forces and angular rates from an Inertial Measurement Unit (IMU) are transformed into posi-tion, velocity and attitude by solving a set of differential equations called INS mechanization.On the other hand, GPS and magnetometer deliver positions, velocities and earth magnetic field measurements with relatively low accuracy but with a bounded error.INS, GPS and magnetometer have complementary properties and are therefore well suited for integration.There are different modes of integration [15]- [17].In this work, a loosely coupled INS/GPS/Mag system has been implemented [18] [19].This means primarily that GPS positions and velocities information are used as inputs to the navigation filter rather than pseudo-ranges measurements.

Process Model
The state vector k x is composed of quaternion, velocity, position and sensor errors (bias, scale factor and mi- salignments errors).The state model is a nonlinear system process represented by The system process model [ ] .g describes the effect of the compensation of the inertial sensor measurement on the navigation terms computation (position, velocity and attitude).It refers to the overall INS navigation algorithm which is described below.

Navigation Error Model Compensation
The compensation of inertial sensor measurement is accomplished to express the true integrated inertial sensor measurement, as follows: ( ) ,  are the 3 3 × matrices of gyro and accelerometer scale factors (SF)/misalignments (MA) [16].The sensor error model can be applied as follows: ( ) are the parameters that can represent the random walk, the random constant and the first-order Gauss-Markov processes.

Navigation Digital Attitude Quaternion Integration
The digital attitude quaternion integration model, n b q , from the body frame to the navigation frame can be updated as follows [12]: where is attitude quaternion that accounts for the b-frame rotation relative to inertial space from its orientation at time 1 k t − to its orientation at time k t , which can be expressed in terms of rotation vector as fol- lows [fusion]: where k ϕ is the b-frame rotation vector which can be obtained as: The attitude quaternion q − , that accounts for the n-frame rotation relative to inertial space from its orien- tation at time k t to its orientation at time 1 k t − , can be also expressed in terms of a rotation vector as follows [20]: where k ζ is the n-frame rotation vector.k ζ is computed as follows: where Φ represents dynamic latitude, h is the height, , N E v v are velocities in the north and east direction in the navigation frame, N R is the radius of curvature in the prime vertical and M R is the meridian radius of curvature, e ω is the magnitude of the Earth's rotation rate.All terms in this paper, can be calculated by a linear extrapolation formula

Digital Velocity Integration
The digital velocity integration model, n v , can be written as [13]: The velocity increment, due to the specific force, , where  ω in z-frame [21].

Digital Position Integration
The INS computes position vector by integrating velocity vector [3] [12] [13].The general position can be updated as follows where n zn u is unit vector upward along the geodetic vertical (the z axis of the n-frame), is the velocity at the midway and λ is the longitude geodetic coordinate.

Measurement Model
The position and velocity, inclination angles and magnetic field obtained from GPS, the 3 accelerometers of INS and magnetometer respectively, are considered as the measurements set in the Kalman filter.

GPS Measurement Model
Since the GPS antenna and the INS cannot be installed at the same place in the host vehicle, the position of the INS is different from that of the GPS, which is known as the lever-arm effect b GPS l .The lever arm effect in the position and velocity measurements is written as: GPS GPS 1 0 0 where GPS b l is the offset vector of the GPS antenna from the center of the IMU in the body frame.GPS e p is the GPS position measured in the Earth-fixed frame (e-frame).e p is the position of the inertial navigation system calculated in the Earth-fixed frame.GPS n v is the velocity of GPS measured in the navigation frame and n v is the velocity of the inertial navigation system calculated in the navigation frame. 1 L and 2 L are the time delay coming from the GPS sensor latency, defined as follow:

GPS velocity latency
The position vector in the e-frame e p can be expressed in terms of the geodetic coordinates as follows: where ε is the first eccentricity of the reference ellipsoid, and n R is the radius of curvature in the prime ver- tical terminated by minor axis " a ".

Accelerometer Measurement Model
The difference between the estimates, at time k t , of tilt angles (pitch and roll) supplied by the inertial naviga- tion system and the tilt angles measured by the three accelerometers constitute the accelerometer observation equations, written as follows: ( ) ( ) The tilt angles estimated by the inertial navigation system can be determined by the following equation: ( )     in the body frame.As for the inertial sensors, magnetometer measurement errors can be compensated by using the following measurement model:

Magnetometer Measurement Model
,  η are the parameters of the evolution models.The compensated Earth magnetic field vector is used to correct the yaw angle or heading compensation.The basic idea for yaw angle compensation is as follows.The residual quantity in the measurement of a declination angle will be the same as the residual of a yaw angle.Hence, we first compute the residual value of the declination which is the difference between the ideal declination angle given by the world magnetic model 2005 [23],

The Quaternion Scaled Unscented Kalman Filter (Q-SUKF)
In order to fuse aiding GPS and Magnetometer measurements with the INS navigation algorithm to guarantee the accuracy of attitude information, the Unscented Kalman Filter (UKF) based on a quaternion parametrization is used.A description of the UKF can be found in [24]- [26].The UKF is based on the unscented transformation which is referred to the procedure of obtaining a set of deterministically chosen points, called the sigma points, to represent the untransformed mean and covariance of the variable to be estimated [27].By choosing appropriate weights, which obey the constraint that the sum of the weights is equal to 1, the weighted average and the weighted outer product of the transformed points give the mean and covariance of the transformed distribution.This is true when the state vector elements lie in a vector space.However, if the state vector is quaternion, the predicted quaternion mean should be calculated in the rotational space to preserve the nonlinear nature of unit quaternion.The straightforward implementation of the UKF with quaternions is possible when a weighted mean of quaternions produces a unit quaternion estimate, and the predicted covariance computation and quaternion guarantees that quaternion in filter lies on unit sphere.Generally, the number of sigma points is (2p + 1) (augmented state dimension p).However, in the reduced sigma point filter, a set of (p + 2) sigma points can be constructed that fully captures all of the known statistics of the distribution [26].In this paper the spherical simplex scaled sigma points are utilized to implement the UKF with quaternions because it generates better numerical properties [28].
Let us consider the general nonlinear system process and measurement dynamic models: where k x , k u and k z are the state, input and outputs vectors at time k t and xk η and zk η are the system process and measurement noises.The system process state and noise components together, k x and xk η and zk η , respectively, are concatenated to create the augmented state vector, a k x .So that the effect of process and measurement noises can be used to better capture the odd-order moment information.Note that xk Σ and zk Σ are the process and measurement general noise covariance matrices, respectively.
We begin by defining the following state vector, where the superscript "+" denotes an update: where ˆk q + is the estimated quaternion, ˆk p + is the estimated position consisting of the latitude, Φ , longitude, λ , and altitude where 0 P + is the initial covariance matrix and the superscript "-" denotes the predicted variables.The weights factors for the mean m i w and for covariance computation c i w are then initialized according to ( ) and is usually set to a small positive value to minimize higher order effects ( ) The parameter β is used to make further higher order effects to be incorporated by adding the weighting of the 0th sigma point of the calculation of the covariance and it is minimized when 2 β = [16].Next the sigma point set 1 a k χ − are generated, along with their associated weights factors, according to the desired selection scheme.For the spherical simplex scaled sigma points this give: P − is positive definite or semidefinite matrix.Two efficient orthogonalization algorithms can be found in [28].Note that the elements of ∆ .However, the unit quaternion is not mathematically closed for addition and subtraction.The sigma points for the quaternion are also quaternions satisfying the normalization constraints and should be scattered around the current quaternion estimate on the unit sphere.Therefore, the quaternion sigma points are generated by multiplying the error quaternion by the current quaternion estimate.Then the set of p + 1 scaled sigma points is split up and constructed as where , 1 q i k χ − is the quaternion scaled sigma points corresponding to the quaternion attitude part, \ , 1 a q i k χ − is the scaled sigma points corresponding to the rest part of the augmented state-vector and , 1 i k q δ + − is the predicted error quaternion.Since the role of process noise for the quaternion in Equation (30) increases the uncertainty in attitude orientation, the process noise for the quaternion can be modeled as a rotation vector, that leading to orientation uncertainty in the attitude.Since the quaternion process noise is a 3-dimensional noise vector, it has to be expanded into a 4 1 × dimensional unit quaternion to apply the multiplicative quaternion-error approach as follows , sin 0.5 0.5 0.5 cos 0.5 where , 1 i k ϕ − is the rotation vector modeling the process noise for the quaternion.During the prediction stage, the transformed spherical simplex scaled sigma point vectors are propagated through the system process model , , where we have denoted the components of sigma points which correspond to actual state variables and process noise with matrices , 1  is composed of the compensated outputs of the gyro and the accelerometer, respectively, defined in Equation (2), Equation (3).The system process model [.] g refer the overall INS navigation algorithm which is described in Sections 2. Propagation stage is completed, the predicted state vector ˆk x − is calculated using a weighted sample mean of the propagated sigma points, , 1 i k χ − − , which is given by: However, the weighted sum of unit quaternions is generally not a quaternion of unit norm because the unit quaternion is not mathematically closed for addition and scalar multiplication.Therefore, the predicted attitude quaternion ˆk q − is determined as the barycentric mean with renormalization as follows ˆk As in averaging attitude quaternions, the predicted covariance of posterior quaternions cannot be directly computed because the unit quaternion is not closed for subtraction.We can use the quaternion-error for predicted covariance computation because it represents the distance from the predicted mean quaternion ˆk q − .The quaternion-error for predicted covariance computation is given by: ( )  denote the difference between the sigma points and the predicted state vector as , , \ , ∆ is the predicted rotation vector corresponding to the quaternion-error , i k q δ − , i.e. the rotation which turns the orientation part of ˆk x − into , i k χ − , the predicted covariance k P − can be then computed as In additive noise case, the process noise is directly added to the state variables, but other forms of noise effect are now also allowed.Once the predicted state and covariance are computed, the sigma points need to be regenerated using Equation (36) in the update stage.Next, the sigma points are transformed through the measure-ment model: where we have denoted the component of sigma points corresponding to measurement noise with matrix , 1 z i k η χ − .Similarly, the predicted measurement vector ˆk z − is also calculated as: The predicted covariance of the measurement zz P is given by 1 T , , 0 The cross-covariance matrix of the state and measurements vectors is determined using: 1 T , , 0 The Kalman gain matrix k K is then computed by: Finally, the estimated state vector ˆk x + and updated covariance k P + are given by ˆˆk and where is the measurement vector.However, the quaternion should be updated using quaternion multiplication.Then, the estimated quaternion ˆk q + is given by ˆk k k q q q δ + + − = ⊗ (50) where k q δ + is represented by sin 0.5 0.5 , cos 0.5

Numerical Simulation and Experimental Results
The quaternion scaled unscented Kalman filter (Q-SUKF) developed in this paper is demonstrated using the experimental results in this section for a moving vehicle's attitude, position and velocity, as well as the initial sensors biases, scale factors.We assume that the misalignment errors of the initial sensors are considered equal to zeros in our case.The experimental platform consists of an INS aided by GPS and magnetometer sensors.The approach corresponds to a loosely integrated INS/GPS/magnetometer system.The experiment was conducted using car driving (reference trajectory) for 10 mimutes.This reference trajectory and their incremental angles and velocities at each moment was generated by the functions "progencar, gendv and genthet" of INS toolbox version 0 0 0 1 .The gyro and accelerometer noises are given by their spectral densities ( ) ± error covariance boundaries.We can see the better error convergence of the UKF-based INS/GPS/Magnetometer system than the INS/GPS system.The table (Table 1)   shows the root mean square errors (RMSE) of the components of attitude, velocity and position resulting from the two hybridizations.
So we can already conclude that the integration of the magnetometer with the inertial navigation system and GPS provided a better behavior of the convergence of Euler angles, which in turn lead to a better estimate of the velocity and position.All these results indicate that the algorithm Q-SUKF proposed is able to provide a reliable information of the inertial navigation states when it is aided by GPS data (position and velocity) and magnetometer data whatever the initial values on the attitude angles.

Conclusion
Since the behavior and the accuracy of an INS navigation algorithm are strongly influenced by INS errors, this paper develops an integrating Inertial Navigation aided by GPS and magnetometer measurements for large attitude errors using quaternion parameterization of the attitude, velocity and position.Differing from other quaternion models, the weighted mean computation for quaternions is derived in rotational space as a barycentric mean with renormalization and a multiplicative quaternion-error is used for predicted covariance computation of the quaternion because it represents the distance from the predicted mean quaternion.The updates are performed using quaternion multiplication which guarantees that quaternion normalization is maintained in the filter.Since the quaternion process noise resulted in increase of the uncertainty in attitude orientation, it is modeled as a rotation vector.The Q-SUKF solves the multi-rate sensor fusion problem in which a series of aiding sensor data with different measurement vector size and different update-rate are fused with high-rate IMU sensor measurements.Numerical simulation results showed that attitude, velocity and position accuracy can be achieved using the proposed approach for large attitude errors.
specific forces represent the incremental angle and velocities measured by the inertial sensors respectively., the gyro and the accelerometer measurement noises.
η η and MA η is the gravity vector expressed in n-frame an ( ) a skew-symmetric matrix function with components of the angular velocity z xy are the elements of the direction cosine matrix ( ) n bC q .The tilt angles measured by the accelerometers can be determined by the following equation[22]:

g
are the components of the gravity vector b g , expressed in the body frame, issued by the internal accelerometers of the inertial navigation system.The components of the gravity vector b g can be written as follows:

v
are the projections of the GPS velocity vector in the body frame., the components of the real acceleration expressed in the body coordinate system.
A three-axes magnetometer measures the Earth magnetic field , magnetic field measurement measured by the magnetometer.The model of evolution of the residual biases, magnetic disturbance , m k d , non-orthogonality, and scale factors may be given by the following expressions: measured declination angle is computed as follows: the horizontal components of the projection of the compensated magnetic field on the navigation frame.Then this residual is used to compensate for the yaw angle in the framework of the unscented Kalman filter.The following measurement model is used for the yaw angle measurement: scaling parameter α determines the spread of the sigma points around sigma point.The augmented system state square-root matrix 1 a k S − can be obtained using lower triangular matrix of the Cholesky factorization such that 34.d) are similar to the process noise xk η of Equation (30.a).If we interpret the sigma points as a set of disturbed state vectors, then Equation (34.d) describes how these disturbed state vectors are build from the set 3.0 created by GPSoft.Its motion is described in the navigation frame coordinates with origin (point of interest) location at 0 rotations about x-axis and y-axis are very small.The initial attitude is given by the quaternion [ ] T

3 0.
respectively.The incremental angles and velocities are corrupted by the gyro and acelerometer noises and integrated through the mechanization equations to give the inertial navigation sates.The integration of white noise components of the gyroscopes and accelerometers increase the uncertainty of the position, velocity and attitude of INS.The figures (Figure1), (Figure2) and (Figure3) show the drift over time of attitude, velocity and position of the inertial navigation system trajectory.The gyro and accelerometer biases are considered as Random Walks and are given by their spectral densitity , the gyro and accelerometer scale factor are modeled as Random Constants and are equals to ( ) 3 3 0.01 I × and ( ) 3 .005I × respectively.Initial biases for the gyros and accelerometers are given by 10 deg/hr and 0.003 m/s 2 respectively, for each axis.The GPS data was generated by adding to the true positions and velocities of the reference trajectory a white Gaussian noise.The initial standard deviation of the horizontal position expressed in geographic coordinates and vertical position are equal to 8 × 10 -7 rd and 5 m respectively.The initial standard deviation of the horizontal and vertical velocities expressed in the navigation frame is equal to 15 m/s and 3 m/s respectively.The sampling frequency of the GPS is 10 Hz (100 ms).Simulated data of Earth's magnetic field in the navigation frame were generated by the World Magnetic Model 2005 (WMM-2005) that uses the geographic position of the vehicle to determine its components.This reference magnetic field is transformed into the body frame and corrupted by a Gaussian white noise of zero-mean and standard deviation equal to 0.02 Gauss/s 1/2 .During the experiment, the raw data of the inertial navigation system (incremental angles and velocities) are provided at 200 Hz (5ms) whereas the magnetometer data (magnetic field) is supplied at 50 Hz (20 ms).To test the robustness of the Q-SUKF an initial attitude error of 60 degrees is given in each axis.The diagonal terms of initial covariance matrix represent variances or mean squared errors.The off-diagonal terms are set to be zeros.The parameters used in the Q-SUKF developed are given by scaling parameters 0Figures 4-6 show performance comparisons of the newly developed multi-sensor fusion system based on the Q-SUKF between INS/GPS and INS/GPS/Magnetometer fusion.Comparison of time histories of position errors is given in Figure 4.The 3σ ± values of the corresponding error covariance are also included so that the error time histories can be compared to the error covariance boundaries.Similarly, velocity error histories are presented in Figure 5.The position and velocity error histories of the new Q-SUKF-based sensor fusion system (INS/GPS and INS/GPS/Magnetometer) show that, after 60 seconds, the error histories are well-behaved within the 3σ ± error covariance.Despite that the Q-SUKF is working properly with INS/GPS, the navigation Filter performance based on INS/GPS/Magnetometer system provides better estimation performance by reducing the peak values of both position and velocity errors estimation.The error histories of the Euler angles differences are presented in Figure 6 with corresponding 3σ
h , ˆn k , ˆa F k S + are the the estimated gyro and accelerometer scale factors, respec-