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

Authors: Canhui Tao; Zhiping Song; Zhenping Weng
Journal: Mathematical Problems in Engineering
 (2021)

---

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


---

## Body

## 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 Section2. 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 byb 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(1)ϕ˙=ϕ×ωinn+δωinn−δωibn,where ϕ=ϕNϕUϕETdenotes 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. ωinn represents the angular rate of navigation frame with respected to inertial frame expressed in navigation frame with δωinn being the corresponding error. δωibn is the angle rate error measured by the gyroscope expressed in the navigation frame.The velocity error model is given by [28](2)δv˙n=fsfn×ϕ+vn×2δωien+δωenn−2ωien+ωenn×δvn+δfsfn,where fsfn and δfsfn are, respectively, the specific force measured by the accelerometer expressed in the navigation frame and the corresponding measurement error, ωien and δωien are, respectively, the angular rate of earth rotation expressed in navigation frame and the corresponding error, ωenn and δωenn are, respectively, the angular rate of earth frame with respect to earth frame expressed in navigation frame and the corresponding error, vn=vNvUvET denotes the velocity relative to earth with vN, vU, and vE being, respectively, the velocity in north, up, and east direction, and δvn=δvNδvUδvET is the corresponding error of vn.The position error model is written as [29](3)δL˙=1RM+hδvN−vNRM+h2δh,(4)δλ˙=secLRN+hδvE+vEsecLtanLRN+hδL−vEsecLRN+h2δh,(5)δh˙=δvU,where pn=LλhT denotes the position with L, λ, and h being, respectively, the latitude, longitude, and height and δpn=δLδλδhT is the corresponding error of pn.The inertial sensor error model is formulated as(6)ε˙b=−1τgεb+wg,(7)∇˙b=−1τa∇b+wa,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; wg and wa 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:(8)x=ϕδvnδpnεb∇bT.Based on the above defined state vector, the system dynamic can be expressed in accordance with the following continuous-time state-space model:(9)x˙t=Atxt+Btwt,where At and wt 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); wt 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:(10)zt=VGPSN−VSinsNVGPSU−VSinsUVGPSE−VSinsELGPS−LSinshGPS−hSinsλGPS−λSins=Hxt+vt,where the subscripts GPS and Sins are, respectively, the velocity and geographical position obtained from GPS and SINS. H=06×3,I6×6,06×12 is the observation matrix with I being the identity matrix and 0 being the zero matrix. vt is the measurement noise.Defining the state vector at time steptk as xk=xtk, the process noise at time step tk as wk=wtk, the observation vector at time step tk as zk=ztk, and the observation noise at time step tk as vk=vtk, the discrete-time state linear state-space model and observation model for equations (9) and (10) can be formulated as follows:(11)xk=Φkxk−1+Gkwk−1,zk=Hkxk+vk,where Ts is the sampling interval, k is the discrete-time index, and tk=kTs; the state transition matrix Φk=Ι15×15+TsAt, the observation matrix Hk=H. Both the process noise wk−1 and measurement noise vk obey the Gaussian distribution with the process noise covariance Qk and measurement noise covariance Rk.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 epochk 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

(12)xk|k−1=Φkxk−1,Pk|k−1=ΦkPk−1ΦkT+Qk,where xk|k−1 and Pk|k−1 are, respectively, the predicted state vector and the corresponding predicted error covariance matrix.


#### 2.2.2. Measurement Update

(13)Kk=Pk|k−1HkTHkPk|k−1HkT+Rk−1,xk|k=xk|k−1+Kkzk−Hkxk|k−1,Pk|k=Ι−KkHkPk|k−1.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.


