# SINS/BDS Integrated Navigation for Hypersonic Boost-Glide Vehicles in the Launch-Centered Inertial Frame

Authors: Kai Chen; Fuqiang Shen; Jun Zhou; Xiaofeng Wu
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

According to the trajectory specialty of hypersonic boost-glide vehicles, a strapdown inertial navigation system/BeiDou navigation satellite system (SINS/BDS) algorithm based on the launch-centered inertial (LCI) frame for hypersonic vehicles is proposed. First, the related frame system, especially the launch earth-centered inertial (LECI) frame, and the SINS mechanization in the LCI frame are introduced. Second, SINS discrete updating algorithms in the LCI frame for the compensation of coning, sculling, and scrolling effects are deduced in the attitude, velocity, and position updating algorithms, respectively. Subsequently, the Kalman filter of the SINS/BDS integrated navigation in the LCI frame is obtained. The method of converting BDS receiver position and velocity from the Earth-centered Earth-fixed (ECEF) frame to the LCI frame is deduced through the LECI frame. Finally, taking the typical hypersonic boost-glide vehicles as the object, the SINS/BDS algorithm vehicle field test and hardware-in-the-loop simulation are performed.


---

## Body

## 1. Introduction

A hypersonic vehicle is an aircraft that has the ability to fly and cruise for a long time at more than five times the speed of sound in near space; it is also called a “near-space hypersonic vehicle” [1]. The near space is the atmospheric space 20–100 km from the ground, and it is located below the low-orbit satellite operation area and above the traditional aircraft flight area. It has great military and civilian application value [2, 3].Scholars from all over the world have studied many different navigation frames for different hypersonic vehicle navigation algorithms [4]. Stephen studied the strapdown algorithm in the earth-centered inertial frame for the SHEFEX2 hypersonic vehicle [5]. Mu et al. studied an integrated navigation algorithm of hypersonic aircraft in the ECI frame [6]. Wei et al. studied a hypersonic vehicle GFSINS/GPS/CNS algorithm using the ECEF frame [7]. Yu et al. used the NEU geography coordinate system as the hypersonic vehicle navigation reference system [8]. The X-43A used an INS/GPS integrated navigation system and the geography coordinate system navigation algorithm [9]. Kang et al. studied an INS/BDS integrated navigation method in the ENU geography coordinate system [10]. Li studied the aerial alignment method of the integrated navigation system in the local horizontal coordinate system [11].Because a hypersonic boost-glide vehicle is launched vertically, its trajectory is similar to that of the launch vehicle. If the local horizontal frame is used as the navigation coordinate system of the hypersonic boost-glide vehicle, the attitude angle will be singular during the navigation calculation process. Therefore, the LCI frame is often used as the navigation coordinate system of the launch vehicle, which will not cause the attitude angle to be singular, making the LCI frame more advantageous over the local horizontal frame [4, 12, 13]. According to the navigation solutions in the LCI frame, scholars from all over the world have designed some hypersonic vehicle flight control algorithms. Zhang et al. studied stellar-inertial integrated guidance to responsive launch vehicles in an LCI frame [12]. Zhao et al. built an INS/CNS/GPS system in the LCI frame for vehicles for reducing the effects of an imprecise Earth model and completing long-duration maneuvers within a wide flight envelope [13]. Our group proposed an algorithm of SINS in the LCI frame for hypersonic boost-glide vehicles and presented a solution in the local horizontal frame from the LCI frame.In the near-space hypersonic flight, factors such as aerodynamic disturbance, scramjet operation, and steering gear operation affect the accuracy of the SINS of hypersonic vehicles. The Chinese BDS navigation system has become a world-class global navigation satellite system (GNSS) [14, 15]. The application of BDS is less in hypersonic vehicles. In the present study, based on the high-dynamic characteristics of hypersonic vehicles, we propose updating algorithms for the attitude, velocity, and position of strapdown inertial navigation as well as a SINS/BDS integrated navigation system in the LCI frame. In addition, we conduct a test on the SINS/BDS integrated navigation vehicle and hardware-in-the-loop (HWIL) simulation of the LCI frame.


## 2. SINS Mechanization in Launch-Centered Inertial Frame

### 2.1. Common Frame Systems

In the SINS, the commonly used frame systems include the Earth-centered inertial frame (ECI,i frame), Earth-centered Earth-fixed frame (ECEF, e frame), launch Earth-centered inertial frame (LECI, t frame), launch-centered Earth-fixed frame (LCEF, g frame), launch-centered inertial frame (LCI, a frame), and body-fixed frame (BF, b frame) [4]. This study uses the frame systems described below and the LCI frame as the navigation frame system.


### 2.2. Relationship between Frame Systems

#### 2.2.1. LCI Frame and BF Frame

The direction cosine matrix (DCM) from the LCI frame to the BF frame isRab, as shown in(1)Rab=RxγRyψRzφ,where φ, ψ, and γ are the vehicle pitch, yaw, and roll angles in the LCI frame, respectively. Riθ is the DCM rotating around the i-axis (i = x, y, z) by angle θ.


#### 2.2.2. LCEF Frame and ECEF Frame

The DCM from the ECEF frame to the LECF frame isReg, as shown in(2)Reg=Ry−90o−α0Rxϕ0Rzλ0−90o,where ϕ0, λ0, and α0 are the initial latitude, initial longitude, and launch azimuth of the vehicle. In addition, at the time of launch, the coordinates of the LCEF, and LCI frames are in the same direction, and the coordinates of the LECI and ECEF frames are in the same direction. Therefore, the DCMs Rta from the LECI frame to the LCI frame and Reg are the same. During flight, Rta and Reg are constant values.


### 2.3. Initial Value of Launch Time

#### 2.3.1. Initial Values of the Launch Point

The launch point position in the ECEF frame isP0e=xe0,ye0,ze0T, as given by(3)xe0=N0+H0cosϕ0cosλ0,ye0=N0+H0cosϕ0sinλ0,ze0=N01−e2+H0sinϕ0,where H0 is the initial height, N0=ae/1−e2sin2L0, e is the eccentricity, and ae is the semimajor axis.At the time of the launch, the coordinates of the LECI and ECEF frames point in the same direction, soP0t=P0e.


#### 2.3.2. Geocentric Vector of the Launch Point

The geocentric vector of the launch point in the LCEF frame isP0eg:(4)Pe0g=RegP0e.At the time of launch, the coordinates of the LCEF and LCI frames point in the same direction, soP0ea=P0eg.


#### 2.3.3. Rotational Angular Velocity in the LCI Frame

The Earth’s rotation angular velocity in the LECF frame relative to the LCI frame isωagg.(5)ωagg=ωiecosϕ0cosα0sinϕ0−cosϕ0sinα0=ωieωe0.


### 2.4. SINS Mechanization in the LCI Frame

The navigation equation of the SINS in the LCI frame is given by the following equation, and its mechanization is shown in Figure1:(6)P˙aV˙aR˙ba=VaRbafb+gaRbaΩabb.Figure 1
Mechanization of SINS in the LCI frame.As shown in (6) and Figure 1, the SINS algorithm is executed by the navigation computer, and steps involved are as follows. First, the vehicle attitude is obtained by integrating angular velocity ωb measured by gyroscopes, and then attitude matrix Rba is obtained. Second, specific force fb measured by accelerometers is converted to the LCI frame through attitude matrix Rba; considering the effect of gravity ga, velocity Va of the vehicle is obtained by integrating the sum of fa and ga in the LCI frame. Finally, the position Pa of the vehicle is obtained by integrating velocity Va, and Pg=x,y,zT. These three integral processes are termed SINS attitude updating, velocity updating, and position updating, respectively.The formula for calculating gravity in the LCI frame is found in reference:(7)ga=gr′r0+gωeωe0,where gr′ is calculated by (8) and gωe is calculated by (9). r0 is the unit vector of r, and r is calculated by (10):(8)gr′=−μr21+Jaer21−5sin2ϕ,(9)gωe=−2μr2Jaer2sinϕ,(10)r=P0ea+Pa.In these equations,μ is the coefficient of gravity; J=3/2J2, where J2 is the harmonic coefficient; r=x2+y2+z2; ϕ is the Earth-centered latitude, ϕ=arcsinrw/r∗ωie; and rw=r⋅ωe.The initial value of positionPa is P0a=0,0,0T, and the initial value of attitude matrix Rab is determined by the initial attitude angles φ0, ψ0, and γ0 in the initial alignment. The initial value V0a is obtained as follows:(11)V0a=ωagg×P0eg.


