# Integrated Navigation Method of Aerospace Vehicle Based on Rank Statistics

Authors: Jun Kang; Zhi Xiong; Rong Wang; Xinrui Zhang
Journal: International Journal of Aerospace Engineering
 (2023)

---

## Abstract

The large dynamic and high-speed flight of aerospace vehicle will bring unpredictable conditions to its navigation system, resulting in that its system random noise probability distribution will no longer meet the preconditions of Gaussian distribution preset by the existing filter algorithm, thus reducing the accuracy of the navigation system. So, it is very important to propose an effective method to solve the filter problem of the navigation system in non-Gaussian distribution to improve the accuracy of the navigation system. Therefore, an integrated navigation method of aerospace vehicle based on rank statistics (LRF) has been proposed in this paper. Firstly, based on the flight characteristics of aerospace vehicles, an accurate gravity calculation model has been established to improve the accuracy of system modelling. Then, the state equation and measurement equation of integrated navigation system have been established. In combination with the rank filter algorithm as well as the determined weights, sampling points are calculated and nonlinearly propagated through the transition matrix to achieve an accurate estimation about the predicted values of the state quantities and measurement quantities and the covariance matrix. In turn, it simulates the probability distribution of the system state effectively. Therefore, when the system random noise probability distribution of the aerospace vehicle does not meet the Gaussian distribution due to various interference factors in the actual flight process, the algorithm can simulate the probability distribution of the actual system to the greatest extent, to improve the accuracy of the integrated navigation system and enhance the reliability of the navigation system ultimately.


---

## Body

## 1. Introduction

In recent years, the research on aerospace vehicles represented by the X37-B series of the United States has been carried out continuously. Countries around the world have also focused their strategic research on aerospace integration. As a new type of aircraft with dual functions of aircraft and spacecraft, aerospace vehicles (ASV) are gradually occupying an important strategic position in the future battlefield due to its characteristics of cross-airspace, multimission, multiworking mode, and large-scale high-speed manoeuvres [1, 2]. The reusable aerospace vehicles represented by X-37B, IXV, X-33, etc. have attracted more and more attention, promoted the development of low-cost space technology, and become a research hotspot in recent years [3, 4]. However, there are still many important problems to be solved in the field of aerospace vehicles. Among them, how to ensure the reliable flight of aerospace vehicles in complex flight environment has become a research hotspot. To improve the flight reliability of aerospace vehicle, it is necessary to design its control system perfectly. In the control system, the navigation system is an important part; its accuracy and reliability have an important impact on the subsequent guidance and control parts. Therefore, relying on high-precision navigation sensors, designing a navigation system with high reliability, high precision, and strong robustness has become one of the key problems to be solved.At present, the multisource fusion navigation system with inertial navigation system as the core has become the primary choice of large aircraft navigation system. Inertial navigation system has the advantages of complete autonomy and output of complete navigation parameters, which can provide continuous navigation information for aerospace vehicles. However, the problem of its own error drifting with time needs to be corrected by high-precision external measurement information. The mature applications include global satellite navigation system and celestial navigation system. The effectiveness of the multisource fusion navigation system constructed by diverse navigation sensors has been verified on multiple types and models of aircraft, such as the embedded GPS/INS system (EGI), which has been used more widely as a central navigation equipment of aircraft [5]. Moreover, the use of INS/GPS/CNS integrated navigation can achieve complementary advantages between navigation systems and improve the accuracy and reliability of the navigation system output, pointed out by Magree and Johnson and Vetrella et al. [6, 7]. Researchers have made fruitful results in this field [8]. However, through the research on the existing integrated navigation filter algorithms, we found that most of the current navigation system solutions are applicable to near ground aircraft. Because the flight altitude of this type of aircraft is generally low, the earth gravity model is usually simplified when the geographic coordinate system is used as the reference coordinate system for the calculation of the navigation system. In addition, in the above algorithm, due to the simplified modelling of the earth motion, when calculating the conversion matrix from the inertial coordinate system to the earth fixed coordinate system, it is usually realized by multiplying the earth rotation angle rate by the earth rotation time. But as we all know, the earth is an irregular ellipsoid, and its poles are flatter. For the modelling of aerospace vehicles navigation system, there are three main problems: The first is the gravity problem, and aerospace vehicles usually have high dynamic flight characteristics. Therefore, using a single gravity value to describe the gravity change in the whole process obviously cannot meet the accuracy requirements of aerospace vehicle navigation system. The second is the curvature radius of the earth. The high-speed flight of aerospace vehicle leads to the rapid change of longitude and latitude. Therefore, modelling using the curvature radius of the ellipsoid surface in all directions is also an important part of improving the accuracy of the navigation system. The third is that the navigation system model of aerospace vehicle is inconsistent with the filter model due to large dynamic flight, which leads to the decline of filter estimation accuracy. In recent years, researchers have solved the problem that the accuracy of integrated navigation system decreases due to the inaccurate error model through the research of the adaptive filter [9, 10] and IMM filter algorithm [11]. According to the accurate modelling of aerospace vehicle navigation system, it can provide high-precision state quantity and measurement quantity for the subsequent filtering algorithm and then improve the accuracy of navigation parameter estimation.The navigation system of aerospace vehicle is a typical nonlinear system because of its complex flight characteristics. How to estimate the parameters of the system accurately has become an important problem. At present, the Kalman filter is the most widely used, but it is limited by its application, that is, the system needs to meet the linear and the noise obeys the Gaussian distribution. Therefore, it is difficult to ensure the accuracy of the parameters estimation of the aerospace vehicle navigation system. In recent years, many researchers have proposed some filter algorithms including EKF and UKF. Among them, the nonlinear function of EKF, by expanding the nonlinear function with Taylor odd numbers, retaining its linear term, and ignoring the high-order term, is transformed into a linear function for the Kalman filter. However, EKF still uses the standard Kalman filter algorithm for linear systems, so it is only applicable to nonlinear objects weakly. UKF is also suitable for nonlinear functions, which estimates the parameters by approximating the probability distribution [12]. Unlike the Kalman filter, which uses the prior information of the measured quantity and the prediction mean square error matrix to determine the filter gain, it determines the filter gain through the estimated state quantity and the covariance matrix of the measured quantity. In recent years, based on the standard Kalman filter algorithm, many filter algorithms to solve the estimation of nonlinear systems have been proposed. Arasaratnam et al. have used the Ito-Taylor expansion of order 1.5 to transform the process equation, modelled in the form of stochastic ordinary differential equations, into a set of stochastic difference equations [13]. Building on this transformation and assuming that all conditional densities are Gaussian-distributed, and the solution to the Bayesian filter reduces to the problem of how to compute Gaussian-weighted integrals. Chuang et al. have used the central difference Kalman filter (CDKF) transform to deal with the nonlinear problem of the model, avoiding to solve the complex Jacobian matrix [14]. However, the above methods are still designed from the perspective that the system noise in Kalman filter obeys Gaussian distribution.The complex flight environment of aerospace vehicles may make it difficult to model all measurement processes and obtain the statistical characteristics of noise accurately. Therefore, the traditional Kalman filter based on Gaussian distribution will be difficult to adapt to this situation. Filter algorithms for non-Gaussian distribution have also been studied in recent years. The representative algorithms include the particle filter and the filter algorithm based on the maximum correlation entropy criterion. Because of its unique theoretical advantages in nonlinear non-Gaussian systems, particle filter has gradually become the focus of many fusion methods. However, particle degradation and sample depletion restrict the application of particle filter in complex engineering [15, 16]. As a local similarity measure, the filter algorithm based on the maximum correlation entropy criterion can make full use of the higher order information of the measured quantity compared with the classical least mean square error criterion which only uses the second-order moment information of the measured quantity. Therefore, the correlation entropy theory is applicable to the stochastic systems contaminated by non-Gaussian noise [17, 18]. Zhang et al. have proposed a cubature Kalman filter based on maximum correntropy criterion (MCCKF) [19]. MCCKF took the maximum correntropy criterion as the optimization criterion, which considers the high-order moments of estimation errors. Then, the estimated state was updated with fixed-point iteration algorithms iteratively, and the cubature quadrature rule was used to approximate a Gaussian-weighted integral. In addition, Zhang et al. have developed the minimum kernel risk-sensitive loss (MKRSL) algorithm to improve the filter accuracy and robustness of kernel least mean square (KLMS) in non-Gaussian noises [20]. From the above analysis, we can see that in the face of complex non-Gaussian systems such as aerospace vehicle navigation system, the traditional filter algorithm based on Gaussian distribution cannot be estimated effectively. Therefore, researchers have carried out a lot of research on the filter algorithm of non-Gaussian noise. However, the above algorithm still has the problems of complex calculation and low efficiency, which is difficult to meet the requirements of aerospace vehicle navigation system. In recent years, a rank sampling method has been proposed [21], which can simulate the probability distribution of system states effectively. Based on this, a rank filter method has been proposed. It can be applied to nonlinear filter problems in which noise conforms to Gaussian distribution or non-Gaussian distribution. In addition, compared with UKF, PF, and other algorithms, this method has the advantages of simple calculation and small amount of calculation, which is convenient for engineering application.In a word, the aerospace vehicle can adopt a multisource navigation system to meet the needs of its complete navigation parameter measurement. On this basis, other auxiliary navigation sensors can be selected according to the changes of the actual flight environment to improve the reliability of the navigation system. At the same time, different navigation sensors can be added according to the needs of practical projects. Based on the above navigation configuration, this paper carries out the algorithm research on the integrated navigation system of aerospace vehicles. The main innovations include the following two parts: firstly, according to the characteristics of aerospace vehicle flying in large dynamic, long distance, and far away from the earth’s surface, the influence of gravity and earth’s radius of curvature on navigation results is fully considered in the modelling stage, and an accurate model is established to improve the accuracy of navigation calculation. At the same time, in order to conform to the actual flight environment of aerospace vehicles, the transformation matrix from the earth-centred inertial coordinate system to the earth fixed coordinate system is established in combination with the earth motion parameters such as pole shift, vernal equinox time angle, nutation, and precession of the equinoxes, to improve the accuracy of the model. On this basis, combined with the multisource navigation sensor configuration of aerospace vehicles and the problem that the system noise no longer obeys the Gaussian distribution during large dynamic flight, according to using the rank filter algorithm and combining with the principle of rank statistics, the real distribution has been simulated, and the rank Kalman filter is used to realize the accurate estimation of navigation parameters. The proposed algorithm can meet the accuracy and reliability requirements of the navigation system for aerospace vehicles in a large-scale and high-maneuverer flight.


