A SINS/SRS/GNS Autonomous Integrated Navigation System Based on Spectral Redshift Velocity Measurements

Abstract

In order to meet the requirements of autonomy and reliability for the navigation system, combined with the method of measuring speed by using the spectral redshift information of the natural celestial bodies, a new scheme, consisting of Strapdown Inertial Navigation System (SINS)/Spectral Redshift (SRS)/Geomagnetic Navigation System (GNS), is designed for autonomous integrated navigation systems. The principle of this SINS/SRS/GNS autonomous integrated navigation system is explored, and the corresponding mathematical model is established. Furthermore, a robust adaptive central difference particle filtering algorithm is proposed for this autonomous integrated navigation system. The simulation experiments are conducted and the results show that the designed SINS/SRS/GNS autonomous integrated navigation system possesses good autonomy, strong robustness and high reliability, thus providing a new solution for autonomous navigation technology.

1. Introduction

At present, navigation methods mainly include inertial navigation, surface radio navigation, celestial navigation and satellite navigation. Each of these methods possess their own characteristics, scope of application and limitations, and cannot achieve completely autonomous or high precision autonomous navigation [1,2].

Strapdown inertial navigation system (SINS) has the advantages of simple structure and strong autonomy, providing continuous support of position, velocity and attitude information, under all-weather conditions. However, the navigation errors of SINS accumulate with time, thus the SINS cannot obtain high precision navigation information [3,4]. Radio navigation is limited by the coverage area of ground stations. The operation of this system is related to the radiowave propagation conditions, and vulnerable to the influence of artificial interference, which make radio navigation a non-autonomous navigation method [5,6,7]. Satellite navigation is the combination of celestial navigation and radio navigation, with the advantages of convenient application and high precision. However, because the satellite navigation depends on artificial beacons, it is also vulnerable to the influence of artificial interference and cannot achieve fully autonomy [8,9,10]. The advantages of celestial navigation are high accuracy of attitude measurement and strong ability to resist electromagnetic interference. Its disadvantages are the low rate of data updating, indirectly measured velocity and the limited navigation performance due to the number, distance and space environment of target celestial bodies [1,2,11,12].

The spectral redshift of natural light sources contains the velocity information of the celestial body relative to the moving object [13]. Based on this principle, spectral redshift navigation (SRS) becomes a forward-looking navigation method, with the advantages of simple principle, high navigation accuracy, strong autonomy and good real-time performance. The SRS can provide a new technological method to improve the autonomy of navigation systems. However, when the carrier is in the process of attitude maneuvering, the navigation accuracy will be worse and the navigation result may even be divergent due to the insufficient or interrupted observation information. Therefore, it is necessary to combine SRS with some other navigation method to constitute an integrated navigation system, thus compensating for the defects of SRS alone.

Nonlinear filtering algorithms are commonly used in autonomous navigation systems, however, these filtering algorithms have their own defects. For example, when the practical probability function has multiple peak values, the extended Kalman filtering (EKF) may be divergent because the nonlinear system equations arelinearized by the Taylor expansion and the linearized states are required to obey the Gaussian distribution [14,15,16,17]. The unscented Kalman filtering (UKF) method also demands the states obey the Gaussian distribution, which is not applicable for nonlinear systems with non-Gaussian distribution [18,19]. The particle filtering (PF) method is prone to particle degeneracy phenomena, and the accuracy depends heavily on the choice of importance sampling density and resampling scheme [20,21,22,23,24]. By robustly estimating the covariance matrix of observation noise and adaptively adjusting the covariance matrix of the state noise by augmenting the adaptive factor into the covariance matrix of the state prediction, the robust adaptive filtering can deal with observation and model noises to obtain reliable filtering results, especially in the presence of abnormal observations [25,26,27,28]. Therefore, combined with robust adaptive filtering and particle filtering, a new nonlinear filtering algorithm for autonomous navigation system is designed to improve the accuracy and reliability of the autonomous navigation system.

Based on the principle of velocity measurement by using spectral redshift of celestial bodies in space, and combined with the advantages of Geomagnetic Navigation System (GNS), this paper proposes a new SINS/SRS/GNS autonomous integrated navigation system. The principle, scheme and mathematical model of this autonomous integrated navigation system are established, and a high-precision nonlinear filtering algorithm for the autonomous navigation system is proposed. Subsequently, all of the models and algorithms are verified by experiments.

2. The Principle of the Spectral Redshift Navigation

We assume the carrier can receive optical signals from celestial bodies during the space flight process. According to the Doppler effect principle, the spectral frequency of carrier received signals is not equal to the spectral frequency of the celestial bodies, and the variation of spectral frequency is related to the motion state of the carrier relative to the celestial bodies. Therefore, the relative velocity of the carrier can be obtained by measuring the redshift of the spectral frequency. According to the space vector relation, if the number of observed celestial bodies (which are non-collinear) is greater than three, the velocity vector of the carrier in inertial coordinate system can be determined by integrating the ephemeris of the celestial bodies and the inertial attitude information, and then the position vector can be obtained by integration.

The total redshift measured by spectral sensors is actually the sum of gravitational redshift, cosmological redshift and Doppler redshift. The gravitational redshift and cosmological redshift can be obtained by astronomical ephemeris, and both of them need to be removed. Only the Doppler redshift can be used to navigation calculation. We define the Doppler redshift as:

$$z=\frac{\lambda -{\lambda }_0}{{\lambda }_0}=\frac{f_0-f}{f}$$

(1)

where $z$ represents Doppler redshift, ${\lambda }_0$ is the original wavelength of the spectral line, $\lambda$ is the observed wavelength, $f_0$ is the original frequency of the spectral line, f is the observed frequency.

Accordingly, the redshift equation is:

$$1+z=\frac{f_0}{f}=\frac{1+v\cos \theta /c}{\sqrt{1-\raisebox{1ex}{$v^2$}\!\left/ \!\raisebox{-1ex}{$c^2$}\right.}}$$

