#### Abstract

In this paper, a robust heading determination method is proposed for low-cost attitude and heading reference system (AHRS) aided by the global positioning system (GPS). As compared with the traditional GPS/SINS-integrated navigation-based heading determination method, in the proposed method, the heading information obtained from the GPS velocity outputs is first incorporated into the observation vector, which constructs a novel completed GPS/SINS integration framework and greatly improves the observability of azimuthal misalignment in the Kalman filter. Moreover, a multivariate constrained total least square (MCTLS) method is proposed and integrated into the completed integration framework to deal with the measurement error in both input and output data of GPS velocity measurement model, which improves the accuracy of the observed heading information and yields a robust heading estimation at each time instant. Simulation and experiment results demonstrate that the proposed robust heading determination method can outperform the related state-of-the-art methods for the GPS-aided attitude and heading reference system.

#### 1. Introduction

Widely applied in civilian and military fields, attitude and heading reference system (AHRS) has been attracting significant attention [1–3]. AHRS aims mainly to track the attitude of an object based on the outputs of the Micro-Electronic-Mechanical-System (MEMS)-based Inertial Measurement Unit (IMU), which consists of a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer [4, 5]. The core of the AHRS to determine the attitude is to propagate the attitude by integrating the gyroscope measurement and then correct the horizontal angle (pitch and roll) through gravity field measurements from the accelerometer and the heading through the magnetic field measurements from magnetometer [6–8]. The AHRS performs well in quasi-static or low-dynamic condition without the magnetic disturbance [9]. However, in practical applications, the nonmagnetic working environment cannot always be guaranteed, which will lead to large heading errors, thereby making it necessary to develop an effective method to address the problem of heading correction of the low-cost AHRS under the condition of magnetic distortion.

Several efforts have been made to improve the heading measurement accuracy so far, with magnetic distortion eliminating as one of the most common strategies [10]. Over recent years, many researchers have devoted themselves to eliminating the negative effects of the magnetic distortion [11], and existing methods for the magnetic disturbance rejection can be categorized into two groups, namely, threshold-based approach and model-based approach [12, 13]. To be specific, threshold-based approach can contribute to eliminating the magnetic distortion by comparing the measured magnetic field intensity with the threshold value chosen based on the prior knowledge available. If the measured magnetic field intensity is greater than the threshold value, the current measurement of the magnetic field will be assumed unreliable and then discarded. In short, the threshold-based approach is easy to implement, without requiring too much extra computation. However, it is hard to determine the perfect threshold value, since the performance of the algorithm may be somewhat degraded and the magnetic characteristic value is close to the set threshold value. By contrast, the model-based approach eliminates the magnetic distortion by adding the magnetic disturbance in the state vector in the sensor fusion algorithm and estimating the magnetic disturbance based on the assumption that the magnetic distortion obeys a special model. Overall, the model-based approach can help avoid the determination of the appropriate threshold value, but its performance relies heavily on the accurate magnetic sensor model. In other words, when the external magnetic disturbance does not obey the given model in practical applications, large errors will be caused and the performance of the model-based approach may be destroyed.

Another way to improve the heading performance of AHRS under condition of magnetic disturbance is to incorporate the low-cost GPS module into the AHRS and utilize the GPS/SINS-integrated navigation-based heading determination method [14–21]. The core of the GPS/SINS-integrated navigation method is to suppress the drift of gyroscope integration by making use of the aided information provided by the GPS, thus yielding more accurate heading information for a long time [22–25]. To further improve the heading accuracy, Ziebold et al. introduced the multiple GPS antennas scheme to determine the vehicle heading [26]. The heading determination accuracy is determined largely by the baseline length between the two antennas, and the heading accuracy can be improved through increasing the baseline length. However, the baseline length is limited by the vehicle dimension in practical application, which may degrade the accuracy of the heading determination. In addition, there are jump errors in velocity information provided by the GPS due to degradation of tracking performance of GPS in strong disturbance condition, leading to a significantly prominent heading error.

To address the above issues, a robust heading determination method for the low-cost attitude and heading reference system is proposed in this paper under the guidance of total least square theory. Specially, we first formulate the heading unobservable problem by incorporating the heading information obtained from the GPS velocity outputs into the observation vector, which completes GPS/SINS integration framework and greatly improves the observability of azimuthal misalignment in the Kalman filter. Subsequently, a multivariate constrained total least square theory is proposed and integrated into the completed integration framework to address the measurement errors involved in both input and output data of GPS velocity measurement model. This helps improve the accuracy of the observed heading information, yielding a robust heading estimation at each time instant. Simulation and experiment results demonstrate that the proposed robust heading determination method can outperform the related state-of-the-art methods for the GPS-aided attitude and heading reference system.