## 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 byb 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(1)ϕ˙=ϕ×ωinn+δωinn−δωibn,where ϕ=ϕNϕUϕETdenotes 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. ωinn represents the angular rate of navigation frame with respected to inertial frame expressed in navigation frame with δωinn being the corresponding error. δωibn is the angle rate error measured by the gyroscope expressed in the navigation frame.The velocity error model is given by [28](2)δv˙n=fsfn×ϕ+vn×2δωien+δωenn−2ωien+ωenn×δvn+δfsfn,where fsfn and δfsfn are, respectively, the specific force measured by the accelerometer expressed in the navigation frame and the corresponding measurement error, ωien and δωien are, respectively, the angular rate of earth rotation expressed in navigation frame and the corresponding error, ωenn and δωenn are, respectively, the angular rate of earth frame with respect to earth frame expressed in navigation frame and the corresponding error, vn=vNvUvET denotes the velocity relative to earth with vN, vU, and vE being, respectively, the velocity in north, up, and east direction, and δvn=δvNδvUδvET is the corresponding error of vn.The position error model is written as [29](3)δL˙=1RM+hδvN−vNRM+h2δh,(4)δλ˙=secLRN+hδvE+vEsecLtanLRN+hδL−vEsecLRN+h2δh,(5)δh˙=δvU,where pn=LλhT denotes the position with L, λ, and h being, respectively, the latitude, longitude, and height and δpn=δLδλδhT is the corresponding error of pn.The inertial sensor error model is formulated as(6)ε˙b=−1τgεb+wg,(7)∇˙b=−1τa∇b+wa,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; wg and wa 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:(8)x=ϕδvnδpnεb∇bT.Based on the above defined state vector, the system dynamic can be expressed in accordance with the following continuous-time state-space model:(9)x˙t=Atxt+Btwt,where At and wt 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); wt 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:(10)zt=VGPSN−VSinsNVGPSU−VSinsUVGPSE−VSinsELGPS−LSinshGPS−hSinsλGPS−λSins=Hxt+vt,where the subscripts GPS and Sins are, respectively, the velocity and geographical position obtained from GPS and SINS. H=06×3,I6×6,06×12 is the observation matrix with I being the identity matrix and 0 being the zero matrix. vt is the measurement noise.Defining the state vector at time steptk as xk=xtk, the process noise at time step tk as wk=wtk, the observation vector at time step tk as zk=ztk, and the observation noise at time step tk as vk=vtk, the discrete-time state linear state-space model and observation model for equations (9) and (10) can be formulated as follows:(11)xk=Φkxk−1+Gkwk−1,zk=Hkxk+vk,where Ts is the sampling interval, k is the discrete-time index, and tk=kTs; the state transition matrix Φk=Ι15×15+TsAt, the observation matrix Hk=H. Both the process noise wk−1 and measurement noise vk obey the Gaussian distribution with the process noise covariance Qk and measurement noise covariance Rk.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 epochk 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

(12)xk|k−1=Φkxk−1,Pk|k−1=ΦkPk−1ΦkT+Qk,where xk|k−1 and Pk|k−1 are, respectively, the predicted state vector and the corresponding predicted error covariance matrix.


### 2.2.2. Measurement Update

(13)Kk=Pk|k−1HkTHkPk|k−1HkT+Rk−1,xk|k=xk|k−1+Kkzk−Hkxk|k−1,Pk|k=Ι−KkHkPk|k−1.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.2.1. Time UpdateX

(12)xk|k−1=Φkxk−1,Pk|k−1=ΦkPk−1ΦkT+Qk,where xk|k−1 and Pk|k−1 are, respectively, the predicted state vector and the corresponding predicted error covariance matrix.


## 2.2.2. Measurement Update