## 2.1. Common Frame Systems

In the SINS, the commonly used frame systems include the Earth-centered inertial frame (ECI,i frame), Earth-centered Earth-fixed frame (ECEF, e frame), launch Earth-centered inertial frame (LECI, t frame), launch-centered Earth-fixed frame (LCEF, g frame), launch-centered inertial frame (LCI, a frame), and body-fixed frame (BF, b frame) [4]. This study uses the frame systems described below and the LCI frame as the navigation frame system.


## 2.2. Relationship between Frame Systems

### 2.2.1. LCI Frame and BF Frame

The direction cosine matrix (DCM) from the LCI frame to the BF frame isRab, as shown in(1)Rab=RxγRyψRzφ,where φ, ψ, and γ are the vehicle pitch, yaw, and roll angles in the LCI frame, respectively. Riθ is the DCM rotating around the i-axis (i = x, y, z) by angle θ.


### 2.2.2. LCEF Frame and ECEF Frame

The DCM from the ECEF frame to the LECF frame isReg, as shown in(2)Reg=Ry−90o−α0Rxϕ0Rzλ0−90o,where ϕ0, λ0, and α0 are the initial latitude, initial longitude, and launch azimuth of the vehicle. In addition, at the time of launch, the coordinates of the LCEF, and LCI frames are in the same direction, and the coordinates of the LECI and ECEF frames are in the same direction. Therefore, the DCMs Rta from the LECI frame to the LCI frame and Reg are the same. During flight, Rta and Reg are constant values.


## 2.2.1. LCI Frame and BF Frame

The direction cosine matrix (DCM) from the LCI frame to the BF frame isRab, as shown in(1)Rab=RxγRyψRzφ,where φ, ψ, and γ are the vehicle pitch, yaw, and roll angles in the LCI frame, respectively. Riθ is the DCM rotating around the i-axis (i = x, y, z) by angle θ.


## 2.2.2. LCEF Frame and ECEF Frame

The DCM from the ECEF frame to the LECF frame isReg, as shown in(2)Reg=Ry−90o−α0Rxϕ0Rzλ0−90o,where ϕ0, λ0, and α0 are the initial latitude, initial longitude, and launch azimuth of the vehicle. In addition, at the time of launch, the coordinates of the LCEF, and LCI frames are in the same direction, and the coordinates of the LECI and ECEF frames are in the same direction. Therefore, the DCMs Rta from the LECI frame to the LCI frame and Reg are the same. During flight, Rta and Reg are constant values.


## 2.3. Initial Value of Launch Time

### 2.3.1. Initial Values of the Launch Point

The launch point position in the ECEF frame isP0e=xe0,ye0,ze0T, as given by(3)xe0=N0+H0cosϕ0cosλ0,ye0=N0+H0cosϕ0sinλ0,ze0=N01−e2+H0sinϕ0,where H0 is the initial height, N0=ae/1−e2sin2L0, e is the eccentricity, and ae is the semimajor axis.At the time of the launch, the coordinates of the LECI and ECEF frames point in the same direction, soP0t=P0e.


### 2.3.2. Geocentric Vector of the Launch Point

The geocentric vector of the launch point in the LCEF frame isP0eg:(4)Pe0g=RegP0e.At the time of launch, the coordinates of the LCEF and LCI frames point in the same direction, soP0ea=P0eg.


### 2.3.3. Rotational Angular Velocity in the LCI Frame

The Earth’s rotation angular velocity in the LECF frame relative to the LCI frame isωagg.(5)ωagg=ωiecosϕ0cosα0sinϕ0−cosϕ0sinα0=ωieωe0.


## 2.3.1. Initial Values of the Launch Point

The launch point position in the ECEF frame isP0e=xe0,ye0,ze0T, as given by(3)xe0=N0+H0cosϕ0cosλ0,ye0=N0+H0cosϕ0sinλ0,ze0=N01−e2+H0sinϕ0,where H0 is the initial height, N0=ae/1−e2sin2L0, e is the eccentricity, and ae is the semimajor axis.At the time of the launch, the coordinates of the LECI and ECEF frames point in the same direction, soP0t=P0e.


## 2.3.2. Geocentric Vector of the Launch Point

The geocentric vector of the launch point in the LCEF frame isP0eg:(4)Pe0g=RegP0e.At the time of launch, the coordinates of the LCEF and LCI frames point in the same direction, soP0ea=P0eg.


## 2.3.3. Rotational Angular Velocity in the LCI Frame

The Earth’s rotation angular velocity in the LECF frame relative to the LCI frame isωagg.(5)ωagg=ωiecosϕ0cosα0sinϕ0−cosϕ0sinα0=ωieωe0.


## 2.4. SINS Mechanization in the LCI Frame

The navigation equation of the SINS in the LCI frame is given by the following equation, and its mechanization is shown in Figure1:(6)P˙aV˙aR˙ba=VaRbafb+gaRbaΩabb.Figure 1
Mechanization of SINS in the LCI frame.As shown in (6) and Figure 1, the SINS algorithm is executed by the navigation computer, and steps involved are as follows. First, the vehicle attitude is obtained by integrating angular velocity ωb measured by gyroscopes, and then attitude matrix Rba is obtained. Second, specific force fb measured by accelerometers is converted to the LCI frame through attitude matrix Rba; considering the effect of gravity ga, velocity Va of the vehicle is obtained by integrating the sum of fa and ga in the LCI frame. Finally, the position Pa of the vehicle is obtained by integrating velocity Va, and Pg=x,y,zT. These three integral processes are termed SINS attitude updating, velocity updating, and position updating, respectively.The formula for calculating gravity in the LCI frame is found in reference:(7)ga=gr′r0+gωeωe0,where gr′ is calculated by (8) and gωe is calculated by (9). r0 is the unit vector of r, and r is calculated by (10):(8)gr′=−μr21+Jaer21−5sin2ϕ,(9)gωe=−2μr2Jaer2sinϕ,(10)r=P0ea+Pa.In these equations,μ is the coefficient of gravity; J=3/2J2, where J2 is the harmonic coefficient; r=x2+y2+z2; ϕ is the Earth-centered latitude, ϕ=arcsinrw/r∗ωie; and rw=r⋅ωe.The initial value of positionPa is P0a=0,0,0T, and the initial value of attitude matrix Rab is determined by the initial attitude angles φ0, ψ0, and γ0 in the initial alignment. The initial value V0a is obtained as follows:(11)V0a=ωagg×P0eg.


## 3. SINS Numerical Updating Algorithm in the LCI Frame

