Information Fusion Based on Complementary Filter for SINS/CNS/GPS Integrated Navigation System of Aerospace Plane

Abstract

In order to solve the problems of heavy computational load and poor real time of the information fusion method based on the federated Kalman filter (FKF), a novel information fusion method based on the complementary filter is proposed for strapdown inertial navigation (SINS)/celestial navigation system (CNS)/global positioning system (GPS) integrated navigation system of an aerospace plane. The complementary filters are designed to achieve the estimations of attitude, velocity, and position in the SINS/CNS/GPS integrated navigation system, respectively. The simulation results show that the proposed information fusion method can effectively realize SINS/CNS/GPS information fusion. Compared with FKF, the method based on complementary filter (CF) has the advantages of simplicity, small calculation, good real-time performance, good stability, no need for initial alignment, fast convergence, etc. Furthermore, the computational efficiency of CF is increased by 94.81%. Finally, the superiority of the proposed CF-based method is verified by both the semi-physical simulation and real-time system experiment.

1. Introduction

The aerospace plane is a reusable aerospace vehicle with the features of aircraft, spacecraft, and carrier, in which the accuracy and reliability requirements of the navigation system are very high due to its wide flight envelope, complex flight environment, and special mission [1]. The common navigation systems in aerospace vehicle include the strapdown inertial navigation system (SINS), global positioning system (GPS), celestial navigation system (CNS), and so on. Considering the black-out phenomenon in atmospheric reentry, the single SINS navigation using single-axis rotation modulation [2] or a second-order damping scheme [3] was adopted for the navigation of hypersonic vehicles. In order to serve the needs of spacecraft, the global navigation satellite system (GNSS) based on GPS [4], global navigation satellite system (GLONASS) [5], GPS/Galileo [6], or GPS/beidou navigation satellite system (BDS) [7] was proposed. An approach using the measurements from GNSS was proposed for the navigation of circumlunar spacecraft [8] or high-Earth-orbits spacecraft [9]. References [10,11] studied the high-precision autonomous celestial navigation based on astronomical information for spacecraft.

In view of the advantages and disadvantages of each navigation subsystem, it is difficult for any single navigation system to provide high-precision navigation parameters independently. In this background, the method of integrated navigation was proposed. Compared with a single navigation system, the integrated navigation system has higher accuracy as well as better reliability and information completeness; therefore, it can be applied for the scenario of an aerospace plane. In [12], a GPS/SINS integrated navigation algorithm in the launch-centered Earth-fixed (LCEF) frame was proposed to suit the features of hypersonic vehicles and meet the requirements of their flight control system. For the navigation applications of a hypersonic vehicle, the design of a tightly coupled SINS/GNSS integrated navigation system was presented to resist the disturbance of measurement errors, which adopted the innovation orthogonality-based robust unscented Kalman filter (IO-RUKF) [13] or order-reduction robust filter [14]. Reference [15] reviewed the inertial navigation system (INS)/CNS integrated navigation technology. For space vehicles of probes and hypersonic cruise vehicles, two tightly coupled INS/CNS integrated navigation methods were proposed, which adopted the accurate CNS celestial measurements assisted by the corrected infrared Earth sensor [16] and the weighted multi-stars observations according to the different error levels of starlight observations [17], respectively. Considering the high navigation requirements and complex practical environment of high dynamic aircraft, the INS/CNS/GPS integrated navigation approach was presented, which used the robust filtering algorithm based on extended H∞ filter [18], Kalman filter assisted by neural network ensembles [19], or unscented Kalman filter (UKF)-based Federated filter [20]. In [21], a new INS/GNSS/CNS integrated navigation method for hypersonic cruising aircrafts (HCVs) based on non-Keplerian orbits mode was presented. For autonomous SINS/GPS/star sensor (SS) navigation of space vehicle, the information fusion algorithms of the extend Kalman filter [22] and federated Kalman filter [23] were designed, respectively. In [24], a federated Sage–Husa adaptive filter was proposed for a SINS/CNS/GNSS navigation system with time-varying noise. For INS/GNSS/CNS integration, Hu et al. proposed two novel information fusion methods: the modified federated Kalman filter (MFKF) based on state decomposition [25] and the matrix weighted multi-sensor data fusion methodology with a two-level structure [26]. Gao et al. presented two multi-sensor optimal data fusion methods with “bottom–top-levels” structures for INS/GNSS/CNS integrated navigation, of which one was based on a UKF filter [27], and another was based on the adaptive fading unscented Kalman filter [28]. However, the data fusion methods for INS/CNS/GPS integrated navigation in the existing literature are mostly implemented by Kalman filter (KF) or federated Kalman filter (FKF). Since the state dimension of FKF in an INS/CNS/GPS integrated navigation system is large, the computational load is heavy and the computational efficiency is low, and the precise filtering model and parameters are required for FKF. In the case of actual non-Gaussian noise or a large model error, the filtering divergence may occur. State observer is also a method of state estimation for a nonlinear system (particularly speed estimation), such as the approximately-reconstruct-state-variables observer [29], approximated-velocity-switching observer [30], designed-for-Lipschitz-nonlinear-system observer [31], etc. However, the estimation method based on state observer has the following disadvantages: (1) It is difficult to select the feedback gain matrix of the general state observer, which needs to be determined according to experiments or experience; (2) Without consideration of the random error of the system, the estimation accuracy of the observer is affected by the stochastic noise of the system; (3) The design of the state observer requires the system to be completely observable, but the integrated navigation system generally does not meet the requirement of complete observability. Therefore, the estimation method based on the state observer is not suitable for a complex multi-source integrated navigation system.

Compared with FKF, the design of complementary filter (CF) is based on the frequency-domain characteristics of navigation errors of each subsystem, and it does not require the exact system model and noise statistics. The CF filter has the advantages of a simple algorithm, low computational load, good real-time computations, etc. When it is used in an integrated navigation system, CF can achieve the closely same filtering precision as FKF [32,33,34]. In this paper, a novel information fusion method based on complementary filter is proposed for an SINS/CNS/GPS integrated navigation system to improve both the computational efficiency and real-time performance. By utilizing the method of CF filtering, the attitude/speed/position estimations of a SINS/CNS/GPS integrated navigation system are realized, respectively. In addition, the proposed information fusion method has the advantages of low computation load, good stability, and steady-state accuracy, which meet the requirements of an integrated navigation system for an aerospace plane.

The rest of this paper is organized as follows: Section 2 introduces the coordinate frames in use, inertial navigation mechanization, and CNS measurement transformation from the $i$ frame to the $n$ frame. Section 3 introduces the principle of complementary filter and designs the information fusion algorithms for an SINS/CNS/GPS integrated navigation system. Section 4 analyzes the frequency characteristics of navigation errors of each subsystem and selects the appropriate cut-off frequencies of complementary filters. A semi-physical simulation experiment and result analysis are provided in Section 5. In addition, the content of this paper is summarized in Section 6.

2. Coordinate Frames, INS Mechanization and CNS Measurement Transformation

2.1. Coordinate Frames in Use

The coordinate frames in use are defined as follows:

• Navigation coordinate frame ($n$ frame): locally level geographic coordinates, which are defined with its $z_n$-axis upward along the local geodetic vertical, $y_n$-axis north (and horizon), and $x_n$-axis east (and horizon);
• Body-fixed coordinate frame ($b$ frame): located at the centroid of the vehicle, defined with its $y_b$-axis forward along the longitudinal axis of the vehicle, $x_b$-axis right, and $z_b$-axis upward;
• Earth-centered Earth-fixed coordinate frame ($e$ frame): located at the center of the Earth, the $x_e$-axis extends from the origin to the intersection of the prime meridian (Longitude $0°$) and the Equator (Latitude $0°$), the $z_e$-axis is along the spin axis of the Earth, pointing to the north pole, the $y_e$-axis is orthogonal to both the $x_e$-axis and $z_e$-axis; the three coordinate axes follow the right-handed rule.
• Earth-centered inertial coordinate frame ($i$ frame): located at the center of the Earth; the $z_i$-axis is along the spin axis of the Earth, pointing to the north pole; the $x_i$-axis extends from the origin to the spring equinox in the equatorial plane; the $y_i$-axis is orthogonal to both the $x_i$-axis and $z_i$-axis; $x_i$, $y_i$, and $z_i$ follow the right-handed rule.

2.2. SINS Mechanization

The navigation parameters of SINS include the attitude quaternion $q_b^n={\left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right]}^{\mathrm{T}}$, velocity ${\mathit{v}}_{}^n={\left[\begin{array}{ccc}{v}_E& v_N& v_U\end{array}\right]}^{\mathrm{T}}$, and position ${\mathit{p}}_{}^n={\left[\begin{array}{ccc}L& \lambda & h\end{array}\right]}^{\mathrm{T}}$; the superscript $n$ denotes the navigation frame and the subscript $b$ denotes the body-fixed frame, $q_i$ ($i=0,1,2,3$) denotes the ith component of attitude quaternion. $v_E$, $v_N$, and $v_U$ represent the eastern velocity, northern velocity, and vertical velocity, respectively. $L$, $\lambda$, and $h$ represent the latitude, longitude, and altitude, respectively, and the mechanization equations for SINS are expressed in Equation (1).

