Received 30 April 2014; revised 26 May 2014; accepted 26 June 2014

ABSTRACT

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 de- veloped that incorporates measurements from an Inertial Navigation System (INS), Global Posi- tioning 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 fun- damental 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 estima- tion 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.

Keywords:

Inertial Navigation System, Inertial Sensor Model, GPS, Magnetometer, Quaternion Attitude Parameterization, Rotation Vector, Scaled Augmented Unscented Kalman Filter

1. 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 at- titude algorithm). The attitude information is used to transform the linear acceleration signal into a suitable na- vigation coordinate frame where it is double integrated in order to obtain the velocity and position states (de- noted as velocity and position algorithms). Thus three integration functions (INS algorithm) are involved: atti- tude, 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 po- sition. The benefits of this integration are well known. The supporting sensors (GPS and magnetometers) pro- vide 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 larg- est 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 Kal- man 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 qu- atemion 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.

2. 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 there- fore 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.

2.1. Process Model

The state vector 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

(1)

The system process model 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 al- gorithm which is described below.

2.1.1. Navigation error Model Compensation

The compensation of inertial sensor measurement is accomplished to express the true integrated inertial sensor measurement, as follows:

(2)

(3)

where, due to the angular rates and due to the specific forces represent the incremental angle and

velocities measured by the inertial sensors respectively. represent the gyro and the accelerometer biases,

respectively. represent the gyro and the accelerometer measurement noises.

are the matrices of gyro and accelerometer scale factors (SF)/misalignments (MA) [16]. The sensor error model can be applied as follows:

(4)

(5)

(6)

where and are the parameters that can represent the random walk, the random con-

stant and the first-order Gauss-Markov processes.

2.1.2. Navigation Digital Attitude quaternion integration

The digital attitude quaternion integration model, , from the body frame to the navigation frame can be up-

dated as follows [12] :

(7.a)

(7.b)

where is attitude quaternion that accounts for the b-frame rotation relative to inertial space from its

orientation at time to its orientation at time, which can be expressed in terms of rotation vector as fol-

lows [fusion]:

(8)

where is the b-frame rotation vector which can be obtained as:

(9)

The attitude quaternion, that accounts for the n-frame rotation relative to inertial space from its orien-

tation at time to its orientation at time, can be also expressed in terms of a rotation vector as follows [20] :

(10)

where is the n-frame rotation vector. is computed as follows:

(11)

where represents dynamic latitude, is the height, are velocities in the north and east direction in the navigation frame, is the radius of curvature in the prime vertical and is the meridian radius of curvature, is the magnitude of the Earth’s rotation rate. All terms in this paper, can be calculated by a linear extrapolation formula

(12)

2.1.3. Digital velocity integration

The digital velocity integration model, , can be written as [13] :

(13)

The velocity increment, due to the specific force, can be computed as follows:

(14)

where and are the compensated outputs of the gyroscope and accelerometer, respectively. The

gravity and Coriolis correction term can be computed as follows:

(15)

is the gravity vector expressed in n-frame an denotes a skew-symmetric matrix function

with components of the angular velocity in z-frame [21] .

2.1.4. Digital position integration

The INS computes position vector by integrating velocity vector [3] [12] [13] . The general position can be up- dated as follows

(16)

where 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.

2.2. 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.

2.2.1. 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. The lever arm effect in the

position and velocity measurements is written as:

(17.a)

(17.b)

where is the offset vector of the GPS antenna from the center of the IMU in the body frame. is the GPS position measured in the Earth-fixed frame (e-frame). is the position of the inertial navigation system calculated in the Earth-fixed frame. is the velocity of GPS measured in the navigation frame and is the velocity of the inertial navigation system calculated in the navigation frame. and are the time delay coming from the GPS sensor latency, defined as follow:

(18)

(19)

The position vector in the e-frame can be expressed in terms of the geodetic coordinates as follows:

(20.a)

(20.b)

(20.c)

where is the first eccentricity of the reference ellipsoid, and is the radius of curvature in the prime ver- tical terminated by minor axis “”.

(21)

2.2.2. Accelerometer Measurement model

The difference between the estimates, at time, 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:

(22.a)

(22.b)

The tilt angles estimated by the inertial navigation system can be determined by the following equation:

(23)

where are the elements of the direction cosine matrix.The tilt angles measured by the accelero-

meters can be determined by the following equation [22] :

(24.a)

(24.b)

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

(25)

where and are the projections of the GPS velocity vector in the body frame. and are the components of the real acceleration expressed in the body coordinate system.

2.2.3. Magnetometer measurement model

A three-axes magnetometer measures the Earth magnetic field in the body frame. As for the inertial

sensors, magnetometer measurement errors can be compensated by using the following measurement model:

(26)

where is the compensated magnetic field measurement measured by the magnetometer. The model of evolution of the residual biases, magnetic disturbance, non-orthogonality, and scale factors may be given by the following expressions:

(27.a)

(27.b)

(27.c)

(27.d)

where and are the parameters of the evolution models. The com-

pensated 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] , ,

and the measured declination angle. The measured declination angle is computed as follows:

(28)

where are the horizontal components of the projection of the compensated magnetic field on the navi-