## 2. Algorithm Design

ASV has a broad flight envelope, and the entire flight process can be divided into several stages: launch phase, on-orbit phase, reentry phase, and landing phase. Considering the difference of flight characteristics between aerospace vehicles and unmanned aerial vehicle, aircraft, and other near ground vehicles, aerospace vehicles generally fly in low-earth orbit about 300 km away from the earth. At present, the algorithms designed for the near ground vehicle using the geographic coordinate system as the reference coordinate system for navigation calculation often ignore the impact of the fact that the earth is an imperfect ellipse on the navigation results. This processing method will not have a great impact on the navigation calculation accuracy of the near ground vehicle. However, in the navigation calculation process of the aerospace vehicle, on the one hand, because its flight is far away from the earth’s surface, parameters related to the earth have an important impact on its navigation calculation. On the other hand, it requires higher accuracy of the navigation system. Therefore, the launch inertial coordinate system is selected as the reference coordinate system of this algorithm.Then, since the noise distribution of the system state quantity and measurement quantity is no longer consistent with the traditional Gaussian distribution caused by many uncertain factors in the actual flight process of the aerospace vehicle, the rank sampling method is adopted to simulate the real system distribution further effectively, to improve the output accuracy of the filter. The overall scheme of the algorithm is shown in Figure1.Figure 1
The overall scheme of the algorithm.Figure1 is an algorithm scheme diagram. It can be seen from Figure 1 that the algorithm is divided into three parts. The blue part is inertial navigation system modelling. Firstly, the angular rate of the aerospace vehicle is obtained by the gyroscope, and the attitude is calculated by the quaternion method. The specific force is obtained by the accelerometer, and the position and speed are calculated. At the same time, the specific force of the body coordinate system relative to the inertial coordinate system is projected in the geographic coordinate system to calculate the acceleration in the up direction, and the gravity potential function is calculated by using the spherical harmonic function, which provides the basis for the subsequent filtering equation modelling. The green part is the filter modelling part. Firstly, the navigation parameters are obtained by measuring sensors, and the Kalman filter model is established by an indirect method. In view of the fact that the system noise does not conform to the Gaussian distribution. The orange part is used for rank statistics analysis. Firstly, the rank sampling points are generated to obtain the variance matrix of a one-step prediction error. Then, the updated filter gain matrix is obtained through the state matrix transition and measurement matrix transition, and the navigation parameter error is corrected.


### 2.1. Influence of Earth Gravity

Unlike aviation aircraft, aerospace vehicles fly far away from the earth’s surface, which means that the simplified earth model used by traditional inertial navigation algorithms in the geographic coordinate system is no longer suitable for ASV navigation systems. During the matrix conversion of aerospace vehicle coordinates, an important conversion matrix is the conversion from WGS84 coordinate system to the J2000 coordinate system. Usually, the global satellite navigation receiver receives the rectangular coordinates under the WGS84 coordinate system, so the difference between the WGS84 coordinate system and the J2000 coordinate system is mainly caused by the pole shift, vernal equinox time angle, nutation, and precession of the equinoxes. The conversion steps from the WGS84 coordinate system to the J2000 coordinate system are shown in Figure2:Figure 2
Schematic diagram of the conversion process between the J2000 coordinate system and WGS84 coordinate system.As can be seen from Figure2, in order to calculate the transformation matrix from the J2000 coordinate system to WGS84 coordinate system, it is necessary to fully consider the relevant parameters of the earth motion. Firstly, the influence of precession is considered to calculate the coordinate transformation matrix from the J2000 coordinate system to the instantaneous horizontal celestial coordinate system, while the nutation effect is required to be considered in the transformation from instantaneous horizontal celestial coordinate system to the instantaneous true celestial coordinate system. Then, the influence of the vernal equinox time angle shall be considered when converting from the instantaneous true horizontal celestial coordinate system to the instantaneous ground fixed coordinate system. Finally, according to the coordinates of the pole, the instantaneous ground fixed coordinate system is converted to the WGS84 coordinate system.Therefore, based on the previous research on the strapdown inertial navigation system model in the launch inertial coordinate system [22], this paper further deduces the universal gravitation model which affects the accuracy of the aerospace vehicle navigation system and then improves the original model.The specific force measured by the accelerometer is calculated from the difference between the absolute acceleration and the gravitational acceleration of the aerospace vehicle relative to the inertial space. Combined with Newton’s second law, the specific force equation can be obtained as follows:(1)f=R¨−G.In Equation (1), G is the gravitational acceleration and R is the position vector of aerospace vehicle in inertial coordinate system. The derivative of R in the geocentric inertial coordinate system can be expressed as
(2)dRdti=dRdte+ωie×R.In Equation (2), dR/dte is the velocity of the aerospace vehicle relative to the earth, ωie is the rotation speed of the earth, and ωie×R is the implicated acceleration caused by the earth’s rotation.Supposevep represents the velocity of the aerospace vehicle relative to the earth:
(3)vep=dRdte.The two sides of Equation (2) are derived in the inertial coordinate system; then:
(4)d2Rdt2i=d2vepdti+ddtωie×Ri.Due toωie is a constant, so
(5)d2Rdt2i=v̇ep+2ωie+ωep×vep+ωie×ωie×R.From Equations (1) and (5):
(6)f+G=d2Rdt2i=v̇ep+2ωie+ωep×vep+ωie×ωie×R.Then the relationship between the specific force output by the accelerometer and the motion acceleration can be obtained:(7)v̇ep=f−2ωie+ωep×vep−g.According to Equation (7), the up direction geographic velocity of the aerospace vehicle relative to the earth surface can be obtained:
(8)v̇zn=fzn−2ωiecosL+vxnRn×vxn+vynRmvyn−g.It can be seen from Equation (8) that the real-time altitude of the aerospace vehicle can be calculated by v̇zn through two integrals.From Equation (8), it can be seen that v̇zn includes the gravitational acceleration. Therefore, G is not constant. There is a functional relationship between G and height h, and its value decreases with the increase of h. R is the radius of the earth, and when h≪R, the gravity model utilized in conventional inertial navigation algorithm is as follows:
(9)g=00g01−2hrT.where g is the local gravity vector and g0 is the launch point gravity value.From the above equation, it can be found that the gravity model is greatly simplified. The gravity model utilized in conventional inertial navigation algorithm is only related tog0 and h.Since the orbital height of ASV is about 300 km, it can be roughly regarded as a low-earth orbit satellite, which means that the greatest influence factor on the gravitational field is the ellipticity of the earth.According to Newton’s law of universal gravitation, the gravitational potential function can be written as:(10)dV=Gdmρ.In Equation (10), dm is the mass element of the total mass of the earth. The distance between the mass of a particle outside the earth and dm is ρ. Therefore, the potential function generated by the mass of the whole earth is equal to the sum of the potential functions of its various mass elements:
(11)V=∫MdV=G∫Mdmρ.Combining Equations (10) and (11), the second-order partial derivative of the three-dimensional coordinates of the potential function can be obtained:
(12)∂2V∂x2=G∫M∂2∂x21ρdm∂2V∂y2=G∫M∂2∂y21ρdm∂2V∂z2=G∫M∂2∂z21ρdm.Using the spherical coordinates of a particle outside the earthq,ψ,σ and the spherical coordinates of a mass element inside the earth Q,ψ′,σ′, Q is the radial diameter, and its included angle with q is ϕ.So, the functionPncosϕ composed of the above four spherical angular coordinate variables can be calculated:
(13)Pncosϕ=∑k=0n2n−k!1+δkn+k!PnkcosψPnkcosψ′coskσ−σ′.In Equation (13), ψ, ψ′, σ, and σ′ are the four spherical angular coordinate variables.By integrating the spatial dynamic coordinatesQ,ψ′,σ′ and combining the trigonometric function identity, the potential function can be rewritten as:
(14)V=μr1−∑n=1∞RernJnPncosψ+∑k=1nJnkPnkcosψcoskσ+σnk.In Equation (14), Jn is the second-order harmonic coefficient.According to Newton’s second law of motion, the motion equation of aerospace vehicle in the earth’s gravitational field is:(15)r¨=f.In Equation (15), the displacement vector of aerospace vehicle in earth-centred inertial coordinate system is r=xyzT, and the earth’s gravitation to aerospace vehicles is f=fxfyfzT. Therefore, the gravity field model is as follows:
(16)G=−μxr31−J2Rer27.5z2r2−1.5−μyr31−J2Rer27.5z2r2−1.5−μzr31−J2Rer27.5z2r2−4.5.where x,y,z is the position of aerospace vehicles in the inertial coordinate system, J2=1.08263×10−3 is the second-order harmonic coefficient, μ=3.986×1014m3s−2 is the gravitational constant, Re=6378137m is the equatorial radius, and r is the distance from aerospace vehicles to the geocentric. If a higher precise gravitational field is needed, the formula using multiorder harmonic coefficients can be applied.Comparing Equations (9) and (16), we can find that the gravity model calculation equation in Equation (9) only has a mathematical relationship with the flying height of the aircraft and does not consider the impact of the earth’s ellipticity on the gravity field, because it is generally applicable to the navigation solution of near ground aircraft. When the altitude h of the aircraft is far less than the radius R of the earth, this equation can calculate the gravity of the aircraft approximately. However, it is well known that the aerospace vehicle is in the orbit for a long time, and its flying height is about 300 km. Therefore, the influence of the earth ellipticity on its gravity field cannot be ignored. In combination with Equation (14), we can deduce the calculation Equation (16) of the gravity field of the aerospace vehicle during flight. This model fully considers the influence of the earth ellipticity on the gravity field and improves the accuracy of the navigation system model of the aerospace vehicle.