$$\{\begin{array}{l}\left[\begin{array}{c}{\dot{\mathit{q}}}_b^n\\ {\dot{\mathit{v}}}_{}^n\\ {\dot{\mathit{p}}}_{}^n\end{array}\right]=\left[\begin{array}{c}\frac{1}{2}{\mathit{q}}_b^n\otimes {\mathbf{\omega }}_{nb}^b\\ {\mathit{f}}_{}^n-({\mathbf{\omega }}_{en}^n+2{\mathbf{\omega }}_{ie}^n)\times {\mathit{v}}_{}^n+{\mathit{g}}^n\\ M_{pv}{\mathit{v}}_{}^n\end{array}\right]\\ M_{pv}=\left[\begin{array}{ccc}0& 1/\left(R_M+h\right)& 0\\ \sec L/\left(R_N+h\right)& 0& 0\\ 0& 0& 1\end{array}\right]\end{array}$$

(1)

where ${\mathbf{\omega }}_{nb}^b$ represents the angular velocity of the $b$ frame relative to the $n$ frame expressed in the $b$ frame, $f_{}^n$ represents the measurement of a specific force in the $n$ frame, ${\mathbf{\omega }}_{ie}^n$ represents the Earth rotation rate denoted in the $n$ frame, ${\mathbf{\omega }}_{en}^n$ represents the angular velocity of the $n$ frame relative to the $e$ frame coordinated in the $n$ frame, $g_{}^n$ represents the gravity acceleration expressed in the $n$ frame, $R_M$ and $R_N$ are the radii of curvature in meridian and prime vertical, ($L$, $\lambda$, $h$) is the position of the vehicle in latitude, longitude and altitude.

2.3. CNS Measurement Transformation

The output of the CNS is the attitude quaternion $q_b^{i,cns}$ from the $b$ frame to the $i$ frame, but the output of SINS is the attitude quaternion $q_b^{n,ins}$ from the $b$ frame to the $n$ frame. If the complementary filter is selected for the information fusion of SINS/CNS integration, the parameter conversion is required firstly; that is to say, $q_b^{i,cns}$ (the attitude quaternion from the $b$ frame to the $i$ frame) must be converted into $q_b^{n,cns}$ (the attitude quaternion from the $b$ frame to the $n$ frame). The algorithm of CNS quaternion transformation from $q_b^{i,cns}$ to $q_b^{n,cns}$ is given in this subsection. According to the chain rule, the attitude quaternion $q_b^{n,cns}$ is

$$q_b^{n,cns}=q_e^n\otimes q_i^e\otimes q_b^{i,cns}$$

(2)

where $q_e^n$ is the transformation quaternion from the $e$ frame to the $n$ frame, and $q_i^e$ is the transformation quaternion from the $i$ frame to the $e$ frame.

2.3.1. The Method for Calculating $q_e^n$

Given the longitude ${\lambda }^{gps}$ and latitude $L^{gps}$ of GPS, the position matrix $C_e^{n,gps}$ can be calculated according to the relationship between the $n$ frame and $e$ frame, and the expression of $C_e^{n,gps}$ is shown in Equation (3):

$$C_e^{n,gps}=\left[\begin{array}{ccc}-\sin ({\lambda }^{gps})& \cos ({\lambda }^{gps})& 0\\ -\sin (L^{gps})cos({\lambda }^{gps})& -\sin (L^{gps})sin({\lambda }^{gps})& \cos (L^{gps})\\ \cos (L^{gps})cos({\lambda }^{gps})& \cos (L^{gps})sin({\lambda }^{gps})& \sin (L^{gps})\end{array}\right]$$

(3)

According to the relationship between the direction cosine matrix and corresponding quaternion, the expression of quaternion $q_e^n$ is as follows:

$$q_e^n=Mat2Quat(C_e^{n,gps})$$

(4)

where $Mat2Quat(•)$ represents the function that calculates the quaternion according to the direction cosine matrix, and its expression is shown in Equation (5).

For any direction cosine matrix $C={\left[T_{ij}^{}\right]}_{3\times 3}$ ($i,j=1,2,3$), the corresponding attitude quaternion is defined as $q\triangleq {\left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right]}^{\mathrm{T}}$; then, the method of calculating $q_i\left(i=0,1,2,3\right)$ is as follows:

$$\{\begin{array}{l}{q}_0=\frac{1}{2}\sqrt{1+T_{11}+T_{22}+T_{33}}\\ q_1=\frac{1}{2}\mathrm{sign}\left(T_{32}-T_{23}\right)\sqrt{1+T_{11}-T_{22}-T_{33}}\\ q_2=\frac{1}{2}\mathrm{sign}\left(T_{13}-T_{31}\right)\sqrt{1-T_{11}+T_{22}-T_{33}}\\ q_3=\frac{1}{2}\mathrm{sign}\left(T_{21}-T_{12}\right)\sqrt{1-T_{11}-T_{22}+T_{33}}\end{array}$$

(5)

where $\mathrm{sign}(•)$ represents the sign function.

2.3.2. The Method for Calculating $q_i^e$

The quaternion $q_i^e$ can be calculated according to the Greenwich Mean Time and Earth rotation rate. Given Greenwich Mean Time $t_G$ and Earth rotation rate ${\omega }_{ie}$, the Greenwich Hour Angle $G_{ST}$ (the rotation angle of the $e$ frame relative to the $i$ frame along the $z_i$ axis) is obtained by multiplying ${\omega }_{ie}$ and $t_G$ together:

$$G_{ST}={\omega }_{ie}{t}_G$$

(6)

Then, the transformation matrix $C_i^e$ can be calculated, as shown in Equation (7):

$$C_i^e=\left[\begin{array}{ccc}\cos G_{ST}& \sin G_{ST}& 0\\ -\sin G_{ST}& \cos G_{ST}& 0\\ 0& 0& 1\end{array}\right].$$

(7)

According to the relationship between the direction cosine matrix and corresponding attitude quaternion, the quaternion $q_i^e$ can be calculated using $C_i^e$, and its expression is shown in Equation (8):

$$q_i^e=Mat2Quat(C_i^e).$$

(8)

3. Design of Complementary Filters for Integrated Navigation

3.1. The Principle of Complementary Filter

The complementary filtering method can restrain the error divergence of the integrated navigation system, which is designed according to the frequency-domain characteristics of errors of different subsystems. For any signal variable $X$, when it is measured using two uncorrelated methods (for convenience, they are denoted as Method #1 and Method #2, respectively), two measurement results $Z_1$ and $Z_2$ can be obtained, and they can be expressed as:

$$\{\begin{array}{c}{Z}_1=X+V_1\\ Z_2=X+V_2\end{array}$$

(9)

where $Z_1$ denotes the measurement result using Method #1, $V_1$ denotes the measuring error of $Z_1$; $Z_2$ denotes the measurement result using Method #2, $V_2$ denotes the measuring error of $Z_2$; $V_1$ and $V_2$ are assumed to be the low-frequency error and high-frequency error, respectively.

In order to obtain an accurate estimate of variable $X$, the measurements $Z_1$ and $Z_2$ are filtered using the high-pass filter (H.P.F. for short) and low-pass filter (L.P.F for short), respectively, so that the errors $V_1$ and $V_2$ could be eliminated.

In the design of a complementary filter, the transfer functions of H.P.F. and L.P.F are denoted as $G_1^{}\left(s\right)$ and $G_2^{}\left(s\right)$, respectively. By selecting appropriate parameters for H.P.F. and L.P.F, the condition $G_1^{}\left(s\right)+G_2^{}\left(s\right)=1$ can be satisfied; then, the estimate $\widehat{X}(s)$ of $X(s)$ is obtained by applying the signal reconstruction (S.R.) method (i.e., the sum of the outputs of $G_1^{}\left(s\right)$ and $G_2^{}\left(s\right)$). For the low-pass filter $G_2^{}\left(s\right)$ and high-pass filter $G_1^{}\left(s\right)$, the appropriate cut-off frequencies are selected so that $G_1^{}\left(s\right)V_1\left(s\right)$ and $G_2^{}\left(s\right)V_2\left(s\right)$ are approximate to zero, and the sum of the outputs of $G_1^{}\left(s\right)$ and $G_2^{}\left(s\right)$ will be close to the true value of signal variable $X$, as shown in Equation (10):

$$\widehat{X}\left(s\right)=G_1^{}\left(s\right)Z_1\left(s\right)+G_2^{}\left(s\right)Z_2\left(s\right)=X\left(s\right)+G_1^{}\left(s\right)V_1\left(s\right)+G_2^{}\left(s\right)V_2\left(s\right)\approx X\left(s\right).$$

(10)

The principle of the complementary filter is shown in Figure 1.

3.2. Design of Complementary Filters for SINS/CNS/GPS Integration

There are two sources of attitude information for SINS/CNS/GPS integrated navigation: the attitude in the $n$ frame calculated by SINS (denoted as $q_b^{n,ins}$), the attitude in the $i$ frame measured by CNS (denoted as $q_b^{i,cns}$), and the velocity information sources include the velocity in the $n$ frame calculated by SINS (denoted as $v_{ins}^n$) and the velocity in the $n$ frame measured by GPS (denoted as $v_{gps}^n\left(s\right)$). Similarly, the position information sources include the position in the $n$ frame calculated by SINS (denoted as $p_{ins}^{}$) and the position in the $n$ frame measured by GPS (denoted as $p_{gps}^{}$). The navigation information (including the attitude, velocity, and position information) obtained by SINS has the advantages of good dynamic performance, high real-time stability, and good short-term stability, but the navigation errors accumulate over time, and the long-term stability is poor. The error sources of SINS mainly include the gyro drifts/accelerometer biases, iterative algorithm errors, initial alignment errors, etc. Due to the cumulative effect of integral calculation, the errors of SINS navigation parameters, which include the attitude error, velocity error, and position error, will accumulate over time, and all the frequency spectrums are mainly in the low frequency segment. The navigation information measured by CNS and GPS (including the attitude from CNS, the velocity and position from GPS) has the advantages of high precision, error convergence, and good long-term stability, but the data update rate is low, and the real-time performance is poor. The measurement errors of CNS and GPS do not accumulate over time and can be approximated as Gaussian white noise. By analyzing the frequency-domain characteristics of the navigation errors of each subsystem, the complementary filters are designed to realize the information fusion of an SINS/CNS/GPS integrated navigation system, and the navigation results with higher precision are obtained. The process of complementary filtering information fusion for an SINS/CNS/GPS integrated navigation system is shown in Figure 2.

3.2.1. Complementary Filter for Attitude Estimation

As shown in Section 2.3, the attitude calculated by SINS is $q_b^{n,ins}$, and the attitude measured by CNS is $q_b^{i,cns}$. By using the position information of GPS, the attitude of CNS in the $n$ frame can be calculated, as shown in Equation (11):

$$q_b^{n,cns}=q_e^{n,gps}\otimes q_i^e\otimes q_b^{i,cns}$$

(11)

where $q_e^{n,gps}$ represents the transformation quaternion from the $e$ frame to the $n$ frame; $q_i^e$ represents the transformation quaternion from the $i$ frame to the $e$ frame; $q_e^{n,gps}$ can be calculated based on the position of GPS; $q_i^e$ can be calculated based on the UTC time and EOP parameters, in which the EOP parameters represent the Earth orientation parameters provided by the International Earth Rotation and Reference Systems Service (IERS).

For the attitude estimation, the complementary filter is denoted as CF1, which is composed of a first-order high-pass filter and a first-order low-pass filter. The transfer function of a first-order high-pass filter is denoted as $G_1^H\left(s\right)=s/\left(s+{\omega }_1^c\right)$, and the transfer function of a first-order low-pass filter is denoted as $G_1^L\left(s\right)={\omega }_1^c/\left(s+{\omega }_1^c\right)$, where ${\omega }_1^c$ is the cut-off angular frequency of CF1, and the cut-off frequency corresponding to ${\omega }_1^c$ is $f_1^c={\omega }_1^c/\left(2\pi \right)$. Then, the attitude estimation in the s-domain is as follows:

$${\widehat{q}}_b^n\left(s\right)=G_1^H\left(s\right)q_b^{n,ins}\left(s\right)+G_1^L\left(s\right)q_b^{n,cns}\left(s\right).$$

(12)

Suppose that the filtering period is $T_f^{}=t_k^{}-t_{k-1}^{}$. Discretize Equation (12) to obtain the recursive form in time domain for attitude estimation, as shown in Equation (13):

$${\widehat{q}}_b^n\left(t_k^{}\right)=\frac{1}{1+{\omega }_1^c{T}_f}{\widehat{q}}_b^n\left(t_{k-1}^{}\right)+\frac{T_f}{1+{\omega }_1^c{T}_f}{\dot{q}}_b^{n,ins}\left(t_k^{}\right)+\frac{{\omega }_1^c{T}_f}{1+{\omega }_1^c{T}_f}{q}_b^{n,cns}\left(t_k^{}\right)$$

(13)

where ${\omega }_1^c$ represents the angular frequency corresponding to CF1′s cut-off frequency $f_1^c={\omega }_1^c/\left(2\pi \right)$.

3.2.2. Complementary Filter for Velocity Estimation

As mentioned, the velocity of SINS is $v_{ins}^n$, and the velocity of GPS is ${\mathit{v}}_{gps}^n$. For the velocity estimation, the complementary filter is denoted as CF2, which is composed of a first-order high-pass filter and a first-order low-pass filter. The transfer function of a first-order high-pass filter is denoted as $G_2^H\left(s\right)=s/\left(s+{\omega }_2^c\right)$, and the transfer function of a first-order low-pass filter is denoted as $G_2^L\left(s\right)={\omega }_2^c/\left(s+{\omega }_2^c\right)$, where ${\omega }_2^c$ represents the angular frequency corresponding to CF2′s cut-off frequency $f_2^c={\omega }_2^c/\left(2\pi \right)$. Then, the velocity estimation in the s-domain is as follows:

$${\widehat{\mathit{v}}}_{}^n\left(s\right)=G_2^H\left(s\right){\mathit{v}}_{ins}^n\left(s\right)+G_2^L\left(s\right){\mathit{v}}_{gps}^n\left(s\right).$$

(14)

Similarly, the filtering period is also $T_f$. Discretize Equation (14) to obtain the recursive form in the time domain for velocity estimation, as shown in Equation (15):

$${\widehat{\mathit{v}}}_{}^n\left(t_k^{}\right)=\frac{1}{1+{\omega }_2^c{T}_f}{\widehat{\mathit{v}}}_{}^n\left(t_{k-1}^{}\right)+\frac{T_f}{1+{\omega }_2^c{T}_f}{\dot{\mathit{v}}}_{ins}^n\left(t_k^{}\right)+\frac{{\omega }_2^c{T}_f}{1+{\omega }_2^c{T}_f}{\mathit{v}}_{gps}^n\left(t_k^{}\right).$$

(15)

3.2.3. Complementary Filter for Position Estimation

For position estimation, the position of SINS is ${\mathit{p}}_{ins}$, the position of GPS is ${\mathit{p}}_{gps}$, and the complementary filter is denoted as CF3, which is also composed of a first-order high-pass filter and a first-order low-pass filter. The transfer function of a first-order high-pass filter is $G_3^H\left(s\right)=s/\left(s+{\omega }_3^c\right)$, and the transfer function of a first-order low-pass filter is $G_3^L\left(s\right)={\omega }_3^c/\left(s+{\omega }_3^c\right)$, where ${\omega }_3^c$ is the angular frequency corresponding to CF3′s cut-off frequency $f_3^c={\omega }_3^c/\left(2\pi \right)$. Then, the position estimation in the s-domain is as follows:

$$\widehat{\mathit{p}}\left(s\right)=G_3^H\left(s\right){\mathit{p}}_{ins}^{}\left(s\right)+G_3^L\left(s\right){\mathit{p}}_{gps}^{}\left(s\right).$$

(16)

Similarly, take the filtering period $T_f^{}$ as a discrete step, and discretize Equation (16) to obtain the recursive form in the time domain for position estimation, as shown in Equation (17):

$$\widehat{\mathit{p}}\left(t_k^{}\right)=\frac{1}{1+{\omega }_3^c{T}_f}\widehat{\mathit{p}}\left(t_{k-1}^{}\right)+\frac{T_f}{1+{\omega }_3^c{T}_f}{\dot{\mathit{p}}}_{ins}^{}\left(t_k^{}\right)+\frac{{\omega }_3^c{T}_f}{1+{\omega }_3^c{T}_f}{\mathit{p}}_{gps}^{}\left(t_k^{}\right).$$

(17)

Each time after the process of complementary filtering is completed, the navigation parameters of SINS are corrected by feedback of the estimation results, as shown in Equation (18):

$$\{\begin{array}{l}{q}_b^{n,ins}\left(t_k^{}\right)={\widehat{q}}_b^n\left(t_k^{}\right)\\ {\mathit{v}}_{ins}^n\left(t_k^{}\right)={\widehat{\mathit{v}}}_{}^n\left(t_k^{}\right)\\ {\mathit{p}}_{ins}^{}\left(t_k^{}\right)=\widehat{\mathit{p}}\left(t_k^{}\right)\end{array}.$$

(18)

4. Error Analysis in Frequency Domain and Selection of Cut-Off Frequency

The cut-off frequency is the most key parameter of a complementary filter, which determines the effect of information fusion. In order to obtain good estimation results, it is necessary to select the cut-off frequencies properly for the complementary filters by analyzing the errors of each navigation subsystem in the frequency domain. According to the principle of complementary filter in Section 3.1, the analysis shows that the selection of cut-off frequency of the complementary filter determines the influential weight of each navigation subsystem (SINS, CNS, or GPS) on the SINS/CNS /GPS integrated navigation system. In conclusion, the larger the cut-off frequency $f_i^c$ ($i=1,2,3$), the greater the influence of the measurement noise of the auxiliary subsystem (CNS or GPS) on the estimation result, the smaller the influence of the errors of SINS, and vice versa. During the on-orbit flight of an aerospace plane, the navigation errors of CNS and GPS are mainly the measurement noises, which can be approximated as Gaussian white noise. Under the premise of satisfying the dynamic performance requirements of SINS, the cut-off frequency of the complementary filter should be as small as possible, so that the measurement noise errors of CNS and GPS can be filtered out in the largest frequency range. In this paper, the errors of each subsystem are analyzed in the time and frequency domain. According to the relation between the cut-off frequencies and statistical errors, the cut-off frequencies of complementary filters for attitude/velocity/position estimation are optimally selected, respectively.

4.1. Selection of Cut-Off Frequency Based on Nonlinear Optimization Theory

In this subsection, the analysis method based on power spectral density is used to analyze the influence of the noise of each subsystem on the output noise of the SINS/CNS/GPS integrated navigation system theoretically, and an optimal selection scheme of the cut-off frequency of the complementary filter based on the constrained nonlinear optimization method is proposed, so as to provide a theoretical reference for the selection of the optimal cut-off frequency.

In SINS/CNS/GPS integrated navigation, the measurement noises of navigation sensors such as gyro, accelerometer, CNS, and GPS are all stationary random noise. For the convenience of theoretical analysis, these noises are assumed to be Gaussian white noise with zero mean. In this paper, the measurement noises of a gyro, accelerometer, CNS, GPS velocity, and GPS position are denoted as $w_{gro}\left(t\right)$, $w_{acc}\left(t\right)$, $w_{cns\_att}\left(t\right)$, $w_{gps\_vel}\left(t\right)$, and $w_{gps\_pos}\left(t\right)$, respectively. The random walk noises in SINS attitude, velocity, and position are denoted as $w_{ins\_att}\left(t\right)$, $w_{ins\_vel}\left(t\right)$, and $w_{ins\_pos}\left(t\right)$. $q_{w_{gro}}$, $q_{w_{acc}}$, $q_{w_{cns\_att}}$, $q_{w_{gps\_vel}}$, and $q_{w_{gps\_pos}}$ represent the variance intensities of gyro angular velocity measurement noise, accelerometer specific force measurement noise, CNS attitude measurement noise, GPS velocity measurement noise, and GPS position measurement noise, respectively. $\delta ()$ represents the Dirac delta function. Then, the covariances and autocorrelation functions of measurement noises of navigation sensors are expressed as:

$$\{\begin{array}{ll}E\left[w_{gro}\left(t\right)w_{gro}^{\mathrm{T}}\left(\mathbf{\tau }\right)\right]=q_{w_{gro}}\mathbf{\delta }\left(t-\mathbf{\tau }\right)\hfill & R_{w_{gro}}\left(\mathbf{\mu }\right)=q_{w_{gro}}\mathbf{\delta }\left(\mathbf{\mu }\right)\hfill \\ E\left[w_{acc}\left(t\right)w_{acc}^{\mathrm{T}}\left(\mathbf{\tau }\right)\right]=q_{w_{acc}}\mathbf{\delta }\left(t-\mathbf{\tau }\right)\hfill & R_{w_{acc}}\left(\mathbf{\mu }\right)=q_{w_{acc}}\mathbf{\delta }\left(\mathbf{\mu }\right)\hfill \\ E\left[w_{cns\_att}\left(t\right)w_{cns\_att}^{\mathrm{T}}\left(\mathbf{\tau }\right)\right]=q_{cns\_att}\mathbf{\delta }\left(t-\mathbf{\tau }\right)\hfill & R_{w_{cns\_att}}\left(\mathbf{\mu }\right)=q_{w_{cns\_att}}\mathbf{\delta }\left(\mathbf{\mu }\right)\hfill \\ E\left[w_{gps\_vel}\left(t\right)w_{gps\_vel}^{\mathrm{T}}\left(\mathbf{\tau }\right)\right]=q_{gps\_vel}\mathbf{\delta }\left(t-\mathbf{\tau }\right)\hfill & R_{w_{gps\_vel}}\left(\mathbf{\mu }\right)=q_{w_{gps\_vel}}\mathbf{\delta }\left(\mathbf{\mu }\right)\hfill \\ E\left[w_{gps\_pos}\left(t\right)w_{gps\_pos}^{\mathrm{T}}\left(\mathbf{\tau }\right)\right]=q_{gps\_pos}\mathbf{\delta }\left(t-\mathbf{\tau }\right)\hfill & R_{w_{gps\_pos}}\left(\mathbf{\mu }\right)=q_{w_{gps\_pos}}\mathbf{\delta }\left(\mathbf{\mu }\right)\hfill \end{array}$$

(19)

where $\mathbf{\mu }=t-\mathbf{\tau }$ represents the time interval between the time instants $t$ and $\mathbf{\tau }$.

The power spectral density functions of measurement noises of these navigation sensors are

$$\{\begin{array}{l}{S}_{w_{gro}}\left(\omega \right)=\mathcal{F}\left\{R_{w_{gro}}\left(\mu \right)\right\}={\displaystyle {\int }_{-\infty }^{\infty }{q}_{w_{gro}}\delta \left(\mu \right){\mathrm{e}}^{-j\omega \mu }\mathrm{d}}\mu =q_{w_{gro}}\hfill \\ S_{w_{acc}}\left(\omega \right)=\mathcal{F}\left\{R_{w_{acc}}\left(\mu \right)\right\}={\displaystyle {\int }_{-\infty }^{\infty }{q}_{w_{acc}}\delta \left(\mu \right){\mathrm{e}}^{-j\omega \mu }\mathrm{d}}\mu =q_{w_{acc}}\hfill \\ S_{w_{cns\_att}}\left(\omega \right)=\mathcal{F}\left\{R_{w_{cns\_att}}\left(\mu \right)\right\}={\displaystyle {\int }_{-\infty }^{\infty }{q}_{w_{cns\_att}}\delta \left(\mu \right){\mathrm{e}}^{-j\omega \mu }\mathrm{d}}\mu =q_{w_{cns\_att}}\hfill \\ S_{w_{gps\_vel}}\left(\omega \right)=\mathcal{F}\left\{R_{w_{gps\_vel}}\left(\mu \right)\right\}={\displaystyle {\int }_{-\infty }^{\infty }{q}_{w_{gps\_vel}}\delta \left(\mu \right){\mathrm{e}}^{-j\omega \mu }\mathrm{d}}\mu =q_{w_{gps\_vel}}\hfill \\ S_{w_{gps\_pos}}\left(\omega \right)=\mathcal{F}\left\{R_{w_{gps\_pos}}\left(\mu \right)\right\}={\displaystyle {\int }_{-\infty }^{\infty }{q}_{w_{gps\_pos}}\delta \left(\mu \right){\mathrm{e}}^{-j\omega \mu }\mathrm{d}}\mu =q_{w_{gps\_pos}}\hfill \end{array}$$

(20)

where $\mathcal{F}\left\{\right\}$ is the Fourier transform.

For the convenience of analysis, the motion state of the vehicle and the random error propagation model of SINS are simplified. Assuming that the vehicle is in an on-orbit cruise phase and its motion state is uniform straight-line flight, then the error model of SINS can be simplified as a linear time-invariant system.

The attitude random walk noise $w_{ins\_att}\left(t\right)$ of SINS is the integral of gyro measurement noise $w_{gro}\left(t\right)$, and the frequency response function of the integrator is $1/\left(j\mathit{\omega }\right)$. According to the response characteristics of the linear system to stationary process, the power spectral density of $w_{ins\_att}$ is

$$S_{w_{ins\_att}}\left(\omega \right)={\left|1/\left(j\omega \right)\right|}^2{S}_{w_{gro}}\left(\omega \right)=q_{w_{gro}}/{\omega }^2.$$

(21)

Similarly, the SINS velocity random walk noise $w_{ins\_vel}$ is the integral of measurement noise of the accelerometer, and the power spectral density of $w_{ins\_vel}$ is

$$S_{w_{ins\_vel}}\left(\omega \right)={\left|1/\left(j\mathit{\omega }\right)\right|}^2{S}_{w_{acc}}\left(\omega \right)=q_{w_{acc}}/{\mathit{\omega }}^2.$$

(22)

Due to the measurement noises of inertial sensors and the calculation error of the SINS update algorithm, there will be white noise in the velocity information of SINS, which could be denoted as $w_{ins\_nv}^{}$, and its variance intensity is denoted as $q_{w_{ins\_nv}}$. The power spectral density of $w_{ins\_nv}^{}$ is $S_{w_{ins\_nv}}\left(\omega \right)=q_{w_{ins\_nv}}$, and the position random walk noise $w_{ins\_pos}$ of SINS is the integral of velocity white noise; its power spectral density is as follows:

$$S_{w_{ins\_pos}}\left(\omega \right)={\left|1/\left(j\omega \right)\right|}^2{S}_{w_{ins\_nv}}\left(\omega \right)=q_{w_{ins\_nv}}/{\omega }_{}^2.$$

(23)

In this paper, the random errors of CF1, CF2, and CF3 are denoted as $w_{CF\_att}$, $w_{CF\_vel}$, and $w_{CF\_pos}$, respectively. Firstly, the power spectral density of a random error of attitude estimation complementary filter CF1 is analyzed. According to the design of complementary filters in Section 3.2, the transfer functions of the high-pass filter and low-pass filter of CF1 are rewritten as follows:

$$\{\begin{array}{l}{G}_1^H\left(s\right)=s/\left(s+{\omega }_1^c\right)\\ G_1^L\left(s\right)={\omega }_1^c/\left(s+{\omega }_1^c\right)\end{array}.$$

(24)

From the transfer functions, the corresponding frequency response functions can be obtained as follows:

$$\{\begin{array}{l}{G}_1^H\left(j\omega \right)={G_1^H\left(s\right)|}_{s=j\omega }=j\omega /\left(j\omega +{\omega }_1^c\right)\\ G_1^L\left(j\omega \right)={G_1^L\left(s\right)|}_{s=j\omega }={\omega }_1^c/\left(j\omega +{\omega }_1^c\right)\end{array}.$$

(25)

According to the principle of complementary filter, the random error of CF1 is the sum of two random errors, of which one is the output of $G_1^H\left(j\omega \right)$ under the input of SINS attitude random walk noise, and the other is the output of $G_1^L\left(j\omega \right)$ under the input of CNS attitude measurement noise. Since SINS and CNS are two independent subsystems, the random error of SINS is orthogonal to that of CNS, so the power spectral density $S_{w_{CF\_att}}\left(\omega \right)$ of the random error of CF1 is

$$S_{w_{CF\_att}}\left(\omega \right)={\left|G_1^H\left(j\omega \right)\right|}_{}^2{S}_{w_{ins\_att}}\left(\omega \right)+{\left|G_1^L\left(j\omega \right)\right|}_{}^2{S}_{w_{cns\_att}}\left(\omega \right).$$

(26)

Substituting $G_1^H\left(j\omega \right)$, $G_1^L\left(j\omega \right)$, $S_{w_{ins\_att}}\left(\omega \right)$, and $S_{w_{cns\_att}}\left(\omega \right)$ into Equation (26), we can obtain

$$S_{w_{CF\_att}}\left(\omega \right)={\left|j\omega /\left(j\omega +{\omega }_1^c\right)\right|}_{}^2{q}_{w_{gro}}/{\omega }^2+{\left|{\omega }_1^c/j\omega +{\omega }_1^c\right|}_{}^2{q}_{w_{cns\_att}}=\left(q_{w_{gro}}+q_{w_{cns\_att}}{\left({\omega }_1^c\right)}_{}^2\right)/\left({\omega }_{}^2+{\left({\omega }_1^c\right)}_{}^2\right).$$

(27)

By substituting $\omega =2\pi f$ into Equation (27), the expression of the power spectral density function of $w_{CF\_att}$ with respect to frequency $f$ can be obtained.

$$S_{w_{CF\_att}}\left(f\right)={S_{w_{CF\_att}}\left(\omega \right)|}_{\omega =2\pi f}^{}=\left(q_{w_{gro}}+q_{w_{cns\_att}}{\left({\omega }_1^c\right)}_{}^2\right)/\left({\left(2\pi f\right)}_{}^2+{\left({\omega }_1^c\right)}_{}^2\right)$$

(28)

In a SINS/CNS/GPS integrated navigation system, the maximum frequency of a filter’s bandwidth is limited due to the discrete sampling of a signal. Among the three subsystems of SINS, CNS, and GPS, the sampling frequency $f_{ins}^s$ of SINS is the highest. According to the sampling theorem of a stationary random process, the power spectral density of the output noise of a SINS/CNS/GPS integrated navigation system only needs to be considered in the frequency range of $\left[0,f_{ins}^s/2\right]$. By calculating the definite integral of $S_{w_{CF\_att}}\left(f\right)$ with respect to $f$ from $0$ to $f_s^{ins}/2$, the average power $P_{w_{CF\_att}}$ of the random error $w_{CF\_att}$ of CF1 in the frequency band of $\left[0,f_s^{ins}/2\right]$ could be obtained, as shown in Equation (29):

$$P_{w_{CF\_att}}={\displaystyle {\int }_0^{f_s^{ins}/2}{S}_{w_{CF\_att}}\left(f\right)\mathrm{d}f}=\arctan \left(\pi f_s^{ins}/{\mathit{\omega }}_1^c\right)\left(q_{w_{gro}}+q_{w_{cns\_att}}{\left({\mathit{\omega }}_1^c\right)}_{}^2\right)/\left(2\mathit{\pi }{\mathit{\omega }}_1^c\right).$$

(29)

By substituting ${\mathit{\omega }}_1^c=2\mathit{\pi }{f}_1^c$ into Equation (29), the expression of average power $P_{w_{CF\_att}}$ with respect to the cut-off frequency $f_1^c$ can be obtained, as shown in Equation (30):

$$P_{w_{CF\_att}}=\arctan \left(f_s^{ins}/\left(2f_1^c\right)\right)\left(4q_{w_{cns\_att}}{\pi }_{}^2{\left(f_1^c\right)}_{}^2+q_{w_{gro}}\right)/\left(4{\mathit{\pi }}_{}^2{f}_1^c\right).$$

(30)

In the algorithm design of a practical integrated navigation system, the parameters of $f_s^{ins}$, $q_{w_{gro}}$, and $q_{w_{cns\_att}}$ are constant, and $P_{w_{CF\_att}}$ is a nonlinear function of $f_1^c$. The optimal value of $f_1^c$ is equivalent to the value that minimizes $P_{w_{CF\_att}}$. By differentiating $P_{w_{CF\_att}}$ with respect to $f_1^c$, we can obtain

$$\begin{array}{ccc}\hfill \frac{\mathrm{d}{P}_{w_{CF\_att}}}{\mathrm{d}{f}_1^c}& =& 2q_{w_{cns\_att}}\arctan \left(f_s^{ins}/\left(2f_1^c\right)\right)\hfill \\ & & -\left(\arctan \left(f_s^{ins}/\left(2f_1^c\right)\right)\left(4q_{w_{cns\_att}}{\mathit{\pi }}_{}^2{\left(f_1^c\right)}_{}^2+q_{w_{gro}}\right)\right)/\left(4{\mathit{\pi }}_{}^2{\left(f_1^c\right)}_{}^2\right)\hfill \\ & & -\left(f_s^{ins}\left(4q_{w_{cns\_att}}{\mathit{\pi }}_{}^2{\left(f_1^c\right)}_{}^2+q_{w_{gro}}\right)\right)/\left(8{\mathit{\pi }}_{}^2{\left(f_1^c\right)}_{}^3\left({\left(f_s^{ins}\right)}^2/\left(4{\left(f_1^c\right)}_{}^2\right)+1\right)\right).\hfill \end{array}$$

(31)

It can be seen from Equation (31) that the derivative of $P_{w_{CF\_att}}$ with respect to $f_1^c$ has a very complex form, so it is difficult to find the minimum value of $P_{w_{CF\_att}}$ in analytical form by using the stationary point method. Considering that in a specific SINS/CNS/GPS integrated navigation system, the values of $f_s^{ins}$, $q_{w_{gro}}$, $q_{w_{cns\_att}}$ are known or can be obtained by calibration, thus, the minimum value of $P_{w_{CF\_att}}$ with respect to $f_1^c$ can be solved by the optimization method, and the nonlinear optimization function of the MATLAB Optimization Toolbox can be used to obtain the numerical solution of the optimization problem.

Combined with optimization theory, the optimal solution of $f_1^c$ can be regarded as a constrained nonlinear optimization problem, and the optimal mathematical model is established as

$$\{\begin{array}{l}\underset{f_1^c}{\min }{P}_{w_{CF\_att}}=\arctan \left(f_s^{ins}/\left(2f_1^c\right)\right)\left(4q_{w_{cns\_att}}{\mathit{\pi }}_{}^2{\left(f_1^c\right)}_{}^2+q_{w_{gro}}\right)/\left(4{\mathit{\pi }}_{}^2{f}_1^c\right)\\ \mathrm{s}.\mathrm{t}.f_L^c\le f_1^c\le f_s^{cns}/2\end{array}$$

(32)

where $f_L^c$ is the infimum of the range of the CF’s cut-off frequency, which can be determined by analyzing the error propagation characteristics of the SINS. Under the condition of $f_1^c\ge f_L^c$, the high-pass filter of CF1 can effectively restrain the oscillation and divergence low-frequency errors in the SINS attitude. Many studies about inertial navigation analyzed the error propagation characteristics of the inertial navigation system, which are not outlined here. Furthermore, the selection of $f_1^c$ should meet the requirement of “minimizing the influence of measurement noise of CNS”, thus $f_1^c\le f_s^{cns}/2$, i.e., the constraint condition in Equation (32). For programming implementation, the MATLAB built-in function “fmincon” can be used to solve Equation (32).

The power spectral density analyses of random errors of CF2 and CF3 are similar to that of CF1. The detailed analysis process will not be introduced here, the analysis results of random noises of CF2 and CF3 are given directly (including the power spectral density, average power, and optimal mathematical model).

According to the power spectral density analysis of random noise of CF2, the following conclusions can be obtained:

Power spectral density of random error of CF2 $S_{w_{CF\_vel}}\left(f\right)$:

$$S_{w_{CF\_vel}}\left(f\right)=\left(q_{w_{acc}}+q_{w_{gps\_vel}}{\left({\omega }_2^c\right)}_{}^2\right)/\left({\left(2\pi f\right)}_{}^2+{\left({\omega }_2^c\right)}_{}^2\right).$$

(33)

Average power of random error of CF2 $P_{w_{CF\_vel}}$:

$$P_{w_{CF\_vel}}=\arctan \left(f_s^{ins}/\left(2f_2^c\right)\right)\left(4q_{w_{gps\_vel}}{\mathit{\pi }}_{}^2{\left(f_2^c\right)}_{}^2+q_{w_{acc}}\right)/\left(4{\mathit{\pi }}_{}^2{f}_2^c\right).$$

(34)

In order to solve the optimization problem of $f_2^c$, the mathematical model of constrained nonlinear optimization is established as follows:

$$\{\begin{array}{l}\underset{f_2^c}{\min }{P}_{w_{CF\_vel}}=\arctan \left(f_s^{ins}/\left(2f_2^c\right)\right)\left(4q_{w_{gps\_vel}}{\mathit{\pi }}_{}^2{\left(f_2^c\right)}_{}^2+q_{w_{acc}}\right)/\left(4{\mathit{\pi }}_{}^2{f}_2^c\right)\\ \mathrm{s}.\mathrm{t}.f_L^c\le f_2^c\le f_s^{gps}/2\end{array}$$

(35)

where $f_L^c$ is the spectrum bandwidth of INS errors. Under the condition of $f_2^c\ge f_L^c$, the high-pass filter of CF2 can effectively eliminate the oscillation and divergence terms of SINS velocity errors. $f_s^{gps}$ is the sampling frequency of GPS.

According to the power spectral density analysis of random noise of CF3, the following conclusions can be obtained:

Power spectral density of random error of CF3 $S_{w_{CF\_pos}}\left(f\right)$:

$$S_{w_{CF\_pos}}\left(f\right)=\left(q_{w_{ins\_n\mathit{v}}}+q_{w_{gps\_pos}}{\left({\omega }_3^c\right)}_{}^2\right)/\left({\left(2\pi f\right)}_{}^2+{\left({\omega }_3^c\right)}_{}^2\right).$$

(36)

Average power of random error of CF3 $P_{w_{CF\_pos}}$:

$$P_{w_{CF\_pos}}=\arctan \left(f_s^{ins}/\left(2f_3^c\right)\right)\left(q_{w_{gps\_pos}}{\left(2\mathit{\pi }{f}_3^c\right)}_{}^2+q_{w_{ins\_n\mathit{v}}}\right)/\left(4{\mathit{\pi }}_{}^2{f}_3^c\right).$$

(37)

To solve the optimization problem of $f_3^c$, the mathematical model of constrained nonlinear optimization is established as follows:

$$\{\begin{array}{l}\underset{f_3^c}{\min }{P}_{w_{CF\_pos}}=\arctan \left(f_s^{ins}/\left(2f_3^c\right)\right)\left(q_{w_{gps\_pos}}{\left(2\mathit{\pi }{f}_3^c\right)}_{}^2+q_{w_{ins\_nv}}\right)/\left(4{\mathit{\pi }}_{}^2{f}_3^c\right)\\ \mathrm{s}.\mathrm{t}.f_L^c\le f_3^c\le f_s^{gps}/2\end{array}$$

(38)

where $f_L^c$ is the spectrum bandwidth of SINS errors. Under the condition of $f_3^c\ge f_L^c$, the high-pass filter of CF3 can effectively eliminate the oscillation and divergence terms in INS position errors. $f_s^{gps}$ is the sampling frequency of GPS.

4.2. Selection of Cut-Off Frequency Based on Simulation Test

4.2.1. Spectrum Analysis Based on DFT

According to the theory of digital signal processing, the discrete Fourier transform (DFT) is often used for signal spectrum analysis. If an N-point time-domain sampling sequence is denoted as $x\left(n\right)$ ($0\le n\le N-1$), then the corresponding N-point DFT of $x\left(n\right)$ is defined as:

$$X\left(k\right)=\mathrm{DFT}\left[x\left(n\right)\right]={\displaystyle {\sum }_{n=0}^{N-1}x\left(n\right)}{W}_N^{nk}0\le k\le N$$

(39)

where $W_N=\exp \left(-j2\pi /N\right)$ is the twiddle factor.

In order to study the frequency distribution of error, DFT is often used to analyze the frequency spectrum of an error signal, mainly focusing on the amplitude-frequency characteristics. For DFT, the sampling frequency of the signal is denoted as $f_s$; then, the steps of Fourier analysis on the $N$-point sampling data $x\left(n\right)$ are as follows:

Step 1: The DFT transform on the $N$-point sampling data $x\left(n\right)$ is performed to obtain the $N$-point DFT $X\left(k\right)$.

Step 2: Calculate the spectral amplitude $\left|X\left(k\right)\right|$ for the spectrum data $X\left(k\right)$ of the ordinal number $0~fix\left(N/2\right)$, where $fix(•)$ denotes the function rounding a number to the nearest integer.

Step 3: Divide all amplitude data of the spectra obtained in Step 2 by $N$; then, multiply the amplitude data of ordinal number $2~fix\left(N/2\right)+1$ by 2, convert the two-sided spectrum into a one-sided spectrum, and plot the amplitude-frequency curve.

Step 4: The frequencies of spectral lines labeled on the abscissa axis of the amplitude-frequency figure are sequentially $k{f}_s/N$ $\mathrm{Hz}$, ($k=0~fix\left(N/2\right)$).

Step 5: The ordinate of spectral line corresponds to the harmonic amplitude of sampling signal $x\left(n\right)$ at the corresponding frequency, the unit of amplitude is the same as that of time-domain sampling data, and the amplitude of DC component is at zero frequency.

4.2.2. Selection of Cut-Off Frequencies Based on Simulation Test

In order to determine the cut-off frequencies of complementary filters for the attitude/velocity/position estimation in an SINS/CNS/GPS integrated navigation system, the frequency-domain analyses were conducted on the measurement errors of SINS, CNS, and GPS subsystems. The analysis results, which include the error curves, amplitude-frequency diagrams, and relationship graphs between the estimation errors and selected cut-off frequencies, are shown in Figure 3, Figure 4, Figure 5, Figure 6, Figure 7, Figure 8, Figure 9, Figure 10 and Figure 11, respectively.

Firstly, analyze and select the cut-off frequency of the complementary filter for attitude estimation (i.e., the cut-off frequency of CF1). The attitude errors of SINS and CNS subsystems obtained in the static test are analyzed in time and frequency domain, and the analysis results are shown in Figure 3, Figure 4 and Figure 5 where ${\varphi }_x$, ${\varphi }_y$, and ${\varphi }_z$ denote the pitch, roll, and yaw misalignment angles, respectively, the unit of which is arcsecond (″); $t$ denotes the time axis, and its unit is second (s); $\left|{\varphi }_x\right|$, $\left|{\varphi }_y\right|$ and $\left|{\varphi }_z\right|$ denote the amplitudes of the pitch, roll, and yaw error spectrums, the unit of which is arcsecond (″); $f$ denotes the frequency axis, its unit is Hertz ($Hz$); $\sigma \left({\varphi }_x\right)$, $\sigma \left({\varphi }_y\right)$, and $\sigma \left({\varphi }_z\right)$ denote the standard deviations of attitude errors in SINS/CNS integration, and $f_1^c$ denotes the cut-off frequency of CF1. Figure 3 shows the time curves and amplitude spectrums of CNS attitude errors, of which the three subfigures on the left are the time curves, and three subfigures on the right are the amplitude spectrums. Figure 4 shows the time curves and amplitude spectrums of SINS attitude errors. Similarly, the three subfigures on the left are the time curves, and the three subfigures on the right are the amplitude spectrums. It can be seen from Figure 3 to Figure 4 that the attitude errors of CNS do not diverge and are less than 100″, while the time curves of attitude errors are characterized by disorderly jumping noise. According to the corresponding amplitude spectrums, the errors of CNS can be approximated as white noise. The attitude errors of SINS have a tendency to oscillate and diverge, which are close to $20^{\prime }$ in 3600 s. From the amplitude spectrums, it can be seen that the attitude errors of SINS are mainly distributed in the low frequency band of 0–1 Hz. In order to obtain the statistical relationship between the cut-off frequency $f_1^c$ and attitude estimation accuracy $\sigma \left({\varphi }_i\right)$, let $f_1^c$ take different values, and Monte-Carlo simulation is performed to record the corresponding values of $\sigma \left({\varphi }_i\right)$ ($i=x,y,z$) and $f_1^c$ every time. Selection of $f_1^c$ value points: the cut-off frequency value points are in the frequency range of 0 to 2.5 Hz, while the interval between two adjacent cut-off frequency points is taken as 0.03 Hz. In addition, the three frequency points of 0.001, 0.005, and 0.01 Hz are also tested. This means that the set of $f_1^c$ value points is ${\left[\begin{array}{cccc}0.001& 0.005& 0.01& \left[0.03:0.03:2.5\right]\end{array}\right]}^{\mathrm{T}}\mathrm{Hz}$. According to the statistical results of Monte-Carlo simulation, the relationships between $f_1^c$ and $\sigma \left({\varphi }_i\right)$ ($i=x,y,z$) are obtained. As shown in Figure 5, the effect of cut-off frequency on the attitude error can be seen. The data pairs including the minimum values of $\sigma \left({\varphi }_i\right)$ ($i=x,y,z$) and corresponding $f_1^c$ are pitch (0.09 Hz, 0.2116’), roll (0.09 Hz, 0.2140’), and yaw (0.12 Hz, 0.2121’), which are marked with red asterisks(*) in Figure 5. It can be seen that the cut-off frequencies corresponding to the minimum values of three attitude errors are very close. In order to reduce the algorithmic complexity and computational load, the same cut-off frequency values are selected for the pitch, roll, and yaw estimations, i.e., $f_1^c=0.1\mathrm{Hz}$, ${\omega }_1^c=2\pi f_1^c=0.2\pi$.

In order to select the appropriate cut-off frequency of a complementary filter for velocity estimation (i.e., the cut-off frequency of CF2, $f_2^c$), the velocity errors of the SINS and GPS subsystems obtained in the static test are analyzed in the time and frequency domains, and the analysis results are shown in Figure 6, Figure 7 and Figure 8, where $\delta V_E$, $\delta V_N$, and $\delta V_U$ denote the velocity errors in the East, North, and Up directions, respectively, the unit of which is meters per second ($\mathrm{m}/\mathrm{s}$); $t$ denotes the time axis, and its unit is second (s); $\left|\delta V_E\right|$, $\left|\delta V_N\right|$, and $\left|\delta V_U\right|$ denote the amplitude spectrums of velocity errors in the East, North, and Up directions, the unit of which is meters per second ($\mathrm{m}/\mathrm{s}$); $f$ denotes the frequency axis, its unit is Hertz ($Hz$); $\sigma \left(\delta V_E\right)$, $\sigma \left(\delta V_N\right)$, and $\sigma \left(\delta V_U\right)$ denote the standard deviations of velocity errors in SINS/GPS integration, and $f_2^c$ denotes the cut-off frequency of CF2. Figure 6 shows the time curves and amplitude spectrums of GPS velocity errors, of which three subfigures on the left are the time curves, and three subfigures on the right are the amplitude spectrums. Figure 7 shows the time curves and amplitude spectrums of SINS velocity errors. Similarly, three subfigures on the left are the time curves, and three subfigures on the right are the amplitude spectrums. It can be seen from Figure 6 and Figure 7 that the velocity errors of GPS do not diverge and are all less than 0.5 $\mathrm{m}/\mathrm{s}$, the time curves of GPS velocity errors are characterized by disorderly jumping noise. According to the corresponding amplitude spectrums, the velocity errors of GPS can be approximated as Gaussian white noise. The velocity errors of SINS have a tendency to oscillate and diverge, which are close to 20 $\mathrm{m}/\mathrm{s}$ in 3600 s. From the amplitude spectrum, it can be seen that the velocity errors of SINS are mainly distributed in the low frequency band of 0 to 0.5 Hz. In order to obtain the statistical relationship between the cut-off frequency value and velocity estimation accuracy, let the cut-off frequency $f_2^c$ take different values, and Monte-Carlo simulation is performed to record the values of $\sigma \left(\delta V_i\right)$ ($i=E,N,U$) and corresponding $f_2^c$. For Monte-Carlo simulation of CF2, the selection of $f_2^c$ value points is the same as that of $f_1^c$. According to the statistical results of Monte-Carlo simulation, the relationships between $f_2^c$ and $\sigma \left(\delta V_i\right)$ ($i=E,N,U$) are obtained, as shown in Figure 8, so the effect of cut-off frequency on the error of velocity estimation can be seen. The data pairs including the minimum values of $\sigma \left(\delta V_i\right)$ and corresponding $f_2^c$ are East $\left(0.03\mathrm{Hz},0.0413\mathrm{m}/\mathrm{s}\right)$, North $\left(0.03\mathrm{Hz},0.0408\mathrm{m}/\mathrm{s}\right)$, and Up $\left(0.03\mathrm{Hz},0.0381\mathrm{m}/\mathrm{s}\right)$, which are marked with red asterisks(*) in Figure 8. It can be seen that the cut-off frequencies corresponding to the minimum values of the three velocity errors are very close. Therefore, the same cut-off frequency value can be selected for velocity estimation; there, it is selected as $f_2^c=0.03\mathrm{Hz}$, ${\omega }_2^c=2\pi f_2^c=0.06\pi$.

Finally, the cut-off frequency is selected for position estimation (i.e., the cut-off frequency of CF3, $f_3^c$). The position errors of SINS and GPS subsystems obtained in the static test are analyzed in time and frequency domain, and the analysis results are shown in Figure 9, Figure 10 and Figure 11. Thereinto, $\delta L$, $\delta \lambda$, and $\delta H$ denote the latitude, longitude, and altitude errors, respectively, the unit of which is meter ($\mathrm{m}$); $t$ denotes the time axis, and its unit is second ($\mathrm{s}$); $\left|\delta L\right|$, $\left|\delta \lambda \right|$, and $\left|\delta H\right|$ denote the amplitude spectrums of latitude, longitude, and altitude errors, the unit of which is meter ($\mathrm{m}$); $f$ denotes the frequency axis, its unit is Hertz ($\mathrm{Hz}$); $\sigma \left(\delta L\right)$, $\sigma \left(\delta \lambda \right)$, and $\sigma \left(\delta H\right)$ denote the standard deviations of position errors in SINS/GPS integration; and $f_3^c$ denotes the cut-off frequency of CF3. Figure 9 shows the time curves and amplitude spectrums of GPS position errors, of which three subfigures on the left are the time curves, and three subfigures on the right are the amplitude spectrums. Figure 10 shows the time curves and amplitude spectrums of SINS position errors. Similarly, the three subfigures on the left are the time curves, and the three subfigures on the right are the amplitude spectrums. It can be seen from Figure 9 and Figure 10 that the position errors of GPS do not diverge and are less than $30\mathrm{m}$, while the time curves of the GPS position errors are characterized by disorderly jumping noise. According to the corresponding amplitude spectrums, the position errors of GPS can be approximated as Gaussian white noise. The position errors of SINS will oscillate and diverge, which are close to the order of magnitude of $2\times {10}^4\mathrm{m}$ at 3600 s. As seen from the amplitude spectrum, the position errors of SINS are mainly distributed in the low frequency band of 0 to 2 Hz. Similarly, in order to obtain a statistical relationship between the cut-off frequency and position estimation accuracy, let the cut-off frequency $f_3^c$ take different values, and Monte-Carlo simulation is performed to record the values of $\sigma \left(\delta P_i\right)$ ($i=L,\lambda ,H$) and corresponding $f_3^c$; the selection of $f_3^c$ value points is the same as that of $f_1^c$. According to the statistical results of Monte-Carlo simulation, the relationships between $f_3^c$ and $\sigma \left(\delta P_i\right)$ ($i=L,\lambda ,H$) are obtained. As shown in Figure 11, the effect of cut-off frequency on the position errors of SINS/GPS integration can be seen. The data pairs including the minimum values of $\sigma \left(\delta P_i\right)$ and corresponding $f_3^c$ are latitude $\left(0.15\mathrm{Hz},6.7436\mathrm{m}\right)$, longitude $\left(0.15\mathrm{Hz},5.6282\mathrm{m}\right)$, and altitude $\left(0.09\mathrm{Hz},6.2559\mathrm{m}\right)$, which are marked with red asterisks(*) in Figure 11. The cut-off frequencies corresponding to the minimum values of the latitude, longitude, and altitude errors are very close. Based on the analysis above, the value of $f_3^c$ can be selected as $f_3^c=0.12\mathrm{Hz}$, ${\omega }_3^c=2\pi f_3^c=0.24\pi$.

The minimum standard deviations of navigation parameters and corresponding cut-off frequencies are summarized in

Table 1. Statistics of minimum standard deviations and corresponding cut-off frequencies.

Errors	${\mathit{\sigma }}_{\mathit{m}\mathit{i}\mathit{n}}^{}$	${\mathit{f}}_{\mathit{c}}^{}\left(\mathbf{Hz}\right)$	${\mathit{f}}_{\mathit{c}}^{}\left(\mathbf{Hz}\right)$
${\varphi }_x$  ${(}^{\prime })$	0.2116	0.0900	0.1
${\varphi }_y$  ${(}^{\prime })$	0.2140	0.0900
${\varphi }_z$  ${(}^{\prime })$	0.2121	0.1200
$\delta V_E^{}$  $(\mathrm{m}/\mathrm{s})$	0.0413	0.0300	0.03
$\delta V_N^{}$  $(\mathrm{m}/\mathrm{s})$	0.0408	0.0300
$\delta V_U^{}$  $(\mathrm{m}/\mathrm{s})$	0.0381	0.0300
$\delta L$  $(\mathrm{m})$	6.7436	0.1500	0.12
$\delta \lambda$  $(\mathrm{m})$	5.6282	0.1500
$\delta H$  $(\mathrm{m})$	6.2559	0.0900

. Finally, the values of $f_1^c$, $f_2^c$, and $f_3^c$ are determined as 0.1, 0.03, and 0.12 Hz, respectively.

5. Simulation and Experiment

The real-time data of an SINS/CNS/GPS integrated navigation system of an aerospace plane in flight are difficult to obtain. In this paper, the method of trajectory simulation combined with the error characteristics of a real sensor’s outputs is used to generate the simulation data of each navigation subsystem, and the semi-physical simulation of an SINS/CNS/GPS integrated navigation algorithm based on CF is performed to verify the validity. The semi-physical simulation experiment using this system has important practical significance for studying the characteristics of a SINS/CNS/GPS integrated navigation system of spacecraft under actual noise. In order to fully verify the priority of CF in real-time performance, an experimental test based on a real-time system was done.

5.1. Design and Implementation of Semi-Physical Simulation System

Combined with the application background of spacecraft, the semi-physical simulation platform of the SINS/CNS/GPS integrated navigation system is constructed, which consists of the hardware part (including IMU, GPS, CNS, navigation computer, etc.) and software part (including the trajectory simulation, integrated navigation algorithm, SINS updating algorithm, etc.). By modeling the sampling data of SINS, GPS, and CNS, the error characteristics of each navigation subsystem are obtained. According to the presupposed maneuver conditions, the required error-free data of navigation sensors are generated by using the method of trajectory simulation. According to the error characteristics of each subsystem, the real outputs of subsystems can be simulated by adding errors to the corresponding error-free data. Finally, the algorithm software of SINS and SINS/CNS/GPS integration are performed, and the navigation results are output.

The flow chart of semi-physical simulation for an SINS/CNS/GPS integrated navigation system is shown in Figure 12. The process is as follows:

(1)

Set the initial parameters of trajectory simulation, perform the trajectory simulation, and generate the error-free trajectory data (including the error-free attitude, velocity, and position of spacecraft.)

(2)

According to the obtained trajectory data, calculate the error-free output of IMU. Collect the IMU data in a static test, and subtract the corresponding mean value from the IMU data to obtain the error data of inertial sensors. Then, add the error data to the error-free IMU data, thus obtaining the simulated output data of IMU with real errors. Then, calculate the attitude, velocity, and position of the SINS.

(3)

The speed and position data of GPS in the static test are collected, and the means are subtracted from collected data to obtain the error data of GPS. Add the GPS’s error data to the error-free position/velocity data of spacecraft from trajectory simulation generated in step (1), thereby obtaining the simulated output data of GPS with real errors.

(4)

Similarly, the error data of the CNS can be obtained by subtracting the true value of attitude from the collected off-line attitude data of CNS. Then, add the error data of the CNS to the error-free attitude data of the spacecraft from the trajectory simulation generated in step (1), thereby obtaining the simulated output data of CNS with real errors.

(5)

By using the data from navigation subsystems generated in steps (2), (3), and (4), the information fusion based on complementary filter is performed to obtain the optimal estimations of attitude/velocity/ position parameters of an SINS/CNS/GPS integrated navigation system.

5.2. Simulation Conditions

Based on CF and FKF respectively, the simulations of SINS/CNS/GPS integrated navigation are performed using MATLAB software (MATLAB 2009a, MathWorks, Natick, MA, USA) with the simulation conditions as follows:

(1) Initial State of Aerospace Plane

The initial position (the longitude, latitude, and altitude in WGS84 frame) is $165{.36}^{\mathrm{o}}\mathrm{W}$, ${34}^{\mathrm{o}}\mathrm{N}$, and $406655.29\mathrm{m}$; the initial velocity (including the velocity components in the East, North, and Up directions) is $2995.2\mathrm{m}/\mathrm{s}$, $6665.7\mathrm{m}/\mathrm{s}$, and $8.6\mathrm{m}/\mathrm{s}$; the initial attitude (including the pitch, roll, and azimuth angles) is ${0.15}^{°}$, ${0.1}^{°}$, and ${67.3}^{°}$.

(2) Initial Errors of INS

The initial position errors (including the initial errors of longitude, latitude, and altitude) are $300\mathrm{m}$, $300\mathrm{m}$, and $300\mathrm{m}$. The initial attitude errors (including the initial errors of pitch, roll, and azimuth) are ${10}^{\prime }$, ${10}^{\prime }$, ${10}^{\prime }$. The initial velocity errors (the velocity errors in the East, North, and Up directions) are $1\mathrm{m}/\mathrm{s}$, $1\mathrm{m}/\mathrm{s}$, and $1\mathrm{m}/\mathrm{s}$.

(3) IMU, GPS, and CNS Errors

Gyro errors: the random constant drift is ${0.03}^{°}/\mathrm{h}$ ($1\sigma$), and the angular random walk coefficient is ${0.005}^{°}/\sqrt{\mathrm{h}}$ ($1\sigma$). Accelerometer errors: the random constant bias is $30\mathsf{\mu }\mathrm{g}$ ($1\sigma$), and the velocity random walk coefficient is $5\mathsf{\mu }\mathrm{g}\sqrt{\mathrm{Hz}}\left(1\sigma \right)$. The sampling frequency of IMU is $200\mathrm{Hz}$.

GPS errors: the horizontal position error is $10\mathrm{m}$ ($1\sigma$), the altitude error is $10\mathrm{m}$ ($1\sigma$), and the velocity error is $0.1\mathrm{m}/\mathrm{s}$ ($1\sigma$). The sampling frequency of GPS is $1\mathrm{Hz}$.

CNS errors: the attitude error is ${20}^{″}$ ($1\sigma$). The sampling frequency is $5\mathrm{Hz}$.

(4) Parameter Setting of CF

The cut-off frequencies of complementary filters for attitude/velocity/position estimations are set as $f_1^c=0.1\mathrm{Hz}$, $f_2^c=0.03\mathrm{Hz}$, and $f_3^c=0.12\mathrm{Hz}$, respectively. The simulation time is $3600\mathrm{s}$.

5.3. Simulation Results

The semi-physical simulation of the proposed CF-based SINS/CNS/GPS integrated navigation algorithm is performed, and the navigation errors obtained by comparing the integrated navigation results with the corresponding trajectory parameters generated by the trajectory generator are shown in Figure 13, Figure 14 and Figure 15. Figure 13, Figure 14 and Figure 15 show the time curves of attitude errors, velocity errors, and position errors, respectively. According to the statistics of errors, the standard deviations of pitch, roll, and azimuth errors are ${0.2124}^{\prime }$, ${0.2137}^{\prime }$, ${0.2100}^{\prime }$; the standard deviations of velocity errors in the East, North, and Up directions are $0.0426\mathrm{m}/\mathrm{s}$, $0.0420\mathrm{m}/\mathrm{s}$, and $0.0401\mathrm{m}/\mathrm{s}$; the standard deviations of position errors in latitude, longitude, and altitude are $6.8851\mathrm{m}$, $5.5885\mathrm{m}$, and $6.4637\mathrm{m}$.

From Figure 13, Figure 14, Figure 15, when the CF processes of INS/CNS/GPS integration navigation start, the curves of attitude errors, velocity errors, and position errors converge quickly, and the noise components of navigation errors are basically filtered out. Throughout the process of simulation, the navigation parameters output by the CF-based SINS/CNS/GPS integrated navigation system remain convergent. The attitude errors are less than ${0.7}^{\prime }$, the velocity errors are less than $0.15\mathrm{m}/\mathrm{s}$, and the position errors are less than $10\mathrm{m}$. The errors of navigation parameters are all within the acceptable range. So, the proposed method of CF-based information fusion can meet the requirements of long-term and high-precision navigation for an SINS/CNS/GPS integrated navigation system of an aerospace plane.

In order to further analyze the performance of the proposed algorithm, the semi-physical simulation of SINS/CNS/GPS integrated navigation algorithm based on FKF (federated Kalman filter) is carried out under the same simulation conditions. The estimation errors of CF are compared with that of FKF, as shown in

Table 2. Comparison of estimation errors between CF and federated Kalman filter (FKF).

$\mathbf{Errors}\left(1\mathit{\sigma }\right)$	CF	FKF	CNS	GPS
${\varphi }_x{(}^{\prime })$	0.2124	0.1442	0.3333	N/A
${\varphi }_y{(}^{\prime })$	0.2137	0.1538	0.3333	N/A
${\varphi }_z{(}^{\prime })$	0.2100	0.1449	0.3333	N/A
$\delta V_E^{}(\mathrm{m}/\mathrm{s})$	0.0426	0.0275	N/A	0.1
$\delta V_N^{}(\mathrm{m}/\mathrm{s})$	0.0420	0.0164	N/A	0.1
$\delta V_U^{}(\mathrm{m}/\mathrm{s})$	0.0401	0.0168	N/A	0.1
$\delta L(\mathrm{m})$	6.8851	2.3852	N/A	10
$\delta \lambda (\mathrm{m})$	5.5885	2.3878	N/A	10
$\delta H(\mathrm{m})$	6.4637	3.9670	N/A	10

. For both CF and FKF, the mean value of each navigation parameter error is one order of magnitude smaller than the standard deviation of the corresponding error. The means of errors are no longer listed in the table, and only the standard deviations of errors are compared and analyzed. The simulation results show that the statistical errors of the CF-based integrated navigation algorithm are close to that of the FKF-based integrated navigation algorithm. In terms of filtering accuracy, the estimation errors of the two filtering methods are in the same order of magnitude; however, in terms of the real-time and complexity, because FKF needs to perform a large number of high-order matrix operations, the computational complexity of FKF is much higher than that of CF, and the real-time performance of FKF is much poorer than that of CF.

5.4. Analysis of Real-Time Performance

In order to investigate the real-time performance of the proposed CF-based information fusion algorithm for SINS/CNS/GPS integrated navigation, the average single-time elapsed times of the FKF and CF are recorded by using TIC and TOC functions of MATLAB. Denote the average single-time elapsed times of the FKF and CF as $t_{FKF}$ and $t_{CF}$, respectively. According to the recorded time, we have $t_{CF}=3.3836\times {10}^{-5}\mathrm{s}$ and $t_{FKF}=6.5180\times {10}^{-4}\mathrm{s}$. In order to eliminate the effect resulted from the difference of computer performances, the relative elapsed times of FKF and CF are defined as:

$$\{\begin{array}{l}{t}_{CF}^R=t_{CF}^{}/t_{FKF}\\ t_{FKF}^R=t_{FKF}^{}/t_{FKF}^{}\end{array}.$$

(40)

Obviously, $t_{FKF}^R=1$, $t_{CF}^R=0.0519$. The relative elapsed times of FKF and CF are shown in Figure 16. From Figure 16, it can be seen that for the information fusion of SINS/CNS/GPS integrated navigation, the elapsed time of the CF is much shorter than that of the FKF, and the average single-time elapsed time of the CF is just $5.19\%$ of the average single-time elapsed time of the FKF. The above simulation results demonstrate that the proposed CF-based method of information fusion for multi-sensor integrated navigation system successfully overcomes the drawback of heavy computational load of the FKF-based method. When used for SINS/CNS/GPS integrated navigation, the proposed CF-based method can save 94.81% computational time compared with the FKF-based method.

5.5. Experimental Test Based on Real-Time System

In order to further verify the priorities of a proposed CF-based information fusion algorithm in terms of computational complexity and real-time performance, an experimental test based on a real-time system was conducted. The data processing architecture of a real-time system is shown in Figure 17, in which the navigation computer mainly includes Digital Signal Processor (DSP, Texas Instruments, TMS320C6748), Field Programmable Gate Array (FPGA, Altera, Cyclone V 5CEFA9F23I7N), Analog-to-Digital Converter (ADC, Analog Devices Inc., Norwood, MA, USA, AD9288), etc. The DSP chip is used to calculate the information fusion algorithms of SINS/CNS/GPS integrated navigation. The experimental process is as follows: Firstly, a frame of experimental data (including the data of SINS, CNS, and GPS) generated by semi-physical simulation is input into the DSP chip through the RS232/422 serial communication interface of the navigation computer. Subsequently, the two information fusion algorithms of SINS/CNS/GPS integrated navigation based on CF and FKF are carried out by the DSP chip, respectively. Eventually, the recorded time consumptions are output to a terminal display device.

The computational times of CF and FKF are shown in

Table 3. Time consumptions of CF and FKF.

Filters	Average Time Consumption(s)
CF	$3.4795\times {10}^{-6}$
FKF	$5.5088\times {10}_{}^{-5}$

. It can be seen that the time consumptions of CF and FKF are $3.4795\times {10}^{-6}$ s and $5.5088\times {10}_{}^{-5}$ s, respectively. So, the timing performance of CF is better than that of FKF. There, the time consumption refers to the average time to process a frame of experimental data.

6. Conclusions

In this paper, a novel information fusion method based on complementary filter is proposed for the SINS/CNS/GPS integrated navigation system of an aerospace plane. The transformation algorithm of the CNS quaternion from the $i$ frame to the $n$ frame is designed, and the CF-based attitude/speed/position estimations are designed and implemented for SINS/CNS/GPS integrated navigation system, respectively. By utilizing the time-frequency domain analysis, the optimal cut-off frequencies of complementary filters are obtained. Compared with the FKF-based method, it can improve the real-time performance and computational efficiency of information fusion of spacecraft-borne multi-sensor integrated navigation. In order to verify the proposed CF-based information fusion algorithm, a semi-physical simulation experiment platform is designed, and the algorithm validity is verified by a semi-physical simulation experiment. The experiment results show that the CF-based integration method has the advantages of fast convergence speed, acceptable noise suppression, and acceptable navigation accuracy. By comparing the two methods of CF-based and FKF-based, it can be concluded that the accuracy of the CF-based method is close to that of the most commonly used FKF-based method; however, the CF has better real-time performance and lower computational complexity than FKF. For a SINS/CNS/GPS integrated navigation system of spacecraft, the elapsed time of CF is decreased to $5.19\%$ of FKF, while the computational efficiency of CF is increased by 94.81%. The experiment results prove that the computational efficiency of information fusion of SINS/CNS/GPS integrated navigation is improved.