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

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.


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 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: where z represents Doppler redshift, λ 0 is the original wavelength of the spectral line, λ is the observed wavelength, f 0 is the original frequency of the spectral line, f is the observed frequency.
Accordingly, the redshift equation is: where v is the velocity of the carrier relative to the light source, θ 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 θ 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: 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, v p is the velocity vector of the carrier in the inertial coordinate system, 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: From the geometric relationship of the celestial bodies, the relationship between v P and v r1 , v r2 , v r3 satisfies the following relationship: where v 1 , v 2 and v 3 are the velocity vectors of the celestial bodies in the inertial system determined by the ephemeris, respectively, u 1 , u 2 and 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: After the initial value is given, the velocity vector v P of the carrier in the inertial coordinate system can be obtained by solving (6), and then the position vector r P can be obtained by integration.

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

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: where δq = [δq 0 , δq 1 , δq 2 , δq 3 ] T is the attitude error quaternion of SINS, δV = [δV E , δV N , δV U ] T is the velocity error of SINS, δP = [δL, δλ, δh] T is the latitude error, longitude error and height error of SINS, ε = [ε x , ε y , ε x ] T represents the random drift of the gyroscope, ∇ = [∇ x , ∇ y , ∇ z ] 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: .
where f (x, t) is nonline state function of the system, G(t) is the coefficient matrix of system noise. w(t) = [w gx , w gy , w gz , w ax , w ay , w az ] 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: where C c n and C c b are the attitude transition matrixes of the navigation coordinate system n and the carrier coordinate system b to the computing coordinate system c, respectively. ω n in represents the projection of the angular velocity of the carrier in the navigation coordinate system, ω n ie and ω n en are the rotation angle velocity and the position angular velocity of the earth, respectively. V n is the velocity of the carrier, ∇ b is the error of the accelerometer, f b is the accelerometer's specific force. M and N are the coefficient matrixes of the velocity error and position error respectively, δ·represents the corresponding error of the parameter. B = 1 2 M(Q n p ), M(Q n p ) is the matrix representation of quaternion multiplication, and:

(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: where C n b is the attitude transition matrixes of the body coordinate system to the navigation coordinate system, V n is velocity vector of the carrier in navigation coordinate system, 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: where 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, V v is velocity observation noise. H v can be expressed as: Accordingly, the observation equation of SINS/SRS subsystem can be expressed as: where (

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: where L I , λ I and h I are latitude, longitude, and height of the SINS output, respectively. L G , λ G and h G are the latitude, longitude, and height of the GNS, respectively. δ·represents the corresponding parameter error. According to (18), the observation equation of the SINS/GNS subsystem can be obtained as: where

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 estimationX i and local optimal error covariance matrix Σ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 estimationX g and the global optimal error covariance matrix ΣX ,g of the system state vector are obtained. Finally, the SINS error is corrected in real time by usingX 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:

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.

Algorithm Steps
Consider the nonlinear system model: The main steps of the robust adaptive central difference particle filtering algorithm are as follows.
Time update: Observation update: 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 ith predicted residual vector V i k at time k can be expressed as: Correspondingly, the error discriminant statistic of the kinematic model is written as: Then, the adaptive factor that is based on the error discriminant statistic can be obtained as [25]: where α i k is the ith 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: It can be seen from (38) that the adaptive factor α 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 α 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(x i k , p i x k ) be the importance density function of the particle sampling, the new sample x i k ∼ N(x i k , P i x k ) 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: and normalize the weight to w i (4) Calculate the threshold estimate:N Compare the result with the established threshold to determine the degree of the particle degradation.
The smallerN e f f 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: 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.

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(x i k , P i k ), the weight of each particle is calculated by: Then the maximum weight w i * k−max and the minimum weight w i * k−min is recorded, and the observation innovation y i k−max , y i k−min , and Euclidean distance L max and L i can be calculated, which are expressed as: and x i k−min represent the particles that correspond to the maximum weight and the minimum weight.
The weight of particles can be modified as: where β is the adaptive coefficient determined by the statistical characteristics of observation noise, and: where α is the dilution of precision that reflects statistical characteristics of observation noise. By changing the size of α , we can adaptively adjust the distribution of weights and increase the weights of useful particles. When α is large, the observation accuracy is low, otherwise, the observation accuracy is relatively high. ε is the threshold that determined by experience, K is the proportional constant, and K α > 0. When the observation noise is small, let β = 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 β = K α , that is, artificially make the likelihood distribution wider.

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. 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.     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 Figures 6 and 7. Table 3 shows the error statistics of the autonomous integrated navigation system.   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 Figures 8 and 9. In order to facilitate statistical results, the position error can be defined as: the velocity error can be defined as: Then, the error statistics of the three filtering algorithms can be obtained, as shown in Table 4. (2) Repeat Step (1) in the case of particle number M = 50 and M = 500 respectively, and the results are shown in Tables 5 and 6.   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 Tables 4-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. 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 2 /s 2 and 4 m 2 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. 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.

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.