# An Improved Attitude Compensation Algorithm for SINS/GNS Integrated Navigation System

Authors: Feng Xie; Minzhou Dong
Journal: Journal of Sensors
 (2021)

---

## Abstract

In order to suppress the error caused by the drift of the gyroscope and further improve accuracy of the navigation system, combined with the method of measuring attitude by using the three-axis components of geomagnetic, a new scheme, consisting of Strapdown Inertial Navigation System (SINS)/Geomagnetic Navigation System (GNS), is designed for autonomous integrated navigation systems. The principle of this SINS/GNS integrated navigation system is explored, and the corresponding mathematical model is established. Furthermore, a Marginalized Particle Filter (MPF) is designed for this autonomous integrated navigation system. The simulation experiments are conducted, and the results show that the improved SINS/GNS autonomous integrated navigation system possesses strong robustness and high reliability, thus providing a new reference solution for autonomous navigation technology.


---

## Body

## 1. Introduction

Since the middle of the last century, the inertial navigation system has been widely used in the navigation of various sports carriers due to its autonomy, concealment, and strong anti-interference ability [1]. However, the inertial navigation system also has shortcomings that cannot be ignored: the error diverges with time, so how to suppress the cumulative error of the inertial navigation system has become a current research hotspot. There are two main ways to improve the performance of inertial systems; one is to improve the measurement accuracy of inertial components and to develop new types of inertial devices, such as laser gyroscopes and quantum gyroscopes; the other is to use external information sources to calibrate the navigation system based on information fusion theory. Commonly used auxiliary information sources include satellite, astronomy, radio, geomagnetic field, and gravitational field [2].The geomagnetic field is an inherent resource of the earth, which provides a natural reference coordinate system for aviation, aerospace, and navigation. The magnetic navigation technology, using the spatial distribution of the earth’s magnetic field, is simple, efficient, reliable, and has strong anti-interference performance [3], so it is often used with inertial navigation together to form a high-precision autonomous navigation system. Scholars have made extensive research on the application of geomagnetism in navigation. For example, the Cao’s team of NUC early proposed the use of geomagnetic field vector and gyroscope to calculate the attitude of the missile body [4, 5]. Taking the geomagnetic entropy and the geomagnetic difference entropy as the reference criteria for the selection of the geomagnetic auxiliary navigation area, combined with the ICCP algorithm can effectively improve the accuracy of the auxiliary navigation [6]. The geomagnetic matching navigation algorithm based on affine parameter estimation, while ensuring the matching accuracy, effectively improves the real-time performance of the algorithm [7]. Applying intelligent algorithms such as quantum particle swarm algorithm and genetic algorithm to geomagnetic matching navigation can effectively improve the convergence speed and accuracy of geomagnetic matching [8–12].In this paper, the geomagnetic field component is introduced as observation information on the basis of position correction, and the observation equation directly related to the attitude matrix is established. The principle, scheme, and mathematical model of this autonomous integrated navigation system are established. In view of the nonlinear characteristics of the model, a high-precision nonlinear filtering algorithm for the autonomous navigation system is proposed. Subsequently, all of the models and algorithms are verified by experiments.


## 2. Materials and Methods

### 2.1. Scheme of SINS/GNS Integrated Navigation System