### 2.2. Modelling of Integrated Navigation System

After we have established an accurate gravity model, the strapdown inertial navigation model is further established. According to the design scheme shown in Figure1, the strapdown inertial navigation model can be divided into attitude calculation module and velocity/position calculation module. Since the launch inertial coordinate system is different from the geocentric inertial coordinate system only in the coordinate origin, therefore, combining with gyroscope outputs, ωlbb can be written as:
(17)ωlbb=ωibb.In Equation (17), ωibb is the projection of the angular rate of the body coordinate system relative to the inertial coordinate system under the body coordinate system.In order to calculate the direction cosine matrix between the launch inertial coordinate system and the geographic coordinate system and further calculate the attitude of the aerospace vehicle in the launch inertial coordinate system, the quaternion method is adopted in this paper. Compared with the traditional Euler angle method, this method can greatly reduce the amount of calculation.The quaternion differential equation of attitude calculation is as follows:(18)q̇=12×q0−q1−q2−q3q1q0−q3q2q2q3q0−q1q3−q2q1q0×0ωbxωbyωbz.In Equation (18), q is the attitude quaternion.The quaternionq can be obtained by discretizing Equation (18) and calculating it by the Picard successive approximation method. By approximating the exponential integral, the following quaternion recurrence expression can be obtained:
(19)qit+T=cosΔθ02I+sinΔθ0/2Δθ0Δθqit.In Equation (19), qit+T is the attitude quaternion of the carrier at time t+T. I is a 4×4 identity matrix.
(20)Δθ02=Δθx2+Δθy2+Δθz2.After normalizingq, the attitude transfer matrix Clb can be obtained:
(21)Clb=q02+q12−q22−q322q1q2+q0q32q1q3−q0q22q1q2−q0q3q02−q12+q22−q322q2q3+q0q12q1q3+q0q22q2q3−q0q1q02−q12−q22+q32.Furthermore, the expressions of attitude angles are obtained as follows:(22)γ=−arctanClb13Clb33θ=arctanClb23ψ=arctanClb21Clb22.In Equation (22), γ is the roll angle, θ is the pitch angle, and ψ is the yaw angle.The output of accelerometerfibb is the vector formed by the axial component of the acceleration of the body coordinate system relative to the inertial coordinate system under the body coordinate system; similarly, combining with Equation (21), we can project the output of the accelerometer to the emission inertial coordinate system, and get:
(23)fibl=Cblfibb.Among them,Cbl is the transformation matrix of the body coordinate system relative to the launch inertial coordinate system.From Equation (23), the specific force equation in launch inertial coordinate system can be written as follows:
(24)fibl=v̇−CilG.In Equation (24), v is the velocity value under the launch inertial coordinate system, and Cil is the attitude transformation matrix of the inertial system relative to the launch inertial coordinate system. G is calculated by Equation (16); then, the position value in the launch inertial coordinate system can be calculated:
(25)ṗ=v.Combining Equations (17), (18), (24), and (25), the navigation model can be established.
(26)q̇=0.5q⊗ωlbbṗ=vv̇=fibl+CilG.After we have established the strapdown inertial navigation system model, the state model and observation model of SINS/GNSS/CNS-integrated navigation system are established directly.The state equation of the navigation system after discretization is as follows:(27)Ẋ=FtXt+GtWt.In Equation (27), Ft is the system matrix, Xt is the state vector, Gt is the system noise matrix, and Wt is the system noise vector.Then, we can get the state vectorX is:
(28)X=δq1δq2δq3δpxδpyδpzδvxδvyδvzωrxωryωrzfrxfryfrz.In Equation (28), δq1, δq2, and δq3 are the three-dimensional vector error part of attitude quaternion q; δpx, δpy, and δpz are the position error; δvx, δvy, and δvz are the velocity error; ωrx, ωry, and ωrz are the gyro random walk error; and frx,fry,frz are the accelerometer random walk error.The system noise vectorW is:
(29)W=ωεxωεyωεzωnxωnyωnzfnxfnyfnz.The discrete equation of state for the navigation system is obtained by discretizing Equation (27) as follows:
(30)Xk=Φkk−1Xk−1+Γk−1Wk−1.In Equation (30), Xk is the state quantity at moment k, Xk−1 is the state quantity at moment k−1, Φkk−1 is the system state transfer matrix from moment k−1 to moment k, Γk−1 is the corresponding discrete system noise matrix, and Wk−1 is the system noise at moment k−1.In the observation equation of integrated navigation system, position information from GPS and attitude information from CNS are regarded as observation measurements. Then the observation vector of SINS/CNS subsystem can be written as follows:(31)Zt=Ht×Xt+R.R is the measurement noise matrix of measurement.


## 2.1. Influence of Earth Gravity

