SINS / CNS Nonlinear Integrated Navigation Algorithm for Hypersonic Vehicle

Celestial Navigation System (CNS) has characteristics of accurate orientation and strong autonomy and has been widely used in Hypersonic Vehicle. Since the CNS location and orientation mainly depend upon the inertial reference that contains errors caused by gyro drifts and other error factors, traditional Strap-down Inertial Navigation System (SINS)/CNS positioning algorithm setting the position error between SINS and CNS as measurement is not effective. The model of altitude azimuth, platform error angles, and horizontal position is designed, and the SINS/CNS tightly integrated algorithm is designed, in which CNS altitude azimuth is set as measurement information. GPF (Gaussian particle filter) is introduced to solve the problem of nonlinear filtering.The results of simulation show that the precision of SINS/CNS algorithm which reaches 130m using three stars is improved effectively.


Introduction
Hypersonic Vehicle (HV) which refers to a vehicle flying at Mach 5 or above has already been the research focus in aeronautic and aerospace fields with its great strategic military application values [1,2].
Although Hypersonic Vehicle has many advantages, such as large flight envelope, high maneuverability, and well penetrability, the dynamic model of an HV is fast time varying and highly nonlinear because of its Mach numbers [3].Largescale variations of altitude and velocity lead to uncertainties in the aerodynamic parameters [4,5].As a result, HV is a highly nonlinear and uncertain system.Consequently, it is difficult to measure or estimate the dynamic state and characteristics of the vehicle [6].Autonomous navigation system with high accuracy and reliability has been a major constraint on the improvement in performance of HV.
In recent years, owing to the development of microelectronics and computer technology, as well as the accuracy improvement of Charge Coupled Device (CCD), Charge Inject Device (CID) star trackers, and inertial components, the Strap-down Inertial Navigation System (SINS) and Celestial Navigation System (CNS) are widely used in in aircrafts [7].
The celestial navigation method is a kind of autonomous navigation technology which can determine the vehicle's position and attitude [8].Since Celestial Navigation System (CNS) has characteristics of accurate orientation and strong autonomy, it has become an important component of integrated navigation system of HV [9].
The conventional celestial navigation utilizes the inertial navigation platform technology to realize the vertical vector and compute the vehicle's navigation information by measuring the relative position changes between the vertical vector and the celestial vector.The navigation accuracy of this method depends largely on the accuracy of horizontal reference and celestial sensor measurements [10].CNS usually gets the inertial horizon reference by inertial navigation platform.Considering that the strap-down type replacing the platform type has been the development trend of INS, it has become extremely difficult to improve the accuracy of the inertial horizon references due to the impact of INS core instruments (gyros and accelerometers) error [11].
In traditional SINS/CNS integrated mode, CNS utilizes the position and attitude information of INS to calculate celestial positions and heading attitude and then realize periodic correction of the INS drifts.This mode can damp  the divergence of INS position errors; however, since the CNS location and orientation mainly depend upon the inertial reference that contains errors caused by gyro drifts and other error factors, this postcorrection method is not effective.
A number of classical approaches, the Kalman filter (KF), extended Kalman filter (EKF), unscented Kalman filter (UKF), particle filtering (PF), and so forth, have been proposed to the information fusion.Kalman filtering is commonly used algorithms for information integration.A prerequisite for using Kalman filter is that the system dynamics and noise statistics are known [12].But considering the HV is a highly nonlinear system, the dynamic characteristics of the HV and external environment make these premise conditions often not met.
PF can effectively solve the problem of nonlinear filtering [13,14].However, its limitation is obvious.
(2) The number of particles will increase at a rapid rate along with the increase of the system dimensions.
The Gaussian PF avoids the PI problem that is the disadvantageous feature of PF in the estimation of a static parameter.Furthermore, resampling process is not required in the GPF algorithm.Therefore, its computational complexity (CC) is significantly reduced compared to particle filtering.
This paper will carry out the research on the SINS/CNS integrated navigation algorithm for the HV.In order to improve the accuracy and reliability of SINS/CNS integrated systems, the scheme and algorithm of airborne SINS/CNS integrated navigation based on celestial angle observation have been presented.The theory of SINS/CNS integrated navigation system based on celestial altitude angle observation information has been discussed adequately; a model with celestial altitude angle, platform error angles, and horizontal position is deduced.Meanwhile, a new SINS/CNS tightly integrated localization algorithm using Gaussian particle filter (GPF) is presented, which makes full use of SINS and CNS navigation information to achieve higher accuracy of the SINS/CNS integration.