The hypersonic flight is generally affected by coning, sculling, and scrolling effects [16, 17]. Therefore, the compensation algorithms for the three effects are derived in attitude updating [18], velocity updating [19], and position updating [19], respectively.In this study, the two-sample updating algorithm is used; in other words, the angular velocity vectorωb from the gyroscope and the specific force measurement vector fb from the accelerometer are both assumed to be linear models; that is,(12)ωbt=a+2bt−tk−1,fbt=A+2Bt−tk−1,where a,b,A,B are constant vectors; then, we can get the angular and velocity increment expressions,(13)Δθk=θbt,tk−1=∫tk−1tωbτdτ=at−tk−1+bt−tk−12,ΔVk=Vbt,tk−1=∫tk−1tfbτdτ=At−tk−1+Bt−tk−12.If the IMU is sampled twice at equal intervals intk−1,tk, the sampling time is tk−1/2 and tk, respectively, T=tk−tk−1 and h=T/2 are recorded; then we can get(14)Δθ1=∫tk−1tk−1+hωbτdτ=ha+h2b=12aT+14bT2,Δθ2=∫tk−1+htkωbτdτ=ha+3h2b=12aT+34bT2,ΔV1=∫tk−1tk−1+hfbτdτ=hA+h2B,ΔV2=∫tk−1+htkfbτdτ=hA+3h2B,where Δθ1 and ΔV1 are the angular and velocity increments in the time period tk−1,tk−1+h, and Δθ2 and ΔV2 are the angular and velocity increments in the time period tk−1+h,tk. Meanwhile, Δθk=Δθ1+Δθ2 and ΔVk=ΔV1+ΔV2.


### 3.1. Attitude Updating Algorithm

The quaternion attitude updating algorithm based on rotation vector is completed in two steps: (1) calculation of a rotation vector, which describes the state change of the vehicle attitude, and (2) update of quaternions.The recurrence formula of attitude quaternion updating is as follows:(15)qbka=qbk−1aqbkbk−1,where the time before and after attitude updating is tk−1 and tk, during which the quaternions of attitude are qbk−1a and qbka, respectively. Quaternion qbkbk−1 is calculated by angular velocity ωabbt, which is angular velocity ωb.Note thatΦk is the equivalent rotation vector of BF frame relative to the LCI frame, and the corresponding equivalent vector differential equation approximated as follows:(16)Φ˙k≈ωabb+12Φk×ωabb+112Φk×Φk×ωabb≈ωabb+12Φk×ωabb.Since the output of the optical gyroscope is an angle increment, it is necessary to solve the rotation vector by the Taylor series expansion method. TakeΦk as the equivalent rotation vector in the time period tk−1,tk; then, the Taylor series expansion of ΦkT is shown in (17)ΦkT=Φk−10+TΦ˙k−10+T22!Φ¨k−10+T33!Φ⃛k−10+⋯From (12), the derivative of each order of the vehicle angular velocity is obtained as follows:(18)ωabbtk−1=ωabbtk−1+τ|τ=0=a,ω˙abbtk−1=ω˙abbtk−1+τ|τ=0=2b,ωabbitk−1=ωabbitk−1+τ|τ=0=0,i=2,3,4,…,From (13), the derivative of each order of the angular increment is obtained as follows:(19)Δθ0=Δθτ|τ=0=aτ+bτ2=0,Δθ˙0=Δθ˙τ|τ=0=ωabbtk+τ|τ=0=a,Δθ¨0=Δθ¨τ|τ=0=ω˙abbtk+τ|τ=0=2b,Δθi0=Δθiτ|τ=0=ωabbi−1tk+τ|τ=0=0,i=3,4,5,…,Further, because the attitude updating periodT is generally in milliseconds, Φk can also be regarded as a small magnitude. Therefore, when calculating the derivative of Φkτ at τ=0, according to (16), replace the second term of Φkτ with the angle increment; then, we can get(20)Φkτ≈Δθτ.Thus, we can get the following equation from (16):(21)Φ˙kτ=ωabbtk−1+τ+12Δθτ×ωabbtk−1+τ,0≤τ≤T.Considering (18) and (19), the derivative of each order of (21) is obtained:(22)Φ¨kτ=ω˙abbtk−1+τ+12Δθ˙τ×ωabbtk−1+τ+12Δθτ×ω˙abbtk−1+τ,Φ⃛kτ=12Δθ¨τ×ωabbtk−1+τ+Δθ˙τ×ω˙abbtk−1+τΦk4τ=32θ¨τ×ω˙abbtk−1+τ,Φkiτ=0,i=5,6,7,….According to (18) and (19), τ=0 is substituted into (22); then, we have(23)Φ˙k0=a,Φ¨k0=2b,Φ⃛k0=a×b,Φki0=0,i=4,5,6,….Substituting (23) into (19), we can get(24)ΦkT=Φk0+aT+bT2+16a×bT3,where Φk0 is the rotation vector in the time period T. Since the time interval is 0, Φk0=0; therefore,(25)ΦkT=aT+bT2+16a×bT3.From (14), the model coefficients a and b of the linear fitting of the vehicle angular velocity can be obtained by inverse solution; that is,(26)a=3Δθ1−Δθ2T,b=2Δθ2−Δθ1T2.Substituting (26) into (24), we can get(27)ΦkT=Δθ1+Δθ2+23Δθ1×Δθ2.To solve the rotation vector according to (27), the angle increments Δθ1 and Δθ2 in the two time periods are used. Therefore, this formula is called the two-sample updating algorithm of the rotation vector.Therefore, the solution of (16) by using the two-sample algorithm is as follows:(28)Φk=Δθ1+Δθ2+23Δθ1×Δθ2.Rotation vectorΦk and its corresponding quaternion qbkbk−1 are related as follows:(29)qbk+1bk=cosΦk2+ΦkΦsinΦk2.Then, the calculatedqbkbk−1 is substituted into (16) to complete the attitude updating.


### 3.2. Velocity Updating Algorithm