Unlike aviation aircraft, aerospace vehicles fly far away from the earth’s surface, which means that the simplified earth model used by traditional inertial navigation algorithms in the geographic coordinate system is no longer suitable for ASV navigation systems. During the matrix conversion of aerospace vehicle coordinates, an important conversion matrix is the conversion from WGS84 coordinate system to the J2000 coordinate system. Usually, the global satellite navigation receiver receives the rectangular coordinates under the WGS84 coordinate system, so the difference between the WGS84 coordinate system and the J2000 coordinate system is mainly caused by the pole shift, vernal equinox time angle, nutation, and precession of the equinoxes. The conversion steps from the WGS84 coordinate system to the J2000 coordinate system are shown in Figure2:Figure 2
Schematic diagram of the conversion process between the J2000 coordinate system and WGS84 coordinate system.As can be seen from Figure2, in order to calculate the transformation matrix from the J2000 coordinate system to WGS84 coordinate system, it is necessary to fully consider the relevant parameters of the earth motion. Firstly, the influence of precession is considered to calculate the coordinate transformation matrix from the J2000 coordinate system to the instantaneous horizontal celestial coordinate system, while the nutation effect is required to be considered in the transformation from instantaneous horizontal celestial coordinate system to the instantaneous true celestial coordinate system. Then, the influence of the vernal equinox time angle shall be considered when converting from the instantaneous true horizontal celestial coordinate system to the instantaneous ground fixed coordinate system. Finally, according to the coordinates of the pole, the instantaneous ground fixed coordinate system is converted to the WGS84 coordinate system.Therefore, based on the previous research on the strapdown inertial navigation system model in the launch inertial coordinate system [22], this paper further deduces the universal gravitation model which affects the accuracy of the aerospace vehicle navigation system and then improves the original model.The specific force measured by the accelerometer is calculated from the difference between the absolute acceleration and the gravitational acceleration of the aerospace vehicle relative to the inertial space. Combined with Newton’s second law, the specific force equation can be obtained as follows:(1)f=R¨−G.In Equation (1), G is the gravitational acceleration and R is the position vector of aerospace vehicle in inertial coordinate system. The derivative of R in the geocentric inertial coordinate system can be expressed as
(2)dRdti=dRdte+ωie×R.In Equation (2), dR/dte is the velocity of the aerospace vehicle relative to the earth, ωie is the rotation speed of the earth, and ωie×R is the implicated acceleration caused by the earth’s rotation.Supposevep represents the velocity of the aerospace vehicle relative to the earth:
(3)vep=dRdte.The two sides of Equation (2) are derived in the inertial coordinate system; then:
(4)d2Rdt2i=d2vepdti+ddtωie×Ri.Due toωie is a constant, so
(5)d2Rdt2i=v̇ep+2ωie+ωep×vep+ωie×ωie×R.From Equations (1) and (5):
(6)f+G=d2Rdt2i=v̇ep+2ωie+ωep×vep+ωie×ωie×R.Then the relationship between the specific force output by the accelerometer and the motion acceleration can be obtained:(7)v̇ep=f−2ωie+ωep×vep−g.According to Equation (7), the up direction geographic velocity of the aerospace vehicle relative to the earth surface can be obtained:
(8)v̇zn=fzn−2ωiecosL+vxnRn×vxn+vynRmvyn−g.It can be seen from Equation (8) that the real-time altitude of the aerospace vehicle can be calculated by v̇zn through two integrals.From Equation (8), it can be seen that v̇zn includes the gravitational acceleration. Therefore, G is not constant. There is a functional relationship between G and height h, and its value decreases with the increase of h. R is the radius of the earth, and when h≪R, the gravity model utilized in conventional inertial navigation algorithm is as follows:
(9)g=00g01−2hrT.where g is the local gravity vector and g0 is the launch point gravity value.From the above equation, it can be found that the gravity model is greatly simplified. The gravity model utilized in conventional inertial navigation algorithm is only related tog0 and h.Since the orbital height of ASV is about 300 km, it can be roughly regarded as a low-earth orbit satellite, which means that the greatest influence factor on the gravitational field is the ellipticity of the earth.According to Newton’s law of universal gravitation, the gravitational potential function can be written as:(10)dV=Gdmρ.In Equation (10), dm is the mass element of the total mass of the earth. The distance between the mass of a particle outside the earth and dm is ρ. Therefore, the potential function generated by the mass of the whole earth is equal to the sum of the potential functions of its various mass elements:
(11)V=∫MdV=G∫Mdmρ.Combining Equations (10) and (11), the second-order partial derivative of the three-dimensional coordinates of the potential function can be obtained:
(12)∂2V∂x2=G∫M∂2∂x21ρdm∂2V∂y2=G∫M∂2∂y21ρdm∂2V∂z2=G∫M∂2∂z21ρdm.Using the spherical coordinates of a particle outside the earthq,ψ,σ and the spherical coordinates of a mass element inside the earth Q,ψ′,σ′, Q is the radial diameter, and its included angle with q is ϕ.So, the functionPncosϕ composed of the above four spherical angular coordinate variables can be calculated:
(13)Pncosϕ=∑k=0n2n−k!1+δkn+k!PnkcosψPnkcosψ′coskσ−σ′.In Equation (13), ψ, ψ′, σ, and σ′ are the four spherical angular coordinate variables.By integrating the spatial dynamic coordinatesQ,ψ′,σ′ and combining the trigonometric function identity, the potential function can be rewritten as:
(14)V=μr1−∑n=1∞RernJnPncosψ+∑k=1nJnkPnkcosψcoskσ+σnk.In Equation (14), Jn is the second-order harmonic coefficient.According to Newton’s second law of motion, the motion equation of aerospace vehicle in the earth’s gravitational field is:(15)r¨=f.In Equation (15), the displacement vector of aerospace vehicle in earth-centred inertial coordinate system is r=xyzT, and the earth’s gravitation to aerospace vehicles is f=fxfyfzT. Therefore, the gravity field model is as follows:
(16)G=−μxr31−J2Rer27.5z2r2−1.5−μyr31−J2Rer27.5z2r2−1.5−μzr31−J2Rer27.5z2r2−4.5.where x,y,z is the position of aerospace vehicles in the inertial coordinate system, J2=1.08263×10−3 is the second-order harmonic coefficient, μ=3.986×1014m3s−2 is the gravitational constant, Re=6378137m is the equatorial radius, and r is the distance from aerospace vehicles to the geocentric. If a higher precise gravitational field is needed, the formula using multiorder harmonic coefficients can be applied.Comparing Equations (9) and (16), we can find that the gravity model calculation equation in Equation (9) only has a mathematical relationship with the flying height of the aircraft and does not consider the impact of the earth’s ellipticity on the gravity field, because it is generally applicable to the navigation solution of near ground aircraft. When the altitude h of the aircraft is far less than the radius R of the earth, this equation can calculate the gravity of the aircraft approximately. However, it is well known that the aerospace vehicle is in the orbit for a long time, and its flying height is about 300 km. Therefore, the influence of the earth ellipticity on its gravity field cannot be ignored. In combination with Equation (14), we can deduce the calculation Equation (16) of the gravity field of the aerospace vehicle during flight. This model fully considers the influence of the earth ellipticity on the gravity field and improves the accuracy of the navigation system model of the aerospace vehicle.


## 2.2. Modelling of Integrated Navigation System

After we have established an accurate gravity model, the strapdown inertial navigation model is further established. According to the design scheme shown in Figure1, the strapdown inertial navigation model can be divided into attitude calculation module and velocity/position calculation module. Since the launch inertial coordinate system is different from the geocentric inertial coordinate system only in the coordinate origin, therefore, combining with gyroscope outputs, ωlbb can be written as:
(17)ωlbb=ωibb.In Equation (17), ωibb is the projection of the angular rate of the body coordinate system relative to the inertial coordinate system under the body coordinate system.In order to calculate the direction cosine matrix between the launch inertial coordinate system and the geographic coordinate system and further calculate the attitude of the aerospace vehicle in the launch inertial coordinate system, the quaternion method is adopted in this paper. Compared with the traditional Euler angle method, this method can greatly reduce the amount of calculation.The quaternion differential equation of attitude calculation is as follows:(18)q̇=12×q0−q1−q2−q3q1q0−q3q2q2q3q0−q1q3−q2q1q0×0ωbxωbyωbz.In Equation (18), q is the attitude quaternion.The quaternionq can be obtained by discretizing Equation (18) and calculating it by the Picard successive approximation method. By approximating the exponential integral, the following quaternion recurrence expression can be obtained:
(19)qit+T=cosΔθ02I+sinΔθ0/2Δθ0Δθqit.In Equation (19), qit+T is the attitude quaternion of the carrier at time t+T. I is a 4×4 identity matrix.
(20)Δθ02=Δθx2+Δθy2+Δθz2.After normalizingq, the attitude transfer matrix Clb can be obtained:
(21)Clb=q02+q12−q22−q322q1q2+q0q32q1q3−q0q22q1q2−q0q3q02−q12+q22−q322q2q3+q0q12q1q3+q0q22q2q3−q0q1q02−q12−q22+q32.Furthermore, the expressions of attitude angles are obtained as follows:(22)γ=−arctanClb13Clb33θ=arctanClb23ψ=arctanClb21Clb22.In Equation (22), γ is the roll angle, θ is the pitch angle, and ψ is the yaw angle.The output of accelerometerfibb is the vector formed by the axial component of the acceleration of the body coordinate system relative to the inertial coordinate system under the body coordinate system; similarly, combining with Equation (21), we can project the output of the accelerometer to the emission inertial coordinate system, and get:
(23)fibl=Cblfibb.Among them,Cbl is the transformation matrix of the body coordinate system relative to the launch inertial coordinate system.From Equation (23), the specific force equation in launch inertial coordinate system can be written as follows:
(24)fibl=v̇−CilG.In Equation (24), v is the velocity value under the launch inertial coordinate system, and Cil is the attitude transformation matrix of the inertial system relative to the launch inertial coordinate system. G is calculated by Equation (16); then, the position value in the launch inertial coordinate system can be calculated:
(25)ṗ=v.Combining Equations (17), (18), (24), and (25), the navigation model can be established.
(26)q̇=0.5q⊗ωlbbṗ=vv̇=fibl+CilG.After we have established the strapdown inertial navigation system model, the state model and observation model of SINS/GNSS/CNS-integrated navigation system are established directly.The state equation of the navigation system after discretization is as follows:(27)Ẋ=FtXt+GtWt.In Equation (27), Ft is the system matrix, Xt is the state vector, Gt is the system noise matrix, and Wt is the system noise vector.Then, we can get the state vectorX is:
(28)X=δq1δq2δq3δpxδpyδpzδvxδvyδvzωrxωryωrzfrxfryfrz.In Equation (28), δq1, δq2, and δq3 are the three-dimensional vector error part of attitude quaternion q; δpx, δpy, and δpz are the position error; δvx, δvy, and δvz are the velocity error; ωrx, ωry, and ωrz are the gyro random walk error; and frx,fry,frz are the accelerometer random walk error.The system noise vectorW is:
(29)W=ωεxωεyωεzωnxωnyωnzfnxfnyfnz.The discrete equation of state for the navigation system is obtained by discretizing Equation (27) as follows:
(30)Xk=Φkk−1Xk−1+Γk−1Wk−1.In Equation (30), Xk is the state quantity at moment k, Xk−1 is the state quantity at moment k−1, Φkk−1 is the system state transfer matrix from moment k−1 to moment k, Γk−1 is the corresponding discrete system noise matrix, and Wk−1 is the system noise at moment k−1.In the observation equation of integrated navigation system, position information from GPS and attitude information from CNS are regarded as observation measurements. Then the observation vector of SINS/CNS subsystem can be written as follows:(31)Zt=Ht×Xt+R.R is the measurement noise matrix of measurement.