gation 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:

(29)

3. 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 appropri- ate 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) (aug- mented state dimension p). However, in the reduced sigma point filter, a set of (p + 2) sigma points can be con- structed 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:

(30.a)

(30.b)

where, and are the state, input and outputs vectors at time and and are the system process and measurement noises. The system process state and noise components together, and and, respectively, are concatenated to create the augmented state vector,. So that the effect of process and measurement noises can be used to better capture the odd-order moment information. Note that and are the process and measurement general noise covariance matrices, respectively.

We begin by defining the following state vector, where the superscript “+” denotes an update:

(31)

where is the estimated quaternion, is the estimated position consisting of the latitude, , longitude,

, and altitude, is the estimated velocity vector, and are the estimated gyro and accelero-

meter biases, respectively, and are the the estimated gyro and accelerometer scale factors, respec-

tively, and and the estimated gyro and accelerometer misalignments, respectively. The first

step is the initialization of the augmented state and augmented state error covariance as follows:

(32)

where is the initial covariance matrix and the superscript “-“ denotes the predicted variables. The weights factors for the mean and for covariance computation are then initialized according to

(33)

where and. The scaling parameter determines the spread of the sigma points

around 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 [16] . Next the sigma point set are generated, along with their associated weights factors, according to the desired selection scheme. For the spherical simplex scaled sigma points this give:

(34.a)

(34.b)

(34.c)

(34.d)

With and where is the weights factors and indicates

the ith scaled sigma point. The augmented system state square-root matrix can be obtained using lower triangular matrix of the Cholesky factorization such that

(35)

where is positive definite or semidefinite matrix. Two efficient orthogonalization algorithms can be found

in [28] . Note that the elements of in Equation (34.d) are similar to the process noise 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. However, the unit quaternion is not mathematically

closed for addition and subtraction. The sigma points for the quaternion are also quaternions satisfying the nor- malization constraints and should be scattered around the current quaternion estimate on the unit sphere. There- fore, the quaternion sigma points are generated by multiplying the error quaternion by the current quaternion es- timate. Then the set of p + 1 scaled sigma points is split up and constructed as

(36)

where is the quaternion scaled sigma points corresponding to the quaternion attitude part, is the

scaled sigma points corresponding to the rest part of the augmented state-vector and 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 dimensional unit quaternion to apply the multiplicative quaternion-error approach as follows

(37)

where 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

(38)

where we have denoted the components of sigma points which correspond to actual state variables and process

noise with matrices and, respectively. The input vector is composed of the

compensated outputs of the gyro and the accelerometer, respectively, defined in Equation (2), Equation (3). The system process model refer the overall INS navigation algorithm which is described in Sections 2.

Propagation stage is completed, the predicted state vector is calculated using a weighted sample mean of

the propagated sigma points, , which is given by:

(39)

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 is determined as the barycentric mean with renormalization as follows

(40)

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 pre- dicted covariance computation because it represents the distance from the predicted mean quaternion. The quaternion-error for predicted covariance computation is given by:

(41)

Let denote the difference between the sigma points and the predicted state vector as

(42)

Since is the predicted rotation vector corresponding to the quaternion-error, i.e. the rotation which turns the orientation part of into, the predicted covariance can be then computed as

(43)

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 rege- nerated using Equation (36) in the update stage. Next, the sigma points are transformed through the measure- ment model:

(44)

where we have denoted the component of sigma points corresponding to measurement noise with matrix. Similarly, the predicted measurement vector is also calculated as:

(45)

The predicted covariance of the measurement is given by

(46)

With. The cross-covariance matrix of the state and measurements vectors is determined

using:

(47)

The Kalman gain matrix is then computed by:

(48)

Finally, the estimated state vector and updated covariance are given by

(49.a)

(49.b)

With and where is the measurement vector. However, the quaternion

should be updated using quaternion multiplication. Then, the estimated quaternion is given by

(50)

where is represented by

(51)

4. Numerical simulation and Experimental Results

The quaternion scaled unscented Kalman filter (Q-SUKF) developed in this paper is demonstrated using the ex- perimental results in this section for a moving vehicle’s attitude, position and velocity, as well as the initial sen- sors 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 3.0 created by GPSoft. Its motion is described in the navigation frame coordinates with origin (point of

interest) location at and. The initial velocity is given by. We

assume that the rotations about x-axis and y-axis are very small. The initial attitude is given by the quaternion

. The gyro and accelerometer noises are given by their spectral densities

and, respectively. The incremental angles and velocities are corrupted by the gyro and ace-

lerometer 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 po- sition, velocity and attitude of INS. The figures (figure 1), (figure 2) and (figure 3) 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

and respectively. Also, the gyro and accelerometer scale factor are

modeled as Random Constants and are equals to and respectively. Initial biases for

the gyros and accelerometers are given by 10 deg/hr and 0.003 m/s² 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 posi- tion 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 ini- tial 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 and, and by weight of 0th point. Figures 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 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 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 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 magneto- meter data whatever the initial values on the attitude angles.

5. 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 atti- tude errors using quaternion parameterization of the attitude, velocity and position. Differing from other quater- nion 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.

References