The gyroscopes and accelerometers used in Strapdown Inertial Navigation Systems are not ideal measurement components. There are errors in both the components themselves and the installation process. Among them, zero offset and scale factor errors are the main error sources of inertial measurement components, and there is also axis cross coupling error and random noise interference [13]; therefore, usually, there is a divergence when working for a long time.During the actual navigation, there are angular deviations between the coordinate system of the mathematical platform and the actual navigation coordinate system, which called error angle of mathematical platformϕ, and satisfies the following equation [14]:
(1)ϕ̇n=δωien+δωenn−ωien+ωenn×ϕn+Cbnεb,where εb is the zero offset of the gyroscope and ωien and ωenn are the rotation rate and position rate of the earth, respectively. Cbn is the attitude transition matrixes of the body coordinate system to the navigation coordinate system. Based on the specific force equation of the accelerometer
(2)V̇n=Cbnfb−2ωien+ωenn×Vn+gn.The velocity error equation of the carrier can be defined as
(3)δV̇n=δfn−2δωien+δωenn×Vn−2ωien+ωenn×δVn,where δVis the error of velocity in the navigation coordinate system and f is the output of the accelerometer. Correspondingly, the traditional position error equation of SINS is shown as
(4)δL̇=δVNRM+h,δḢ=δVU,δλ̇=δVE+VEsecLtanLδLRN+h,where RM and RN represent the radius of curvature of the ellipsoid model of earth in different direct, respectively.Location information of GNS is often used to assist Strapdown Inertial Navigation Systems, which can suppress the accumulation of errors of sensors. We can get two location informationPm by GNS and Psins by SINS, respectively. Consider the error in the positioning process [15], the matching position can be expressed as
(5)Psins=P+δPsins,Pm=P+δPm,where P is the real position and δPm and δPsins are the position error of GNS and SINS, respectively.Take the difference of the position between SINS and GNS output as the observation, and the position observation equation is obtained as(6)ZP=Psins−Pm=HPX+VP,where VP=δPsins−δPm is the position observation error and HP is the position observation matrix.The geomagnetic field has the following characteristics: the vector of each location is unique. Suppose that the positionP and the total geomagnetism G satisfy the relationship: G=GP+V, so when the position Psins of the inertial navigation system is known, the total amount of geomagnetic field can be obtained: Gsins=GPsins+γsins, where γsins is error of geomagnetic matching algorithm.Take the difference of the geomagnetism between SINS and magnetometer output as the observation, the new observation equation can be obtained as(7)Zm=GP+δPsins−GP+Vm1,where Vm1=γsins−γm is the observation error.Therefore, according to the above method, the accuracy of the inertial navigation system, especially position, can be effectively improved. Based on formulas (6) and (7), the position information is directly involved in the observation, but the attitude information is lacking. In order to make full use of the geomagnetic field component information and further improve the attitude accuracy of the integrated navigation system, this paper takes the three-axis geomagnetic field component as a new observation and proposes an improved integrated navigation algorithm. Figure 1 shows the scheme of the improved integrated navigation system.Figure 1
Scheme of improved integrated navigation system.In the improved navigation system, the geomagnetic field componentsHx,Hy,Hz are used as the new observation information for the estimation of attitude and velocity; the geomagnetism G at a certain point has the following relationship with the geomagnetic components:
(8)G=Hx2+Hy2+Hz2.Given the nonlinear characteristic of the measurement equation, this paper introduces the particle filter to estimate the position information.


### 2.2. State Equation of Integrated Navigation System

The improved integrated navigation system involves multiple sensors, including a three-axis magnetometer, a three-axis gyroscope, and a three-axis accelerometer fixedly. Each axis of the sensor has its own error model, and this article defines the error model of all sensors as a random constant plus white noise:V=m+η. Suppose the random constant error vector of the magnetometer is mb, the paper selects the following parameters as state variables: attitude angle error φ, horizontal velocity error δV, zero offset of accelerometer εb, magnetometer constant error mb, gyroscope drift ∇b, and horizontal position error δP=δLat,δLon.Using the E-N-U (East-North-Up) geographic coordinate system as the base coordinate system for navigation, the system state vectorX of SINS/GNS autonomous integrated navigation system can be defined as
(9)X=φδVεb∇bmbδPT.Based on formula (9) and the error model of SINS, the kinematic model of the improved SINS/GNS autonomous integrated navigation system is described as
(10)X=FtXt+NtWt,Xk+1=ΦkXk+Γkwk,where F is the system state matrix and N is the systematic error matrix. The discretized state matrix Φk and error matrix Γk satisfies equation can be expressed as equation (11), where T is the cycle of the navigation system.
(11)Φk≈I+FkΔt+Τ2Fk22,Γ2≈ΤNk.


### 2.3. Observation Equation of Integrated Navigation System