## 3. Integrated Navigation Filter Algorithm

### 3.1. Rank Kalman Filter Algorithm

After the models of strapdown inertial navigation system and integrated navigation system are established, the filter algorithm of integrated navigation system needs to be further studied. Aerospace vehicles need to complete the task of cross-airspace, high dynamic, and large range; therefore, the noise statistical characteristics of its navigation system no longer obey the Gaussian distribution, which will lead to the inconsistency between the filter model and the actual navigation system model. At this time, the performance of the existing filter algorithms based on the assumption of Gaussian distribution will decline. According to the principle of the rank Kalman filter [21], it adopts the rank sampling algorithm, which collects the sampling points according to the correlation principle of rank statistics, and then simulates the probability distribution of the system state effectively, which can solve the problems of system model error and noise uncertainty in the actual system.


#### 3.1.1. Principle of Rank Sampling

The rank sampling is a deterministic sampling method, which is based on the mean of input vectorX¯ and its covariance matrix Pkx, to calculate the mean Y¯ and covariance matrix Pky of the nonlinear function.Firstly, according to the mean valueX¯ and covariance matrix PX of input vector X. Computing the set of rank sampling points:χi:
(32)χi=X¯+rjλpjPkxl,(33)i=j−1n+lj=1,2,⋯ui=j−2n+lj=u+2,u+3,⋯2u+1.In Equation (32), χi is the i-th sampling point of X, with a total of 2un sampling points, n is the dimension of state variable X, u is the number of sampling layers, rj is the correction coefficient of sampling points in layer J, pj is the j-th sampling point, χi is the corresponding probability, λpj is the quantile of probability pj in the one-dimensional standard distribution probability of X distribution, and Pkxl is the l-th vector of the square root of covariance matrix Pkx. Due to the singular values that often appear in practical use, which leads to the decline of numerical stability, therefore, singular-value decomposition is adopted to improve the stability of the algorithm. The specific algorithm is as follows:
(34)Pkx=UkxSkx000VkxT.In Equation (34), S is a diagonal matrix, S=diags1,s2,⋯,sn. After SVD decomposition, the covariance matrix of Equation (34) can be written as follows:
(35)Pkx=UkxSkxVkxT.For each pointχi of the rank sampling point set χi, the point set Yi of the output variable Y is obtained by nonlinear transformation. According to Yi, calculate the mean Y¯ and the covariance matrix PY.
(36)Y¯=12un∑i=12unYi,(37)PY=1ω∑i=12unri∗Yi−Y¯Yi−Y¯T.Among Equation (37), ri∗ is the covariance correction coefficient corresponding to the i-th sampling point; the covariance weight coefficient ω can be written as follows:
(38)ω=∑j=1,j≠u+12u+1rj2λpj2.


### 3.2. Filter Equation

Let the estimated value of state vectorX at time k−1 be X^k−1, and the covariance matrix is Pk−1. According to the principle of rank sampling in Equation (32), we can get the set of sampling points χi. Therefore, the estimated value of X at time k is obtained X^kk−1:
(39)X^kk−1=12un∑i=12unΦχk−1,ii=1,2,⋯,2un.One-step prediction covariance matrix is:(40)Pkk−1=ri∗ω∑i=12unX~kk−1X~kk−1T+Qk−1.In Equation (40):
(41)X~kk−1=Xkk−1,i−X^kk−1.Measurement update: according to Equation (37), the new set of sampling points χkk−1,i is obtained by rank sampling again:
(42)χkk−1,i=X^kk−1+rjλpjPkk−1l.χkk−1,i is the i-th sampling point of X^kk−1, a total of 2un; Pkk−1l is the l-th column vector of Pkk−1. After SVD decomposition, the covariance matrix of Equation (34) can be written as follows:
(43)Pkk−1=Ukk−1Skk−1Vkk−1T.The set of sampling pointsχkk−1,i calculated by Equation (42) is introduced into the measurement equation and nonlinear propagation is carried out:
(44)Zkk−1,i=Hχkk−1,ii=1,2,⋯,2un.From Equation (44), we can get the estimated value Z^kk−1 of observation measurement:
(45)Z^kk−1=12un∑i=12unZkk−1,i.From Equations (44) and (45), we can get the covariance matrix of forecasting innovation:
(46)Pzz=ri∗ω∑i=12unZkk−1,i−Z^kk−1Zkk−1,i−Z∧kk−1T+Rk.From Equations (41), (44), and (45), the prediction cross-covariance matrix can be obtained:
(47)Pxz=ri∗ω∑i=12unχkk−1,i−X^kk−1χkk−1,i−X∧kk−1T.From Equations (46) and (47), the filter gain can be obtained:
(48)K=PxzPzz−1.Thek-time state estimation is:
(49)X^k=X^kk−1+KZk−Z^kk−1.The variance matrix ofk-time estimation error is as follows:
(50)Pk=Pkk−1−KPzzKT.


## 3.1. Rank Kalman Filter Algorithm

After the models of strapdown inertial navigation system and integrated navigation system are established, the filter algorithm of integrated navigation system needs to be further studied. Aerospace vehicles need to complete the task of cross-airspace, high dynamic, and large range; therefore, the noise statistical characteristics of its navigation system no longer obey the Gaussian distribution, which will lead to the inconsistency between the filter model and the actual navigation system model. At this time, the performance of the existing filter algorithms based on the assumption of Gaussian distribution will decline. According to the principle of the rank Kalman filter [21], it adopts the rank sampling algorithm, which collects the sampling points according to the correlation principle of rank statistics, and then simulates the probability distribution of the system state effectively, which can solve the problems of system model error and noise uncertainty in the actual system.


### 3.1.1. Principle of Rank Sampling

The rank sampling is a deterministic sampling method, which is based on the mean of input vectorX¯ and its covariance matrix Pkx, to calculate the mean Y¯ and covariance matrix Pky of the nonlinear function.Firstly, according to the mean valueX¯ and covariance matrix PX of input vector X. Computing the set of rank sampling points:χi:
(32)χi=X¯+rjλpjPkxl,(33)i=j−1n+lj=1,2,⋯ui=j−2n+lj=u+2,u+3,⋯2u+1.In Equation (32), χi is the i-th sampling point of X, with a total of 2un sampling points, n is the dimension of state variable X, u is the number of sampling layers, rj is the correction coefficient of sampling points in layer J, pj is the j-th sampling point, χi is the corresponding probability, λpj is the quantile of probability pj in the one-dimensional standard distribution probability of X distribution, and Pkxl is the l-th vector of the square root of covariance matrix Pkx. Due to the singular values that often appear in practical use, which leads to the decline of numerical stability, therefore, singular-value decomposition is adopted to improve the stability of the algorithm. The specific algorithm is as follows:
(34)Pkx=UkxSkx000VkxT.In Equation (34), S is a diagonal matrix, S=diags1,s2,⋯,sn. After SVD decomposition, the covariance matrix of Equation (34) can be written as follows:
(35)Pkx=UkxSkxVkxT.For each pointχi of the rank sampling point set χi, the point set Yi of the output variable Y is obtained by nonlinear transformation. According to Yi, calculate the mean Y¯ and the covariance matrix PY.
(36)Y¯=12un∑i=12unYi,(37)PY=1ω∑i=12unri∗Yi−Y¯Yi−Y¯T.Among Equation (37), ri∗ is the covariance correction coefficient corresponding to the i-th sampling point; the covariance weight coefficient ω can be written as follows:
(38)ω=∑j=1,j≠u+12u+1rj2λpj2.


## 3.1.1. Principle of Rank Sampling