The rest of the paper is organized as follows: The system model and traditional heading determination method of the low-cost attitude and heading reference system are briefly discussed in Section 2. After that, we present the proposed robust heading determination method in Section 3. Simulations and experiments to evaluate the performance of the proposed approach are reported in Section 4. Conclusion and remarks are drawn in Section 5.

#### 2. Problem Formulation

##### 2.1. System Model For the Attitude and Heading Reference System

The system model of the strap-down inertial navigation system (SINS) is introduced in this section, including attitude error model, velocity error model, position error model, and inertial sensor error model.

Denote by *b* the body frame (*b*-frame) with the *x*-*y*-*z* axis pointing towards forward-up-right, by *n* the local level navigation frame (*n*-frame) with the *x*-*y*-*z* axis pointing towards north-up-east, by *e* the earth frame (*e*-frame), and by *i* the inertial frame. According to [27], the attitude error model is formulated aswhere denotes the misalignment angle between true navigation frame and the calculated navigation frame with , , and being, respectively, the roll error, heading error, and pitch error. represents the angular rate of navigation frame with respected to inertial frame expressed in navigation frame with being the corresponding error. is the angle rate error measured by the gyroscope expressed in the navigation frame.

The velocity error model is given by [28]where and are, respectively, the specific force measured by the accelerometer expressed in the navigation frame and the corresponding measurement error, and are, respectively, the angular rate of earth rotation expressed in navigation frame and the corresponding error, and are, respectively, the angular rate of earth frame with respect to earth frame expressed in navigation frame and the corresponding error, denotes the velocity relative to earth with , , and being, respectively, the velocity in north, up, and east direction, and is the corresponding error of .

The position error model is written as [29]where denotes the position with , , and being, respectively, the latitude, longitude, and height and is the corresponding error of .

The inertial sensor error model is formulated aswhere and denote the constant drift of gyroscope and accelerometer, respectively; and are, respectively, the corresponding correlation time constant of gyroscope and accelerometer; and represent the Gaussian white noise with unity power spectral density, respectively.

##### 2.2. The Traditional Heading Determination Method

In this section, the 15-dimension state vector including alignment angles, velocity error, position error, and inertial sensors error is defined as follows:

Based on the above defined state vector, the system dynamic can be expressed in accordance with the following continuous-time state-space model:where and are, respectively, the state transition matrix and noise distribution matrix, which can be determined based on the SINS error model explained in equations (1)–(7); is the process white noise.

The loosely coupled method is exploited in the GPS-aided AHRSdue to its ease of implementation and appropriate performance. Therefore, the measurement model of the GPS-aided AHRS can be formulated as follows:where the subscripts GPS and Sins are, respectively, the velocity and geographical position obtained from GPS and SINS. is the observation matrix with being the identity matrix and being the zero matrix. is the measurement noise.

Defining the state vector at time step as , the process noise at time step as , the observation vector at time step as , and the observation noise at time step as , the discrete-time state linear state-space model and observation model for equations (9) and (10) can be formulated as follows:where is the sampling interval, is the discrete-time index, and ; the state transition matrix , the observation matrix . Both the process noise and measurement noise obey the Gaussian distribution with the process noise covariance and measurement noise covariance .

Kalman filter is the most common method to estimate the attitude of the GPS-aided AHRS based on the predicted information from the state-space model and the observation information from the observation model. The implementation of the Kalman filter contains two steps: time update and measurement update. The time update calculates the predicted states at epoch *k* according to the discrete-time state-space model based on the optimal estimated state at epoch *k*−1. Meanwhile, the measurement update fuses the current observation information and the predicted states at epoch *k* to yield an improved a posteriori state estimation. The calculations for time update and measurement update are as follows.

###### 2.2.1. Time UpdateX

where and are, respectively, the predicted state vector and the corresponding predicted error covariance matrix.

###### 2.2.2. Measurement Update

Based on the above estimated up misalignment angle (heading error) in the state vector, the heading obtained from the SINS-based AHRS can be corrected to some extent.

##### 2.3. Problem Formulation