The selection of the measurement vector of the integrated navigation system is directly related to the measurement accuracy of the entire system. Based on the known matching results, the position and attitude observation equations are established.(1) Establishment of Position Observation Equation. Based on equations (6) and (9), the position observation matrix HP can be obtained as
(12)HP=01×13I2×2.Perform a first-order Taylor expansion of equation (7):
(13)Zm=∂G∂LatδLat+∂G∂LonδLon+μ+Vm1,where ∂G/∂Lat and ∂G/∂Lon are the slope values of geomagnetic field in the longitude and latitude directions, respectively. In the actual calculation, the slope value of each direction is calculated based on the data in the geomagnetic database and the measured magnetic field at the position where the carrier has flown. First of all, it is necessary to use plane fitting method to fit the discrete data in the database. In order to improve the accuracy of slope, the least square method is usually used to fit the magnetic field data in a certain range [16]. For the area as shown in Figure 2, let the half length of the fitting region be 2.5δx and 2.5δy, where δx and δy are the root mean square of position error in different directions, respectively.Figure 2
Schematic diagram of full plane fitting.Then, the slope of a pointi,j in the area can be expressed as
(14)∂G∂Lon=122M+112+22+⋯+N2d∑i,jjGti,j,∂G∂Lat=122N+112+22+⋯+M2d∑i,jiGti,j,wherei=m−M,⋯,m+M, j=n−N,⋯,n+N, and d is the data grid spacing on the geomagnetic map.LetVm=Vm1+μ, the observation equation can be obtained as
(15)Zm=∂G∂Lat∂G∂LonδLatδLon+Vm,where μ is the error caused by the first-order Taylor expansion. Then, the linearized observation equation can be expressed as Zm=HmX+Vm, where:
(16)Hm=01×14∂G∂Lat∂G∂Lon.(2) Establishment of Attitude Measurement Equation. Common SINS/GNS does not establish a measurement equation about the attitude. This article uses the three-axis geomagnetic field component as the observation information according to the vector characteristics of the geomagnetic field. Since the geomagnetic field component is measured in the carrier coordinate system, if the local geomagnetic field component in the navigation coordinate system is Hn, the following relational equation can be obtained
(17)Hn=C^bnHm+ζ.where C^bn is the ideal attitude matrix and ζ is the measurement error matrix of the magnetometer and satisfies the equation ζ=mb+ηm. Suppose ΔC is the attitude error matrix of the inertial system, equation (18) can be established.
(18)C^bn=ΔC⋅Cbn,when the attitude error angle is very small [17], the following equation holds: ΔC=I+φ×, where φ× is the cross-multiplying antisymmetric matrix of φ, so equation (17) can be expressed as the following:
(19)Hn=I+φ×CbnH+ζ.According to the characteristics of the cross-multiplying antisymmetric matrix:φ×⋅C=−C×⋅φ, equation (18) can be organized into the following form:
(20)CbnHm−Hn=CbnHm×⋅φ−Cbnmb+V1,V1=−I+φ×⋅Cbn⋅ηm−φ×⋅Cbn⋅mb,where V1 is the corresponding noise and Hn satisfies the following equation: Hn=Hsins+ξ,ξ is the error of geomagnetic matching system, so equation (20) can be organized into
(21)CbnHm−Hsins=CbnHm×⋅φ−Cbnmb+VH,where VH=V1+ξ, so the observation equation can be obtained as
(22)ZH=CbnHm−Hsins=HHX+VH.LetHC=CbnHm=H1,H2,H3, the cross-multiplying antisymmetric matrix H~C of HC can be expressed as
(23)H~C=0H3−H2−H30H1H2−H10.Accordingly, the observation matrixHH can be expressed as
(24)HH=H~C03×8−Cbn03×2.Based on equations (11), (15), and (23), the observation equation of the integrated navigation system can be obtained: Z=HX+V, where the observation matrix H satisfies
(25)H=HHHPHm′.


### 2.4. Filter Design