When a carrier operates in the environment of line vibration and angular vibration, the sculling error may occur, which should not be ignored in a high-precision SINS algorithm.The specific force given by (6) indicates the time parameters of each component as follows:(30)V˙at=Rbatfbt+gat.Equation (30) integrates in tk−1,tk:(31)∫tk−1tkV˙atdt=∫tk−1tkRbatfbt+gatdt.From (31),(32)Vka−Vk−1a=∫tk−1tkRbatfbtdt+∫tk−1tkgatdt,where Vka and Vk−1a are velocities of the LCI frame at tk and tk−1, respectively, and are recorded as(33)ΔVsfka=∫tk−1tkRbatfbtdt,(34)ΔVgka=∫tk−1tkgatdt,where ΔVsfka and ΔVgka are the velocity increments of specific force and gravity in time period T, respectively. The following recurrence forms can be obtained:(35)Vka=Vk−1a+ΔVsfka+ΔVgka.The numerical integration algorithms forΔVsfka and ΔVgka are discussed below:(1)
First, we have the gravity velocity incrementΔVgka.Even for hypersonic vehicles, the change in the gravitational vector in a short time periodT is very small, so the integrand function of ΔVgka is generally considered to be a slow time variable, which can be replaced with the value at tk−1/2=tk−1+tk/2, and ΔVgka is approximated as follows:(36)ΔVgka≈gk−1/2aT.Since the navigation velocity and position at timetk are unknown, gk−1/2a needs to be calculated by linear extrapolation:(37)xk−1/2=xk−1+xk−1−xk−22,x=ga.(2)
Second, we have the specific force velocity incrementΔVsfka.The integrand matrix of (33) is transformed to the matrix chain multiplication decomposition as follows.(38)ΔVsfka=∫tk−1tkRbk−1aRbtbk−1fbtdt=Rbk−1a∫tk−1tkRbtbk−1fbtdt.Hence,(39)ΔVsfkbk−1=∫tk−1tkRbtbk−1fbtdt.Fortk−1≤t≤tk, the relationship between the coordinate transformation matrix and the equivalent rotation vector is as follows:(40)Rbtbk−t=I+sinΦΦΦ×+1−cosΦΦ2Φ×Φ×,Equation (42) can be approximated as(41)sinΦΦ≈1,1−cosΦΦ2≈12,Φ×Φ× can be regarded as a second-order small quantity. Equations (20) and (40) can be approximated as(42)Rbtbk−1=I+Δθ×.By substituting (42) into (39), we obtain(43)ΔVsfkb=∫tk−1tkfbt+Δθ×fbtdt.Hence,(44)ΔVk=∫tk−1tkfbtdt.From (14),(45)Δθ˙t=ωabbt,ΔV˙t=fbt.Since(46)ddtΔθt×ΔVt=Δθ×ΔV˙t−ΔVt×Δθ˙t,the following formula can be obtained:(47)Δθt×fbt=Δθt×ΔV˙t=ddtΔθt×ΔVt+ΔVt×Δθ˙t=12ddtΔθt×ΔVt+12Δθtfbt+ΔVt×ωabbtEquations (44) and (47) are substituted into (43), when t=tk+1, ΔVtk−1=0, and Δθtk−1=0 and when t=tk, ΔVtk=ΔVk, and Δθtk=Δθk.Hence,(48)ΔVsfkbk−1=ΔVk+12Δθk×ΔVk+12∫tk−1tkΔθt×fbt+ΔVt×ωabbtdt=ΔVk+ΔVrotk+ΔVsculk,where(49)ΔVrotk=12Δθk×ΔVk.Here,ΔVrotk is the compensation quantity of the velocity rotation effect, caused by the rotation of the linear motion direction of the carrier.(50)ΔVsculk=12∫tk−1tkΔθt×fbt+ΔVt×ωabbtdt,ΔVsculk is the compensation term of the velocity sculling effect when the carrier has linear and angular vibrations.Equations (12) and (13) are substituted into (50) and integrated to obtain(51)ΔVsculk=12∫tk−1tkat−tk−1+bt−tk−12×A+2Bt−tk−1+At−tk−1+Bt−tk−12×a+2bt−tk−1dt=a×B+A×btk−tk−136.From (14),(52)A=3ΔV1−ΔV22h,B=ΔV2−ΔV12h2.By substituting (26) and (52) into (51), the two-sample algorithm for the compensation of the velocity sculling error is obtained as follows:(53)ΔVsculk=3Δθ1−Δθ22h×ΔV2−ΔV12h2+3ΔV1−ΔV22h×Δθ2−Δθ12h22h36=23Δθ1×ΔV2+ΔV1×Δθ2.Thus, velocity updating algorithm derivation is completed.


### 3.3. Position Updating Algorithm

When the hypersonic vehicle is in sculling flight, a scrolling effect error is present in the solution of the SINS position. According to (35) and (36),(54)Vat=Vk−1a+ΔVsfat+ΔVgat−tk−1T,tk−1≤t≤tk,where(55)ΔVsfka=∫tk−1tkRbatfbtdt.By integrating (54) in time period tk−1,tk, we get(56)ΔPka=Vk−1a+12ΔVgkaT+ΔPsfka.From (38), we obtain(57)ΔPsfka=∫tk−1tkΔVsfkadt=Rbk−1a∫tk−1tkΔVsfkbk−1dt.Using (48), the following can be calculated:(58)ΔPsfkb=∫tk−1tkΔVsfkbk−1dt=∫tk−1tkΔVt+12Δθbt×ΔVbt+ΔVscultdt.Note thatγ1=1/2∫tk−1tkΔθbt×ΔVbtdt.The above integral is obtained by a step integral method:(59)γ2=12∫tk−1tkΔθbt×ΔVbtdt=12SΔθkb×ΔVkb−12∫tk−1tkbΔθbt×fbtdt,γ3=12∫tk−1tkΔθbt×ΔVbtdt=12θkb×SΔvkb+12∫tk−1tkSΔvbt×ωabbtdt.Thus,(60)12∫tk−1tkΔθbt×ΔVbtdt=13γ1+γ2+γ3=16SΔθkb×ΔVkb+12θkb×SΔvkb+16∫tk−1tkSΔvbt×ωabbt−SΔθbt×fbt+Δθbt×ΔVbtdt.The above equation is substituted into (58) to obtain(61)ΔPsfkb=∫tk−1tkΔVsfkbdt=SΔvkb+ΔProtkb+ΔPscrlkb,where(62)SΔvkb=∫tk−1tkΔVtdt=∫tk−1tk∫tk−1tkfbτdτdt.That is, the second integral increment of specific force is obtained.(63)ΔProtkb=16SΔθkb×ΔVkb+Δθkb×SΔvkb.Equation (63) is the compensation of the rotation effect in position calculation.(64)ΔPscrlkb=16∫tk−1tkSΔvbt×ωabbt−SΔθbt×fbt+Δθbt×ΔVbt+6ΔVscultdt.Equation (64) is the compensation of the scrolling effect in position calculation, and it indicates that the factors affecting the scrolling effect are the sculling effect and the coupling effect between the angular and linear motions of the vehicle.From (12) and (13), we obtain the following:(65)Δθk=∫tk−1tkωabbtdt=Ta+T2b,(66)ΔVk=∫tk−1tkftdt=TA+T2B,(67)SΔθk=∫tk−1tk∫tk−1τωabbμdμdτ=T22a+T33b,(68)SΔvk=∫tk−1tk∫tk−1τfμdμdτ=T22A+T33B.Equations (26) and (52) as well as (65)–(68) are substituted into (62)–(64) to obtain the following.(69)SΔvkb=T56ΔV1+16ΔV2,ΔProtkb=TΔθ1×518ΔV1+16ΔV2+Δθ2×16ΔV1+118ΔV2,ΔPscrlkb=TΔθ1×1190ΔV1+110ΔV2+Δθ2×190ΔV2−730ΔV1.Thus, position updating algorithm derivation is completed.


## 3.1. Attitude Updating Algorithm