The optimal heading can be obtained from the traditional GPS/SINS-integrated navigation scheme under special conditions such as purposeful maneuvering and better satellite signal. However, the performance of the GPS/SINS-integrated navigation-based heading determination method may be degraded seriously due to the poor quality of the GPS measurements caused by the multipath and low satellite visibility in the densely built-up urban environment and purposeful maneuverings are always unfeasible in practical applications. Therefore, the aforementioned challenges represent the main motivation of this work.

#### 3. The Proposed Robust Heading Determination Approach

The proposed robust heading determination approach is developed in the following two major steps: the completed integration framework construction and multivariate constrained total least square theory (MCTLS)-based robust filtering design.

##### 3.1. The Completed Integration Framework Construction

To avoid the unobservable heading in the traditional GPS/SINS-integrated navigation-based determination method, we construct the completed integration framework by employing the heading information obtained from the GPS velocity outputs. The main difference between the traditional GPS/SINS loosely coupled integrated and the completed integration framework lies in the measurement equation construction. To construct the measurement equation of completed integration, we first derive the relationship between the attitude error angle and platform misalignment angle as follows.

Denote by the true attitude matrix between the true navigation frame (*n*-frame) and body frame (*b*-frame) and by the true attitude matrix between the computed navigation frame (*c*-frame) and body frame (*b*-frame). According to the matrix chain multiplication rule, we can obtain the following relationship between and :where represents the derivation between *n*-frame and *c*-frame caused by the platform misalignment angle . Assuming that is small, the matrix can be approximated by

Meanwhile, we can obtain the attitude transformation matrices and as follows [30]:where and , respectively, denote the computed attitude and true attitude resolving in *n*-frame. By defining the attitude error , we can obtain

Taking the heading for example and calculating the sine and cosine values at both sides of equation yields

Considering that the attitude error is small and substituting and into (19) yield

Combining (14)∼(20), we obtain the following relationship between the attitude error angle and platform misalignment angle :where is the coefficient between and , which is used in the construction of measurement matrix.

In this paper, we choose the position, velocity, and heading error obtained, respectively, from the GPS and SINS as the observation to construct the completed integration framework. Assuming that the velocity and position information resolving in ECEF (Earth-Centered, Earth-Fixed) frame obtained from GPS are, respectively, and , the geographical position can be given as follows:where is the prime radius of ellipsoid, is the eccentricity of ellipsoid, and is the semimajor axis of ellipsoid.

The velocity in local navigation frame can be given as follows:where is the attitude transformation matrix from ECEF frame to local navigation frame. To reduce the computational complexity, we define , and obtain the relationship between and as follows:where is the submatrix of . Based on the velocity information obtained from equation (23), the heading information from GPS can be calculated as follows:

Combining the navigation information (heading, velocity, and geographical position) calculated by the SINS [30], we can construct the completed integrated observation model as follows:where is the completed integration measurement matrix.

*Remark 1. *In the traditional GPS/SINS-integrated navigation method, the pitch and roll can be estimated accurately due to the indirect observability of north and east misalignment angle. Nevertheless, the estimated performance of heading is poor, since the up misalignment angle is unobservable in GPS/SINS-integrated system model. Even though the purposeful maneuverings can somewhat improve the degree of observability in up misalignment angle. To address the problem, we employ the heading information obtained from the GPS velocity outputs to make the up misalignment angle directly observable, which constructs the completed integrated system and significantly improves the estimating accuracy of heading. As compared with the existing attitude determination methods for low-cost AHRS aided by GPS, the major advantage of our proposed method is that it can effectively improve the heading measurement accuracy in the process of GPS and SINS information fusion [7, 31].

##### 3.2. The Multivariate Constrained Total Square Theory-Based Robust Filtering

In general, equation (24) can be resolved by employing the traditional least square method. However, considering both of the coefficient matrix and observation vector existing measurement error, the estimation results of the traditional least square method may be biased and the estimation performance may be seriously degraded. Therefore, in order to reduce the influence of coefficient on the least square algorithm, we proposed in this paper the multivariate constrained total least square method to estimate the state vector optimally. The main idea of the multivariate constrained total least square algorithm is to estimate the state vector by minimizing the sum of and , which are, respectively, the error matrix of the coefficient matrix and the observation vector . Therefore, the linearization equation (24) can be accurately expressed as

Equation (27) can be further rewritten in the following matrix form:where is the unit matrix. To reduce the rank of the augmented matrix , we employ the singular value decomposition (SVD) as follows:where and are, respectively, the orthogonal matrices, which are composed of the eigenvectors of matrices and . is the singular value of matrix . Similarly, we can obtain

Multiplying both sides of equation (30) by matrix yieldsthat is,

We have the following relationship:

Substituting (29) and (30) into (33) yields

Thus, combining (30) and (34), we can obtain

Multiplying both sides of equation (35) by yields

Then, by combining (38) and (28), we can obtain the following result:

*Remark 2. *In most related literatures, the heading is determined roughly by the GPS velocity information using the traditional least square method. Obviously, this will lead to a degraded performance due to the disturbance existing in the practical measurement. In the proposed robust heading determination method, a multivariate constrained total least square is proposed to bound the influence of both coefficient matrix and observation vector . As compared with the state-of-the-art adaptive and robust Kalman filtering methods, the major advantage of our proposed method is that it can deal with the observation outliers under the condition of unknown noise distribution.

#### 4. Numerical Results

In this section, the simulation results are given to illustrate the performance of the proposed algorithm. The simulations are run on a platform of Intel(R) Core(TM) i5-3320 [email protected] GHz PC with 8G RAM and Matlab 2019a. To test the effectiveness of the completed integration framework in the proposed algorithm, the traditional loosely coupled integrated framework is compared based on the GPS-aided attitude and heading reference system, whose simulation data can be found in [32]. In this simulation, the true trajectory of the AHRS is shown in Figures 1. The total simulation time is 480 s. The constant drift and random drift of gyroscope and accelerometer used in the AHRS are, respectively, 12 deg/h, 5 mg and 0.24 deg/sqrt(h), 0.3 m/s^{2}/sqrt(Hz). The velocity and position error of GPS used in the AHRS are, respectively, 0.1 m/s and 10 m. The metrics used for the comparison between the two schemes is the root mean square error (RMSE) of heading for the AHRS, which is defined as follows:where and denote the true and the estimated value of heading at time *k* of the th Monte Carlo run, respectively. *M* denotes the total simulation samples.

The estimated position, velocity, attitude, and the corresponding error via the traditional GPS/SINS loosely coupled integrated and the completed integration framework are, respectively, shown in Figures 2–4, and the corresponding RMSEs are shown in Table 1. It can be seen from Figures 2–4 and Table 1 that the traditional GPS/SINS loosely coupled integration framework and the completed integration framework have similar performance in the estimation of the position and velocity. This is because both the velocity error and position error in the state vector of GPS/SINS-integrated model are directly observable due to the velocity and position information provided by the GPS. Nevertheless, the traditional GPS/SINS loosely coupled integration plays an unsatisfactory performance in the attitude estimation, especially the heading information since the platform misalignment angle is unobservable or indirectly unobservable in the GPS/SINS-integrated navigation. In contrast, the proposed method using the completed integration framework can achieve better attitude estimation performance than the GPS/SINS loosely coupled integration. This is because the proposed completed integration employing the heading information obtained from the GPS velocity outputs as the element of measurement vector, which significantly improves the observability of heading misalignment angle and finally leads to a better heading estimation accuracy. Therefore, the simulation results indicate that the proposed completed integration framework GPS/SINS can effectively improve the observability of heading misalignment angle and achieve better performance in the estimation of attitude.

**(a)**

**(b)**

**(a)**

**(b)**

**(a)**

**(b)**

The GPS velocity measurement information in ECEF frame with the outliers is shown in Figure 5. The GPS velocity information in NED navigation frame obtained via least square and multivariate constrained total least square is shown in Figure 6. Compared with the situation without disturbance, the north and east velocity information estimated by the traditional least square method at the time instant with disturbance is significantly biased as it is unable to handle the successive outliers. By contrast, the multivariate constrained total least square method is capable of coping with the unknown disturbance thanks to its robustness provided by the special robust mechanism. The heading information obtained via the traditional SINS/GPS-integrated method and the proposed robust heading determination method is shown in Figure 7(a). The local enlarged drawing of Figure 7(a) is displayed in Figure 7(b). It is observed that traditional heading determination method fails to resolve the unknown disturbance. We can also see that the azimuth results obtained from the traditional heading determination method exhibit significant drift error. This is expected since the platform misalignment angle is unobservable or indirectly unobservable in the traditional GPS/SINS-integrated navigation. By contrast, our proposed completed integration is able to utilize the heading information obtained from GPS velocity information to suppress the azimuth drift while exhibiting higher estimation accuracy than the traditional heading determination method.

**(a)**

**(b)**

**(c)**

**(a)**

**(b)**

**(a)**

**(b)**