In this paper, two filters are designed. According to equations (12), (16), and (24), all observation equations are linear, so the Kalman filter can be designed directly to complete data fusion. However, nonlinear processing was performed during the derivation of formula (13), which produces more error, so an improved particle filter algorithm (Marginalized Particle Filter (MPF)) is proposed in this paper, which is the combination of Kalman filter and particle filter, and the method not only avoids linearization errors but also ensures the real-time performance of the algorithm. The linear part Xl of the model is estimated with a Kalman filter, and the nonlinear part Xn is estimated with a particle filter, which reduces the dimensions of the PF. The state vector of the system can be divided into
(26)X=XlXn=φδVεb∇bmbTlatlonT.Correspondingly, the discretized state matrix of integrated navigation system can be written as follows [18].
(27)Φk=Φl,klΦl,knΦn,klΦn,kn.According toζ=mb+ηm, we can know that the error model of the magnetometer is in the carrier coordination system, now suppose that the total amount of magnetic field generated by the carrier satisfies the model VG=mG+ρ, mG is the constant error, and ρ is the white noise. Add the constant error mG as one of the state variables to improve the stability of system. Then, the new observation model can be established as
(28)Z=01×3h1Xn+HH−1Xl+V,where:
(29)Z=ZHTZmT,Xl=φδVεb∇bmbmGT,h1Xn=∂G∂LatδLat+∂G∂LonδLon.Then, the general form of the observation equation of MPF is expressed as(30)Zk=hkXkn+HklXkl+Vk,where:
(31)hkXkn=01×3h1Xn,Hkl=HH−1,where Vk is the comprehensive observation error and the specific steps of the design of the MPF are as follows [19] (Algorithm 1).Algorithm 1: Implementation process of MPF.
a) Initialization:X0l, P0l, X0n,i∼P0X0ni=0,1,⋯N and N is the number of particles.b) Measurement update of Particle filter:Calculate weightsω~ki, resample Nk times, and calculate the minimum variance estimate X^kn.X^kn=∑i=1Nkω~kiXkn,ic) Measurement update of Kalman filter:X^k+1l=X^kl+KkZk−hX^kn−HklX^klKk=Pk/k−1HklTHklPk/k−1HklT+RkTPk=Pk/k−1−KkHklPk/k−1d) Time update of Particle filter:X^k+1n,i∼pXk+1n∣X0:kn,i,Z0:ke) Time update of Kalman filter:X^k+1/kl=Φl,klX^kl+Φl,knX^kn+KkX^k+1n−Φn,klX^kl−Φn,knX^knPk+1/k=Φl,klPkΦl,klT+ΓklQklΓklT−KkSkKkTKk=Fl.klPkFn,klTSk−1Sk=Fn,klPkFn,klT


### 2.5. Simulation

In the above section, we have established a new observation model for the integrated navigation system, designed the corresponding filter, and listed the specific implementation. Next, we will perform numerical simulation based on the above model and analyze the characteristics of the proposed algorithm. Table1 is the sensor error model parameters. The errors can be considered to be composed of constant deviation and white noise in this paper.Table 1
Sensor error model parameters.
Sensor typeConstant deviationWhite noiseUnitGyroscope0.020.01deg/hAccelerometer10-410-5gMagnetometer105mGIn the experiment, the simulation data are from a flight of a spacecraft in Earth-Centered Earth-Fixed coordinate system (ECEF). The initial latitude is 45.7508°, the initial longitude is 122.6715°, and the simulation time is 2000 s. The flight trajectory is shown in Figure3.Figure 3
Flight trajectory.In the simulation process, the initial horizontal position error is 5 m, the initial horizontal velocity error is 1 m/s, the initial attitude angles are 0.1°, 0.1°, and 0.1°. The northbound position error of GNS is 10 m, and the eastbound position error is 20 m. Perform simulation experiments based on the above parameters compare the navigation effects between the traditional model and the improved mathematical model and use MATLAB as the simulation tool to obtain the following simulation results.


## 2.1. Scheme of SINS/GNS Integrated Navigation System