The quaternion attitude updating algorithm based on rotation vector is completed in two steps: (1) calculation of a rotation vector, which describes the state change of the vehicle attitude, and (2) update of quaternions.The recurrence formula of attitude quaternion updating is as follows:(15)qbka=qbk−1aqbkbk−1,where the time before and after attitude updating is tk−1 and tk, during which the quaternions of attitude are qbk−1a and qbka, respectively. Quaternion qbkbk−1 is calculated by angular velocity ωabbt, which is angular velocity ωb.Note thatΦk is the equivalent rotation vector of BF frame relative to the LCI frame, and the corresponding equivalent vector differential equation approximated as follows:(16)Φ˙k≈ωabb+12Φk×ωabb+112Φk×Φk×ωabb≈ωabb+12Φk×ωabb.Since the output of the optical gyroscope is an angle increment, it is necessary to solve the rotation vector by the Taylor series expansion method. TakeΦk as the equivalent rotation vector in the time period tk−1,tk; then, the Taylor series expansion of ΦkT is shown in (17)ΦkT=Φk−10+TΦ˙k−10+T22!Φ¨k−10+T33!Φ⃛k−10+⋯From (12), the derivative of each order of the vehicle angular velocity is obtained as follows:(18)ωabbtk−1=ωabbtk−1+τ|τ=0=a,ω˙abbtk−1=ω˙abbtk−1+τ|τ=0=2b,ωabbitk−1=ωabbitk−1+τ|τ=0=0,i=2,3,4,…,From (13), the derivative of each order of the angular increment is obtained as follows:(19)Δθ0=Δθτ|τ=0=aτ+bτ2=0,Δθ˙0=Δθ˙τ|τ=0=ωabbtk+τ|τ=0=a,Δθ¨0=Δθ¨τ|τ=0=ω˙abbtk+τ|τ=0=2b,Δθi0=Δθiτ|τ=0=ωabbi−1tk+τ|τ=0=0,i=3,4,5,…,Further, because the attitude updating periodT is generally in milliseconds, Φk can also be regarded as a small magnitude. Therefore, when calculating the derivative of Φkτ at τ=0, according to (16), replace the second term of Φkτ with the angle increment; then, we can get(20)Φkτ≈Δθτ.Thus, we can get the following equation from (16):(21)Φ˙kτ=ωabbtk−1+τ+12Δθτ×ωabbtk−1+τ,0≤τ≤T.Considering (18) and (19), the derivative of each order of (21) is obtained:(22)Φ¨kτ=ω˙abbtk−1+τ+12Δθ˙τ×ωabbtk−1+τ+12Δθτ×ω˙abbtk−1+τ,Φ⃛kτ=12Δθ¨τ×ωabbtk−1+τ+Δθ˙τ×ω˙abbtk−1+τΦk4τ=32θ¨τ×ω˙abbtk−1+τ,Φkiτ=0,i=5,6,7,….According to (18) and (19), τ=0 is substituted into (22); then, we have(23)Φ˙k0=a,Φ¨k0=2b,Φ⃛k0=a×b,Φki0=0,i=4,5,6,….Substituting (23) into (19), we can get(24)ΦkT=Φk0+aT+bT2+16a×bT3,where Φk0 is the rotation vector in the time period T. Since the time interval is 0, Φk0=0; therefore,(25)ΦkT=aT+bT2+16a×bT3.From (14), the model coefficients a and b of the linear fitting of the vehicle angular velocity can be obtained by inverse solution; that is,(26)a=3Δθ1−Δθ2T,b=2Δθ2−Δθ1T2.Substituting (26) into (24), we can get(27)ΦkT=Δθ1+Δθ2+23Δθ1×Δθ2.To solve the rotation vector according to (27), the angle increments Δθ1 and Δθ2 in the two time periods are used. Therefore, this formula is called the two-sample updating algorithm of the rotation vector.Therefore, the solution of (16) by using the two-sample algorithm is as follows:(28)Φk=Δθ1+Δθ2+23Δθ1×Δθ2.Rotation vectorΦk and its corresponding quaternion qbkbk−1 are related as follows:(29)qbk+1bk=cosΦk2+ΦkΦsinΦk2.Then, the calculatedqbkbk−1 is substituted into (16) to complete the attitude updating.


## 3.2. Velocity Updating Algorithm

When a carrier operates in the environment of line vibration and angular vibration, the sculling error may occur, which should not be ignored in a high-precision SINS algorithm.The specific force given by (6) indicates the time parameters of each component as follows:(30)V˙at=Rbatfbt+gat.Equation (30) integrates in tk−1,tk:(31)∫tk−1tkV˙atdt=∫tk−1tkRbatfbt+gatdt.From (31),(32)Vka−Vk−1a=∫tk−1tkRbatfbtdt+∫tk−1tkgatdt,where Vka and Vk−1a are velocities of the LCI frame at tk and tk−1, respectively, and are recorded as(33)ΔVsfka=∫tk−1tkRbatfbtdt,(34)ΔVgka=∫tk−1tkgatdt,where ΔVsfka and ΔVgka are the velocity increments of specific force and gravity in time period T, respectively. The following recurrence forms can be obtained:(35)Vka=Vk−1a+ΔVsfka+ΔVgka.The numerical integration algorithms forΔVsfka and ΔVgka are discussed below:(1)
First, we have the gravity velocity incrementΔVgka.Even for hypersonic vehicles, the change in the gravitational vector in a short time periodT is very small, so the integrand function of ΔVgka is generally considered to be a slow time variable, which can be replaced with the value at tk−1/2=tk−1+tk/2, and ΔVgka is approximated as follows:(36)ΔVgka≈gk−1/2aT.Since the navigation velocity and position at timetk are unknown, gk−1/2a needs to be calculated by linear extrapolation:(37)xk−1/2=xk−1+xk−1−xk−22,x=ga.(2)
Second, we have the specific force velocity incrementΔVsfka.The integrand matrix of (33) is transformed to the matrix chain multiplication decomposition as follows.(38)ΔVsfka=∫tk−1tkRbk−1aRbtbk−1fbtdt=Rbk−1a∫tk−1tkRbtbk−1fbtdt.Hence,(39)ΔVsfkbk−1=∫tk−1tkRbtbk−1fbtdt.Fortk−1≤t≤tk, the relationship between the coordinate transformation matrix and the equivalent rotation vector is as follows:(40)Rbtbk−t=I+sinΦΦΦ×+1−cosΦΦ2Φ×Φ×,Equation (42) can be approximated as(41)sinΦΦ≈1,1−cosΦΦ2≈12,Φ×Φ× can be regarded as a second-order small quantity. Equations (20) and (40) can be approximated as(42)Rbtbk−1=I+Δθ×.By substituting (42) into (39), we obtain(43)ΔVsfkb=∫tk−1tkfbt+Δθ×fbtdt.Hence,(44)ΔVk=∫tk−1tkfbtdt.From (14),(45)Δθ˙t=ωabbt,ΔV˙t=fbt.Since(46)ddtΔθt×ΔVt=Δθ×ΔV˙t−ΔVt×Δθ˙t,the following formula can be obtained:(47)Δθt×fbt=Δθt×ΔV˙t=ddtΔθt×ΔVt+ΔVt×Δθ˙t=12ddtΔθt×ΔVt+12Δθtfbt+ΔVt×ωabbtEquations (44) and (47) are substituted into (43), when t=tk+1, ΔVtk−1=0, and Δθtk−1=0 and when t=tk, ΔVtk=ΔVk, and Δθtk=Δθk.Hence,(48)ΔVsfkbk−1=ΔVk+12Δθk×ΔVk+12∫tk−1tkΔθt×fbt+ΔVt×ωabbtdt=ΔVk+ΔVrotk+ΔVsculk,where(49)ΔVrotk=12Δθk×ΔVk.Here,ΔVrotk is the compensation quantity of the velocity rotation effect, caused by the rotation of the linear motion direction of the carrier.(50)ΔVsculk=12∫tk−1tkΔθt×fbt+ΔVt×ωabbtdt,ΔVsculk is the compensation term of the velocity sculling effect when the carrier has linear and angular vibrations.Equations (12) and (13) are substituted into (50) and integrated to obtain(51)ΔVsculk=12∫tk−1tkat−tk−1+bt−tk−12×A+2Bt−tk−1+At−tk−1+Bt−tk−12×a+2bt−tk−1dt=a×B+A×btk−tk−136.From (14),(52)A=3ΔV1−ΔV22h,B=ΔV2−ΔV12h2.By substituting (26) and (52) into (51), the two-sample algorithm for the compensation of the velocity sculling error is obtained as follows:(53)ΔVsculk=3Δθ1−Δθ22h×ΔV2−ΔV12h2+3ΔV1−ΔV22h×Δθ2−Δθ12h22h36=23Δθ1×ΔV2+ΔV1×Δθ2.Thus, velocity updating algorithm derivation is completed.


## 3.3. Position Updating Algorithm

