MCTLS-Assisted Completed SINS/GPS Integrated and Applied to Low-Cost Attitude and Heading Reference System

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.


Introduction
Widely applied in civilian and military fields, attitude and heading reference system (AHRS) has been attracting significant attention [1][2][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]. e 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][7][8].
e 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, thresholdbased 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][15][16][17][18][19][20][21]. e 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][23][24][25]. To further improve the heading accuracy, Ziebold et al. introduced the multiple GPS antennas scheme to determine the vehicle heading [26]. e 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. is 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. e rest of the paper is organized as follows: e 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.

System Model For the Attitude and Heading Reference
System.
e 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 as where ϕ � ϕ N ϕ U ϕ E T denotes the misalignment angle between true navigation frame and the calculated navigation frame with ϕ N , ϕ U , and ϕ E being, respectively, the roll error, heading error, and pitch error. ω n in represents the angular rate of navigation frame with respected to inertial frame expressed in navigation frame with δω n in being the corresponding error. δω n ib is the angle rate error measured by the gyroscope expressed in the navigation frame. e velocity error model is given by [28] δ _ v n � f n sf × ϕ + v n × 2δω n ie + δω n en − 2ω n ie + ω n en × δv n + δf n sf , where f n sf and δf n sf are, respectively, the specific force measured by the accelerometer expressed in the navigation frame and the corresponding measurement error, ω n ie and δω n ie are, respectively, the angular rate of earth rotation expressed in navigation frame and the corresponding error, ω n en and δω n en are, respectively, the angular rate of earth frame with respect to earth frame expressed in navigation frame and the corresponding error, v n � v N v U v E T denotes the velocity relative to earth with v N , v U , and v E being, respectively, the velocity in north, up, and east direction, and δv n � δv N δv U δv E T is the corresponding error of v n . e position error model is written as [29] δ _ L � 1 where p n � L λ h T denotes the position with L, λ, and h being, respectively, the latitude, longitude, and height and δp n � δL δλ δh T is the corresponding error of p n . e inertial sensor error model is formulated as where ε b and ∇ b denote the constant drift of gyroscope and accelerometer, respectively; τ g and τ a are, respectively, the corresponding correlation time constant of gyroscope and accelerometer; w g and w a represent the Gaussian white noise with unity power spectral density, respectively.

e 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 A(t) and w(t) 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); w(t) is the process white noise. e loosely coupled method is exploited in the GPSaided AHRSdue to its ease of implementation and appropriate performance. erefore, 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. H � [0 6×3 , I 6×6 , 0 6×12 ] is the observation matrix with I being the identity matrix and 0 being the zero matrix. v(t) is the measurement noise. Defining the state vector at time step t k as x k � x(t k ), the process noise at time step t k as w k � w(t k ), the observation vector at time step t k as z k � z(t k ), and the observation noise at time step t k as v k � v(t k ), the discrete-time state linear state-space model and observation model for equations (9) and (10) can be formulated as follows: where T s is the sampling interval, k is the discrete-time index, and t k � kT s ; the state transition matrix Both the process noise w k−1 and measurement noise v k obey the Gaussian distribution with the process noise covariance Q k and measurement noise covariance R k . 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. e implementation of the Kalman filter contains two steps: time update and measurement update. e 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. e calculations for time update and measurement update are as follows.

Time UpdateX.
where x k|k−1 and P k|k−1 are, respectively, the predicted state vector and the corresponding predicted error covariance matrix.

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.

Problem Formulation.
e 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 builtup urban environment and purposeful maneuverings are always unfeasible in practical applications. erefore, the aforementioned challenges represent the main motivation of this work.
Mathematical Problems in Engineering 3

The Proposed Robust Heading Determination Approach
e 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.