The gyroscopes and accelerometers used in Strapdown Inertial Navigation Systems are not ideal measurement components. There are errors in both the components themselves and the installation process. Among them, zero offset and scale factor errors are the main error sources of inertial measurement components, and there is also axis cross coupling error and random noise interference [13]; therefore, usually, there is a divergence when working for a long time.During the actual navigation, there are angular deviations between the coordinate system of the mathematical platform and the actual navigation coordinate system, which called error angle of mathematical platformϕ, and satisfies the following equation [14]:
(1)ϕ̇n=δωien+δωenn−ωien+ωenn×ϕn+Cbnεb,where εb is the zero offset of the gyroscope and ωien and ωenn are the rotation rate and position rate of the earth, respectively. Cbn is the attitude transition matrixes of the body coordinate system to the navigation coordinate system. Based on the specific force equation of the accelerometer
(2)V̇n=Cbnfb−2ωien+ωenn×Vn+gn.The velocity error equation of the carrier can be defined as
(3)δV̇n=δfn−2δωien+δωenn×Vn−2ωien+ωenn×δVn,where δVis the error of velocity in the navigation coordinate system and f is the output of the accelerometer. Correspondingly, the traditional position error equation of SINS is shown as
(4)δL̇=δVNRM+h,δḢ=δVU,δλ̇=δVE+VEsecLtanLδLRN+h,where RM and RN represent the radius of curvature of the ellipsoid model of earth in different direct, respectively.Location information of GNS is often used to assist Strapdown Inertial Navigation Systems, which can suppress the accumulation of errors of sensors. We can get two location informationPm by GNS and Psins by SINS, respectively. Consider the error in the positioning process [15], the matching position can be expressed as
(5)Psins=P+δPsins,Pm=P+δPm,where P is the real position and δPm and δPsins are the position error of GNS and SINS, respectively.Take the difference of the position between SINS and GNS output as the observation, and the position observation equation is obtained as(6)ZP=Psins−Pm=HPX+VP,where VP=δPsins−δPm is the position observation error and HP is the position observation matrix.The geomagnetic field has the following characteristics: the vector of each location is unique. Suppose that the positionP and the total geomagnetism G satisfy the relationship: G=GP+V, so when the position Psins of the inertial navigation system is known, the total amount of geomagnetic field can be obtained: Gsins=GPsins+γsins, where γsins is error of geomagnetic matching algorithm.Take the difference of the geomagnetism between SINS and magnetometer output as the observation, the new observation equation can be obtained as(7)Zm=GP+δPsins−GP+Vm1,where Vm1=γsins−γm is the observation error.Therefore, according to the above method, the accuracy of the inertial navigation system, especially position, can be effectively improved. Based on formulas (6) and (7), the position information is directly involved in the observation, but the attitude information is lacking. In order to make full use of the geomagnetic field component information and further improve the attitude accuracy of the integrated navigation system, this paper takes the three-axis geomagnetic field component as a new observation and proposes an improved integrated navigation algorithm. Figure 1 shows the scheme of the improved integrated navigation system.Figure 1
Scheme of improved integrated navigation system.In the improved navigation system, the geomagnetic field componentsHx,Hy,Hz are used as the new observation information for the estimation of attitude and velocity; the geomagnetism G at a certain point has the following relationship with the geomagnetic components:
(8)G=Hx2+Hy2+Hz2.Given the nonlinear characteristic of the measurement equation, this paper introduces the particle filter to estimate the position information.


## 2.2. State Equation of Integrated Navigation System

The improved integrated navigation system involves multiple sensors, including a three-axis magnetometer, a three-axis gyroscope, and a three-axis accelerometer fixedly. Each axis of the sensor has its own error model, and this article defines the error model of all sensors as a random constant plus white noise:V=m+η. Suppose the random constant error vector of the magnetometer is mb, the paper selects the following parameters as state variables: attitude angle error φ, horizontal velocity error δV, zero offset of accelerometer εb, magnetometer constant error mb, gyroscope drift ∇b, and horizontal position error δP=δLat,δLon.Using the E-N-U (East-North-Up) geographic coordinate system as the base coordinate system for navigation, the system state vectorX of SINS/GNS autonomous integrated navigation system can be defined as
(9)X=φδVεb∇bmbδPT.Based on formula (9) and the error model of SINS, the kinematic model of the improved SINS/GNS autonomous integrated navigation system is described as
(10)X=FtXt+NtWt,Xk+1=ΦkXk+Γkwk,where F is the system state matrix and N is the systematic error matrix. The discretized state matrix Φk and error matrix Γk satisfies equation can be expressed as equation (11), where T is the cycle of the navigation system.
(11)Φk≈I+FkΔt+Τ2Fk22,Γ2≈ΤNk.


## 2.3. Observation Equation of Integrated Navigation System