The rank sampling is a deterministic sampling method, which is based on the mean of input vectorX¯ and its covariance matrix Pkx, to calculate the mean Y¯ and covariance matrix Pky of the nonlinear function.Firstly, according to the mean valueX¯ and covariance matrix PX of input vector X. Computing the set of rank sampling points:χi:
(32)χi=X¯+rjλpjPkxl,(33)i=j−1n+lj=1,2,⋯ui=j−2n+lj=u+2,u+3,⋯2u+1.In Equation (32), χi is the i-th sampling point of X, with a total of 2un sampling points, n is the dimension of state variable X, u is the number of sampling layers, rj is the correction coefficient of sampling points in layer J, pj is the j-th sampling point, χi is the corresponding probability, λpj is the quantile of probability pj in the one-dimensional standard distribution probability of X distribution, and Pkxl is the l-th vector of the square root of covariance matrix Pkx. Due to the singular values that often appear in practical use, which leads to the decline of numerical stability, therefore, singular-value decomposition is adopted to improve the stability of the algorithm. The specific algorithm is as follows:
(34)Pkx=UkxSkx000VkxT.In Equation (34), S is a diagonal matrix, S=diags1,s2,⋯,sn. After SVD decomposition, the covariance matrix of Equation (34) can be written as follows:
(35)Pkx=UkxSkxVkxT.For each pointχi of the rank sampling point set χi, the point set Yi of the output variable Y is obtained by nonlinear transformation. According to Yi, calculate the mean Y¯ and the covariance matrix PY.
(36)Y¯=12un∑i=12unYi,(37)PY=1ω∑i=12unri∗Yi−Y¯Yi−Y¯T.Among Equation (37), ri∗ is the covariance correction coefficient corresponding to the i-th sampling point; the covariance weight coefficient ω can be written as follows:
(38)ω=∑j=1,j≠u+12u+1rj2λpj2.


## 3.2. Filter Equation

Let the estimated value of state vectorX at time k−1 be X^k−1, and the covariance matrix is Pk−1. According to the principle of rank sampling in Equation (32), we can get the set of sampling points χi. Therefore, the estimated value of X at time k is obtained X^kk−1:
(39)X^kk−1=12un∑i=12unΦχk−1,ii=1,2,⋯,2un.One-step prediction covariance matrix is:(40)Pkk−1=ri∗ω∑i=12unX~kk−1X~kk−1T+Qk−1.In Equation (40):
(41)X~kk−1=Xkk−1,i−X^kk−1.Measurement update: according to Equation (37), the new set of sampling points χkk−1,i is obtained by rank sampling again:
(42)χkk−1,i=X^kk−1+rjλpjPkk−1l.χkk−1,i is the i-th sampling point of X^kk−1, a total of 2un; Pkk−1l is the l-th column vector of Pkk−1. After SVD decomposition, the covariance matrix of Equation (34) can be written as follows:
(43)Pkk−1=Ukk−1Skk−1Vkk−1T.The set of sampling pointsχkk−1,i calculated by Equation (42) is introduced into the measurement equation and nonlinear propagation is carried out:
(44)Zkk−1,i=Hχkk−1,ii=1,2,⋯,2un.From Equation (44), we can get the estimated value Z^kk−1 of observation measurement:
(45)Z^kk−1=12un∑i=12unZkk−1,i.From Equations (44) and (45), we can get the covariance matrix of forecasting innovation:
(46)Pzz=ri∗ω∑i=12unZkk−1,i−Z^kk−1Zkk−1,i−Z∧kk−1T+Rk.From Equations (41), (44), and (45), the prediction cross-covariance matrix can be obtained:
(47)Pxz=ri∗ω∑i=12unχkk−1,i−X^kk−1χkk−1,i−X∧kk−1T.From Equations (46) and (47), the filter gain can be obtained:
(48)K=PxzPzz−1.Thek-time state estimation is:
(49)X^k=X^kk−1+KZk−Z^kk−1.The variance matrix ofk-time estimation error is as follows:
(50)Pk=Pkk−1−KPzzKT.


## 4. Simulation and Analysis

In order to verify the performance of the algorithm, this paper adopts the Monte Carlo simulation method. At the same time, two simulation scenarios are set. Firstly, the performance of the algorithm is verified when the random noise distribution of the system does not meet the Gaussian distribution. Then, on the basis that the random noise distribution does not meet the Gaussian distribution, the system noise error is further increased by simulating the inconsistency between the system model and the filter model. Finally, the performance of the algorithm is verified. This paper selects UKF as the comparison algorithm.


### 4.1. Simulation Parameter Setting

The initial longitude, latitude, and altitude of aerospace vehicles launch are118°,32°,100m, the initial yaw angle is 90°, the launching azimuth angle is 30°, the launch time is 0 h 0 min 0 s on November 15, 2020, the flight time is 300 s, and the filter period is 1 s. In Table 1, the actual system model random walk parameters are inconsistent with the filter random walk parameters in Equation (51), and the driving white noise parameters of the accelerometer are also different from the corresponding parameters in the filter. Therefore, the simulation conditions can be regarded as the inconsistency between the filtering model and the actual system model.Table 1
Error parameter setting.
Noise objectNoise typeNoise parametersUpdate rateGyroscopeRandom walk4°/h0.2 sWhite noise0.2°/hAccelerometerRandom walk5×10−2gGPSWhite noise15 m1 sCNSWhite noise21′′Filter parameters are set as follows:System noise variance:(51)Q=diag0.1°/h0.1°/h0.1°/h0.1°/h0.1°/h0.1°/h1×10−4g1×10−4g1×10−4g2.Measurement noise variance:(52)R=diag21″21″21″15m15m15m2.From Table1, it can be found that the actual system model parameters are inconsistent with the parameters in the filter. This simulation condition can be considered that there is an error in the filter model.


### 4.2. Simulation Analysis

#### 4.2.1. Non-Gaussian Random Noise

Based on the above simulation conditions, the flight path of aerospace vehicles is simulated, as shown in Figure3. The simulation flight path of aerospace vehicle includes vertical launch, constant speed rise, turning 45 degrees, and incline acceleration climbing. The actual flight trajectory of the aerospace vehicle is simulated. At the same time, we adjust the random errors of gyroscope and accelerometer to make them no longer conform to Gaussian distribution, to verify the effectiveness of this algorithm.Figure 3
Flight path of aerospace vehicles.Figures4 and 5 show the simulation results of the random error distribution of the gyroscope and accelerometer. The black line in the figure represents the Gaussian distribution curve, and the red part is the noise set in the simulation experiment in this paper. After adjustment, the random error noise distribution of the gyroscope and accelerometer obviously no longer conforms to the Gaussian distribution. It can also be seen from the data in Table 2 that the noise of the gyroscope and accelerometer set in this paper meets the characteristics of non-Gaussian noise in terms of mean and variance. In subsequent simulation experiments, this group of noise is used to verify the effectiveness of the algorithm in this paper.Figure 4
Gyroscope noise.Figure 5
Accelerator noise.Table 2
Random noise error distribution.
MeanVarianceSkewnessKurtosisGyroscope−6.3×10−74.8×10−71.15.3Accelerometer−1.9×10−49.8×10−415


#### 4.2.2. Navigation Parameter Error under Non-Gaussian Distribution

From Figures6–14, we can see that due to the addition of non-Gaussian noise, the errors of both UKF algorithm and LRF algorithm fluctuate. In the attitude error comparison curve, the roll angle error and pitch angle error estimated by UKF algorithm are greater than those estimated by the LRF algorithm, and the heading angle error estimated by UKF algorithm is significantly greater than that estimated by the LRF algorithm around 100 seconds. In the position error comparison curve, it shows the same performance as the attitude error comparison curve, that is, the UKF algorithm error is significantly greater than the LRF algorithm error. In this paper, the velocity of aerospace vehicle is calculated from its position, so the variation trend of velocity error is consistent with that of position error. Because the LRF algorithm designed in this paper can better simulate the non-Gaussian distribution of system random noise, it is more suitable for the practical engineering application of aerospace vehicles. From the statistical results of root mean square of navigation parameter estimation error in Table 3, when the navigation system noise does not meet the Gaussian distribution, the root mean square of navigation parameter estimation error of LRF algorithm is significantly smaller than that of the UKF algorithm, which indicates that the LRF algorithm has smaller estimation error, higher accuracy, and more stable when the navigation system noise does not meet the Gaussian distribution.Figure 6
Roll angle error contrast curves.Figure 7
Pitch angle error contrast curves.Figure 8
Yaw angle error contrast curves.Figure 9
X-axis position error contrast curves.Figure 10
Y-axis position error contrast curvesFigure 11
Z-axis position error contrast curvesFigure 12
X-axis velocity error contrast curvesFigure 13
Y-axis velocity error contrast curvesFigure 14
Z-axis velocity error contrast curves.Table 3
Root mean square of navigation parameter estimation error.
UKFLRFRoll angle/(arc-second)8.83738.1344Pitch angle/(arc-second)8.50717.6470Yaw angle/(arc-second)10.49088.9204X-axis position/(m)13.24186.7898Y-axis position/(m)11.85347.9474Z-axis position/(m)20.24925.2763X-axis velocity/(m/s)2.11540.7314Y-axis velocity/(m/s)1.75841.0181Z-axis velocity/(m/s)2.99090.5263


