Tightly Coupled Integrated Navigation Algorithm for Hypersonic Boost-Glide Vehicles in the LCEF Frame

Abstract

According to the trajectory characteristics of hypersonic boost-glide vehicles, a tightly coupled integrated navigation algorithm for hypersonic vehicles based on the launch-centered Earth-fixed (LCEF) frame is proposed. First, the strapdown inertial navigation mechanization algorithm and discrete update algorithm in the LCEF frame are introduced. Subsequently, the attitude, velocity, and position error equations of strapdown inertial navigation in the LCEF frame are introduced. The strapdown inertial navigation system/global positioning system (SINS/GPS) pseudo-range and pseudo-range rate measurement equations in the LCEF frame are derived. Further, the tightly coupled SINS/GPS integrated navigation filter state equation and the measurement equation are presented. Finally, the tightly coupled SINS/GPS integrated navigation algorithm is verified in the hardware-in-the-loop (HWIL) simulation environment. The simulation results indicate that the precision of tightly coupled integrated navigation is better than that of loosely coupled integrated navigation. Moreover, even when the number of effective satellites is less than four, tightly coupled integrated navigation functions well, thus verifying the effectiveness and feasibility of the algorithm.

1. Introduction

Hypersonic vehicles are being developed by various countries in the world today because of their strong strategic deterrence, air–space integrated information support, control capabilities, rapid navy, army, and air strike capabilities, and remote air interception capabilities [1]. Countries with strong economic and military capabilities such as the United States, Russia, and China have conducted a large number of flight tests, while some countries have even formally installed hypersonic missiles in their militaries [2].

The commonly used navigation systems for hypersonic vehicles include the inertial navigation system (INS), global navigation satellite system (GNSS), and celestial navigation system (CNS) [3]. The INS are self-contained and are more accurate in the short term and they can supply data continuously at a very high rate. The main drawback of an INS is the degradation of its performance with time. The GNSS receivers has high precision in long-time navigation and can bound the errors to an acceptable level. The INS and GNSS are natural complementarity. Therefore, the navigation systems of hypersonic vehicles worldwide are mainly inertial/satellite integrated navigation systems [4]. The Unites States’ X-43A hypersonic vehicle of NASA’s Hyper-X program uses a mature INS/GPS integrated navigation system [5]. The navigation system of the Hypersonic Technology Vehicle 2 (HTV-2) uses a tightly coupled INS/GPS navigation, achieving a navigation precision of up to 3 m [6]. Russia’s Avangard hypersonic missile and India’s BrahMos-II also use the INS/GNSS navigation [7].

The inertial/satellite integrated navigation system can be classified into three types based on the integration degree: loosely coupled, tightly coupled, and ultra-tightly coupled [8]. The algorithm for a loosely coupled system is relatively simple to calculate and easy to implement, and it is widely used in engineering practice. However, its navigation precision is not comparable to that of a tightly coupled system and its anti-interference ability is poor. The design of an ultra-tightly coupled system is complicated with large calculation load. Its engineering realization is limited by the development capability of satellite receivers. Tight coupling corrects the inertial navigation system through pseudo-range and pseudo-range rate satellite observations. A tightly coupled system does not require satellite receivers to provide complete positioning information and has high navigation precision. When the effective number of satellites is less than four, it can still perform integrated navigation and has strong anti-interference ability. This can meet the navigation needs of hypersonic vehicles. In recent years, scholars have increasingly studied tightly coupled INS/GNSS integrated navigation [9]. Dai et al. proposed a tightly coupled strapdown inertial navigation system/BeiDou system (SINS/BDS) algorithm based on the fast hybrid Gaussian unscented Kalman filter (UKF) algorithm. While the precision is lower than that of the traditional generalized maximum likelihood-type UKF (GM-UKF), it effectively improves the filtering calculation efficiency and the calculation speed [10]. Feng et al. used tightly coupled INS/GNSS integrated navigation technology to survey and map the surface of a shore collapsed terrain [11]. Zhao et al. studied a faster and more efficient tightly coupled SINS/GNSS navigation algorithm based on the simplified steady state KF (SSKF). Furthermore, the navigation precision is similar to that of the traditional SSKF algorithm [12]. The tightly coupled GPS/INS algorithm proposed by Yu et al. fully combines the characteristics of the aggregation constraint method and the UKF and improves the calculation efficiency [13].

As a hypersonic boost-glide vehicle has dual flight control and navigation requirements for space and aviation [4], a single launch-centered inertial (LCI) frame in space and a local-level frame in aviation cannot meet its navigation requirements simultaneously [14]. Therefore, a navigation reference frame combining the characteristics of the LCI frame and the local-level frame is needed to meet the dual requirements of space and aviation for hypersonic vehicles. The strapdown inertial navigation algorithm in the launch-centered Earth-fixed (LCEF) frame was presented in reference [4], proposing solutions to the abovementioned problems [4]. References [14,15] introduced the loosely coupled integrated navigation algorithm in the LCI frame and the LCEF frame, respectively. Based on references [4,14,15], this paper derives the tightly coupled SINS/GPS integrated navigation algorithm, KF state, and measurement equation. A solution for the tightly coupled integrated navigation of hypersonic boost-glide vehicles is proposed.