When the hypersonic vehicle is in sculling flight, a scrolling effect error is present in the solution of the SINS position. According to (35) and (36),(54)Vat=Vk−1a+ΔVsfat+ΔVgat−tk−1T,tk−1≤t≤tk,where(55)ΔVsfka=∫tk−1tkRbatfbtdt.By integrating (54) in time period tk−1,tk, we get(56)ΔPka=Vk−1a+12ΔVgkaT+ΔPsfka.From (38), we obtain(57)ΔPsfka=∫tk−1tkΔVsfkadt=Rbk−1a∫tk−1tkΔVsfkbk−1dt.Using (48), the following can be calculated:(58)ΔPsfkb=∫tk−1tkΔVsfkbk−1dt=∫tk−1tkΔVt+12Δθbt×ΔVbt+ΔVscultdt.Note thatγ1=1/2∫tk−1tkΔθbt×ΔVbtdt.The above integral is obtained by a step integral method:(59)γ2=12∫tk−1tkΔθbt×ΔVbtdt=12SΔθkb×ΔVkb−12∫tk−1tkbΔθbt×fbtdt,γ3=12∫tk−1tkΔθbt×ΔVbtdt=12θkb×SΔvkb+12∫tk−1tkSΔvbt×ωabbtdt.Thus,(60)12∫tk−1tkΔθbt×ΔVbtdt=13γ1+γ2+γ3=16SΔθkb×ΔVkb+12θkb×SΔvkb+16∫tk−1tkSΔvbt×ωabbt−SΔθbt×fbt+Δθbt×ΔVbtdt.The above equation is substituted into (58) to obtain(61)ΔPsfkb=∫tk−1tkΔVsfkbdt=SΔvkb+ΔProtkb+ΔPscrlkb,where(62)SΔvkb=∫tk−1tkΔVtdt=∫tk−1tk∫tk−1tkfbτdτdt.That is, the second integral increment of specific force is obtained.(63)ΔProtkb=16SΔθkb×ΔVkb+Δθkb×SΔvkb.Equation (63) is the compensation of the rotation effect in position calculation.(64)ΔPscrlkb=16∫tk−1tkSΔvbt×ωabbt−SΔθbt×fbt+Δθbt×ΔVbt+6ΔVscultdt.Equation (64) is the compensation of the scrolling effect in position calculation, and it indicates that the factors affecting the scrolling effect are the sculling effect and the coupling effect between the angular and linear motions of the vehicle.From (12) and (13), we obtain the following:(65)Δθk=∫tk−1tkωabbtdt=Ta+T2b,(66)ΔVk=∫tk−1tkftdt=TA+T2B,(67)SΔθk=∫tk−1tk∫tk−1τωabbμdμdτ=T22a+T33b,(68)SΔvk=∫tk−1tk∫tk−1τfμdμdτ=T22A+T33B.Equations (26) and (52) as well as (65)–(68) are substituted into (62)–(64) to obtain the following.(69)SΔvkb=T56ΔV1+16ΔV2,ΔProtkb=TΔθ1×518ΔV1+16ΔV2+Δθ2×16ΔV1+118ΔV2,ΔPscrlkb=TΔθ1×1190ΔV1+110ΔV2+Δθ2×190ΔV2−730ΔV1.Thus, position updating algorithm derivation is completed.


## 4. SINS/BDS Integrated Navigation Algorithm in the LCI Frame

### 4.1. SINS Error Equation in the Launch-Centered Inertial Frame

#### 4.1.1. Attitude Error Equation

For the SINS, the transformation matrix error from the BF frame to the calculated frame is caused by the angular rate error between the two coordinate systems when they are rotated. The attitude mechanization in the LCI frame is as follows.(70)R˙ba=RbaΩabb.Considering the measurement and calculation errors, the transformation rate of the calculated transformation matrix is given as follows.(71)R^˙ba=R^baΩ^abb.The calculated transformation matrixR^ba can be written as(72)R^ba=Rba+δRba.If(73)δRba=−ΨaRba,then(74)R^ba=I−ΨaRba.There is an error angleφa=ϕxϕyϕzT between the actual navigation frame and the calculated frame, and Ψa is the antisymmetric matrix of φa:(75)Ψa=0ϕz−ϕy−ϕz0ϕxϕy−ϕx0.Derivation of both sides of (74) gives the following:(76)R^˙ba=R˙ba−Ψ˙aRba−ΨaR˙ba,δR˙ba=−Ψ˙aRba−ΨaR˙ba.By contrast, the differential equation of (70) is obtained as(77)δR˙ba=δRbaΩabb+RbaδΩabb=−ΨaRbaΩabb+RbaδΩabb.Comparing (76) and (77), we get(78)Ψ˙a=−RbaδΩabbRab.Equation (78) is written in a vector form as(79)φ˙a=−Rbaδωabb.The gyroscope measurement error isδωabb=δωaib+δωibb, and because δωaib=0, δωibb is assumed to be composed of the random constant drift εb of the equivalent gyroscope. Thus,(80)φ˙a=−Rbaεb.Therefore, the attitude error equation is written in the component form as(81)ϕ˙xaϕ˙yaϕ˙za=−Rabεxbεybεzb.


#### 4.1.2. Velocity Error Equation

From (6), the linear equation of the first-order small disturbance for velocity can be obtained as(82)δV˙a=δRbafb+Rbaδfb+δga.The measurement errorδfb of the accelerometer consists of the random constant zero bias ∇b of the equivalent accelerometer.Therefore, we can get the velocity error equation as follows.(83)δV˙a=δRbafb+δga+Rba∇b.By substituting (73) into (83), we get(84)δV˙a=−ΨRbafb+δga+Rba∇b=Faφa+δga+Rba∇b,where(85)δga=ur33x2r2−13xyr23xzr23yxr23y2r2−13yzr23zxr23zyr23z2r2−1δxδyδz=GPaδP.


#### 4.1.3. Position Error Equation

The position error equation under the LCI frame is(86)δP˙a=δVa.


### 4.2. SINS/BDS Integrated Navigation Equation

The state and measurement equation for SINS/BDS integrated navigation are given as follows.(87)X˙=FX+GW,Zvp=HvpX+Vvp,where Zvp is shown in (88)Zvpt=Va−VsaPa−Psa.It is assumed that the measurement errorδωabb of the gyroscope is composed of the gyroscope’s zero bias εb and that the measurement error δfb of the accelerometer consists of the accelerometer’s zero bias ∇b. The state variable of SINS/BDS is adopted; that is,(89)X=φaδVaδPaεb∇bT.The state equation is(90)ϕaδV˙aδP˙aε˙b∇˙b=03×303×303×3−Rba03×3Fa03×3GPa03×3Rba03×3I3×303×303×303×303×303×303×303×303×303×303×303×303×303×3ϕaδVaδPaεb∇b+−Rba03×303×3Rba09×309×3wgwg.The 15-dimensional measurement matrixHvp is expressed as follows:(91)Hvp=03×3I3×303×303×303×303×303×3I3×303×303×3.


### 4.3. BDS Receiver Position and Velocity Conversion

Since the BDS receiver outputs the position and velocity in the ECEF frame [20], they need to be converted to the LCI frame. Let the position and velocity of the BDS receiver in the ECEF frame be Pse and Vse, respectively. First, by converting these values in to the LECI frame, position Pst and velocity Vst in the LECI are obtained as shown in (92)Pst=RzωietPse,(93)Vst=RzωietVse+00ωie×Pse.Then, positionPsa and velocity Vsa of the BDS receiver in the LCI frame are obtained by converting position Pst and velocity Vst from the LECI frame to the LCI frame, as shown in (94)Psa=RtaPst−P0t,(95)Vsa=RtaVst.


## 4.1. SINS Error Equation in the Launch-Centered Inertial Frame

### 4.1.1. Attitude Error Equation