(13)Kk=Pk|k−1HkTHkPk|k−1HkT+Rk−1,xk|k=xk|k−1+Kkzk−Hkxk|k−1,Pk|k=Ι−KkHkPk|k−1.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δA and platform misalignment angle ϕ as follows.Denote byCbn the true attitude matrix between the true navigation frame (n-frame) and body frame (b-frame) and by Cbc 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 Cbn and Cbc:(14)Cbn=CcnCbc,where Ccn represents the derivation between n-frame and c-frame caused by the platform misalignment angle ϕ=ϕNϕUϕET. Assuming that ϕ is small, the matrix Ccn can be approximated by(15)Ccn≈Ι+ϕ=−1ϕE−ϕUϕE1ϕNϕU−ϕN1.Meanwhile, we can obtain the attitude transformation matricesCbc and Cbn as follows [30]:(16)Cbc=cosθccosψc−cosγccosψcsinθc+sinγcsinψcsinθcsinγccosψc+cosγcsinψcsinθccosθccosγc−sinγccosθc−sinψccosθccosγcsinψcsinθc+sinγccosψc−sinγcsinψcsinθc+cosγccosψc,(17)Cbn=cosθncosψn−cosγncosψnsinθn+sinγnsinψnsinθnsinγncosψn+cosγnsinψnsinθncosθncosγn−sinγncosθn−sinψncosθncosγnsinψnsinθn+sinγncosψn−sinγnsinψnsinθn+cosγncosψn,where Ac=ψcθcγcT and An=ψnθnγnT, respectively, denote the computed attitude and true attitude resolving in n-frame. By defining the attitude error δA=δψδθδγT, we can obtain(18)δψ=ψc−ψn,δθ=θc−θn,δγ=γc−γn.Taking the headingψfor example and calculating the sine and cosine values at both sides of equation yields(19)sinψc=sinψn+δψ=sinψncosδψ+sinψnsinδψ,cosψc=cosψn+δψ=cosψncosδψ−sinψnsinδψ.Considering that the attitude error is small and substitutingsinδψ≈δψ and cosδψ≈1 into (19) yield(20)sinψc=sinψn+δψ=sinψn+cosψnδψ,cosψc=cosψn+δψ=cosψn−sinψnδψ.Combining (14)∼(20), we obtain the following relationship between the attitude error angle δA and platform misalignment angle ϕ:(21)δψδθδγ=1cosθ0cosγ−sinγ0sinγcosθcosγcosθcosθ−sinθcosγsinθsinγϕNϕUϕE,where M=1/cosθ0cosγ−sinγ0sinγcosθcosγcosθcosθ−sinθcosγsinθsinγ is 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,ve=vxvyvzT and pe=pxpypzT, the geographical position pGPSn=LλHT can be given as follows:(22)λGPS=arctan2py,px,LGPS=atanpz+e2RNsinLpx2+py2,HGPS=px2+py2cosϕ−RN,where RN=a/1−e2sin2ϕ is the prime radius of ellipsoid, e=0.08181919 is the eccentricity of ellipsoid, and a=6378137 is the semimajor axis of ellipsoid.The velocityvGPSn=VGPSNVGPSEVGPSUT in local navigation frame can be given as follows:(23)vGPSn=Renve,where Ren=−sinL∗cosλ−sinL∗sinλcosL−sinλcosλ0−cosL∗cosλ−cosL∗sinλ−sinL is the attitude transformation matrix from ECEF frame to local navigation frame. To reduce the computational complexity, we define X˜=VGPSNVGPSET, Z=vxvyT and obtain the relationship between X˜ and Z as follows:(24)Z=A−1X˜,where A=−sinL∗cosλ−sinL∗sinλ−sinλcosλ is the submatrix of Ren. Based on the velocity information obtained from equation (23), the heading information from GPS can be calculated as follows:(25)HeadingGPS=atan2VGPSN,VGPSE.Combining the navigation information (heading, velocity, and geographical position) calculated by the SINS [30], we can construct the completed integrated observation model as follows:(26)zt=HeadingGPS−HeadingSINSvGPSn−vSINSnpGPSn−pSINSn=H¯xt+vt,where H¯=M3×303×12I6×606×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].


### 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 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. 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 X optimally. The main idea of the multivariate constrained total least square algorithm is to estimate the state vector X by minimizing the sum of EA and EZ, which are, respectively, the error matrix of the coefficient matrix A−1 and the observation vector Z. Therefore, the linearization equation (24) can be accurately expressed as(27)A−1+EAX=Z+EZ.Equation (27) can be further rewritten in the following matrix form:(28)A−1+EA|Z+EZX−Ιk=0,where Ιk is the k×k unit matrix. To reduce the rank of the augmented matrix A−1+EA|Z+EZ, we employ the singular value decomposition (SVD) as follows:(29)C=A−1+EA|Z+EZ=UA|UZΣA000VAAVAZVZAVZZT,where UA|UZ and VAAVAZVZAVZZT are, respectively, the orthogonal matrices, which are composed of the eigenvectors of matrices C⋅CT and C⋅CT. ΣA000 is the singular value of matrix A−1+EA|Z+EZ. Similarly, we can obtain(30)A−1|Z=UA|UZΣA00ΣZVAAVAZVZAVZZT.Multiplying both sides of equation (30) by matrix VAAVAZVZAVZZ yields(31)A−1|ZVAAVAZVZAVZZ=UA|UZΣA00ΣZVAAVAZVZAVZZTVAAVAZVZAVZZ,that is,(32)A−1|ZVAZVZZ=UZΣZ.We have the following relationship:(33)EA|EZ=A−1|Z+EA|EZ−A−1|Z=A−1+EA|Z+EZ−A−1|Z.Substituting (29) and (30) into (33) yields(34)EA|EZ=−UA|UZ000ΣZVAZVAZVZAVZZT=−UZΣZVAZVZZT=−A−1|ZVAZVZZVAZVZZT.Thus, combining (30) and (34), we can obtain(35)A−1+EA|Z+EZ=A−1|Z+EA|EZ=A−1|Z−A−1|ZVAZVZZVAZVZZT.Multiplying both sides of equation (35) by VAZVZZT yields(36)A−1+EA|Z+EZVAZVZZ=A−1|ZVAZVZZ−A−1|ZVAZVZZVAZVZZTVAZVZZ,(37)A−1+EA|Z+EZVAZVZZ=A−1|ZVAZVZZ−A−1|ZVAZVZZ,(38)A−1+EA|Z+EZVAZVZZ=0.Then, by combining (38) and (28), we can obtain the following result:(39)X^=−VAZVZZ−1.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 matrixA−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.