2. Strapdown Inertial Navigation Algorithm in the LCEF Frame

2.1. Coordinate System

The following coordinate systems are used in this paper and the LCEF frame is selected as the navigation reference coordinate system [4].

• The Earth-centered inertial (ECI) frame, i;
• the Earth-centered Earth-fixed (ECEF) frame, e;
• the body-fixed (BF) frame, b;
• the launch-centered Earth-fixed (LCEF) frame, g;
• the launch-centered inertial (LCI) frame, a.

2.2. Strapdown Inertial Navigation Mechanization in the LCEF Frame

The strapdown inertial navigation equation in the LCEF frame is as follows and the mechanization is shown in Figure 1.

$$\left[\begin{array}{l}{\dot{\mathit{P}}}^g\\ {\dot{\mathit{V}}}^g\\ {\dot{\mathit{R}}}_b^g\end{array}\right]=\left[\begin{array}{c}{\mathit{V}}^g\\ {\mathit{R}}_b^g{\mathit{f}}^b-2{\mathit{\Omega }}_{ag}^g{\mathit{V}}^g+{\mathit{g}}^g\\ {\mathit{R}}_b^g{\mathit{\Omega }}_{gb}^b\end{array}\right]$$

(1)

where ${\mathit{P}}^g,{\mathit{V}}^g,{\mathit{R}}_b^g$ represent the position, velocity, and attitude matrices of the carrier (hypersonic vehicle) in the LCEF frame, respectively, and ${\mathit{R}}_b^g={\left({\mathit{R}}_g^b\right)}^T$. ${\mathit{f}}^b$ represents the specific force measured by the triaxial accelerometers; ${\mathit{\Omega }}_{ag}^g$ is the anti-symmetric matrix of the carrier relative to the rotational angular velocity ${\mathit{\omega }}_{ag}^g$; ${\mathit{g}}^g$ is the gravity of the carrier in the LCEF frame; ${\mathit{\Omega }}_{gb}^b$ is the anti-symmetric matrix of the carrier angular velocity ${\mathit{\omega }}_{gb}^b$; and ${\mathit{\omega }}_{gb}^b={\mathit{\omega }}_{ab}^b-{\mathit{R}}_g^b{\mathit{\omega }}_{ag}^g$, where ${\mathit{\omega }}_{ab}^b$ is the angular velocity measured by the triaxial gyroscopes, abbreviated as ${\mathit{\omega }}_{}^b$.

2.3. Navigation Numerical Update Algorithm

The strapdown inertial navigation algorithm in the LCEF frame consists of three parts: attitude, velocity, and position update algorithms [15]. Since this navigation numerical update algorithm has been deduced in detail in reference [15], the flow diagram of the update algorithm is presented in Figure 2.

In Figure 2, T represents the calculation cycle; $\Delta {\mathit{\theta }}_1,\Delta {\mathit{\theta }}_2$ are the angular increments of two equal-interval samples in a calculation cycle; $\Delta {\mathit{\theta }}_k,\Delta V_k$ are the angular increment and velocity increment of the calculation cycle, respectively; ${\mathit{\Phi }}_k$ is the rotation vector; ${\mathit{q}}_b^g$ represents the attitude quaternion; and ${\mathit{g}}^g$ represents the gravity vector in the LCEF frame.

3. Tightly Coupled Integrated Navigation in the LCEF Frame

The schematic diagram of the tightly coupled SINS/GPS integrated navigation algorithm in the LCEF frame is shown in Figure 3.

3.1. SINS Error Equation in the LCEF Frame

The SINS navigation error equations in the LCEF frame include attitude, velocity, and position error equations [14]. They are obtained by linearizing the attitude, velocity, and position equations in Equation (1) with first-order small disturbances, as shown in Equation (2).

$$\left[\begin{array}{c}{\dot{\mathit{\varphi }}}^g\\ \delta {\dot{\mathit{V}}}^g\\ \delta {\dot{\mathit{P}}}^g\end{array}\right]=\left[\begin{array}{c}-{\mathit{\Omega }}_{ag}^g{\mathit{\varphi }}^g-{\mathit{R}}_b^g\delta {\mathit{\omega }}_{ab}^b\\ {\mathit{F}}^g{\mathit{\varphi }}^g-2{\mathit{\Omega }}_{ag}^g\delta {\mathit{V}}^g+\delta {\mathit{g}}^g+{\mathit{R}}_b^g\delta {\mathit{f}}^b\\ \delta {\mathit{V}}^g\end{array}\right]$$

(2)

where ${\mathit{F}}^g$ is the anti-symmetric matrix of the specific force ${\mathit{f}}^g$ in the LCEF frame; $\delta {\mathit{g}}^g$ is the gravity error of the carrier in the LCEF frame; and $\delta {\mathit{g}}^g=G_P\delta P^g$.

3.2. Tightly Coupled State Equation in the LCEF Frame

The state equation of the KF system for tightly coupled SINS/GPS is