The Model of CNS Measurement.
The basic principle of astronomical positioning is given by where ℎ  is the observed altitude angle of the navigational stars in Local-Level Frame.,  are longitude and latitude;   ,   are the declination and Greenwich hour angle.
According to (1), using longitude and latitude information from SINS, the computed altitude angle ℎ  in Local-Level Frame is received as where   ,   are latitude and longitude from SINS, and By using first order approximation of Taylor expansion, (3) can be expressed as According to (1) and ( 4), the relation between computed altitude angle ℎ  and observed altitude angle ℎ  can be expressed as follows: where Since CNS gets the inertial horizon reference by SINS, we can not get the observed altitude angle in Local-Level Frame in (1) but the observed altitude angle ℎ  in Computational Frame.
The celestial vector in Local-Level Frame can be set as The celestial vector in Computational-Level Frame can be set as The relationship between Computational Frame and Local-Level Frame can be expressed as follows: where ], and   ,   ,   are east, north, and up platform angle error.From ( 7)∼( 9), According to ( 5) and (10), the relationship between computed altitude angle ℎ  in Local-Level Frame and the observed altitude angle ℎ  in Computational Frame can be expressed as follows:

Equation of State of SINS/CNS Integrated Algorithm.
The navigation frame is chosen as east-north-upward geographic frame.The -- direction of the body frame is coincident with the Right-Front-Upward direction of the vehicle body.The state equations are described as where X() is the state variables vector, and ,   ,   are single-order Markov errors of three gyros, ∇  , ∇  , ∇  are single-order Markov errors of three accelerometers, and F is the system matrix, W() is the system noise matrix, and The attitude equation in the form of a quaternion is written as where the vehicle body angular rate measured by gyros in the body frame, =    +    , and    is the projection of the earth rotation rate in the navigation frame.    is angular rate of the navigation frame with respect to the earth frame, expressed in the navigation frame: The definition of the quaternion error between real quaternion and calculated quaternion is where Q = [ 0 ,  1 ,  2 ,  3 ]  , Q  is calculated quaternion, and Q   is real quaternion.The dynamic vector equation of quaternion error can be presented as where    =    +    , and Dynamic vector equation of velocity errors can be expressed as where V = [  ,   ,   ], and f represents the specific force measured by the three accelerometers in the body frame.
Dynamic equation of position errors can be expressed as where   ,   are the earth curvature radius.From (11), the measurement equation with one navigational star can be set as If there is more than one navigational star, the measurement equation can be set as where H() is the matrix that reflects the relations between X() and Z(), V() is the noise vector of the measurement information, and According to ( 12) and (24), the system equations are described as (26)

SINS/CNS Gaussian Particle Filter Algorithm
The GPF approximates the filtering and predictive densities by Gaussian distributions in a PF framework.The GPF recursively updates only the posterior mean and the covariance of the parameter of interest.The basic idea of PF is to represent a density by generated samples and their associated weights.
The following shows the approximation of a filtering density by particles and their weights.
The equations of nonlinear system model are defined as follows: GPF can be described as the following two steps.
Measurement Update.Generate the sample particles following where (⋅) is the importance sampling function,  = 1 : , and  is the particle number.
And the weights can be approximated by the following: where (⋅) is the filtering density, and   , Σ  are obtained by sample mean and covariance of the particles generated in the previous time update step.
Then compute the   , Σ  following (30) Time Update.Generate x  |+1 and compute x  +1|+1 : x  |+1 ∼  (x   ;   ; Σ  ) , Then compute the  +1 , Σ +1 following According to (26), the state equation is nonlinear and the measurement equation is linear; the measurement update of GPF can be estimated by Kalman filter.Thus, (29) and (30) can be simplified as follows: The GPF flow chart for (26) is given as follows.

Simulation and Analysis
The proposed algorithm is testified by the designed track.The drift of equivalent gyros in inertial navigation system is set to 0.1 ∘ /h, the drift of equivalent accelerometer is 10 −4 g, and the period of SINS algorithm is 5 ms.The navigation stars are Alioth, Arcturus, and Dubhe.The error of star sensor is 10 arc-second, and the period of CNS is 1.0 s; the period of the GPF is 1.0 s.The track is set as showed in Figure 1.The longitude is set as showed in Figure 2.
The errors of longitude and latitude are given as in Figures 3 and 4.
In Figures 3 and 4, "A" represents the results of one navigation star and "B" and "C" represent two and three navigation stars.
In Figures 3 and 4, when there are more navigation stars, the position error is smaller.The mean square error average is shown in Table 1.
Test results indicate that the precision of position is about 1100 m with one star.The precision of position using three stars can reach 130 m, which indicate that the method in the paper can improve the precision of navigation system effectively.
The comparison of GPF and KF using three stars is given in Figures 5 and 6.
Through Figures 5 and 6, both KF and GPF can convergence fast, but the accuracy of GPF is higher, and the steady accuracy is less influenced by the carrier mobility.

Conclusion
The SINS/CNS navigation system is an important autonomous navigation technology of HV for its accurate orientation and strong autonomy.Traditional SINS/CNS integrated mode, which uses celestial positions correcting the SINS drifts, is not effective because the CNS location mainly depends upon the inertial reference that contains errors caused by gyro drifts.In this paper, the theory of SINS/CNS integrated navigation system based on celestial angle observation information has been proposed for HV.The model with altitude angle, platform error angles, and horizontal position is deduced; the SINS/CNS tightly integrated localization algorithm using GPF is presented.The SINS/CNS algorithm is of important value in engineering application.

Table 1 :
Comparison of SINS/CNS navigation error.