e 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. e 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 δA and platform misalignment angle ϕ as follows.
Denote by C n b the true attitude matrix between the true navigation frame (n-frame) and body frame (b-frame) and by C c b 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 C n b and C c b : where C n c represents the derivation between n-frame and cframe caused by the platform misalignment angle Assuming that ϕ is small, the matrix C n c can be approximated by Meanwhile, we can obtain the attitude transformation matrices C c b and C n b as follows [30]: cos θ n cos ψ n −cos c n cos ψ n sin θ n + sin c n sin ψ n sin θ n sin c n cos ψ n + cos c n sin ψ n sin θ n cos θ n cos c n −sin c n cos θ n −sin ψ n cos θ n cos c n sin ψ n sin θ n + sin c n cos ψ n −sin c n sin ψ n sin θ n + cos c n cos ψ n where A c � ψ c θ c c c T and A n � ψ n θ n c n T , respectively, denote the computed attitude and true attitude resolving in n-frame. By defining the attitude error δA � δψ δθ δc T , we can obtain Taking the heading ψfor example and calculating the sine and cosine values at both sides of equation yields sin ψ c � sin ψ n + δψ � sin ψ n cos δψ + sin ψ n sin δψ, cos ψ c � cos ψ n + δψ � cos ψ n cos δψ − sin ψ n sin δψ.

⎧ ⎨ ⎩ (19)
Considering that the attitude error is small and substituting sin δψ ≈ δψ and cos δψ ≈ 1 into (19) yield sin ψ c � sin ψ n + δψ � sin ψ n + cos ψ n δψ, Combining (14)∼ (20), we obtain the following relationship between the attitude error angle δA and platform misalignment angle ϕ: the coefficient between δA 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, where R N � (a/ ��������� � 1 − e 2 sin 2 ϕ ) is the prime radius of ellipsoid, e � 0.08181919 is the eccentricity of ellipsoid, and a � 6378137 is the semimajor axis of ellipsoid.
igation frame can be given as follows: where attitude transformation matrix from ECEF frame to local navigation frame. To reduce the computational complexity, the relationship between X and Z as follows: where A � −sin L * cos λ −sin L * sin λ −sin λ cos λ is the submatrix of R n e . 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 H � M 3×3 0 3×12 I 6×6 0 6×9 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].

e Multivariate Constrained Total Square eory-Based
Robust Filtering. In general, equation (24) can be resolved by employing the traditional least square method. However, considering both of the coefficient matrix A − 1 and observation vector Z existing measurement error, the estimation results of the traditional least square method may be biased and the estimation performance may be seriously degraded. erefore, 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 X optimally. e main idea of the multivariate constrained total least square algorithm is to estimate the state vector X by minimizing the sum of E A and E Z , which are, respectively, the error matrix of the coefficient matrix A − 1 and the observation vector Z. erefore, the linearization equation (24) can be accurately expressed as Equation (27) can be further rewritten in the following matrix form: where Ι k is the k × k unit matrix. To reduce the rank of the augmented matrix [A − 1 + E A |Z + E Z ], we employ the singular value decomposition (SVD) as follows: where [U A |U Z ] and V AA V AZ V ZA V ZZ T are, respectively, the orthogonal matrices, which are composed of the eigenvectors of matrices C · C T and C · C T .
Similarly, we can obtain

Multiplying both sides of equation (30) by matrix
that is, We have the following relationship: Substituting (29) and (30) into (33) yields us, combining (30) and (34), we can obtain Multiplying both sides of equation (35) en, 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 A − 1 and observation vector Z. 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.

Numerical Results
In this section, the simulation results are given to illustrate the performance of the proposed algorithm. e simulations are run on a platform of Intel(R) Core(TM) i5-3320 CPU@ 2.6 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 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 x s k and x s k denote the true and the estimated value of heading at time k of the sth Monte Carlo run, respectively. M denotes the total simulation samples. e 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. is 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.
is 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. erefore, 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. e GPS velocity measurement information in ECEF frame with the outliers is shown in Figure 5. e 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. e heading information obtained via the traditional SINS/ GPS-integrated method and the proposed robust heading determination method is shown in Figure 7(a). e 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. is 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.
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. e bias stabilities of gyroscope and accelerometer are, respectively, 12 deg/h and 5 mg. e 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). e reference heading for the performance comparison of integrated navigation is provided by the highaccuracy integrated navigation system, which can provide a 0.01 deg heading accuracy. e total test time is 400 s. In the   Figure 8.  Table 2. It can be clearly seen from   Table 2 that the position, velocity, and attitude estimated from the traditional SINS/GNSS and the proposed SINS/GNSS have similar estimation accuracy. is 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    Mathematical Problems in Engineering effectively deal with the uncertainty in the GPS velocity and position measurement in ECEF frame, yielding a more highaccuracy velocity and position measurement in NED frame and finally leading to a better SINS/GNSS-integrated navigation heading estimation result. erefore, 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.

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-ofthe-art methods for the GPS-aided attitude and heading reference system.

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