$$\left[\begin{array}{c}\delta {\dot{\mathit{x}}}_{\mathrm{SINS}}\\ \delta {\dot{\mathit{x}}}_{\mathrm{GPS}}\end{array}\right]=\left[\begin{array}{cc}{\mathit{F}}_{\mathrm{SINS}}& 0\\ 0& {\mathit{F}}_{\mathrm{GPS}}\end{array}\right]\left[\begin{array}{c}\delta {\mathit{x}}_{\mathrm{SINS}}\\ \delta {\mathit{x}}_{\mathrm{GPS}}\end{array}\right]+\left[\begin{array}{cc}{\mathit{G}}_{\mathrm{SINS}}& 0\\ 0& {\mathit{G}}_{\mathrm{GPS}}\end{array}\right]\left[\begin{array}{c}{w}_{\mathrm{SINS}}\\ w_{\mathrm{GPS}}\end{array}\right]$$

(3)

The SINS state vector includes the attitude, velocity, position, gyroscope, and accelerometer errors in the LCEF frame [14].

$$\delta {\dot{\mathit{x}}}_{\mathrm{SINS},15\times 1}^{}={[\delta {\mathit{\varphi }}^g,\delta {\mathit{V}}_{3\times 1}^g,\delta {\mathit{P}}_{3\times 1}^g,\delta {\mathit{\epsilon }}_{3\times 1}^b,\delta {\mathbf{𝛻}}_{3\times 1}^b]}^T$$

(4)

The GPS state vector includes the range error and clock drift caused by satellite receiver clock deviation.

$$\delta {\mathit{x}}_{\mathrm{GPS}}={[\begin{array}{cc}\delta b_r& \delta d_r\end{array}]}^T$$

(5)

Moreover:

$$\left\{\begin{array}{l}\delta {\dot{b}}_r=\delta {\dot{d}}_r+w_b\hfill \\ \delta {\dot{d}}_r=w_d\hfill \end{array}\right.$$

(6)

where $w_b$ and $w_d$ are the standard deviations of white noise of clock deviation and clock drift, respectively.

By expanding Equation (3), we obtain:

$$\left[\begin{array}{c}{\dot{\mathit{\varphi }}}^g\\ \delta {\dot{V}}^g\\ \delta {\dot{P}}^g\\ {\dot{\mathit{\epsilon }}}^b\\ {\dot{\mathbf{𝛻}}}^b\\ \delta {\dot{b}}_r\\ \delta {\dot{d}}_r\end{array}\right]=\left[\begin{array}{cc}\begin{array}{ccccc}-{\mathit{\Omega }}_{ag}^g& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& -{\mathit{R}}_b^g& {\mathbf{0}}_{3\times 3}\\ {\mathit{F}}^g& -2{\mathit{\Omega }}_{ag}^g& {\mathit{G}}_P& {\mathbf{0}}_{3\times 3}& {\mathit{R}}_b^g\\ {\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}& {\mathbf{0}}_{15\times 2}\\ {\mathbf{0}}_{2\times 15}& \begin{array}{cc}0& 1\\ 0& 0\end{array}\end{array}\right]\left[\begin{array}{c}{\mathit{\varphi }}^g\\ \delta V^g\\ \delta P^g\\ {\mathit{\epsilon }}^b\\ {\mathbf{𝛻}}^b\\ \delta b_r\\ \delta d_r\end{array}\right]+\left[\begin{array}{cc}\begin{array}{c}\begin{array}{cc}-{\mathit{R}}_b^g& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathit{R}}_b^g\end{array}\\ \begin{array}{cc}{\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\end{array}& {\mathbf{0}}_{15\times 2}\\ {\mathbf{0}}_{2\times 6}& \begin{array}{cc}1& 0\\ 0& 1\end{array}\end{array}\right]\left[\begin{array}{c}{w}_g\\ w_a\\ w_b\\ w_d\end{array}\right]$$

(7)

3.3. Tightly Coupled Measurement Equation in the LCEF Frame

The SINS/GPS tightly coupled KF measurement equation is:

$$\delta {\mathit{z}}_k={\mathit{H}}_k\delta {\mathit{x}}_k+{\mathit{\eta }}_k$$

(8)

For M visible satellites:

$$\delta \mathit{z}=\left[\begin{array}{c}\delta {\mathit{z}}_{\rho }\\ \delta {\mathit{z}}_{\dot{\rho }}\end{array}\right]=\left[\begin{array}{c}{\mathit{\rho }}_{\mathrm{SINS}}-{\mathit{\rho }}_{\mathrm{GPS}}\\ {\dot{\mathit{\rho }}}_{\mathrm{SINS}}-{\dot{\mathit{\rho }}}_{\mathrm{GPS}}\end{array}\right]=\left[\begin{array}{c}{\rho }_{\mathrm{SINS}}^1-{\rho }_{\mathrm{GPS}}^1\\ {\rho }_{\mathrm{SINS}}^2-{\rho }_{\mathrm{GPS}}^2\\ ⋮\\ {\rho }_{\mathrm{SINS}}^M-{\rho }_{\mathrm{GPS}}^M\\ {\dot{\rho }}_{\mathrm{SINS}}^1-{\dot{\rho }}_{\mathrm{GPS}}^1\\ {\dot{\rho }}_{\mathrm{SINS}}^2-{\dot{\rho }}_{\mathrm{GPS}}^2\\ ⋮\\ {\dot{\rho }}_{\mathrm{SINS}}^M-{\dot{\rho }}_{\mathrm{GPS}}^M\end{array}\right]$$

(9)

As shown in Equation (9), the SINS/GPS tightly coupled measurement equation is composed of pseudo-range and pseudo-range rate measurement equations, obtained through ${\mathit{\rho }}_{\mathrm{SINS}}-{\mathit{\rho }}_{\mathrm{GPS}}$ and ${\dot{\mathit{\rho }}}_{\mathrm{SINS}}-{\dot{\mathit{\rho }}}_{\mathrm{GPS}}$, respectively.

3.3.1. Pseudo-Range Measurement Equation in the LCEF Frame

The pseudo-range from the carrier position to the m-th satellite calculated by the strapdown inertial navigation system is

$${\rho }_{\mathrm{SINS}}^m=\sqrt{{(x_{\mathrm{SINS}}^e-x^m)}^2+{(y_{\mathrm{SINS}}^e-y^m)}^2+{(z_{\mathrm{SINS}}^e-z^m)}^2}$$

(10)

where $(x_{\mathrm{SINS}}^e,y_{\mathrm{SINS}}^e,z_{\mathrm{SINS}}^e)$ is the position in the ECEF frame calculated by strapdown inertial navigation, and $(x^m,y^m,z^m)$ is the position of the m-th GPS satellite in the ECEF frame.

The pseudo-range measurement equation of the m-th GPS satellite is

$${\rho }_{\mathrm{GPS}}^m=\sqrt{{(x^e-x^m)}^2+{(y^e-y^m)}^2+{(z^e-z^m)}^2}+\delta b_r+{\tilde{\epsilon }}_{\rho }^m$$

(11)

where $\delta b_r$ is the distance error caused by clock deviation $\delta t_r$; ${\tilde{\epsilon }}_{\rho }^m$ is the pseudo-range measurement noise, which is mainly composed of the ionospheric error, tropospheric error, and multi-path effect error; and ${\mathit{P}}_{\mathrm{GPS}}^e=[x^e,y^e,z^e{]}^T$ is the position of the GPS receiver in the ECEF frame.

After the Taylor series expansion of Equation (11) and subtracting Equation (10) from it, the following can be obtained:

$${\rho }_{\mathrm{SINS}}^m-{\rho }_{\mathrm{GPS}}^m=-\frac{(x_{\mathrm{SINS}}^e-x^m)(x-x_{\mathrm{SINS}}^e)+(y_{\mathrm{SINS}}^e-y^m)(y-y_{\mathrm{SINS}}^e)+(z_{\mathrm{SINS}}^e-z^m)(z-z_{\mathrm{SINS}}^e)}{{\rho }_{\mathrm{SINS}}^m}-\delta b_r+{\tilde{\epsilon }}_{\rho }^m$$

(12)

The line-of-sight unit vector from the m-th satellite to the carrier position calculated by SINS/GPS is defined as:

$${\mathit{1}}_{\mathrm{SINS}}^m=\left[\begin{array}{ccc}\frac{x_{\mathrm{SINS}}^e-x^m}{{\rho }_{\mathrm{SINS}}^m}& \frac{y_{\mathrm{SINS}}^e-y^m}{{\rho }_{\mathrm{SINS}}^m}& \frac{z_{\mathrm{SINS}}^e-z_m}{{\rho }_{\mathrm{SINS}}^m}\end{array}\right]$$

(13)

Substituting Equation (13) into Equation (12), we obtain:

$${\rho }_{\mathrm{SINS}}^m-{\rho }_{\mathrm{GPS}}^m=1_{x,\mathrm{SINS}}^m\delta x^e+1_{y,\mathrm{SINS}}^m\delta y^e+1_{z,\mathrm{SINS}}^m\delta z^e-\delta b_r+{\tilde{\mathit{\epsilon }}}_{\rho }^m$$

(14)

where:

$$\left[\begin{array}{c}\delta x^e\\ \delta y^e\\ \delta z^e\end{array}\right]=\left[\begin{array}{c}{x}_{\mathrm{SINS}}^e-x^e\\ y_{\mathrm{SINS}}^e-y^e\\ z_{\mathrm{SINS}}^e-z^e\end{array}\right]$$

(15)

For M visible satellites, Equation (15) can be written as:

$$\delta {\mathit{z}}_{\rho }={\mathit{\rho }}_{\mathrm{SINS}}-{\mathit{\rho }}_{\mathrm{GPS}}=\left[\begin{array}{c}{\rho }_{\mathrm{SINS}}^1-{\rho }_{\mathrm{GPS}}^1\\ {\rho }_{\mathrm{SINS}}^2-{\rho }_{\mathrm{GPS}}^2\\ ⋮\\ {\rho }_{\mathrm{SINS}}^M-{\rho }_{\mathrm{GPS}}^M\end{array}\right]=\left[\begin{array}{ccc}{1}_{x,\mathrm{SINS}}^1& 1_{y,\mathrm{SINS}}^1& 1_{z,\mathrm{SINS}}^1\\ 1_{x,\mathrm{SINS}}^2& 1_{y,\mathrm{SINS}}^2& 1_{z,\mathrm{SINS}}^2\\ ⋮& ⋮& ⋮\\ 1_{x,\mathrm{SINS}}^M& 1_{y,\mathrm{SINS}}^M& 1_{z,\mathrm{SINS}}^M\end{array}\right]\left[\begin{array}{c}\delta x^e\\ \delta y^e\\ \delta z^e\end{array}\right]-\left[\begin{array}{c}\delta b_r^1\\ \delta b_r^2\\ ⋮\\ \delta b_r^M\end{array}\right]+\left[\begin{array}{c}{\tilde{\epsilon }}_{\rho }^1\\ {\tilde{\epsilon }}_{\rho }^2\\ ⋮\\ {\tilde{\epsilon }}_{\rho }^M\end{array}\right]$$

(16)

Define:

$${\mathit{G}}_{M\times 3}=\left[\begin{array}{ccc}{1}_{x,\mathrm{SINS}}^1& 1_{y,\mathrm{SINS}}^1& 1_{z,\mathrm{SINS}}^1\\ 1_{x,\mathrm{SINS}}^2& 1_{y,\mathrm{SINS}}^2& 1_{z,\mathrm{SINS}}^2\\ ⋮& ⋮& ⋮\\ 1_{x,\mathrm{SINS}}^M& 1_{y,\mathrm{SINS}}^M& 1_{z,\mathrm{SINS}}^M\end{array}\right]$$

(17)

$$\delta {\mathit{b}}_{r,M\times 1}={[\begin{array}{cccc}\delta b_r^1& \delta b_r^2& \cdots & \delta b_r^M\end{array}]}^T$$

(18)

$${\tilde{\mathit{\epsilon }}}_{\rho ,M\times 1}={[\begin{array}{cccc}{\tilde{\epsilon }}_{\rho }^1& {\tilde{\epsilon }}_{\rho }^2& \cdots & {\tilde{\epsilon }}_{\rho }^M\end{array}]}^T$$

(19)

Substituting Equations (17)–(19) into Equation (16), we obtain:

$$\delta {\mathit{z}}_{\rho }={\mathit{\rho }}_{SINS}-{\mathit{\rho }}_{\mathrm{GPS}}={\mathit{G}}_{M\times 3}\left[\begin{array}{c}\delta x^e\\ \delta y^e\\ \delta z^e\end{array}\right]-\delta {\mathit{b}}_{r,M\times 1}+{\tilde{\mathit{\epsilon }}}_{\rho ,M\times 1}$$

(20)

Transforming the position in the LCEF frame to the ECEF frame, we get:

$$\left[\begin{array}{c}\delta x^e\\ \delta y^e\\ \delta z^e\end{array}\right]={\mathit{R}}_g^e\left[\begin{array}{c}\delta x^g\\ \delta y^g\\ \delta z^g\end{array}\right]$$

(21)

where ${\mathit{R}}_g^e={({\mathit{R}}_e^g)}^T$; ${\mathit{R}}_e^g$ is the conversion matrix from the ECEF frame to the LCEF frame, which is relevant to the initial longitude ${\lambda }_0$, latitude $B_0$, and heading direction $A_0$ of the carrier, as shown in Equation (22).

$${\mathit{R}}_e^g={\mathit{R}}_y\left(-\left({90}^{\circ }+A_0\right)\right){\mathit{R}}_x\left(B_0\right){\mathit{R}}_z\left({\lambda }_0-{90}^{\circ }\right)$$

(22)

Substituting Equations (21) and (22) into Equation (20), we obtain:

$$\delta {\mathit{z}}_{\rho }={\mathit{\rho }}_{\mathrm{SINS}}-{\mathit{\rho }}_{\mathrm{GPS}}={\mathit{G}}_{M\times 3}{\mathit{R}}_g^e\left[\begin{array}{c}\delta x_g\\ \delta y_g\\ \delta z_g\end{array}\right]-\delta {\mathit{b}}_{r,M\times 1}+{\tilde{\mathit{\epsilon }}}_{\rho ,M\times 1}$$

(23)

Define:

$${\mathit{H}}_{M\times 3}^{}={\mathit{G}}_{M\times 3}{\mathit{R}}_g^e$$

(24)

Substituting Equation (24) into Equation (23), the pseudo-range measurement equation can be obtained as:

$$\delta {\mathit{z}}_{\rho }={\mathit{\rho }}_{\mathrm{SINS}}-{\mathit{\rho }}_{\mathrm{GPS}}={\mathit{H}}_{M\times 3}\left[\begin{array}{c}\delta x_g\\ \delta y_g\\ \delta z_g\end{array}\right]-\delta {\mathit{b}}_{r,M\times 1}+{\tilde{\mathit{\epsilon }}}_{\rho ,M\times 1}$$

(25)

3.3.2. Pseudo-Range Rate Measurement Equation in the LCEF Frame

The pseudo-range rate is modeled as:

$${\dot{\rho }}_{\mathrm{GPS}}^m=1_x^m(v_x^e-v_x^m)+1_y^m(v_y^e-v_y^m)+1_z^m(v_z^e-v_z^m)+\delta d_r+{\tilde{\epsilon }}_{\dot{\rho }}^m$$

(26)

where $\delta d_r=c\delta {\dot{t}}_r$ is the clock drift (m/s), and ${\mathit{1}}^m$ is the line-of-sight unit vector from the m-th satellite to the carrier, which can be represented as:

$${\mathit{1}}^m={\left[\begin{array}{ccc}{1}_{\mathit{x}}^m& 1_{\mathit{y}}^m& 1_{\mathit{z}}^m\end{array}\right]}^T=\frac{{[(x^e-x^m),(y^e-y^m),({\mathit{z}}^e-{\mathit{z}}^m)]}^T}{\sqrt{{(x^e-x^m)}^2+{(y^e-y^m)}^2+{(z^e-z^m)}^2}}$$

(27)

Considering the position as $\left(x_{\mathrm{SINS}}^e,y_{\mathrm{SINS}}^e,z_{\mathrm{SINS}}^e\right)$ and substituting it in Equation (27), the following can be obtained:

$${\mathit{1}}^m=\frac{{[(x_{\mathrm{SINS}}^e-x^m),(y_{\mathrm{SINS}}^e-y^m),(z_{\mathrm{SINS}}^e-z^m)]}^T}{\sqrt{{(x_{\mathrm{SINS}}^e-x^m)}^2+{(y_{\mathrm{SINS}}^e-y^m)}^2+{(z_{\mathrm{SINS}}^e-z^m)}^2}}={[\begin{array}{ccc}{1}_{x,\mathrm{SINS}}^m& 1_{y,\mathrm{SINS}}^m& 1_{z,\mathrm{SINS}}^m\end{array}]}^T$$

(28)

Taking the derivative of Equation (10), the following can be obtained:

$$\begin{array}{c}{\dot{\rho }}_{\mathrm{SINS}}^m=1_{x,\mathrm{SINS}}^m(v_{x,\mathrm{SINS}}^e-v_x^m)+1_{y,\mathrm{SINS}}^m(v_{y,\mathrm{SINS}}^e-v_y^m)\\ +1_{z,\mathrm{SINS}}^m(v_{z,\mathrm{SINS}}^e-v_z^m)\end{array}$$

(29)

where $(v_{x,\mathrm{SINS}}^e,v_{y,\mathrm{SINS}}^e,v_{z,\mathrm{SINS}}^e)$ is the velocity calculated by the strapdown inertial navigation in the ECEF frame, and $(v_x^m,v_y^m,v_z^m)$ is the velocity of the m-th satellite in the ECEF frame.

The difference between Equations (29) and (26) will give the following:

$$\delta {\mathit{z}}_{\dot{\rho }}^m={\dot{\rho }}_{\mathrm{SINS}}^m-{\dot{\rho }}_{\mathrm{GPS}}^m=\left[\begin{array}{ccc}{1}_{x,\mathrm{SINS}}^m& 1_{y,\mathrm{SINS}}^m& 1_{z,\mathrm{SINS}}^m\end{array}\right]\left[\begin{array}{c}\delta v_x^e\\ \delta v_y^e\\ \delta v_z^e\end{array}\right]-\delta d_r+{\tilde{\epsilon }}_{\dot{\rho }}^m$$

(30)

For M visible satellites, Equation (30) can be written as:

$$\delta {\mathit{z}}_{\dot{\rho }}={\dot{\mathit{\rho }}}_{\mathrm{SINS}}-{\dot{\mathit{\rho }}}_{\mathrm{GPS}}=\left[\begin{array}{c}{\dot{\rho }}_{\mathrm{SINS}}^1-{\dot{\rho }}_{\mathrm{GPS}}^1\\ {\dot{\rho }}_{\mathrm{SINS}}^2-{\dot{\rho }}_{\mathrm{GPS}}^2\\ ⋮\\ {\dot{\rho }}_{\mathrm{SINS}}^M-{\dot{\rho }}_{\mathrm{GPS}}^M\end{array}\right]=\left[\begin{array}{ccc}{1}_{x,\mathrm{SINS}}^1& 1_{y,\mathrm{SINS}}^1& 1_{z,\mathrm{SINS}}^1\\ 1_{x,\mathrm{SINS}}^2& 1_{y,\mathrm{SINS}}^2& 1_{z,\mathrm{SINS}}^2\\ ⋮& ⋮& ⋮\\ 1_{x,\mathrm{SINS}}^M& 1_{y,\mathrm{SINS}}^M& 1_{z,\mathrm{SINS}}^M\end{array}\right]\left[\begin{array}{c}\delta v_x^e\\ \delta v_y^e\\ \delta v_z^e\end{array}\right]-\left[\begin{array}{c}\delta d_r^1\\ \delta d_r^2\\ ⋮\\ \delta d_r^M\end{array}\right]+\left[\begin{array}{c}{\tilde{\epsilon }}_{\dot{\rho }}^1\\ {\tilde{\epsilon }}_{\dot{\rho }}^2\\ ⋮\\ {\tilde{\epsilon }}_{\dot{\rho }}^M\end{array}\right]$$

(31)

Define:

$$\delta {\mathit{d}}_{r,M\times 1}={[\begin{array}{cccc}\delta d_r^1& \delta d_r^2& \cdots & \delta d_r^M\end{array}]}^T$$

(32)

$${\tilde{\mathit{\epsilon }}}_{\dot{\rho },M\times 1}={[\begin{array}{cccc}{\tilde{\epsilon }}_{\dot{\rho }}^1& {\tilde{\epsilon }}_{\dot{\rho }}^2& \cdots & {\tilde{\epsilon }}_{\dot{\rho }}^M\end{array}]}^T$$

(33)

Substituting Equations (17), (32), and (33) into Equation (31), we obtain:

$$\delta {\mathit{z}}_{\dot{\rho }}={\dot{\mathit{\rho }}}_{\mathrm{SINS}}-{\dot{\mathit{\rho }}}_{\mathrm{GPS}}={\mathit{G}}_{M\times 3}\left[\begin{array}{c}\delta v_x^e\\ \delta v_y^e\\ \delta v_z^e\end{array}\right]-\delta {\mathit{d}}_{r,M\times 1}+{\tilde{\mathit{\epsilon }}}_{\dot{\rho },M\times 1}$$

(34)

Transforming the velocity in the LCEF frame to the ECEF frame, we obtain:

$$\left[\begin{array}{c}\delta v_x^e\\ \delta v_y^e\\ \delta v_z^e\end{array}\right]={\mathit{R}}_g^e\left[\begin{array}{c}\delta v_x^g\\ \delta v_y^g\\ \delta v_z^g\end{array}\right]$$

(35)

Substituting Equation (35) into Equation (34), we obtain:

$$\delta {\mathit{z}}_{\dot{\rho }}={\dot{\mathit{\rho }}}_{\mathrm{SINS}}-{\dot{\mathit{\rho }}}_{\mathrm{GPS}}={\mathit{G}}_{M\times 3}{\mathit{R}}_g^e\left[\begin{array}{c}\delta v_x^g\\ \delta v_y^g\\ \delta v_z^g\end{array}\right]-\delta {\mathit{d}}_{r,M\times 1}+{\tilde{\mathit{\epsilon }}}_{\dot{\rho },M\times 1}$$

(36)

Substituting Equation (24) into Equation (36), the pseudo-range rate measurement equation can be obtained as follows:

$$\delta {\mathit{z}}_{\dot{\rho }}={\dot{\mathit{\rho }}}_{\mathrm{SINS}}-{\dot{\mathit{\rho }}}_{\mathrm{GPS}}={\mathit{H}}_{M\times 3}^{\dot{\rho }}\left[\begin{array}{c}\delta v_x^g\\ \delta v_y^g\\ \delta v_z^g\end{array}\right]-\delta {\mathit{d}}_{r,M\times 1}+{\tilde{\mathit{\epsilon }}}_{\dot{\rho },M\times 1}$$

(37)

In summary, the measurement equation of the SINS/GPS tightly coupled integrated navigation system in the LCEF frame is:

$$\delta \mathit{z}=\left[\begin{array}{c}{\mathit{\rho }}_{\mathrm{SINS}}-{\mathit{\rho }}_{\mathrm{GPS}}\\ {\dot{\mathit{\rho }}}_{\mathrm{SINS}}-{\dot{\mathit{\rho }}}_{\mathrm{GPS}}\end{array}\right]=\left[\begin{array}{c}{\rho }_{\mathrm{SINS}}^1-{\rho }_{\mathrm{GPS}}^1\\ {\rho }_{\mathrm{SINS}}^2-{\rho }_{\mathrm{GPS}}^2\\ ⋮\\ {\rho }_{\mathrm{SINS}}^M-{\rho }_{\mathrm{GPS}}^M\\ {\dot{\rho }}_{\mathrm{SINS}}^1-{\dot{\rho }}_{\mathrm{GPS}}^1\\ {\dot{\rho }}_{\mathrm{SINS}}^2-{\dot{\rho }}_{\mathrm{GPS}}^2\\ ⋮\\ {\dot{\rho }}_{\mathrm{SINS}}^M-{\dot{\rho }}_{\mathrm{GPS}}^M\end{array}\right]=\left[\begin{array}{ccccc}{0}_{M\times 3}& 0_{M\times 3}& {\mathit{H}}_{M\times 3}^{}& \begin{array}{cc}{0}_{M\times 6}& -1_{M\times 1}\end{array}& 0_{M\times 1}\\ 0_{M\times 3}& {\mathit{H}}_{M\times 3}^{\dot{\rho }}& 0_{M\times 3}& \begin{array}{cc}{0}_{M\times 6}& 0_{M\times 1}\end{array}& -1_{M\times 1}\end{array}\right]\delta {\mathit{x}}_{17\times 1}+\left[\begin{array}{c}{\tilde{\mathit{\epsilon }}}_{\rho ,M\times 1}\\ {\tilde{\mathit{\epsilon }}}_{\dot{\rho },M\times 1}\end{array}\right]$$

(38)

where:

$$\delta {\dot{\mathit{x}}}_{17\times 1}^{}={[\begin{array}{ccccccc}\delta {\mathit{\varphi }}_{3\times 1}^g& \delta {\mathit{V}}_{3\times 1}^g& \delta {\mathit{P}}_{3\times 1}^g& \delta {\mathit{\epsilon }}_{3\times 1}^b& \delta {\mathbf{𝛻}}_{3\times 1}^b& \delta b_r& \delta d_r\end{array}]}^{\mathit{T}}$$

(39)

4. Simulation Verification

In order to verify the accuracy and superiority of the tightly coupled SINS/GPS integrated navigation in the LCEF frame, a hardware-in-the-loop (HWIL) simulation was performed. In the HWIL system, a three-axis rotation table simulates the flight attitude, while an actuator load simulator simulates the torque of the rudder. An IMU simulator receives real-time information on the theoretical specific force from a real-time 6DOF simulator. A GPS simulator receives real-time information the 6DOF simulator [15,16].

The integrated navigation results of the loosely and tightly coupled SINS/GPS in the LCEF frame were compared. In addition, the results of the tight coupling of the normal number of satellites were compared with those of the tight coupling of less than four satellites. For the simulation verification, we adopted a classic trajectory of a hypersonic vehicle with the duration of 1100 s for simulation verification [15,16]. The initial state of the trajectory was velocity 0 m/s, latitude 34.2°, longitude 108.9°, and height 400 m. In addition, the shooting direction, pitch angle, roll angle, and yaw angle were 200°, 90°, 0°, and 0°, respectively. The flight path is shown in Figure 4. Parameters of SINS/GPS integrated navigation system in the HWIL simulation are shown in

Table 1. Parameters of SINS/GPS integrated navigation simulation.

Simulation Parameters	Index
Gyro constant deviation/(°·h−1)	3.0
Gyro random error/(°·h−1/2)	0.3
Accelerometer constant offset/(g0)	1 × 10−3
White noise measured by accelerometer/(g0)	1 × 10−4
Initial attitude angle errors /(′′)	20,5,5
Initial velocity error/(m·s−1)	0.01
Initial position error/(m)	5
Satellite positioning precision/(m)	15
Satellite velocity measurement precision/(m·s−1)	0.3

.

4.1. Comparison between Loose and Tight Coupling

Figure 5, Figure 6, Figure 7 show the comparison of the HWIL simulation results between loosely and tightly coupled integrated navigation in the LCEF frame. In the SINS/GPS loose coupling simulation results in the LCEF frame, the pitch angle convergence error is 0.0048°, yaw angle convergence error is 0.028°, roll angle convergence error is 0.0021°, X-axis velocity convergence error is 0.08 m/s, Y-axis velocity convergence error is 0.07 m/s, z-axis velocity error is 0.12 m/s, X-axis position convergence error is 3.6 m, Y-axis position convergence error is 4.8 m, and Z-axis position convergence error is 5.2 m. In the SINS/GPS tightly coupled simulation results in the LCEF frame, the pitch angle convergence error is 0.0043°, yaw angle convergence error is 0.012°, roll angle convergence error is 0.0029°, X-axis velocity convergence error is 0.03 m/s, Y-axis velocity convergence error is 0.11 m/s, z-axis velocity convergence error is 0.06 m/s, X-axis position convergence error is 1.2 m, Y-axis position convergence error is 4.9 m, and Z-axis position convergence error is 4.9 m. Overall, the precision of the tightly coupled navigation system is better than that of the loosely coupled navigation system.

4.2. Navigation Comparison between the Normal-Number Satellite and Three-Satellite Integrated Navigation

Figure 8, Figure 9, Figure 10 show the result of the comparison of a SINS/GPS tightly coupled normal-number satellite and three-satellite integrated navigation in the LCEF frame. In the SINS/GPS tightly coupled simulation results of three effective satellites, the pitch angle convergence error is 0.012°, yaw angle convergence error is 0.034°, and roll angle convergence error is 0.029°; the maximum X-axis velocity error is 0.42 m/s, the maximum Y-axis velocity error is 0.86 m/s, and the maximum z-axis velocity error is 0.46 m/s; the maximum X-axis position error is 84 m, the maximum Y-axis position error is 213 m, and the maximum position error of z-axis is 118 m.

Overall, when adopting three satellites for navigation, the navigation precision declines while integrated navigation can still be conducted. Thus, tightly coupled navigation has great advantages compared with loosely coupled navigation.

5. Conclusions

In this paper, the strapdown inertial navigation algorithm and the tightly coupled integrated navigation algorithm of hypersonic vehicles under the LCEF frame were designed and verified by simulation. The integrated navigation system under the LCEF frame cannot be singular in the attitude angle under vertical launch conditions, which is suitable for boost-glide vehicles. The LCEF frame adopts the J2 gravity model, which is different from the normal gravity model in the local-level frame. It is suitable for near-space flying altitude of above 20 km. The digital simulation test verifies that the precision of tightly coupled navigation is better than that of loosely coupled navigation. Tightly coupled navigation functions even when the effective number of satellites is less than four; thus, the issue of vulnerability of satellite navigation to interference is overcome.