#### 4.2.3. System Model Inconsistency and Navigation Parameter Error under Non-Gaussian Distribution

Figures15–23 show the simulation analysis carried out on the basis that the navigation system noise obeys the non-Gaussian distribution and in combination with the actual flight environment of aerospace vehicles, which is prone to lead to inconsistency between the navigation system model and the filter model. Firstly, because the system noise still obeys the non-Gaussian distribution, the navigation parameter error estimated by the UKF algorithm is greater than that estimated by the LRF algorithm. Secondly, because of the inconsistency between the system model and the filter model, the size of the system noise will also change, resulting in the decline of the estimation accuracy of the algorithm. From the curve of estimation error, the estimation accuracy of LRF algorithm is significantly better than that of the UKF algorithm. Its filtering error is smaller, and the error curve is more stable. This is also verified by the statistical results of the estimated mean square error in Table 4. Since the inconsistency between the navigation system model and the filter model is increased, it can be seen by comparing the data in Tables 3 and 4 that the data in Table 4 are larger than those in Table 3, indicating that the system is more unstable. However, it can be seen from the root mean square of navigation parameter estimation error in Table 4 that the LRF algorithm proposed in this paper has a smaller root mean square of navigation parameter estimation error and higher estimation accuracy when the system noise does not meet the Gaussian distribution and the inconsistency between the navigation system model and the filter model, and it can adapt to more complex flight environment and has better robustness.Figure 15
Roll angle error contrast curves.Figure 16
Pitch angle error contrast curves.Figure 17
Yaw angle error contrast curves.Figure 18
X-axis position error contrast curvesFigure 19
Y-axis position error contrast curvesFigure 20
Z-axis position error contrast curvesFigure 21
X-axis velocity error contrast curvesFigure 22
Y-axis velocity error contrast curvesFigure 23
Z-axis velocity error contrast curvesTable 4
Root mean square of navigation parameter estimation error.
UKFLRFRoll angle/(arc-second)16.21788.4622Pitch angle/(arc-second)15.135210.9661Yaw angle/(arc-second)14.861511.7413X-axis position/(m)75.631016.4053Y-axis position/(m)53.035633.4729Z-axis position/(m)53.320124.4474X-axis velocity/(m/s)12.24982.9631Y-axis velocity/(m/s)8.82915.9694Z-axis velocity/(m/s)8.46884.3937


## 4.1. Simulation Parameter Setting

The initial longitude, latitude, and altitude of aerospace vehicles launch are118°,32°,100m, the initial yaw angle is 90°, the launching azimuth angle is 30°, the launch time is 0 h 0 min 0 s on November 15, 2020, the flight time is 300 s, and the filter period is 1 s. In Table 1, the actual system model random walk parameters are inconsistent with the filter random walk parameters in Equation (51), and the driving white noise parameters of the accelerometer are also different from the corresponding parameters in the filter. Therefore, the simulation conditions can be regarded as the inconsistency between the filtering model and the actual system model.Table 1
Error parameter setting.
Noise objectNoise typeNoise parametersUpdate rateGyroscopeRandom walk4°/h0.2 sWhite noise0.2°/hAccelerometerRandom walk5×10−2gGPSWhite noise15 m1 sCNSWhite noise21′′Filter parameters are set as follows:System noise variance:(51)Q=diag0.1°/h0.1°/h0.1°/h0.1°/h0.1°/h0.1°/h1×10−4g1×10−4g1×10−4g2.Measurement noise variance:(52)R=diag21″21″21″15m15m15m2.From Table1, it can be found that the actual system model parameters are inconsistent with the parameters in the filter. This simulation condition can be considered that there is an error in the filter model.


## 4.2. Simulation Analysis

### 4.2.1. Non-Gaussian Random Noise

Based on the above simulation conditions, the flight path of aerospace vehicles is simulated, as shown in Figure3. The simulation flight path of aerospace vehicle includes vertical launch, constant speed rise, turning 45 degrees, and incline acceleration climbing. The actual flight trajectory of the aerospace vehicle is simulated. At the same time, we adjust the random errors of gyroscope and accelerometer to make them no longer conform to Gaussian distribution, to verify the effectiveness of this algorithm.Figure 3
Flight path of aerospace vehicles.Figures4 and 5 show the simulation results of the random error distribution of the gyroscope and accelerometer. The black line in the figure represents the Gaussian distribution curve, and the red part is the noise set in the simulation experiment in this paper. After adjustment, the random error noise distribution of the gyroscope and accelerometer obviously no longer conforms to the Gaussian distribution. It can also be seen from the data in Table 2 that the noise of the gyroscope and accelerometer set in this paper meets the characteristics of non-Gaussian noise in terms of mean and variance. In subsequent simulation experiments, this group of noise is used to verify the effectiveness of the algorithm in this paper.Figure 4
Gyroscope noise.Figure 5
Accelerator noise.Table 2
Random noise error distribution.
MeanVarianceSkewnessKurtosisGyroscope−6.3×10−74.8×10−71.15.3Accelerometer−1.9×10−49.8×10−415


### 4.2.2. Navigation Parameter Error under Non-Gaussian Distribution

From Figures6–14, we can see that due to the addition of non-Gaussian noise, the errors of both UKF algorithm and LRF algorithm fluctuate. In the attitude error comparison curve, the roll angle error and pitch angle error estimated by UKF algorithm are greater than those estimated by the LRF algorithm, and the heading angle error estimated by UKF algorithm is significantly greater than that estimated by the LRF algorithm around 100 seconds. In the position error comparison curve, it shows the same performance as the attitude error comparison curve, that is, the UKF algorithm error is significantly greater than the LRF algorithm error. In this paper, the velocity of aerospace vehicle is calculated from its position, so the variation trend of velocity error is consistent with that of position error. Because the LRF algorithm designed in this paper can better simulate the non-Gaussian distribution of system random noise, it is more suitable for the practical engineering application of aerospace vehicles. From the statistical results of root mean square of navigation parameter estimation error in Table 3, when the navigation system noise does not meet the Gaussian distribution, the root mean square of navigation parameter estimation error of LRF algorithm is significantly smaller than that of the UKF algorithm, which indicates that the LRF algorithm has smaller estimation error, higher accuracy, and more stable when the navigation system noise does not meet the Gaussian distribution.Figure 6
Roll angle error contrast curves.Figure 7
Pitch angle error contrast curves.Figure 8
Yaw angle error contrast curves.Figure 9
X-axis position error contrast curves.Figure 10
Y-axis position error contrast curvesFigure 11
Z-axis position error contrast curvesFigure 12
X-axis velocity error contrast curvesFigure 13
Y-axis velocity error contrast curvesFigure 14
Z-axis velocity error contrast curves.Table 3
Root mean square of navigation parameter estimation error.
UKFLRFRoll angle/(arc-second)8.83738.1344Pitch angle/(arc-second)8.50717.6470Yaw angle/(arc-second)10.49088.9204X-axis position/(m)13.24186.7898Y-axis position/(m)11.85347.9474Z-axis position/(m)20.24925.2763X-axis velocity/(m/s)2.11540.7314Y-axis velocity/(m/s)1.75841.0181Z-axis velocity/(m/s)2.99090.5263


### 4.2.3. System Model Inconsistency and Navigation Parameter Error under Non-Gaussian Distribution

Figures15–23 show the simulation analysis carried out on the basis that the navigation system noise obeys the non-Gaussian distribution and in combination with the actual flight environment of aerospace vehicles, which is prone to lead to inconsistency between the navigation system model and the filter model. Firstly, because the system noise still obeys the non-Gaussian distribution, the navigation parameter error estimated by the UKF algorithm is greater than that estimated by the LRF algorithm. Secondly, because of the inconsistency between the system model and the filter model, the size of the system noise will also change, resulting in the decline of the estimation accuracy of the algorithm. From the curve of estimation error, the estimation accuracy of LRF algorithm is significantly better than that of the UKF algorithm. Its filtering error is smaller, and the error curve is more stable. This is also verified by the statistical results of the estimated mean square error in Table 4. Since the inconsistency between the navigation system model and the filter model is increased, it can be seen by comparing the data in Tables 3 and 4 that the data in Table 4 are larger than those in Table 3, indicating that the system is more unstable. However, it can be seen from the root mean square of navigation parameter estimation error in Table 4 that the LRF algorithm proposed in this paper has a smaller root mean square of navigation parameter estimation error and higher estimation accuracy when the system noise does not meet the Gaussian distribution and the inconsistency between the navigation system model and the filter model, and it can adapt to more complex flight environment and has better robustness.Figure 15
Roll angle error contrast curves.Figure 16
Pitch angle error contrast curves.Figure 17
Yaw angle error contrast curves.Figure 18
X-axis position error contrast curvesFigure 19
Y-axis position error contrast curvesFigure 20
Z-axis position error contrast curvesFigure 21
X-axis velocity error contrast curvesFigure 22
Y-axis velocity error contrast curvesFigure 23
Z-axis velocity error contrast curvesTable 4
Root mean square of navigation parameter estimation error.
UKFLRFRoll angle/(arc-second)16.21788.4622Pitch angle/(arc-second)15.135210.9661Yaw angle/(arc-second)14.861511.7413X-axis position/(m)75.631016.4053Y-axis position/(m)53.035633.4729Z-axis position/(m)53.320124.4474X-axis velocity/(m/s)12.24982.9631Y-axis velocity/(m/s)8.82915.9694Z-axis velocity/(m/s)8.46884.3937