For the SINS, the transformation matrix error from the BF frame to the calculated frame is caused by the angular rate error between the two coordinate systems when they are rotated. The attitude mechanization in the LCI frame is as follows.(70)R˙ba=RbaΩabb.Considering the measurement and calculation errors, the transformation rate of the calculated transformation matrix is given as follows.(71)R^˙ba=R^baΩ^abb.The calculated transformation matrixR^ba can be written as(72)R^ba=Rba+δRba.If(73)δRba=−ΨaRba,then(74)R^ba=I−ΨaRba.There is an error angleφa=ϕxϕyϕzT between the actual navigation frame and the calculated frame, and Ψa is the antisymmetric matrix of φa:(75)Ψa=0ϕz−ϕy−ϕz0ϕxϕy−ϕx0.Derivation of both sides of (74) gives the following:(76)R^˙ba=R˙ba−Ψ˙aRba−ΨaR˙ba,δR˙ba=−Ψ˙aRba−ΨaR˙ba.By contrast, the differential equation of (70) is obtained as(77)δR˙ba=δRbaΩabb+RbaδΩabb=−ΨaRbaΩabb+RbaδΩabb.Comparing (76) and (77), we get(78)Ψ˙a=−RbaδΩabbRab.Equation (78) is written in a vector form as(79)φ˙a=−Rbaδωabb.The gyroscope measurement error isδωabb=δωaib+δωibb, and because δωaib=0, δωibb is assumed to be composed of the random constant drift εb of the equivalent gyroscope. Thus,(80)φ˙a=−Rbaεb.Therefore, the attitude error equation is written in the component form as(81)ϕ˙xaϕ˙yaϕ˙za=−Rabεxbεybεzb.


### 4.1.2. Velocity Error Equation

From (6), the linear equation of the first-order small disturbance for velocity can be obtained as(82)δV˙a=δRbafb+Rbaδfb+δga.The measurement errorδfb of the accelerometer consists of the random constant zero bias ∇b of the equivalent accelerometer.Therefore, we can get the velocity error equation as follows.(83)δV˙a=δRbafb+δga+Rba∇b.By substituting (73) into (83), we get(84)δV˙a=−ΨRbafb+δga+Rba∇b=Faφa+δga+Rba∇b,where(85)δga=ur33x2r2−13xyr23xzr23yxr23y2r2−13yzr23zxr23zyr23z2r2−1δxδyδz=GPaδP.


### 4.1.3. Position Error Equation

The position error equation under the LCI frame is(86)δP˙a=δVa.


## 4.1.1. Attitude Error Equation

For the SINS, the transformation matrix error from the BF frame to the calculated frame is caused by the angular rate error between the two coordinate systems when they are rotated. The attitude mechanization in the LCI frame is as follows.(70)R˙ba=RbaΩabb.Considering the measurement and calculation errors, the transformation rate of the calculated transformation matrix is given as follows.(71)R^˙ba=R^baΩ^abb.The calculated transformation matrixR^ba can be written as(72)R^ba=Rba+δRba.If(73)δRba=−ΨaRba,then(74)R^ba=I−ΨaRba.There is an error angleφa=ϕxϕyϕzT between the actual navigation frame and the calculated frame, and Ψa is the antisymmetric matrix of φa:(75)Ψa=0ϕz−ϕy−ϕz0ϕxϕy−ϕx0.Derivation of both sides of (74) gives the following:(76)R^˙ba=R˙ba−Ψ˙aRba−ΨaR˙ba,δR˙ba=−Ψ˙aRba−ΨaR˙ba.By contrast, the differential equation of (70) is obtained as(77)δR˙ba=δRbaΩabb+RbaδΩabb=−ΨaRbaΩabb+RbaδΩabb.Comparing (76) and (77), we get(78)Ψ˙a=−RbaδΩabbRab.Equation (78) is written in a vector form as(79)φ˙a=−Rbaδωabb.The gyroscope measurement error isδωabb=δωaib+δωibb, and because δωaib=0, δωibb is assumed to be composed of the random constant drift εb of the equivalent gyroscope. Thus,(80)φ˙a=−Rbaεb.Therefore, the attitude error equation is written in the component form as(81)ϕ˙xaϕ˙yaϕ˙za=−Rabεxbεybεzb.


## 4.1.2. Velocity Error Equation

From (6), the linear equation of the first-order small disturbance for velocity can be obtained as(82)δV˙a=δRbafb+Rbaδfb+δga.The measurement errorδfb of the accelerometer consists of the random constant zero bias ∇b of the equivalent accelerometer.Therefore, we can get the velocity error equation as follows.(83)δV˙a=δRbafb+δga+Rba∇b.By substituting (73) into (83), we get(84)δV˙a=−ΨRbafb+δga+Rba∇b=Faφa+δga+Rba∇b,where(85)δga=ur33x2r2−13xyr23xzr23yxr23y2r2−13yzr23zxr23zyr23z2r2−1δxδyδz=GPaδP.


## 4.1.3. Position Error Equation

The position error equation under the LCI frame is(86)δP˙a=δVa.


## 4.2. SINS/BDS Integrated Navigation Equation

The state and measurement equation for SINS/BDS integrated navigation are given as follows.(87)X˙=FX+GW,Zvp=HvpX+Vvp,where Zvp is shown in (88)Zvpt=Va−VsaPa−Psa.It is assumed that the measurement errorδωabb of the gyroscope is composed of the gyroscope’s zero bias εb and that the measurement error δfb of the accelerometer consists of the accelerometer’s zero bias ∇b. The state variable of SINS/BDS is adopted; that is,(89)X=φaδVaδPaεb∇bT.The state equation is(90)ϕaδV˙aδP˙aε˙b∇˙b=03×303×303×3−Rba03×3Fa03×3GPa03×3Rba03×3I3×303×303×303×303×303×303×303×303×303×303×303×303×303×3ϕaδVaδPaεb∇b+−Rba03×303×3Rba09×309×3wgwg.The 15-dimensional measurement matrixHvp is expressed as follows:(91)Hvp=03×3I3×303×303×303×303×303×3I3×303×303×3.


## 4.3. BDS Receiver Position and Velocity Conversion

Since the BDS receiver outputs the position and velocity in the ECEF frame [20], they need to be converted to the LCI frame. Let the position and velocity of the BDS receiver in the ECEF frame be Pse and Vse, respectively. First, by converting these values in to the LECI frame, position Pst and velocity Vst in the LECI are obtained as shown in (92)Pst=RzωietPse,(93)Vst=RzωietVse+00ωie×Pse.Then, positionPsa and velocity Vsa of the BDS receiver in the LCI frame are obtained by converting position Pst and velocity Vst from the LECI frame to the LCI frame, as shown in (94)Psa=RtaPst−P0t,(95)Vsa=RtaVst.


## 5. Test on SINS/BDS Integrated Navigation

### 5.1. SINS/BDS Integrated Navigation Vehicle Field Test

To test the feasibility of the SINS/BDS integrated navigation algorithm, a vehicle field test was conducted, as shown in Figure2. The test was conducted at a high speed around Xi’an, and the track of the vehicle is shown in Figure 3. The initial latitude and longitude of the vehicle test were 34.19785° and 108.8284°, respectively, the initial height was 365.1 m, and the initial velocity was 0 m/s. The vehicle was equipped with a set of high-precision integrated navigation system as the reference, called the master INS. Its main performance indicators are as follows: position accuracy is 1.0 n mile/h, speed accuracy is 0.8 m/s, and attitude accuracy is 0.05°.Figure 2
SINS/BDS integrated navigation system.
(a)(b)Figure 3
Test track of vehicle.The SINS/BDS used in this paper is called the slave INS. The gyroscope drift was 3°/h (1σ), the scale factor was 100 ppm, the variance of white noise was 0.3°/h (1σ), and the output period of gyroscope was 2 ms. The bias of the accelerometer was 1 × 10−3·g (1σ), the scale factor was 100 ppm, the white noise variance was 1 × 10−4·g (1σ), and the output period was 2 ms. The positioning accuracy of the BDS satellite receiver was 10 m (1 σ), the speed accuracy was 0.3 m/s (1σ), and the output period was 1 Hz.The master INS output the navigation solutions were in the local horizontal frame. The slave INS solutions were converted from the LCI frame to the local horizontal frame [21]. Both are then compared and made the difference as shown in Figures 4–10.Figure 4
Error of master and slave SINS/BDS latitude.Figure 5
Error of master and slave SINS/BDS longitude.Figure 6
Error of master and slave SINS/BDS east velocity.Figure 7
Error of master and slave SINS/BDS north velocity.Figure 8
Error of master and slave SINS/BDS azimuth angle.Figure 9
Error of master and slave SINS/BDS pitch angle.Figure 10
Error of master and slave SINS/BDS roll angle.In the vehicle test, the azimuth angle error of SINS/BDS integrated navigation was approximately 0.05°, the pitch angle error was within 0.05°, the roll angle error was within 0.05°, the velocity error of east and north was within 0.2 m/s, and the position error was within 10 m.