(2)

where $v$ is the velocity of the carrier relative to the light source, $\theta$ is the angle between the wave vector of the light source pointing to the carrier and the velocity $v$ in the inertial coordinate system, $v\cos \theta$ represents radial velocity, $c$ is the light speed in vacuum.

In three-dimensional celestial navigation, Equation (2) is transformed and applied to the first reference celestial body, and we can obtain:

$$v_{r1}=\left(1+z_1\right)\sqrt{c^2-{\left|{\mathit{v}}_p-{\mathit{v}}_1\right|}^2}-c$$

(3)

where $v_{r1}$ is the radial velocity of the carrier relative to the first reference celestial body (the light source), $z_1$ is the redshift value of the first reference celestial body, ${\mathit{v}}_p$ is the velocity vector of the carrier in the inertial coordinate system, ${\mathit{v}}_1$ is the velocity vector of the first reference celestial body in the inertial coordinate system. The redshift value can be obtained by spectral preprocessing, spectral line feature extraction and spectral line matching.

Select three reference celestial bodies and the equations are listed as follows:

$$\{\begin{array}{l}{v}_{r1}=\left(1+z_1\right)\sqrt{c^2-{\left|{\mathit{v}}_p-{\mathit{v}}_1\right|}^2}-c\\ v_{r2}=\left(1+z_2\right)\sqrt{c^2-{\left|{\mathit{v}}_p-{\mathit{v}}_2\right|}^2}-c\\ v_{r3}=\left(1+z_3\right)\sqrt{c^2-{\left|{\mathit{v}}_p-{\mathit{v}}_3\right|}^2}-c\end{array}$$

(4)

From the geometric relationship of the celestial bodies, the relationship between ${\mathit{v}}_P$ and $v_{r1}$, $v_{r2}$, $v_{r3}$ satisfies the following relationship:

$$\{\begin{array}{l}{v}_{r1}=\left({\mathit{v}}_P-{\mathit{v}}_1\right)\cdot {\mathit{u}}_1\\ v_{r2}=\left({\mathit{v}}_P-{\mathit{v}}_2\right)\cdot {\mathit{u}}_2\\ v_{r3}=\left({\mathit{v}}_P-{\mathit{v}}_3\right)\cdot {\mathit{u}}_3\end{array}$$

(5)

where ${\mathit{v}}_1$, ${\mathit{v}}_2$ and ${\mathit{v}}_3$ are the velocity vectors of the celestial bodies in the inertial system determined by the ephemeris, respectively, ${\mathit{u}}_1$, ${\mathit{u}}_2$ and ${\mathit{u}}_3$ are the unit vectors of the position vectors of the celestial bodies pointing to the carrier in the inertial coordinate system, which can be measured by the sun sensor or the star sensor.

The state estimation equations for the velocity vector and the position vector are established as:

$$\{\begin{array}{l}{\mathit{v}}_P=f\left({\mathit{v}}_1,{\mathit{v}}_2,{\mathit{v}}_3,{\mathit{u}}_1,{\mathit{u}}_2,{\mathit{u}}_3,v_{r1},v_{r2},v_{r3}\right)\\ {\mathit{r}}_P={\displaystyle \int {\mathit{v}}_{P}dt}\end{array}$$

(6)

After the initial value is given, the velocity vector ${\mathit{v}}_P$ of the carrier in the inertial coordinate system can be obtained by solving (6), and then the position vector ${\mathit{r}}_P$ can be obtained by integration.

3. Design of the SINS/SRS/GNS Autonomous Integrated Navigation System

Since the SINS can provide three-dimensional attitude, speed and position in all weather and all day conditions, and possesses the ability of good concealment and strong anti-interference. it is usually used as the main navigation system. SRS and GNS are used as auxiliary navigation systems to form the SINS/SRS/GNS autonomous integrated navigation system, as shown in Figure 1.

Figure 1. The principle of the SINS/SRS/GNS autonomous integrated navigation system.

The estimation of spectral redshift can be calculated by de-noising and spectral lines separation of spectral signals that obtained by a spectrometer. According to the principle of spectral redshift navigation, the accurate velocity information of the carrier can be obtained by combining the celestial ephemeris and the attitude information measured by the star sensor. The difference between this velocity and the velocity obtained by SINS is made as observation vector and sent to the SINS/SRS subfilter for filtering calculation. GNS can get regional geomagnetic distribution by magnetic sensor, and the position information of the carrier can be obtained by matching the regional geomagnetic distribution and the reference geomagnetic library. The difference between this position and the position from SINS is treated as observation vector, which is sent to SINS/GNS subfilter for filtering calculation. The attitude errors, position errors and velocity errors calculated by the SINS/SRS subfilter and the SINS/GNS subfilter are both sent into the global filter for multi-sensor information fusion. Then the global optimal estimation of navigation errors is calculated to correct the SINS, for obtaining the accurate output of SINS/SRS/GNS autonomous integrated navigation system.

3.1. State Equation of the Autonomous Integrated Navigation System

Using the E-N-U (East-North-Up) geographic coordinate system as the base coordinate system for navigation. The system state vector $x(t)$ of SINS/SRS/GNS autonomous integrated navigation system can be defined as:

$$\mathit{x}(t)={[{(\delta \mathit{q})}^{\mathrm{T}},{(\delta \mathit{V})}^{\mathrm{T}},{(\delta \mathit{P})}^{\mathrm{T}},{\mathit{\epsilon }}^{\mathrm{T}},{\mathbf{\nabla }}^{\mathrm{T}}]}^{\mathrm{T}}$$

(7)

where $\delta \mathit{q}={[\delta q_0,\delta q_1,\delta q_2,\delta q_3]}^{\mathrm{T}}$ is the attitude error quaternion of SINS, $\delta \mathit{V}={[\delta V_E,\delta V_N,\delta V_U]}^{\mathrm{T}}$ is the velocity error of SINS, $\delta \mathit{P}={[\delta L,\delta \lambda ,\delta h]}^{\mathrm{T}}$ is the latitude error, longitude error and height error of SINS, $\mathit{\epsilon }={[{\epsilon }_x,{\epsilon }_y,{\epsilon }_x]}^{\mathrm{T}}$ represents the random drift of the gyroscope, $\mathbf{\nabla }={[{\nabla }_x,{\nabla }_y,{\nabla }_z]}^{\mathrm{T}}$ is the constant bias of the accelerometer.

According to (7) and the error model of SINS [10], the kinematic model of the SINS/SRS/GNS autonomous integrated navigation system is described as:

$$\dot{\mathit{x}}=f(\mathit{x},t)+\mathit{G}(t)\mathit{w}(t)$$

(8)

where $f(\mathit{x},t)$ is nonline state function of the system, $\mathit{G}(t)$ is the coefficient matrix of system noise. $\mathit{w}(t)={[w_{gx},w_{gy},w_{gz},w_{ax},w_{ay},w_{az}]}^{\mathrm{T}}$ represents system noise, $[w_{gx},w_{gy},w_{gz}]$ is the white Gaussian noise of the gyro measurement, $[w_{ax},w_{ay},w_{az}]$ is the white Gaussian noise of the accelerometer measurement:

$$f(\mathit{x},t)=\left[\begin{array}{l}B(I-{\mathit{C}}_n^c){\mathit{\omega }}_{in}^n-B{\mathit{C}}_b^c{\mathit{\epsilon }}^b\\ (\mathit{I}-{\mathit{C}}_c^n){\mathit{C}}_b^c{\mathit{f}}^b-(2{\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n)\times \delta {\mathit{V}}^n-(2\delta {\mathit{\omega }}_{ie}^n+\mathit{\delta }{\omega }_{en}^n)\times {\mathit{V}}^n+{\mathit{C}}_b^c{\nabla }^b\\ \mathit{M}\delta \mathit{V}+\mathit{N}\delta \mathit{L}\\ {\mathbf{0}}_{3\times 1}\\ {\mathbf{0}}_{3\times 1}\end{array}\right]$$

(9)

$$\mathit{G}(t)=\left[\begin{array}{cc}{\mathit{C}}_b^c& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathit{C}}_b^c\\ {\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}\right]$$

where ${\mathit{C}}_n^c$ and ${\mathit{C}}_b^c$ are the attitude transition matrixes of the navigation coordinate system $n$ and the carrier coordinate system $b$ to the computing coordinate system $c$, respectively. ${\mathit{\omega }}_{in}^n$ represents the projection of the angular velocity of the carrier in the navigation coordinate system, ${\mathit{\omega }}_{ie}^n$ and ${\mathit{\omega }}_{en}^n$ are the rotation angle velocity and the position angular velocity of the earth, respectively. ${\mathit{V}}^n$ is the velocity of the carrier, ${\mathbf{\nabla }}^b$ is the error of the accelerometer, ${\mathit{f}}^b$ is the accelerometer’s specific force. $\mathit{M}$ and $\mathit{N}$ are the coefficient matrixes of the velocity error and position error respectively, $\delta \cdot$represents the corresponding error of the parameter. $B=\frac{1}{2}M({\mathit{Q}}_p^n)$, $M({\mathit{Q}}_p^n)$ is the matrix representation of quaternion multiplication, and:

$$M({\mathit{Q}}_p^n)=\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]$$

(10)

3.2. Observation Equation of the Autonomous Integrated Navigation System

(1) Observation equation of SINS/SRS subsystem

Let $v_X$, $v_Y$ and $v_Z$ represent the E-velocity, N-velocity and U-velocity of the carrier in the inertial system, respectively, which are obtained by the spectral redshift navigation, then:

$${\mathit{V}}_n={\mathit{C}}_b^n{\mathit{V}}_b$$

(11)

$${\mathit{V}}_n=\left[\begin{array}{c}{v}_{SE}\\ v_{SE}\\ v_{SE}\end{array}\right],{\mathit{V}}_b=\left[\begin{array}{c}{v}_X\\ v_Y\\ v_Z\end{array}\right]$$

(12)

where ${\mathit{C}}_b^n$ is the attitude transition matrixes of the body coordinate system to the navigation coordinate system, ${\mathit{V}}_n$ is velocity vector of the carrier in navigation coordinate system, ${\mathit{V}}_b$ is velocity vector of the carrier in body coordinate system. $v_{SE}$, $v_{SN}$ and $v_{SU}$ are E-velocity, N-velocity and U-velocity of the carrier in navigation coordinate system, respectively, which are obtained by the spectral redshift navigation.

Take the difference of the velocity between SRS and SINS output as the observation, and the velocity observation equation is obtained as:

$${\mathit{Z}}_v=\left[\begin{array}{c}{Z}_{S1}\\ Z_{S2}\\ Z_{S3}\end{array}\right]=\left[\begin{array}{c}{v}_E-v_{SE}\\ v_N-v_{SN}\\ v_U-v_{SU}\end{array}\right]={\mathit{H}}_v\mathit{X}\left(t\right)+{\mathit{V}}_v\left(t\right)$$

(13)

where ${\mathit{H}}_v$ is the velocity observation matrix, $v_E$, $v_N$ and $v_U$ are the E-velocity, N-velocity and U-velocity of the carrier, respectively, which are obtained by the SINS, ${\mathit{V}}_v$ is velocity observation noise. ${\mathit{H}}_v$ can be expressed as:

$${\mathit{H}}_v={\left[\begin{array}{ccc}{\mathbf{0}}_{3\times 3}& diag\left(\begin{array}{ccc}1& 1& 1\end{array}\right)& {\mathbf{0}}_{3\times 9}\end{array}\right]}_{3\times 15}$$

(14)

Accordingly, the observation equation of SINS/SRS subsystem can be expressed as:

$$\begin{array}{ll}{\mathit{Z}}_1\left(t\right)& =\left[{\mathit{Z}}_v\left(t\right)\right]=\left[{\mathit{H}}_v\right]\mathit{X}\left(t\right)+\left[{\mathit{V}}_v\left(t\right)\right]\hfill \\ & ={\mathit{H}}_1\left(t\right)\mathit{X}\left(t\right)+{\mathit{v}}_1\left(t\right)\hfill \end{array}$$

(15)

where ${\mathit{H}}_1\left(t\right)$ is observation matrix, ${\mathit{V}}_1\left(t\right)$ is white noise vector.

(2) Observation equation of SINS/GNS subsystem

The difference between the positions of SINS and GNS is selected as the observation, and the observation vector can be described as:

$${\mathit{Z}}_2(t)=\left[\begin{array}{cc}\begin{array}{c}{L}_I\\ {\lambda }_I\\ h_I\end{array}& \begin{array}{c}{L}_G\\ {\lambda }_G\\ h_G\end{array}\end{array}\right]=\left[\begin{array}{c}(L+\delta L_I)-(L+\delta L_G)\\ (\lambda +\delta {\lambda }_I)-(\lambda +\delta {\lambda }_G)\\ (h+\delta h_I)-(h+\delta h_G)\end{array}\right]=\left[\begin{array}{c}\delta L_I\\ \delta {\lambda }_I\\ \delta h_I\end{array}\right]-\left[\begin{array}{c}\delta L_G\\ \delta {\lambda }_G\\ \delta h_G\end{array}\right]$$

(16)

where $L_I$, ${\lambda }_I$ and $h_I$ are latitude, longitude, and height of the SINS output, respectively. L_G, ${\lambda }_G$ and $h_G$ are the latitude, longitude, and height of the GNS, respectively. $\delta \cdot$represents the corresponding parameter error.

According to (18), the observation equation of the SINS/GNS subsystem can be obtained as:

$${\mathit{Z}}_2(t)={\mathit{H}}_2(t)\mathit{X}(t)+{\mathit{V}}_2(t)$$

(17)

where ${\mathit{H}}_2(t)$ is the observation matrix, ${\mathit{V}}_2(t)={[\delta L_G,\delta {\lambda }_G,\delta h_G]}^{\mathrm{T}}$ is a white noise vector.

3.3. Information Fusion Algorithm of the Autonomous Integrated Navigation

According to federal filtering technology [29,30], the information fusion principle of SINS/SRS/GNS autonomous integrated navigation system is shown in Figure 2. Firstly, SINS/SRS and SINS/GNS navigation subfilters are designed, and two sets of local optimal estimation ${\widehat{\mathit{X}}}_i$ and local optimal error covariance matrix ${\mathsf{\Sigma }}_{\widehat{X},i}(i=1,2)$ are obtained. Secondly, the federated filtering technology is applied to send two sets of local optimal estimation into the main filter for information fusion, and the global optimal estimation ${\widehat{\mathit{X}}}_g$ and the global optimal error covariance matrix ${\mathsf{\Sigma }}_{\widehat{X},g}$ of the system state vector are obtained. Finally, the SINS error is corrected in real time by using ${\widehat{\mathit{X}}}_g$. Due to the divergence of the SINS height channel, the height of the altimeter output is used to correct the SINS. The optimal fusion algorithm for SINS/SRS/GNS autonomous integrated navigation system is expressed as:

$$\{\begin{array}{c}{\mathsf{\Sigma }}_{\widehat{X},g}={({\mathsf{\Sigma }}_{\widehat{X},1}^{-1}+{\mathsf{\Sigma }}_{\widehat{X},2}^{-1})}^{-1}\\ {\widehat{\mathit{X}}}_g={\mathsf{\Sigma }}_{\widehat{X},g}({\mathsf{\Sigma }}_{\widehat{X},1}^{-1}{\widehat{\mathit{X}}}_1+{\mathsf{\Sigma }}_{\widehat{X},2}^{-1}{\widehat{\mathit{X}}}_2)\end{array}$$

(18)

Figure 2. The information fusion principle of SINS/SRS/GNS autonomous integrated navigation system.

4. Robust Adaptive Central Difference Particle Filtering Algorithm

Combined with the advantages of robust adaptive filtering (RAF), central difference Kalman filtering (CDKF) and particle filtering (PF), a robust adaptive central difference particle filtering algorithm (RACDPF) is proposed. By choosing appropriate equivalent weight function and adaptive factor, the algorithm controls the information of the state model and observation model to suppresse the influence of abnormal interference on the system state estimation, and updates the a priori information, adjusts the filtering gain, for improving the filtering accuracy.

4.1. Algorithm Steps

Consider the nonlinear system model:

$$\begin{array}{l}{\mathit{x}}_k=f({\mathit{x}}_{k-1},{\mathit{v}}_{k-1})\\ {\mathit{y}}_k=h({\mathit{x}}_k,{\mathit{n}}_k)\end{array}$$

(19)

The main steps of the robust adaptive central difference particle filtering algorithm are as follows.

(1)

Initialization. At the time $k=0$:

$$\begin{array}{l}{\widehat{\mathit{x}}}_0=E[{\mathit{x}}_0]\\ {\mathit{P}}_{{\mathit{x}}_0}=E{[({\mathit{x}}_0-{\widehat{\mathit{x}}}_0)({\mathit{x}}_0-{\widehat{\mathit{x}}}_0)]}^T\end{array}$$

(20)

(2)

When $k=1,\cdots ,\infty$, and $j=0,\cdots ,2L$:

$$\begin{array}{l}{W}_0^m=\frac{h^2-L}{h^2},W_j^m=\frac{1}{2h^2}\\ W_j^{c1}=\frac{1}{4h^2},W_j^{c2}=\frac{h^2-1}{4h^4}\end{array}$$

(21)

where $L$ is state dimension, $h$ represents the central differential interval of the scalar, for Gauss prior random variables, the optimal value is $h=\sqrt{3}$.

Time update:

$${\mathit{\chi }}_{k-1}^i=[{\widehat{\mathit{x}}}_{k-1}^i,{\widehat{\mathit{x}}}_{k-1}^i+h\sqrt{{\mathit{P}}_{k-1}^i},{\widehat{\mathit{x}}}_{k-1}^i-h\sqrt{{\mathit{P}}_{k-1}^i}]$$

(22)

$${\mathit{\chi }}_{k|k-1}^i=f({\mathit{\chi }}_{k-1}^i,{\mathit{u}}_{k-1}^i)$$

(23)

$${\widehat{\mathit{x}}}_k^{i-}={\displaystyle \sum _{j=0}^{2L}{W}_j^m{\mathit{\chi }}_{j,k|k-1}^i}$$

(24)

$${\mathit{P}}_{x_k}^{i-}={\displaystyle \sum _{j=0}^L[W_j^{c_1}{({\mathit{\chi }}_{j,k|k-1}^i-{\mathit{\chi }}_{L+j,k|k-1}^i)}^2+}{W}_j^{c_2}{({\mathit{\chi }}_{j,k|k-1}^i+{\mathit{\chi }}_{L+j,k|k-1}^i-2{\mathit{\chi }}_{0,k|k-1}^i)}^2]$$

(25)

$${\mathit{\chi }}_{k|k-1}^{i*}=[{\widehat{\mathit{x}}}_{k|k-1}^i,{\widehat{\mathit{x}}}_{k|k-1}^i+h\sqrt{{\mathit{P}}_{x_{k|k-1}}^{i-}},{\widehat{\mathit{x}}}_{k|k-1}^i-h\sqrt{{\mathit{P}}_{x_{k|k-1}}^{i-}}]$$

(26)

Observation update:

$${\mathit{Y}}_{k|k-1}^i=h({\mathit{\chi }}_{k|k-1}^{i*})$$

(27)

$${\widehat{\mathit{y}}}_k^{i-}={\displaystyle \sum _{j=0}^{2L}{W}_j^m}{\mathit{Y}}_{j,k|k-1}^i$$

(28)

$${\mathit{P}}_{{\overline{y}}_k}^i={\displaystyle \sum _{j=0}^L[W_j^{c_1}{({\mathit{Y}}_{j,k|k-1}^i-{\mathit{Y}}_{L+j,k|k-1}^i)}^2+}{W}_j^{c_2}{({\mathit{Y}}_{j,k|k-1}^i+{\mathit{Y}}_{L+j,k|k-1}^i-2{\mathit{Y}}_{0,k|k-1}^i)}^2]$$

(29)

$${\mathit{P}}_{x_k{y}_k}^i=\sqrt{W_1^{{}_{c_1}}{\mathit{P}}_{x_k}^{i-}}{[{\mathit{Y}}_{1:L,k|k-1}^i-{\mathit{Y}}_{L+1:2L,k|k-1}^i]}^T$$

(30)

The predicted residual vector contains the state information that is not corrected by observation information, and can reflect the disturbance of the dynamic system. Therefore, we can use the predicted residual vector as the variable to construct the error discriminant statistic and the adaptive factor of the kinematic model. The $i$th predicted residual vector ${\overline{\mathit{V}}}_k^i$ at time k can be expressed as:

$${\overline{\mathit{V}}}_k^i={\mathit{Y}}_k^i-{\widehat{\mathit{y}}}_k^i$$

(31)

Correspondingly, the error discriminant statistic of the kinematic model is written as:

$$\Delta {\overline{\mathit{V}}}_k^i={\left(\frac{{\left({\overline{\mathit{V}}}_k^i\right)}^T{\overline{\mathit{V}}}_k^i}{tr({\mathit{P}}_{y_k{y}_k}^i)}\right)}^{\frac{1}{2}}$$

(32)

Then, the adaptive factor that is based on the error discriminant statistic can be obtained as [25]:

$${\alpha }_k^i=\{\begin{array}{l}1\left|\Delta {\overline{\mathit{V}}}_k^i\right|\le c\\ \frac{c}{\left|\Delta {\overline{\mathit{V}}}_k^i\right|}\left|\Delta {\overline{\mathit{V}}}_k^i\right|>c\end{array}$$

(33)

where ${\alpha }_k^i$ is the $i$th adaptive factor at time $k$, $c$ is an empirical constant, and generally $1.0<c<2.5$.

According to the Kalman filtering framework, we can obtain:

$${\mathit{K}}_k^i={\mathit{P}}_{x_k{y}_k}^i{({\mathit{P}}_{{\overline{y}}_k}^i)}^{-1}$$

(34)

$${\widehat{\mathit{x}}}_k^i={\widehat{\mathit{x}}}_k^{i-}+{\mathit{K}}_k^i({\mathit{y}}_k^i-{\widehat{\mathit{y}}}_k^{i-})$$

(35)

$${\mathit{P}}_{x_k}^i={\mathit{P}}_{x_k}^{i-}-{\alpha }_k^i{\mathit{K}}_k^i{\mathit{P}}_{{\overline{y}}_k}^i{\mathit{K}}_k^{iT}$$

(36)

It can be seen from (38) that the adaptive factor ${\alpha }_k$ can influence and adjust $P_{x_k}$ to make the importance density function closer to the actual distribution. When anomalies exist in the observation model, the adaptive factor ${\alpha }_k$ decreases, the use of observation information is reduced in the process of the state estimation, thus weakening the abnormal interference of the observation model, or vice versa.

Let $N({\widehat{\mathit{x}}}_k^i,{\mathit{p}}_{x_k}^i)$ be the importance density function of the particle sampling, the new sample ${\mathit{x}}_k^i\sim \mathit{N}({\widehat{\mathit{x}}}_k^i,{\mathit{P}}_{x_k}^i)$ can be obtained by importance sampling. In addition, compared with the UKF algorithm, the CDKF algorithm only needs to calculate the parameter $h$, therefore the computational complexity is reduced.

(3)

Calculate the weight:

$$w_k^i=w_{k-1}^i\frac{p({\mathit{y}}_k|{\mathit{x}}_k^i)p({\mathit{x}}_k^i|{\mathit{x}}_{k-1}^i)}{q({\mathit{x}}_k^i|{\mathit{x}}_{k-1}^i,{\mathit{y}}_k)}$$

(37)

and normalize the weight to ${\tilde{w}}_k^i=w_k^i/{\displaystyle \sum _{i=1}^n{w}_k^i}$.

(4)

Calculate the threshold estimate:

$${\widehat{N}}_{eff}=1/{\displaystyle \sum _{i=1}^N{({\tilde{w}}_{{}_k}^i)}^2}$$

(38)

Compare the result with the established threshold to determine the degree of the particle degradation. The smaller ${\widehat{N}}_{eff}$ is, the worse the particle degeneracy is. In order to inhibit particle degeneracy, M new particles can be obtained by resampling the posterior density function, and given the same weight 1/M.

(5)

Markov Chain Monte Carlo (MCMC) move. This step can be selected according to the needs.

(6)

Calculate the estimate of the nonlinear state vector and its covariance matrix:

$${\widehat{\mathit{x}}}_k^{}={\displaystyle \sum _{i=1}^N{\tilde{w}}_k^i}{\mathit{x}}^i{}_k$$

(39)

$${\widehat{\mathit{P}}}_k^{}={\displaystyle \sum _{i=1}^N{\tilde{w}}_k^i}({\mathit{x}}_k^i-{\widehat{\mathit{x}}}_k^{}){({\mathit{x}}_k^i-{\widehat{\mathit{x}}}_k^{})}^T$$

(40)

return to Step (2).

In the above steps, the Expectation Maximization (EM) [23] method can be used to replace the resampling, so that the state estimation is converged to the optimal value. In addition, when selecting the importance density function, the proposed RACDPF takes advantage of the important adjustment factor, namely the robust adaptive factor, which controls the contribution of the observed information in the state estimation, and provides better sampling function for the importance sampling process.

4.2. Adaptive Adjustment of the Weight

In the RACDPF algorithm, the adaptive adjustment method of the weight can be used to calculate the weight value, for reducing the amount of calculation while improving the accuracy. The principle of the method is that after the steps of time update and observation update, utilize observation information, Euclidean distance and precision dilution, which reflects statistical characteristics of observation noise to Euclidean distance, to adaptively adjust the weight, thus move the sampled particles from the high priori density region to the high likelihood density region, to obtain a posteriori density function closer to the true distribution.

After sampling the importance density function $N({\overline{\mathit{x}}}_k^i,{\mathit{P}}_k^i)$, the weight of each particle is calculated by:

$$w_k^i=w_{k-1}^i\frac{p({\mathit{y}}_k|{\mathit{x}}_k^i)p({\mathit{x}}_k^i|{\mathit{x}}_{k-1}^i)}{q({\mathit{x}}_k^i|{\mathit{x}}_{k-1}^i,{\mathit{y}}_k)}$$

(41)

Then the maximum weight $w_{k-\max }^{i^{*}}$ and the minimum weight $w_{k-\min }^{i^{*}}$ is recorded, and the observation innovation ${\mathit{y}}_{k-\max }^i$, ${\mathit{y}}_{k-\min }^i$, and Euclidean distance $L_{\max }^{}$ and $L_{}^i$ can be calculated, which are expressed as:

$$L_{\max }={({\mathit{y}}_{k-\min }^i-{\mathit{y}}_{k-\max }^i)}^{\mathrm{T}}\cdot ({\mathit{y}}_{k-\min }^i-{\mathit{y}}_{k-\max }^i)$$

(42)

$$L^i={({\mathit{y}}_k^i-{\mathit{y}}_{k-\max }^i)}^{\mathrm{T}}\cdot ({\mathit{y}}_k^i-{\mathit{y}}_{k-\max }^i)$$

(43)

where ${\mathit{y}}_{k-\max }^i={\mathit{y}}_k-h({\mathit{x}}_{k-\max }^i)$, ${\mathit{y}}_{k-\min }^i={\mathit{y}}_k-h({\mathit{x}}_{k-\min }^i)$, ${\mathit{x}}_{k-\max }^i$ and ${\mathit{x}}_{k-\min }^i$ represent the particles that correspond to the maximum weight and the minimum weight.

The weight of particles can be modified as:

$$w_k^{i^{*}}=w_k^{i^{*}}+(\raisebox{1ex}{$w_{k-\max }^{i^{*}}$}\!\left/ \!\raisebox{-1ex}{$N$}\right.)\cdot \sin (\raisebox{1ex}{$L^i$}\!\left/ \!\raisebox{-1ex}{$L_{\max }\cdot \raisebox{1ex}{$\pi $}\!\left/ \!\raisebox{-1ex}{$2$}\right.$}\right.)\cdot \beta$$

(44)

where $\beta$ is the adaptive coefficient determined by the statistical characteristics of observation noise, and:

$$\beta =\{\begin{array}{cc}\raisebox{1ex}{$K$}\!\left/ \!\raisebox{-1ex}{$\alpha \prime $}\right.& \alpha \prime \le \epsilon \\ 0& \alpha \prime >\epsilon \end{array}$$

(45)

where $\alpha \prime$ is the dilution of precision that reflects statistical characteristics of observation noise. By changing the size of $\alpha \prime$, we can adaptively adjust the distribution of weights and increase the weights of useful particles. When $\alpha \prime$ is large, the observation accuracy is low, otherwise, the observation accuracy is relatively high. $\epsilon$ is the threshold that determined by experience, $K$ is the proportional constant, and $\raisebox{1ex}{$K$}\!\left/ \!\raisebox{-1ex}{$\alpha \prime $}\right.>0$. When the observation noise is small, let $\beta =0$, that is, do not adjust the likelihood distribution. Otherwise, when the likelihood distribution is in the peak or at the tail of the transfer prior distribution, let $\beta =\raisebox{1ex}{$K$}\!\left/ \!\raisebox{-1ex}{$\alpha \prime $}\right.$, that is, artificially make the likelihood distribution wider.

5. Simulation Experiment and Result Analysis

In this section, we compare the navigation errors of the proposed SINS/SRS/GNS autonomous navigation system with the SINS, SINS/SRS subsystem and SINS/GNS subsystem, respectively, by using the proposed RACDPF algorithm, to verify the performance of our SINS/SRS/GNS autonomous navigation system. Furthermore, under the same conditions, the simulation results of UKF, PF, and the proposed RACDPF are also compared to verify the performance of the proposed filtering algorithm, including comparisons of the accuracy, real-time and robustness of the filtering algorithms.

In the experiment, the J2000 geocentric equatorial inertial coordinate system is selected for the coordinate system. Assume that the experimental data are from a flight of a spacecraft. The orbit parameters of the spacecraft are described as Semimajor axis 6947.035365 km, Eccentricity 0.001088, Orbit inclination 22.998°, Right ascension of ascending node 334.87°, Argument of perigee 341.452°, True anomaly 231.43°. We selected a part of the flight, which lasted for 1500 s, and the initial position is (3,330,812, −2,488,259, 5,565,647). The flight trajectory is shown in Figure 3.

Figure 3. Flight trajectory.

In the simulation process, the initial alignment error of SINS is 0, the initial velocity error is 1 m/s, the initial position error is 10 m, and the initial attitude error is $10^{″}$. The parameter for adaptive factor calculation is $c=1.5$ [25,27]. The number of particles is $M=200$, the simulation time is 1500 s, and the filtering period is 1 s. The parameters of the sensors used in the simulation are shown in Table 1.

Table 1. Parameters of the sensors used in the simulation.

Parameter Type	Value
Gyro constant drift	$0.02°/h$
Gyro random drift	$0.005°/\sqrt{h}$
Constant drift of accelerometer	0.05 mg
Random drift of accelerometer	$0.005\mathrm{mg}/\sqrt{h}$
Accuracy of geomagnetic matching	20 m
Accuracy of velocity measurement with SRS [31]	5.5 m/s

(1) Simulation verification of the subsystems

The simulation of SINS, SINS/SRS subsystem and SINS/GNS subsystem are carried out respectively, and the results are compared to verify the performance of the subsystems.

The simulation results are shown in Figure 4 and Figure 5. The error statistics of the subsystems are shown in Table 2.

Figure 4. Velocity errors of the subsystems.

Figure 5. Position errors of the subsystems.

Table 2. The error statistics of the subsystems.

Errors		The Subsystems
		SINS/GNS	SINS/SRS
Velocity RMSE (m/s)	East	9.4491	5.3219
	North	8.7579	5.0692
	Up	8.6688	5.2851
Position RMSE (m)	Longitude	16.8274	65.6695
	Latitude	17.3205	68.7935
	Height	16.7775	70.1250

It can be seen that the velocity error and position error of SINS accumulate with time and diverge, which cannot meet the high accuracy requirement of navigation system. Therefore, other navigation methods need to be used to correct SINS.

For the SINS/GNS subsystem, GNS can obtain high position accuracy, so it is mainly used to correct the position error of SINS. But the velocity accuracy is poor, and the correction effect is not obvious. For the SINS/SRS subsystem, SRS can obtain good velocity accuracy, so the velocity accuracy after correction is high, but the position accuracy is poor. Therefore, single SINS/GNS or SINS/SRS subsystem cannot meet the needs of autonomous navigation and positioning. We need to combine both of them to design the SINS/SRS/GNS autonomous integrated navigation system to improve the navigation accuracy.

(2) Simulation verification of the autonomous integrated navigation system

The simulation of SINS/SRS/GNS autonomous integrated navigation system is carried out, and the results are shown in Figure 6 and Figure 7. Table 3 shows the error statistics of the autonomous integrated navigation system.

Figure 6. Velocity error of the autonomous integrated navigation system.

Figure 7. Position error of the autonomous integrated navigation system.

Table 3. The error statistics of the autonomous integrated navigation system.

	Error		Value
SINS/SRS/GNS autonomous integrated navigation system	Position RMSE(m)	Longitude	17.0375
		Latitude	16.3565
		Height	16.7865
	Velocity RMSE(m/s)	East	5.3698
		North	5.1550
		Up	5.2546

The simulation results show that the designed SINS/SRS/GNS autonomous integrated navigation system adopts information fusion technology to combine SINS with SRS and GNS, and complements the three systems in performance. Therefore, the SINS/SRS/GNS system overcomes the defects of single navigation system, and exhibits a good performance. Therefore, the designed SINS/SRS/GNS system effectively improve the accuracy and reliability of autonomous navigation systems.

(3) Performance verification of filtering algorithm

Based on the above performance comparison and analysis, UKF and PF are also applied to the SINS/SRS/GNS autonomous integrated navigation system for filtering respectively, in order to verify the performance of the proposed RACDPF algorithm and autonomous integrated navigation system. We will compare and analyze the accuracy, real-time and robustness of the nonlinear filtering algorithms (UKF, PF and RACDPF) respectively, so as to evaluate the performance of the filtering algorithms from more aspects.

A. Accuracy Comparison of Filtering Algorithms

We define the filtering accuracy as the difference between the estimated state value and the reference state value. The accuracy of the nonlinear filtering algorithms is compared and analyzed by the following steps:

(1)

Use UKF, PF and RACDPF for filtering and then calculate the velocity error and position error of each filtering algorithm. The results are shown in Figure 8 and Figure 9. In order to facilitate statistical results, the position error can be defined as:

$$\delta p=\sqrt{{(\delta L)}^2+{(\delta \lambda )}^2+{(\delta h)}^2}$$

(46)

the velocity error can be defined as:

$$\delta V=\sqrt{{(\delta V_E)}^2+{(\delta V_N)}^2+{(\delta V_U)}^2}$$

(47)

Figure 8. Velocity errors of the three filtering algorithms ($M=200$).

Figure 9. Position errors of the three filtering algorithms ($M=200$).

Then, the error statistics of the three filtering algorithms can be obtained, as shown in Table 4.

Table 4. The error statistics of the three filtering algorithms ($M=200$).

Filtering Algorithm	Velocity Error/m/s	Position Error/m
UKF	23.8122	56.7445
PF	11.1434	33.4074
RACDPF	5.1529	16.1745

(2)

Repeat Step (1) in the case of particle number $M=50$ and $M=500$ respectively, and the results are shown in Table 5 and Table 6.

Table 5. The error statistics of the three filtering algorithms ($M=50$).

Filtering Algorithm	Velocity Error/m/s	Position Error /m
UKF	23.8122	56.7445
PF	15.2567	39.9238
RACDPF	6.1740	18.2510

Table 6. The error statistics of the three filtering algorithms ($M=500$).

Filtering Algorithm	Velocity Error/m/s	Position Error/m
UKF	23.8122	56.7445
PF	10.9682	28.4450
RACDPF	4.5424	13.5341

It can be seen that the accuracy of UKF is the worst. PF inevitably displays a particle degradation phenomenon after several iterative computations, therefore the filtering accuracy is limited. RACDPF uses the robust adaptive factor to control the kinematic model information and observation model information, for suppressing the influence of abnormal interference, thus the filtering accuracy is better than that of UKF and PF. From Table 4, Table 5 and Table 6, it can be seen that the number of particles can obviously affect the accuracy of PF and RACDPF because PF uses samples to approximate the a priori information and a posteriori information, and the more the number of samples, the closer to the true distribution.

B. Real-Time Comparison of Filtering Algorithms

The factors that affect the real-time performance of the filtering algorithm include algorithm complexity, filtering condition and hardware processing ability. The algorithm complexity is the main parameter to describe the degree of difficulty in algorithm implementation. In this section, The real-time performance of UKF, PF and RACDPF is researched under the same hardware platform, software platform and initial conditions.

The equivalent computational complexity and running time of each filtering algorithm are are shown in Table 7, where $M$ is the number of particles, $n$ is the state dimension, $O(·)$ represents the computational complexity of each recurrence calculation for the algorithms.

Table 7. Real-time performance of the nonlinear filtering algorithms.

Filtering Algorithm	Computational Complexity	Running Time/s
		$\mathit{M}=50$	$\mathit{M}=200$	$\mathit{M}=500$
UKF	$O(n^4)$	0.6728	0.6728	0.6728
PF	$O(M{n}^3)$	1.0795	2.4141	4.8626
RACDPF	$O(M{n}^3)$	1.1589	2.6979	5.0032

It can be seen that PF and its improved algorithms need to sample a large number of particles, allocate weights and resample, which is complex and computationally burdensome, therefore the algorithm running time is obviously higher than that of UKF. Therefore, the number of particles should be determined according to the specific application requirements.

C. Robustness Comparison of Filtering Algorithms

For a given filtering algorithm, its robustness is manifested in the performance that when the system parameters or the external environment change, the filtering algorithm can still maintain certain filtering accuracy.

In order to verify the robustness of the filtering algorithms (UKF, PF and RACDPF), the errors of variance 1 m²/s² and 4 m² are added to the velocity and position observations of the experimental data, respectively. Then, the position RMSEs of the two sets of experimental data are calculated by each filtering algorithm, respectively. The number of particles M = 200, and the other conditions are consistent with the previous. The results are shown in Table 8.

Table 8. Robustness performance of the nonlinear filtering algorithms.

Filtering Algorithm	Position RMSE/m
	Errors Are Added	No Error Is Added	Difference
UKF	61.0342	56.7445	8.2897
PF	38.8110	33.4074	5.4036
RACDPF	17.3852	16.1745	1.2107

The effect of abnormal disturbances on the UKF and PF is more significant than RACDPF, this is because UKF and PF cannot deal with abnormal interferences. However, RACDPF can control the kinematic model information and the observation model information by selecting the appropriate robust adaptive factor to suppress the influence of abnormal interferences. Therefore, the RACDPF is the least affected.

The effect of abnormal disturbances on the UKF and PF is more significant than RACDPF, this is because UKF and PF cannot deal with abnormal interferences. However, RACDPF can control the kinematic model information and the observation model information by selecting the appropriate robust adaptive factor to suppress the influence of abnormal interferences. Therefore, the RACDPF is the least affected.

6. Conclusions

In order to satisfy the requirements of the high precision and reliability of the autonomous navigation system, a new SINS/SRS/GNS autonomous integrated navigation method is proposed in this paper. The scheme of the SINS/SRS/GNS autonomous integrated navigation system is designed, and the corresponding filtering algorithm, a robust adaptive central difference particle filtering (RACDPF) algorithm, is also proposed. This algorithm can control the state model information and observation model information, suppress the influence of abnormal interference, and adaptively adjust the weight of particles after importance sampling, thus improving filtering accuracy. The simulation results show that the proposed method possesses high accuracy, strong robustness and good reliability, and can satisfy the performance requirements of autonomous navigation in a certain degree. Because of the abundant natural celestial bodies and their spectral redshift information in space, the SINS/SRS/GNS autonomous integrated navigation method can be applied to spacecraft, space station, and satellite orbit determination, thus providing a new solution for the autonomous navigation technology.