The selection of the measurement vector of the integrated navigation system is directly related to the measurement accuracy of the entire system. Based on the known matching results, the position and attitude observation equations are established.(1) Establishment of Position Observation Equation. Based on equations (6) and (9), the position observation matrix HP can be obtained as
(12)HP=01×13I2×2.Perform a first-order Taylor expansion of equation (7):
(13)Zm=∂G∂LatδLat+∂G∂LonδLon+μ+Vm1,where ∂G/∂Lat and ∂G/∂Lon are the slope values of geomagnetic field in the longitude and latitude directions, respectively. In the actual calculation, the slope value of each direction is calculated based on the data in the geomagnetic database and the measured magnetic field at the position where the carrier has flown. First of all, it is necessary to use plane fitting method to fit the discrete data in the database. In order to improve the accuracy of slope, the least square method is usually used to fit the magnetic field data in a certain range [16]. For the area as shown in Figure 2, let the half length of the fitting region be 2.5δx and 2.5δy, where δx and δy are the root mean square of position error in different directions, respectively.Figure 2
Schematic diagram of full plane fitting.Then, the slope of a pointi,j in the area can be expressed as
(14)∂G∂Lon=122M+112+22+⋯+N2d∑i,jjGti,j,∂G∂Lat=122N+112+22+⋯+M2d∑i,jiGti,j,wherei=m−M,⋯,m+M, j=n−N,⋯,n+N, and d is the data grid spacing on the geomagnetic map.LetVm=Vm1+μ, the observation equation can be obtained as
(15)Zm=∂G∂Lat∂G∂LonδLatδLon+Vm,where μ is the error caused by the first-order Taylor expansion. Then, the linearized observation equation can be expressed as Zm=HmX+Vm, where:
(16)Hm=01×14∂G∂Lat∂G∂Lon.(2) Establishment of Attitude Measurement Equation. Common SINS/GNS does not establish a measurement equation about the attitude. This article uses the three-axis geomagnetic field component as the observation information according to the vector characteristics of the geomagnetic field. Since the geomagnetic field component is measured in the carrier coordinate system, if the local geomagnetic field component in the navigation coordinate system is Hn, the following relational equation can be obtained
(17)Hn=C^bnHm+ζ.where C^bn is the ideal attitude matrix and ζ is the measurement error matrix of the magnetometer and satisfies the equation ζ=mb+ηm. Suppose ΔC is the attitude error matrix of the inertial system, equation (18) can be established.
(18)C^bn=ΔC⋅Cbn,when the attitude error angle is very small [17], the following equation holds: ΔC=I+φ×, where φ× is the cross-multiplying antisymmetric matrix of φ, so equation (17) can be expressed as the following:
(19)Hn=I+φ×CbnH+ζ.According to the characteristics of the cross-multiplying antisymmetric matrix:φ×⋅C=−C×⋅φ, equation (18) can be organized into the following form:
(20)CbnHm−Hn=CbnHm×⋅φ−Cbnmb+V1,V1=−I+φ×⋅Cbn⋅ηm−φ×⋅Cbn⋅mb,where V1 is the corresponding noise and Hn satisfies the following equation: Hn=Hsins+ξ,ξ is the error of geomagnetic matching system, so equation (20) can be organized into
(21)CbnHm−Hsins=CbnHm×⋅φ−Cbnmb+VH,where VH=V1+ξ, so the observation equation can be obtained as
(22)ZH=CbnHm−Hsins=HHX+VH.LetHC=CbnHm=H1,H2,H3, the cross-multiplying antisymmetric matrix H~C of HC can be expressed as
(23)H~C=0H3−H2−H30H1H2−H10.Accordingly, the observation matrixHH can be expressed as
(24)HH=H~C03×8−Cbn03×2.Based on equations (11), (15), and (23), the observation equation of the integrated navigation system can be obtained: Z=HX+V, where the observation matrix H satisfies
(25)H=HHHPHm′.


## 2.4. Filter Design