## 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δA and platform misalignment angle ϕ as follows.Denote byCbn the true attitude matrix between the true navigation frame (n-frame) and body frame (b-frame) and by Cbc 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 Cbn and Cbc:(14)Cbn=CcnCbc,where Ccn represents the derivation between n-frame and c-frame caused by the platform misalignment angle ϕ=ϕNϕUϕET. Assuming that ϕ is small, the matrix Ccn can be approximated by(15)Ccn≈Ι+ϕ=−1ϕE−ϕUϕE1ϕNϕU−ϕN1.Meanwhile, we can obtain the attitude transformation matricesCbc and Cbn as follows [30]:(16)Cbc=cosθccosψc−cosγccosψcsinθc+sinγcsinψcsinθcsinγccosψc+cosγcsinψcsinθccosθccosγc−sinγccosθc−sinψccosθccosγcsinψcsinθc+sinγccosψc−sinγcsinψcsinθc+cosγccosψc,(17)Cbn=cosθncosψn−cosγncosψnsinθn+sinγnsinψnsinθnsinγncosψn+cosγnsinψnsinθncosθncosγn−sinγncosθn−sinψncosθncosγnsinψnsinθn+sinγncosψn−sinγnsinψnsinθn+cosγncosψn,where Ac=ψcθcγcT and An=ψnθnγnT, respectively, denote the computed attitude and true attitude resolving in n-frame. By defining the attitude error δA=δψδθδγT, we can obtain(18)δψ=ψc−ψn,δθ=θc−θn,δγ=γc−γn.Taking the headingψfor example and calculating the sine and cosine values at both sides of equation yields(19)sinψc=sinψn+δψ=sinψncosδψ+sinψnsinδψ,cosψc=cosψn+δψ=cosψncosδψ−sinψnsinδψ.Considering that the attitude error is small and substitutingsinδψ≈δψ and cosδψ≈1 into (19) yield(20)sinψc=sinψn+δψ=sinψn+cosψnδψ,cosψc=cosψn+δψ=cosψn−sinψnδψ.Combining (14)∼(20), we obtain the following relationship between the attitude error angle δA and platform misalignment angle ϕ:(21)δψδθδγ=1cosθ0cosγ−sinγ0sinγcosθcosγcosθcosθ−sinθcosγsinθsinγϕNϕUϕE,where M=1/cosθ0cosγ−sinγ0sinγcosθcosγcosθcosθ−sinθcosγsinθsinγ is 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,ve=vxvyvzT and pe=pxpypzT, the geographical position pGPSn=LλHT can be given as follows:(22)λGPS=arctan2py,px,LGPS=atanpz+e2RNsinLpx2+py2,HGPS=px2+py2cosϕ−RN,where RN=a/1−e2sin2ϕ is the prime radius of ellipsoid, e=0.08181919 is the eccentricity of ellipsoid, and a=6378137 is the semimajor axis of ellipsoid.The velocityvGPSn=VGPSNVGPSEVGPSUT in local navigation frame can be given as follows:(23)vGPSn=Renve,where Ren=−sinL∗cosλ−sinL∗sinλcosL−sinλcosλ0−cosL∗cosλ−cosL∗sinλ−sinL is the attitude transformation matrix from ECEF frame to local navigation frame. To reduce the computational complexity, we define X˜=VGPSNVGPSET, Z=vxvyT and obtain the relationship between X˜ and Z as follows:(24)Z=A−1X˜,where A=−sinL∗cosλ−sinL∗sinλ−sinλcosλ is the submatrix of Ren. Based on the velocity information obtained from equation (23), the heading information from GPS can be calculated as follows:(25)HeadingGPS=atan2VGPSN,VGPSE.Combining the navigation information (heading, velocity, and geographical position) calculated by the SINS [30], we can construct the completed integrated observation model as follows:(26)zt=HeadingGPS−HeadingSINSvGPSn−vSINSnpGPSn−pSINSn=H¯xt+vt,where H¯=M3×303×12I6×606×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].