### 5.2. SINS/BDS Integrated Navigation Test by HWIL Simulation

Due to the limitation of vehicle velocity, the performance of SINS/BDS integrated navigation was verified for hypersonic velocity by the HWIL simulation of hypersonic vehicles [4], as shown in Figure 11. The HWIL simulation equipment includes the real-time simulator, GNC analysis computer, 3D display, three-axis rotation table, IMU simulator, GNSS simulator, product interface system, and actuator load simulator. The on-board equipment includes the on-board computer, SINS/BDS system, and actuator system.Figure 11
Diagram of HWIL simulation system.We propose a classic boost-glide flight trajectory [22, 23]. The equation describing this trajectory is detailed in [22], and the trajectory data are obtained from the simulation platform in [23]. The initial parameters of the trajectory are shown in Table 1, and the simulation diagram of the trajectory is shown in Figure 12.Table 1
Initial state of the trajectory.
ParametersValueVelocity0 m/sLatitude34.2°Longitude108.9°Height0 mLaunch azimuth200°Pitch90°Roll0°Yaw0°Figure 12
Flight trajectory of a hypersonic vehicle.The simulation results presented in Figures13–17 indicate that the three attitude angle errors of the SINS/BDS integrated navigation system can basically converge within 0.05. The velocity error in the three directions is within 0.2 m/s. The position error in the three directions is less than 10 m. After 200 s, the gyroscope drift in all three directions is estimated to be 3.1°/h. The drift of accelerometers in both x- and y-directions is estimated to be 1.1 mg. Due to the small motion in the z-direction, the drift of accelerometers in this direction is estimated to be 0.7 mg.Figure 13
Position error in the LCI frame.Figure 14
Velocity error in the LCI frame.Figure 15
Attitude error in the LCI frame.Figure 16
Gyroscope drift estimation in the LCI frame.Figure 17
Accelerometer drift estimation in the LCI frame.


## 5.1. SINS/BDS Integrated Navigation Vehicle Field Test

To test the feasibility of the SINS/BDS integrated navigation algorithm, a vehicle field test was conducted, as shown in Figure2. The test was conducted at a high speed around Xi’an, and the track of the vehicle is shown in Figure 3. The initial latitude and longitude of the vehicle test were 34.19785° and 108.8284°, respectively, the initial height was 365.1 m, and the initial velocity was 0 m/s. The vehicle was equipped with a set of high-precision integrated navigation system as the reference, called the master INS. Its main performance indicators are as follows: position accuracy is 1.0 n mile/h, speed accuracy is 0.8 m/s, and attitude accuracy is 0.05°.Figure 2
SINS/BDS integrated navigation system.
(a)(b)Figure 3
Test track of vehicle.The SINS/BDS used in this paper is called the slave INS. The gyroscope drift was 3°/h (1σ), the scale factor was 100 ppm, the variance of white noise was 0.3°/h (1σ), and the output period of gyroscope was 2 ms. The bias of the accelerometer was 1 × 10−3·g (1σ), the scale factor was 100 ppm, the white noise variance was 1 × 10−4·g (1σ), and the output period was 2 ms. The positioning accuracy of the BDS satellite receiver was 10 m (1 σ), the speed accuracy was 0.3 m/s (1σ), and the output period was 1 Hz.The master INS output the navigation solutions were in the local horizontal frame. The slave INS solutions were converted from the LCI frame to the local horizontal frame [21]. Both are then compared and made the difference as shown in Figures 4–10.Figure 4
Error of master and slave SINS/BDS latitude.Figure 5
Error of master and slave SINS/BDS longitude.Figure 6
Error of master and slave SINS/BDS east velocity.Figure 7
Error of master and slave SINS/BDS north velocity.Figure 8
Error of master and slave SINS/BDS azimuth angle.Figure 9
Error of master and slave SINS/BDS pitch angle.Figure 10
Error of master and slave SINS/BDS roll angle.In the vehicle test, the azimuth angle error of SINS/BDS integrated navigation was approximately 0.05°, the pitch angle error was within 0.05°, the roll angle error was within 0.05°, the velocity error of east and north was within 0.2 m/s, and the position error was within 10 m.


## 5.2. SINS/BDS Integrated Navigation Test by HWIL Simulation

Due to the limitation of vehicle velocity, the performance of SINS/BDS integrated navigation was verified for hypersonic velocity by the HWIL simulation of hypersonic vehicles [4], as shown in Figure 11. The HWIL simulation equipment includes the real-time simulator, GNC analysis computer, 3D display, three-axis rotation table, IMU simulator, GNSS simulator, product interface system, and actuator load simulator. The on-board equipment includes the on-board computer, SINS/BDS system, and actuator system.Figure 11
Diagram of HWIL simulation system.We propose a classic boost-glide flight trajectory [22, 23]. The equation describing this trajectory is detailed in [22], and the trajectory data are obtained from the simulation platform in [23]. The initial parameters of the trajectory are shown in Table 1, and the simulation diagram of the trajectory is shown in Figure 12.Table 1
Initial state of the trajectory.
ParametersValueVelocity0 m/sLatitude34.2°Longitude108.9°Height0 mLaunch azimuth200°Pitch90°Roll0°Yaw0°Figure 12
Flight trajectory of a hypersonic vehicle.The simulation results presented in Figures13–17 indicate that the three attitude angle errors of the SINS/BDS integrated navigation system can basically converge within 0.05. The velocity error in the three directions is within 0.2 m/s. The position error in the three directions is less than 10 m. After 200 s, the gyroscope drift in all three directions is estimated to be 3.1°/h. The drift of accelerometers in both x- and y-directions is estimated to be 1.1 mg. Due to the small motion in the z-direction, the drift of accelerometers in this direction is estimated to be 0.7 mg.Figure 13
Position error in the LCI frame.Figure 14
Velocity error in the LCI frame.Figure 15
Attitude error in the LCI frame.Figure 16
Gyroscope drift estimation in the LCI frame.Figure 17
Accelerometer drift estimation in the LCI frame.


## 6. Conclusions

In this work, we considered the hypersonic boost-glide vehicles as the object, proposed the SINS mechanization and SINS numerical update algorithm in the LCI frame, and designed the SINS/BDS integrated algorithm in the LCI frame. In addition, we conducted vehicle field tests and hardware-in-the-loop simulations, and the accuracy of the simulation results was found to meet the navigation requirements. When the pitch angle is 90°, the attitude angle in the LCI frame is not singular, which meets the requirements for the vertical launch of the hypersonic boost-glide vehicles. Therefore, the proposed integrated navigation system is especially suitable for hypersonic boost-glide vehicles and other vertically launched aircrafts.



---

*Source: 7503272-2020-11-16.xml*