To better demonstrate the superior performance of the proposed method, the car-mounted test datasheet is used to evaluate the heading estimate performance in this section. In the experiment, the sample frequency of IMU and the update frequency of GNSS are, respectively, 100 Hz and 10 Hz. The bias stabilities of gyroscope and accelerometer are, respectively, 12 deg/h and 5 mg. The angular rate random walk and velocity random walk of the gyroscope and accelerometer are, respectively, 0.24 deg/sqrt(h) and 0.3 m/s/sqrt(Hz). The reference heading for the performance comparison of integrated navigation is provided by the high-accuracy integrated navigation system, which can provide a 0.01 deg heading accuracy. The total test time is 400 s. In the experiment test, the car run with a good sky view of the GNSS antennas. The test trajectory is shown in Figure 8.

Existing traditional SINS/GNSS-integrated navigation-based heading determination method (traditional SINS/GNSS) and the proposed robust SINS/GPS completed integrated navigation method (proposed SINS/GNSS) are tested and compared in the car-mounted experiment. The initial state estimation and the corresponding estimation error of the Kalman filter in the traditional and the proposed SINS/GNSS are, respectively, set as **X**_{0|0} = 0 and **P**_{0|0} = diag([0.005 rad, 0.015 rad, 0.005 rad, 0.2 m/s, 0.2 m/s, 0.2 m/s, 3 m, 10 m, 3m, 0.2 deg/s, 0.2 deg/s, 0.2 deg/s, 1 mg, 1 mg, 1 mg])^{2}, respectively. The process and measurement noise covariance matrix in the Kalman filter are, respectively, set as **Q**_{k} = diag([0.24 deg/sqrt(h), 0.24 deg/sqrt(h), 0.24 deg/sqrt(h), 0.3 m/s^{2}/sqrt(Hz), 0.3 m/s^{2}/sqrt(Hz), 0.3 m/s^{2}/sqrt(Hz))^{2}, and **R**_{k} = diag([0.05 m/s, 0.05 m/s, 0.05 m/s, 1 m, 3 m, 1 m])^{2}.

The position, velocity, and heading provided by the traditional SINS/GNSS and the proposed SINS/GNSS are, respectively, shown in Figures 9–12. The corresponding RMSEs of the velocity and position are, respectively, listed in Table 2. It can be clearly seen from Figures 9–11 and Table 2 that the position, velocity, and attitude estimated from the traditional SINS/GNSS and the proposed SINS/GNSS have similar estimation accuracy. This is due to the fact that the position and velocity are directly observable and the level attitudes which include the pitch and roll are indirectly observable in both the traditional and proposed SINS/GNSS-integrated navigation systems. By contrast, the heading obtained from the proposed SINS/GNSS can achieve a better estimation performance than the traditional SINS/GNSS, since the complete integration framework in the proposed SINS/GNSS can greatly improve the observability of heading misalignment angle. On the other hand, the multivariate constrained total least square method can effectively deal with the uncertainty in the GPS velocity and position measurement in ECEF frame, yielding a more high-accuracy velocity and position measurement in NED frame and finally leading to a better SINS/GNSS-integrated navigation heading estimation result. Therefore, we can conclude that the proposed low-cost AHRS via the proposed robust completely SINS/GNSS-integrated navigation scheme can achieve better heading determination performance than the state-of-the-art methods.

**(a)**

**(b)**

**(c)**

**(d)**

**(e)**

**(f)**

**(a)**

**(b)**

**(c)**

**(d)**

**(e)**

**(f)**

**(a)**

**(b)**

**(c)**

**(d)**

**(a)**

**(b)**

**(c)**

#### 5. Conclusions

We propose in this paper a robust heading determination method for low-cost attitude and heading reference system (AHRS) aided by the global positioning system (GPS). In the proposed method, the heading information obtained from the GPS velocity outputs is first incorporated into the observation vector, which constructs a novel completed GPS/SINS integration framework and greatly improves the observability of azimuthal misalignment in the Kalman filter. Moreover, a multivariate constrained total least square method is proposed and integrated into the completed integration framework to deal with the measurement error in both input and output data of GPS velocity measurement model, which improves the accuracy of the observed heading information and yields a robust heading estimation at each time instant. Simulation and experiment results demonstrate that the proposed robust heading determination method can outperform the related state-of-the-art methods for the GPS-aided attitude and heading reference system.

#### Data Availability

The data used to support the findings of this study are available from the corresponding author upon request.

#### Conflicts of Interest

The authors declare that there are no conflicts of interest.

#### Acknowledgments

This work was supported in part by the Postdoctoral Science Foundation of China (no. 2019M661069).