## 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 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. 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 X optimally. The main idea of the multivariate constrained total least square algorithm is to estimate the state vector X by minimizing the sum of EA and EZ, which are, respectively, the error matrix of the coefficient matrix A−1 and the observation vector Z. Therefore, the linearization equation (24) can be accurately expressed as(27)A−1+EAX=Z+EZ.Equation (27) can be further rewritten in the following matrix form:(28)A−1+EA|Z+EZX−Ιk=0,where Ιk is the k×k unit matrix. To reduce the rank of the augmented matrix A−1+EA|Z+EZ, we employ the singular value decomposition (SVD) as follows:(29)C=A−1+EA|Z+EZ=UA|UZΣA000VAAVAZVZAVZZT,where UA|UZ and VAAVAZVZAVZZT are, respectively, the orthogonal matrices, which are composed of the eigenvectors of matrices C⋅CT and C⋅CT. ΣA000 is the singular value of matrix A−1+EA|Z+EZ. Similarly, we can obtain(30)A−1|Z=UA|UZΣA00ΣZVAAVAZVZAVZZT.Multiplying both sides of equation (30) by matrix VAAVAZVZAVZZ yields(31)A−1|ZVAAVAZVZAVZZ=UA|UZΣA00ΣZVAAVAZVZAVZZTVAAVAZVZAVZZ,that is,(32)A−1|ZVAZVZZ=UZΣZ.We have the following relationship:(33)EA|EZ=A−1|Z+EA|EZ−A−1|Z=A−1+EA|Z+EZ−A−1|Z.Substituting (29) and (30) into (33) yields(34)EA|EZ=−UA|UZ000ΣZVAZVAZVZAVZZT=−UZΣZVAZVZZT=−A−1|ZVAZVZZVAZVZZT.Thus, combining (30) and (34), we can obtain(35)A−1+EA|Z+EZ=A−1|Z+EA|EZ=A−1|Z−A−1|ZVAZVZZVAZVZZT.Multiplying both sides of equation (35) by VAZVZZT yields(36)A−1+EA|Z+EZVAZVZZ=A−1|ZVAZVZZ−A−1|ZVAZVZZVAZVZZTVAZVZZ,(37)A−1+EA|Z+EZVAZVZZ=A−1|ZVAZVZZ−A−1|ZVAZVZZ,(38)A−1+EA|Z+EZVAZVZZ=0.Then, by combining (38) and (28), we can obtain the following result:(39)X^=−VAZVZZ−1.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 matrixA−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.


## 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 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 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/s2/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:(40)RMSE=1M∑s=1Mxks−x^ks2,where xks and x^ks 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.Figure 1
The true trajectory of the AHRS.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 Figures2–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.Figure 2
The estimated position (a) and position error (b) via the traditional loosely coupled integrated and proposed completed integration.
(a)(b)Figure 3
The estimated velocity (a) and velocity error (b) via traditional loosely coupled integrated and proposed completed integration.
(a)(b)Figure 4
The estimated attitude (a) and attitude error (b) via the traditional loosely coupled integrated and proposed completed integration.
(a)(b)Table 1
RMSEs of attitude, velocity, and position via the traditional and proposed methods.
MethodsPosNPosEHeightVelNVelEVelDHeadingPitchRoll(m)(m/s)(deg)Traditional AHRS0.4290.3640.5460.0310.0310.02213.00.3320.513Proposed AHRS0.4130.3160.5400.0310.0310.0220.80.3400.341The GPS velocity measurement information in ECEF frame with the outliers is shown in Figure5. 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.Figure 5
The GPS velocity measurement information in ECEF frame.
(a)(b)(c)Figure 6
The GPS velocity information in NED navigation frame obtained via least square and multivariate constrained total least square.
(a)(b)Figure 7
The heading information obtained via the traditional and proposed methods.
(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 Figure8.Figure 8
The true experiment trajectory.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 asX0|0 = 0 and P0|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 Qk = diag([0.24 deg/sqrt(h), 0.24 deg/sqrt(h), 0.24 deg/sqrt(h), 0.3 m/s2/sqrt(Hz), 0.3 m/s2/sqrt(Hz), 0.3 m/s2/sqrt(Hz))2, and Rk = 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 Figures9–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.Figure 9
The position estimation from different methods.
(a)(b)(c)(d)(e)(f)Figure 10
The velocity estimation from different methods.
(a)(b)(c)(d)(e)(f)Figure 11
The attitude estimation from different methods.
(a)(b)(c)(d)Figure 12
The heading estimation from different methods.
(a)(b)(c)Table 2
RMSEs of attitude, velocity, and position via the traditional SINS/GNSS loosely coupled integrated and the proposed completed integration framework.
MethodsPosNPosEHeightVelNVelEVelDHeadingPitchRoll(m)(m/s)(deg)Traditional AHRS1.2291.0842.8360.0650.0410.1521.390.3290.233Proposed AHRS1.0631.0612.6520.0520.0400.1220.920.3040.212


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



---

*Source: 4260162-2021-04-14.xml*