## 4.2.1. Non-Gaussian Random Noise

Based on the above simulation conditions, the flight path of aerospace vehicles is simulated, as shown in Figure3. The simulation flight path of aerospace vehicle includes vertical launch, constant speed rise, turning 45 degrees, and incline acceleration climbing. The actual flight trajectory of the aerospace vehicle is simulated. At the same time, we adjust the random errors of gyroscope and accelerometer to make them no longer conform to Gaussian distribution, to verify the effectiveness of this algorithm.Figure 3
Flight path of aerospace vehicles.Figures4 and 5 show the simulation results of the random error distribution of the gyroscope and accelerometer. The black line in the figure represents the Gaussian distribution curve, and the red part is the noise set in the simulation experiment in this paper. After adjustment, the random error noise distribution of the gyroscope and accelerometer obviously no longer conforms to the Gaussian distribution. It can also be seen from the data in Table 2 that the noise of the gyroscope and accelerometer set in this paper meets the characteristics of non-Gaussian noise in terms of mean and variance. In subsequent simulation experiments, this group of noise is used to verify the effectiveness of the algorithm in this paper.Figure 4
Gyroscope noise.Figure 5
Accelerator noise.Table 2
Random noise error distribution.
MeanVarianceSkewnessKurtosisGyroscope−6.3×10−74.8×10−71.15.3Accelerometer−1.9×10−49.8×10−415


## 4.2.2. Navigation Parameter Error under Non-Gaussian Distribution

From Figures6–14, we can see that due to the addition of non-Gaussian noise, the errors of both UKF algorithm and LRF algorithm fluctuate. In the attitude error comparison curve, the roll angle error and pitch angle error estimated by UKF algorithm are greater than those estimated by the LRF algorithm, and the heading angle error estimated by UKF algorithm is significantly greater than that estimated by the LRF algorithm around 100 seconds. In the position error comparison curve, it shows the same performance as the attitude error comparison curve, that is, the UKF algorithm error is significantly greater than the LRF algorithm error. In this paper, the velocity of aerospace vehicle is calculated from its position, so the variation trend of velocity error is consistent with that of position error. Because the LRF algorithm designed in this paper can better simulate the non-Gaussian distribution of system random noise, it is more suitable for the practical engineering application of aerospace vehicles. From the statistical results of root mean square of navigation parameter estimation error in Table 3, when the navigation system noise does not meet the Gaussian distribution, the root mean square of navigation parameter estimation error of LRF algorithm is significantly smaller than that of the UKF algorithm, which indicates that the LRF algorithm has smaller estimation error, higher accuracy, and more stable when the navigation system noise does not meet the Gaussian distribution.Figure 6
Roll angle error contrast curves.Figure 7
Pitch angle error contrast curves.Figure 8
Yaw angle error contrast curves.Figure 9
X-axis position error contrast curves.Figure 10
Y-axis position error contrast curvesFigure 11
Z-axis position error contrast curvesFigure 12
X-axis velocity error contrast curvesFigure 13
Y-axis velocity error contrast curvesFigure 14
Z-axis velocity error contrast curves.Table 3
Root mean square of navigation parameter estimation error.
UKFLRFRoll angle/(arc-second)8.83738.1344Pitch angle/(arc-second)8.50717.6470Yaw angle/(arc-second)10.49088.9204X-axis position/(m)13.24186.7898Y-axis position/(m)11.85347.9474Z-axis position/(m)20.24925.2763X-axis velocity/(m/s)2.11540.7314Y-axis velocity/(m/s)1.75841.0181Z-axis velocity/(m/s)2.99090.5263


## 4.2.3. System Model Inconsistency and Navigation Parameter Error under Non-Gaussian Distribution

Figures15–23 show the simulation analysis carried out on the basis that the navigation system noise obeys the non-Gaussian distribution and in combination with the actual flight environment of aerospace vehicles, which is prone to lead to inconsistency between the navigation system model and the filter model. Firstly, because the system noise still obeys the non-Gaussian distribution, the navigation parameter error estimated by the UKF algorithm is greater than that estimated by the LRF algorithm. Secondly, because of the inconsistency between the system model and the filter model, the size of the system noise will also change, resulting in the decline of the estimation accuracy of the algorithm. From the curve of estimation error, the estimation accuracy of LRF algorithm is significantly better than that of the UKF algorithm. Its filtering error is smaller, and the error curve is more stable. This is also verified by the statistical results of the estimated mean square error in Table 4. Since the inconsistency between the navigation system model and the filter model is increased, it can be seen by comparing the data in Tables 3 and 4 that the data in Table 4 are larger than those in Table 3, indicating that the system is more unstable. However, it can be seen from the root mean square of navigation parameter estimation error in Table 4 that the LRF algorithm proposed in this paper has a smaller root mean square of navigation parameter estimation error and higher estimation accuracy when the system noise does not meet the Gaussian distribution and the inconsistency between the navigation system model and the filter model, and it can adapt to more complex flight environment and has better robustness.Figure 15
Roll angle error contrast curves.Figure 16
Pitch angle error contrast curves.Figure 17
Yaw angle error contrast curves.Figure 18
X-axis position error contrast curvesFigure 19
Y-axis position error contrast curvesFigure 20
Z-axis position error contrast curvesFigure 21
X-axis velocity error contrast curvesFigure 22
Y-axis velocity error contrast curvesFigure 23
Z-axis velocity error contrast curvesTable 4
Root mean square of navigation parameter estimation error.
UKFLRFRoll angle/(arc-second)16.21788.4622Pitch angle/(arc-second)15.135210.9661Yaw angle/(arc-second)14.861511.7413X-axis position/(m)75.631016.4053Y-axis position/(m)53.035633.4729Z-axis position/(m)53.320124.4474X-axis velocity/(m/s)12.24982.9631Y-axis velocity/(m/s)8.82915.9694Z-axis velocity/(m/s)8.46884.3937


## 5. Conclusions

As a new type of aircraft, aerospace vehicle has become research hotspot because of its advantages such as repeatable take-off and landing and large-range flight. In order to meet the high-precision and reliable flight requirements of aerospace vehicles, it is necessary to research the corresponding navigation algorithms based on their high dynamic and high-speed flight characteristics. Unlike the near ground vehicle, the aerospace vehicle can fly at both the aviation level and space level. Therefore, the traditional navigation algorithm based on the geographic coordinate system for the near ground vehicle is difficult to meet the accuracy requirements of the aerospace vehicle navigation system. Based on the previous research, this paper analyses the influence of accurate gravity model on the accuracy of aerospace vehicle navigation system. At the same time, it analyses the main factors that affect the modelling accuracy when converting from inertial coordinate system to earth fixed coordinate system, improves the accuracy of system modelling, and then provides high-precision system model and error model for subsequent filtering algorithms.On this basis, considering that the complex flight environment will bring unpredictability to the aerospace vehicle navigation system, at this time, the system error model is often inconsistent with the preset filter error model. So, the rank filter algorithm is adopted, the weight is determined by using the rank statistics correlation principle, and the sampling points are calculated; then, the nonlinear propagation is carried out through the transfer matrix. Finally, the predicted values of state variables and measurement variables and the covariance matrix are estimated accurately. In turn, it effectively simulates the probability distribution of system states.In this paper, the model and algorithm of aerospace vehicle navigation system are analysed based on the actual flight characteristics of aerospace vehicle. The experimental results show that the probability distribution of the actual system can be simulated to the greatest extent by using rank filter based on the accurate model, to improve the accuracy of the integrated navigation system and improve the reliability of the navigation system ultimately. In the future, the following research can be carried out on the basis of this paper. First, a more accurate navigation system model can be established. In addition to estimating and compensating the zero-bias error of the inertial navigation system, its installation error and scale coefficient error will also cause the inaccuracy of the system model. Therefore, it is necessary to further model the above errors to improve the accuracy of the system modelling. On this basis, the reasons for the inconsistency between the navigation system model and the filtering model are need to be analysed deeply, and the navigation error compensation rules are needed to be refined to improve the accuracy of the navigation system. In addition, for the case that the system noise does not obey the Gaussian distribution, the noise distribution can be analysed, and then the sampling points can be optimized based on the LRF algorithm proposed in this paper to improve the adaptability of the algorithm. This will also lay a foundation for the subsequent application and promotion of aerospace vehicles.



---

*Source: 1897256-2023-03-07.xml*