In this paper, two filters are designed. According to equations (12), (16), and (24), all observation equations are linear, so the Kalman filter can be designed directly to complete data fusion. However, nonlinear processing was performed during the derivation of formula (13), which produces more error, so an improved particle filter algorithm (Marginalized Particle Filter (MPF)) is proposed in this paper, which is the combination of Kalman filter and particle filter, and the method not only avoids linearization errors but also ensures the real-time performance of the algorithm. The linear part Xl of the model is estimated with a Kalman filter, and the nonlinear part Xn is estimated with a particle filter, which reduces the dimensions of the PF. The state vector of the system can be divided into
(26)X=XlXn=φδVεb∇bmbTlatlonT.Correspondingly, the discretized state matrix of integrated navigation system can be written as follows [18].
(27)Φk=Φl,klΦl,knΦn,klΦn,kn.According toζ=mb+ηm, we can know that the error model of the magnetometer is in the carrier coordination system, now suppose that the total amount of magnetic field generated by the carrier satisfies the model VG=mG+ρ, mG is the constant error, and ρ is the white noise. Add the constant error mG as one of the state variables to improve the stability of system. Then, the new observation model can be established as
(28)Z=01×3h1Xn+HH−1Xl+V,where:
(29)Z=ZHTZmT,Xl=φδVεb∇bmbmGT,h1Xn=∂G∂LatδLat+∂G∂LonδLon.Then, the general form of the observation equation of MPF is expressed as(30)Zk=hkXkn+HklXkl+Vk,where:
(31)hkXkn=01×3h1Xn,Hkl=HH−1,where Vk is the comprehensive observation error and the specific steps of the design of the MPF are as follows [19] (Algorithm 1).Algorithm 1: Implementation process of MPF.
a) Initialization:X0l, P0l, X0n,i∼P0X0ni=0,1,⋯N and N is the number of particles.b) Measurement update of Particle filter:Calculate weightsω~ki, resample Nk times, and calculate the minimum variance estimate X^kn.X^kn=∑i=1Nkω~kiXkn,ic) Measurement update of Kalman filter:X^k+1l=X^kl+KkZk−hX^kn−HklX^klKk=Pk/k−1HklTHklPk/k−1HklT+RkTPk=Pk/k−1−KkHklPk/k−1d) Time update of Particle filter:X^k+1n,i∼pXk+1n∣X0:kn,i,Z0:ke) Time update of Kalman filter:X^k+1/kl=Φl,klX^kl+Φl,knX^kn+KkX^k+1n−Φn,klX^kl−Φn,knX^knPk+1/k=Φl,klPkΦl,klT+ΓklQklΓklT−KkSkKkTKk=Fl.klPkFn,klTSk−1Sk=Fn,klPkFn,klT


## 2.5. Simulation

In the above section, we have established a new observation model for the integrated navigation system, designed the corresponding filter, and listed the specific implementation. Next, we will perform numerical simulation based on the above model and analyze the characteristics of the proposed algorithm. Table1 is the sensor error model parameters. The errors can be considered to be composed of constant deviation and white noise in this paper.Table 1
Sensor error model parameters.
Sensor typeConstant deviationWhite noiseUnitGyroscope0.020.01deg/hAccelerometer10-410-5gMagnetometer105mGIn the experiment, the simulation data are from a flight of a spacecraft in Earth-Centered Earth-Fixed coordinate system (ECEF). The initial latitude is 45.7508°, the initial longitude is 122.6715°, and the simulation time is 2000 s. The flight trajectory is shown in Figure3.Figure 3
Flight trajectory.In the simulation process, the initial horizontal position error is 5 m, the initial horizontal velocity error is 1 m/s, the initial attitude angles are 0.1°, 0.1°, and 0.1°. The northbound position error of GNS is 10 m, and the eastbound position error is 20 m. Perform simulation experiments based on the above parameters compare the navigation effects between the traditional model and the improved mathematical model and use MATLAB as the simulation tool to obtain the following simulation results.


## 3. Results and Discussion

This work has already finished the model building and digital simulation. The digital simulation attitude results of integrated navigation system are shown in Figures4–6, and the position results are shown in Figures 7 and 8, under the same simulation conditions; the two simulation results are shown in Table 2. Although both methods can converge with the time going, the improved algorithm has smaller errors and better stability.Figure 4
Comparison of error of roll.Figure 5
Comparison of error of yaw.Figure 6
Comparison of error of pitch.Figure 7
Comparison of error of north position.Figure 8
Comparison of error of east position.Table 2
Comparison of simulation results.
TraditionalImprovedRoll (deg)0.50230.2079Yaw (deg)0.71320.1512Pitch (deg)0.62570.1354North (m)12.5843.8520East (m)7.14505.4730The simulation results show that the improved GNS/SINS has higher navigation accuracy than traditional GNS/SINS in attitude and position and has better stability.


## 4. Conclusions

This study focuses on improving the accuracy of the navigation system, especially the attitude angle, through introducing the geomagnetic field vector. Based on the inertial navigation system, this paper proposes to use the geomagnetic field component and total geomagnetic field in the navigation coordinate system as observations to improve the traditional GNS/SINS and use MPF for information fusion to achieve accurate estimation of position and attitude. From the results of numerical simulation, it can be seen that the navigation accuracy of the improved scheme will be improved to a certain extent, especially in attitude correction. With the improvement of geomagnetic detection accuracy, this navigation method will play a greater role. However, some issues, sample impoverishment and degeneracy problem, related to particle filters need to be further optimized.



---

*Source: 5525481-2021-04-24.xml*