# SINS/CNS Nonlinear Integrated Navigation Algorithm for Hypersonic Vehicle

Authors: Yong-jun Yu; Jin-fa Xu; Zhi Xiong
Journal: Mathematical Problems in Engineering
 (2015)

---

## Abstract

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 130 m using three stars is improved effectively.


---

## Body

## 1. 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]. Large-scale 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.(1)
PF occasionally has the particle impoverishment (PI) problem that results from resampling process [15].(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.


## 2. SINS/CNS Tightly Integrated Navigation Model

### 2.1. The Model of CNS Measurement

The basic principle of astronomical positioning is given by(1)sinhp=sinδAsinL+cosδAcosLcostG+λ,where hp is the observed altitude angle of the navigational stars in Local-Level Frame. λ, L are longitude and latitude; δA, tG are the declination and Greenwich hour angle.According to (1), using longitude and latitude information from SINS, the computed altitude angle hl in Local-Level Frame is received as(2)sinhl=sinLssinδA+cosLscosδAcosλs+tG,where Ls, λs are latitude and longitude from SINS, and(3)hl=hp+δh,Ls=L+δL,λs=λ+δλ.By using first order approximation of Taylor expansion, (3) can be expressed as(4)sinhl=sinLsinδA+cosδAcosLcostG+λ+cosLsinδA-cosδAcostGsinLcosλ+cosδAsintGsinλsinL·ΔL-cosδAsintGcosLcosλ+cosδAcostGcosLsinλ·Δλ.According to (1) and (4), the relation between computed altitude angle hl and observed altitude angle hp can be expressed as follows:(5)sinhl-sinhp=α1·Δλ+β1·ΔL,where(6)α1=-cosδAcostGsinλcosL-cosδAsintGcosλcosL,β1=sinδAcosL-cosδAcostGcosλsinL+cosδAsintGsinλsinL.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 hc in Computational Frame.The celestial vector in Local-Level Frame can be set as(7)Xp=coshpsinApcoshpcosApsinhpT.The celestial vector in Computational-Level Frame can be set as(8)Xc=coshcsinAccoshccosAcsinhcT.The relationship between Computational Frame and Local-Level Frame can be expressed as follows:(9)XP=CcpXc,where Ccp=[1-ϕuϕnϕu1-ϕe-ϕnϕe1], and ϕe, ϕn, ϕu are east, north, and up platform angle error.From (7)~(9),(10)sinhp=sinhc-ϕn×coshcsinAc+ϕe×coshccosAc.According to (5) and (10), the relationship between computed altitude angle hl in Local-Level Frame and the observed altitude angle hc in Computational Frame can be expressed as follows:(11)sinhl-sinhc=α1·Δλ+β1·ΔL-ϕn×coshcsinAc+ϕe×coshccosAc.


### 2.2. Equation of State of SINS/CNS Integrated Algorithm

The navigation frame is chosen as east-north-upward geographic frame. TheX-Y-Z direction of the body frame is coincident with the Right-Front-Upward direction of the vehicle body.The state equations are described as(12)X˙t=FXt,Wt,where X(t) is the state variables vector, and(13)Xt=δq0,δq1,δq2,δq3δVEδVNδVUδLδλδhεbxεbyεbz∇x∇y∇zTεbx, εby, εbz are single-order Markov errors of three gyros, ∇ax, ∇ay, ∇az are single-order Markov errors of three accelerometers, and (14)δω˙ibb=ε˙bxε˙byε˙bzT=-1Tωδωibb+σωt,∇˙b=∇˙x∇˙y∇˙zT=-1T∇∇˙b+σ∇t.F is the system matrix, W(t) is the system noise matrix, and Wt=σωtσ∇t.The attitude equation in the form of a quaternion is written as(15)Q˙bn=12Qbnωnbb=12ωibbQbn-12ωinnQbn,where Qbn=q0q1q2q3T, ωibb the vehicle body angular rate measured by gyros in the body frame, (16)ωibb=0-ωx-ωy-ωzωx0ωz-ωyωy-ωz0ωxωzωy-ωx0,ω
i
n
n
=
ω
i
e
n
+
ω
e
n
n, and ωien is the projection of the earth rotation rate in the navigation frame. ωenn is angular rate of the navigation frame with respect to the earth frame, expressed in the navigation frame:(17)ωinn=0-ωe-ωn-ωuωe0-ωuωnωnωu0-ωeωu-ωnωe0.The definition of the quaternion error between real quaternion and calculated quaternion is(18)δQ=Q^bn-Qbn,where δQ=δq0,δq1,δq2,δq3T, Q^bn is calculated quaternion, and Qbn is real quaternion.The dynamic vector equation of quaternion error can be presented as(19)δQ˙=12ωibbδQ-12ωinnδQ+12UQδωibb-12YQδωinn,where δωinn=δωien+δωenn, and(20)YQ=-q1-q2-q3q0q3-q2-q3q0q1q2-q1q0,UQ=-q1-q2-q3q0-q3q2q3q0-q1-q2q1q0.Dynamic vector equation of velocity errors can be expressed as(21)δV˙=ΔQbnf^b+Qbn∇b-2ωien+ωenn×δV+V+δV×2δωien+δωenn,where δV=δVE,δVN,δVU, and f^b represents the specific force measured by the three accelerometers in the body frame.Dynamic equation of position errors can be expressed as(22)δL˙=δVNRM+h-VNRM+h2δh,δλ˙=δVERN+hsecL+VERN+htgLsecLδL-VEsecLδhRN+h2,δh˙=δVU,where RM, RN are the earth curvature radius.From (11), the measurement equation with one navigational star can be set as(23)Zt=sinhl-sinhc.If there is more than one navigational star, the measurement equation can be set as(24)Zt=sinhl1-sinhc1⋮sinhln-sinhcn=HtXt+Vt,where H(t) is the matrix that reflects the relations between X(t) and Z(t), V(t) is the noise vector of the measurement information, and(25)Vt=sinδh1⋮sinδhn.According to (12) and (24), the system equations are described as (26)X˙t=FXt,Wt,Zt=HtXt+Vt.


## 2.1. The Model of CNS Measurement

The basic principle of astronomical positioning is given by(1)sinhp=sinδAsinL+cosδAcosLcostG+λ,where hp is the observed altitude angle of the navigational stars in Local-Level Frame. λ, L are longitude and latitude; δA, tG are the declination and Greenwich hour angle.According to (1), using longitude and latitude information from SINS, the computed altitude angle hl in Local-Level Frame is received as(2)sinhl=sinLssinδA+cosLscosδAcosλs+tG,where Ls, λs are latitude and longitude from SINS, and(3)hl=hp+δh,Ls=L+δL,λs=λ+δλ.By using first order approximation of Taylor expansion, (3) can be expressed as(4)sinhl=sinLsinδA+cosδAcosLcostG+λ+cosLsinδA-cosδAcostGsinLcosλ+cosδAsintGsinλsinL·ΔL-cosδAsintGcosLcosλ+cosδAcostGcosLsinλ·Δλ.According to (1) and (4), the relation between computed altitude angle hl and observed altitude angle hp can be expressed as follows:(5)sinhl-sinhp=α1·Δλ+β1·ΔL,where(6)α1=-cosδAcostGsinλcosL-cosδAsintGcosλcosL,β1=sinδAcosL-cosδAcostGcosλsinL+cosδAsintGsinλsinL.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 hc in Computational Frame.The celestial vector in Local-Level Frame can be set as(7)Xp=coshpsinApcoshpcosApsinhpT.The celestial vector in Computational-Level Frame can be set as(8)Xc=coshcsinAccoshccosAcsinhcT.The relationship between Computational Frame and Local-Level Frame can be expressed as follows:(9)XP=CcpXc,where Ccp=[1-ϕuϕnϕu1-ϕe-ϕnϕe1], and ϕe, ϕn, ϕu are east, north, and up platform angle error.From (7)~(9),(10)sinhp=sinhc-ϕn×coshcsinAc+ϕe×coshccosAc.According to (5) and (10), the relationship between computed altitude angle hl in Local-Level Frame and the observed altitude angle hc in Computational Frame can be expressed as follows:(11)sinhl-sinhc=α1·Δλ+β1·ΔL-ϕn×coshcsinAc+ϕe×coshccosAc.


## 2.2. Equation of State of SINS/CNS Integrated Algorithm

The navigation frame is chosen as east-north-upward geographic frame. TheX-Y-Z direction of the body frame is coincident with the Right-Front-Upward direction of the vehicle body.The state equations are described as(12)X˙t=FXt,Wt,where X(t) is the state variables vector, and(13)Xt=δq0,δq1,δq2,δq3δVEδVNδVUδLδλδhεbxεbyεbz∇x∇y∇zTεbx, εby, εbz are single-order Markov errors of three gyros, ∇ax, ∇ay, ∇az are single-order Markov errors of three accelerometers, and (14)δω˙ibb=ε˙bxε˙byε˙bzT=-1Tωδωibb+σωt,∇˙b=∇˙x∇˙y∇˙zT=-1T∇∇˙b+σ∇t.F is the system matrix, W(t) is the system noise matrix, and Wt=σωtσ∇t.The attitude equation in the form of a quaternion is written as(15)Q˙bn=12Qbnωnbb=12ωibbQbn-12ωinnQbn,where Qbn=q0q1q2q3T, ωibb the vehicle body angular rate measured by gyros in the body frame, (16)ωibb=0-ωx-ωy-ωzωx0ωz-ωyωy-ωz0ωxωzωy-ωx0,ω
i
n
n
=
ω
i
e
n
+
ω
e
n
n, and ωien is the projection of the earth rotation rate in the navigation frame. ωenn is angular rate of the navigation frame with respect to the earth frame, expressed in the navigation frame:(17)ωinn=0-ωe-ωn-ωuωe0-ωuωnωnωu0-ωeωu-ωnωe0.The definition of the quaternion error between real quaternion and calculated quaternion is(18)δQ=Q^bn-Qbn,where δQ=δq0,δq1,δq2,δq3T, Q^bn is calculated quaternion, and Qbn is real quaternion.The dynamic vector equation of quaternion error can be presented as(19)δQ˙=12ωibbδQ-12ωinnδQ+12UQδωibb-12YQδωinn,where δωinn=δωien+δωenn, and(20)YQ=-q1-q2-q3q0q3-q2-q3q0q1q2-q1q0,UQ=-q1-q2-q3q0-q3q2q3q0-q1-q2q1q0.Dynamic vector equation of velocity errors can be expressed as(21)δV˙=ΔQbnf^b+Qbn∇b-2ωien+ωenn×δV+V+δV×2δωien+δωenn,where δV=δVE,δVN,δVU, and f^b represents the specific force measured by the three accelerometers in the body frame.Dynamic equation of position errors can be expressed as(22)δL˙=δVNRM+h-VNRM+h2δh,δλ˙=δVERN+hsecL+VERN+htgLsecLδL-VEsecLδhRN+h2,δh˙=δVU,where RM, RN are the earth curvature radius.From (11), the measurement equation with one navigational star can be set as(23)Zt=sinhl-sinhc.If there is more than one navigational star, the measurement equation can be set as(24)Zt=sinhl1-sinhc1⋮sinhln-sinhcn=HtXt+Vt,where H(t) is the matrix that reflects the relations between X(t) and Z(t), V(t) is the noise vector of the measurement information, and(25)Vt=sinδh1⋮sinδhn.According to (12) and (24), the system equations are described as (26)X˙t=FXt,Wt,Zt=HtXt+Vt.


## 3. 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:(27)xk=fxk-1,ωk-1,zk=hxk,vk.GPF can be described as the following two steps.Measurement Update. Generate the sample particles following (28)xki~qxki∣z0:k,where q(·) is the importance sampling function, i=1:M, and M is the particle number.And the weights can be approximated by the following:(29)ωki∝pzk∣xkiNxki;μk;Σkqxki∣z0:k,where p(·) is the filtering density, and μk, Σk are obtained by sample mean and covariance of the particles generated in the previous time update step.Then compute theμ-k, Σ-k following (30)μ-k=∑i=1Mωkixki,Σ-k=∑i=1Mωkiμk-xkiμk-xkiT.Time Update. Generate xk|k+1i and compute xk+1|k+1i:(31)xk|k+1i~Nxki;μ-k;Σ-k,xk+1|k+1i~pxk+1∣xk|k+1i.Then compute theμ-k+1, Σ-k+1 following (32)μ-k+1=1M∑i=1Mxk+1i,Σ-k+1=1M∑i=1Mμk+1-xk+1iμk+1-xk+1iT.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:(33)Kk=ΣkHkTHkΣkHkT+Rk-1,μ-k=μk+Kkzk-Hkμk,Σ-k=I-KkHkΣk.The GPF flow chart for (26) is given as follows.Step 1.
Consider(34)xki~qxki∣z0:k=Nxki;μk;Σk.Step 2.
Consider(35)Kk=ΣkHkTHkΣkHkT+Rk-1,μ-k=μk+Kkzk-Hkμk,Σ-k=I-KkHkΣk.Step 3.
Consider(36)xk|k+1i~Nxki;μ-k;Σ-k,xk+1|k+1i~pxk+1∣xk|k+1i.Step 4.
Consider(37)μ-k+1=1M∑i=1Mxk+1i,Σ-k+1=1M∑i=1Mμk+1-xk+1iμk+1-xk+1iT.


## 4. 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 Figure1. The longitude is set as showed in Figure 2.Figure 1
Simulation trace.Figure 2
Simulation longitude.The errors of longitude and latitude are given as in Figures3 and 4.Figure 3
Compression of longitude error.Figure 4
Compression of latitude error.In Figures3 and 4, “A” represents the results of one navigation star and “B” and “C” represent two and three navigation stars.In Figures3 and 4, when there are more navigation stars, the position error is smaller. The mean square error average is shown in Table 1.Table 1
Comparison of SINS/CNS navigation error.
Error
δ
v/m/s
δ
p/m
δ
v
e
δ
v
n
δ
λ
δ
L
One star
0.21
-
0.76
737.7
842.6
Two stars
0.13
-
0.38
161.2
89.5
Three stars
-
0.10
-
0.10
108.9
72.6Test 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 Figures5 and 6.Figure 5
Compression of longitude error.Figure 6
Compression of latitude error.Through Figures5 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.


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



---

*Source: 903054-2015-05-20.xml*