Next Article in Journal
Three-Dimensional Registration for Handheld Profiling Systems Based on Multiple Shot Structured Light
Next Article in Special Issue
PL-VIO: Tightly-Coupled Monocular Visual–Inertial Odometry Using Point and Line Features
Previous Article in Journal
Multi-Objectives Optimization of Ventilation Controllers for Passive Cooling in Residential Buildings
Previous Article in Special Issue
Direct Position Determination of Unknown Signals in the Presence of Multipath Propagation
 
 
Font Type:
Arial Georgia Verdana
Font Size:
Aa Aa Aa
Line Spacing:
Column Width:
Background:
Article

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

1
School of Automatics, Northwestern Polytechnical University, 710072 Xi’an, China
2
School of Geological Engineering and Surveying and Mapping, Chang’an University, 710064 Xi’an, China
*
Author to whom correspondence should be addressed.
Sensors 2018, 18(4), 1145; https://doi.org/10.3390/s18041145
Submission received: 2 March 2018 / Revised: 1 April 2018 / Accepted: 4 April 2018 / Published: 9 April 2018
(This article belongs to the Special Issue Sensor Fusion and Novel Technologies in Positioning and Navigation)

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 = λ λ 0 λ 0 = f 0 f f
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:
1 + z = f 0 f = 1 + v cos θ / c 1 v 2 c 2
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:
v r 1 = ( 1 + z 1 ) c 2 | v p v 1 | 2 c
where v r 1 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:
{ v r 1 = ( 1 + z 1 ) c 2 | v p v 1 | 2 c v r 2 = ( 1 + z 2 ) c 2 | v p v 2 | 2 c v r 3 = ( 1 + z 3 ) c 2 | v p v 3 | 2 c
From the geometric relationship of the celestial bodies, the relationship between v P and v r 1 , v r 2 , v r 3 satisfies the following relationship:
{ v r 1 = ( v P v 1 ) u 1 v r 2 = ( v P v 2 ) u 2 v r 3 = ( v P v 3 ) u 3
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:
{ v P = f ( v 1 , v 2 , v 3 , u 1 , u 2 , u 3 , v r 1 , v r 2 , v r 3 ) r P = v P d t
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.

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.
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:
x ( t ) = [ ( δ q ) T , ( δ V ) T , ( δ P ) T , ε T , T ] T
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:
x ˙ = f ( x , t ) + G ( t ) w ( t )
where f ( x , t ) is nonline state function of the system, G ( t ) is the coefficient matrix of system noise. w ( t ) = [ w g x , w g y , w g z , w a x , w a y , w a z ] T represents system noise, [ w g x , w g y , w g z ] is the white Gaussian noise of the gyro measurement, [ w a x , w a y , w a z ] is the white Gaussian noise of the accelerometer measurement:
f ( x , t ) = [ B ( I C n c ) ω i n n B C b c ε b ( I C c n ) C b c f b ( 2 ω i e n + ω e n n ) × δ V n ( 2 δ ω i e n + δ ω e n n ) × V n + C b c b M δ V + N δ L 0 3 × 1 0 3 × 1 ]
G ( t ) = [ C b c 0 3 × 3 0 3 × 3 C b c 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 ]
where C n c and 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. ω i n n represents the projection of the angular velocity of the carrier in the navigation coordinate system, ω i e n and ω e n n 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 p n ) , M ( Q p n ) is the matrix representation of quaternion multiplication, and:
M ( Q p n ) = [ 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 ]

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:
V n = C b n V b
V n = [ v S E v S E v S E ] , V b = [ v X v Y v Z ]
where C b n 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 S E , v S N and v S U 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:
Z v = [ Z S 1 Z S 2 Z S 3 ] = [ v E v S E v N v S N v U v S U ] = H v X ( t ) + V v ( t )
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:
H v = [ 0 3 × 3 d i a g ( 1 1 1 ) 0 3 × 9 ] 3 × 15
Accordingly, the observation equation of SINS/SRS subsystem can be expressed as:
Z 1 ( t ) = [ Z v ( t ) ] = [ H v ] X ( t ) + [ V v ( t ) ] = H 1 ( t ) X ( t ) + v 1 ( t )
where H 1 ( t ) is observation matrix, V 1 ( t ) 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:
Z 2 ( t ) = [ L I λ I h I L G λ G h G ] = [ ( L + δ L I ) ( L + δ L G ) ( λ + δ λ I ) ( λ + δ λ G ) ( h + δ h I ) ( h + δ h G ) ] = [ δ L I δ λ I δ h I ] [ δ L G δ λ G δ h G ]
where L I , λ I and h I are latitude, longitude, and height of the SINS output, respectively. LG, λ 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:
Z 2 ( t ) = H 2 ( t ) X ( t ) + V 2 ( t )
where H 2 ( t ) is the observation matrix, V 2 ( t ) = [ δ L G , δ λ G , δ h G ] 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 X ^ 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 estimation X ^ 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 using 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:
{ Σ X ^ , g = ( Σ X ^ , 1 1 + Σ X ^ , 2 1 ) 1 X ^ g = Σ X ^ , g ( Σ X ^ , 1 1 X ^ 1 + Σ X ^ , 2 1 X ^ 2 )

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:
x k = f ( x k 1 , v k 1 ) y k = h ( x k , n k )
The main steps of the robust adaptive central difference particle filtering algorithm are as follows.
(1)
Initialization. At the time k = 0 :
x ^ 0 = E [ x 0 ] P x 0 = E [ ( x 0 x ^ 0 ) ( x 0 x ^ 0 ) ] T
(2)
When k = 1 , , , and j = 0 , , 2 L :
W 0 m = h 2 L h 2 , W j m = 1 2 h 2 W j c 1 = 1 4 h 2 , W j c 2 = h 2 1 4 h 4
where L is state dimension, h represents the central differential interval of the scalar, for Gauss prior random variables, the optimal value is h = 3 .
Time update:
χ k 1 i = [ x ^ k 1 i , x ^ k 1 i + h P k 1 i , x ^ k 1 i h P k 1 i ]
χ k | k 1 i = f ( χ k 1 i , u k 1 i )
x ^ k i = j = 0 2 L W j m χ j , k | k 1 i
P x k i = j = 0 L [ W j c 1 ( χ j , k | k 1 i χ L + j , k | k 1 i ) 2 + W j c 2 ( χ j , k | k 1 i + χ L + j , k | k 1 i 2 χ 0 , k | k 1 i ) 2 ]
χ k | k 1 i * = [ x ^ k | k 1 i , x ^ k | k 1 i + h P x k | k - 1 i , x ^ k | k 1 i h P x k | k - 1 i ]
Observation update:
Y k | k - 1 i = h ( χ k | k 1 i * )
y ^ k i = j = 0 2 L W j m Y j , k | k 1 i
P y ¯ k i = j = 0 L [ W j c 1 ( Y j , k | k 1 i Y L + j , k | k 1 i ) 2 + W j c 2 ( Y j , k | k 1 i + Y L + j , k | k 1 i 2 Y 0 , k | k 1 i ) 2 ]
P x k y k i = W 1 c 1 P x k i [ Y 1 : L , k | k 1 i Y L + 1 : 2 L , k | k 1 i ] T
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 V ¯ k i at time k can be expressed as:
V ¯ k i = Y k i y ^ k i
Correspondingly, the error discriminant statistic of the kinematic model is written as:
Δ V ¯ k i = ( ( V ¯ k i ) T V ¯ k i t r ( P y k y k i ) ) 1 2
Then, the adaptive factor that is based on the error discriminant statistic can be obtained as [25]:
α k i = { 1      | Δ V ¯ k i | c    c | Δ V ¯ k i |    | Δ V ¯ k i | > c
where α 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:
K k i = P x k y k i ( P y ¯ k i ) 1
x ^ k i = x ^ k i + K k i ( y k i y ^ k i )
P x k i = P x k i α k i K k i P y ¯ k i K k i T
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 ^ k i , p x k i ) be the importance density function of the particle sampling, the new sample x k i N ( x ^ k i , 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 p ( y k | x k i ) p ( x k i | x k 1 i ) q ( x k i | x k 1 i , y k )
and normalize the weight to w ˜ k i = w k i / i = 1 n w k i .
(4)
Calculate the threshold estimate:
N ^ e f f = 1 / i = 1 N ( w ˜ k i ) 2
Compare the result with the established threshold to determine the degree of the particle degradation. The smaller N ^ 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:
x ^ k = i = 1 N w ˜ k i x i k
P ^ k = i = 1 N w ˜ k i ( x k i x ^ k ) ( x k i x ^ k ) T
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 ( x ¯ k i , P k i ) , the weight of each particle is calculated by:
w k i = w k 1 i p ( y k | x k i ) p ( x k i | x k 1 i ) q ( x k i | x k 1 i , y k )
Then the maximum weight w k max i * and the minimum weight w k min i * is recorded, and the observation innovation y k max i , y k min i , and Euclidean distance L max and L i can be calculated, which are expressed as:
L max = ( y k min i y k max i ) T ( y k min i y k max i )
L i = ( y k i y k max i ) T ( y k i y k max i )
where y k max i = y k h ( x k max i ) , y k min i = y k h ( x k min i ) , x k max i and 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 * + ( w k max i * N ) sin ( L i L max π 2 ) β
where β is the adaptive coefficient determined by the statistical characteristics of observation noise, and:
β = { K α α ε 0 α > ε
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.

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.
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 1 0 . 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.
(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.
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.
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:
δ p = ( δ L ) 2 + ( δ λ ) 2 + ( δ h ) 2
the velocity error can be defined as:
δ V = ( δ V E ) 2 + ( δ V N ) 2 + ( δ V U ) 2
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 Table 5 and Table 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 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.
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 m2/s2 and 4 m2 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.

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.

Acknowledgments

The work of this paper is supported by the National Natural Science Foundation of China (Project No. 41704016), the China Postdoctoral Science Foundation (Project No. 2017M613029), the Postdoctoral Research Project Foundation of Shaanxi Province (Project No. 2017BSHEDZZ84), and Aerospace Science and Technology Fundation (Project No. 2017-HT-XG).

Author Contributions

Wenhui Wei completed the theoretical research and design of the manuscript, including the principle and scheme of the SINS/SRS/GNS autonomous integrated navigation system and the design of the filtering algorithm for the navigation system, and completed the writing of the manuscript. Zhaohui Gao completed the simulation and validation of the models and algorithms in the manuscript, including data collection and charts drawing. Shesheng Gao guided and improved the research content, technical principle and experimental scheme of the manuscript, and analyzed and summarized the experimental results. Ke Jia completed the literature retrieval, and improved the language and grammar expression of the manuscript.

Conflicts of Interest

The authors declare no conflict of interest.

References

  1. Zeng, Q.; Chen, W.; Liu, J.; Wang, H. An Improved Multi-Sensor Fusion Navigation Algorithm Based on the Factor Graph. Sensors 2017, 17, 641. [Google Scholar] [CrossRef] [PubMed]
  2. Hu, G.; Gao, S.; Zhong, Y.; Gao, B.; Subic, A. Matrix weighted multisensor data fusion for INS/GNSS/CNS integration. Proc. Inst. Mech. Eng. Part G J. Aerosp. Eng. 2016, 230, 1011–1026. [Google Scholar] [CrossRef]
  3. Seo, J.; Lee, J.G. Application of nonlinear smoothing to integrated GPS/INS navigation system. J. Glob. Position. Syst. 2005, 4, 88–94. [Google Scholar] [CrossRef]
  4. Jiang, C.; Zhang, S.; Zhang, Q. Adaptive Estimation of Multiple Fading Factors for GPS/INS Integrated Navigation Systems. Sensors 2017, 17, 1254. [Google Scholar] [CrossRef] [PubMed]
  5. Gao, S.; Zhong, Y.; Li, W. Robust adaptive filtering method for SINS/SAR integrated navigation system. Aerosp. Sci. Technol. 2011, 15, 425–430. [Google Scholar] [CrossRef]
  6. Zhong, Y.; Gao, S.; Li, W. A Quaternion-Based Method for SINS/SAR Integrated Navigation System. IEEE Trans. Aerosp. Electron. Syst. 2012, 48, 514–524. [Google Scholar] [CrossRef]
  7. Gao, Z.; Gao, B.; Mu, D.; Gao, S. Robust adaptive SDRE filter and its application to SINS/SAR integration. In Proceedings of the 2016 3rd International Conference on Information Science and Control Engineering, Beijing, China, 8–10 July 2016; pp. 1048–1412. [Google Scholar]
  8. Tawk, Y.; Tomé, P.; Botteron, C.; Stebler, Y.; Farine, P.A. Implementation and Performance of a GPS/INS Tightly Coupled Assisted PLL Architecture Using MEMS Inertial Sensors. Sensors 2014, 14, 3768–3796. [Google Scholar] [CrossRef] [PubMed]
  9. Ding, W.; Wang, J.; Rizos, C. Improving adaptive Kalman estimation in GPS/INS integration. J. Navig. 2007, 60, 517–529. [Google Scholar] [CrossRef]
  10. Hu, G.; Gao, S.; Zhong, Y. A derivative UKF for tightly coupled INS/GPS integrated navigation. ISA Trans. 2015, 56, 135–144. [Google Scholar] [CrossRef] [PubMed]
  11. Wang, R.; Xiong, Z.; Liu, J.; Shi, L. A new tightly-coupled INS/CNS integrated navigation algorithm with weighted multi-stars observations. Proc. Inst. Mech. Eng. Part G J. Aerosp. Eng. 2015, 230, 698–712. [Google Scholar] [CrossRef]
  12. Ning, X.; Gui, M.; Xu, Y.; Bai, X.; Fang, J. INS/VNS/CNS integrated navigation method for planetary rovers. Aerosp. Sci. Technol. 2016, 48, 102–114. [Google Scholar] [CrossRef]
  13. Glazebrook, K.; Offer, A.R.; Deeley, K. Automatic redshift determination by use of prinicipal component analysis. I. Fundamentals. Astrophys. J. 1998, 492, 98–109. [Google Scholar] [CrossRef]
  14. Julier, S.J.; Uhlmann, J.K.; Durrant-Whyte, H.F. A new method for the nonlinear transformation of means and covariances in filters and estimators. IEEE Trans. Autom. Control 2000, 45, 477–482. [Google Scholar] [CrossRef]
  15. Lefebvre, T.; Bruyninckx, H.; Schutter, J.D. Kalman filters for nonlinear systems: A comparison of performance. Int. J. Control 2004, 77, 639–653. [Google Scholar] [CrossRef]
  16. Simon, D. Optimal state estimation. In Infinity, and Nonlinear Approaches; Kalman, H., Ed.; John Wiley & Sons: Hoboken, NJ, USA, 2006. [Google Scholar]
  17. Karamat, T.B.; Atia, M.M.; Noureldin, A. An Enhanced Error Model for EKF-Based Tightly-Coupled Integration of GPS and Land Vehicle’s Motion Sensors. Sensors 2015, 15, 24269–24296. [Google Scholar] [CrossRef] [PubMed]
  18. Julier, S.J.; Uhlmann, J.K. Unscented filtering and nonlinear estimation. Proc. IEEE 2004, 92, 401–422. [Google Scholar] [CrossRef]
  19. Wan, E.A.; van der Merwe, R. The unscented Kalman filter for nonlinear estimation. In Proceedings of the IEEE Symposium on Adaptive Systems for Signal Processing, Communications, and Control, Lake Louise, AB, Canada, 4 October 2000; pp. 153–158. [Google Scholar]
  20. Rawlings, J.B.; Bakshi, B.R. Particle filtering and moving horizon estimation. Comput. Chem. Eng. 2006, 30, 1529–1541. [Google Scholar] [CrossRef]
  21. Zhang, B.; Chen, M.; Zhou, D.; Li, Z. Particle-filter-based estimation and prediction of chaotic states. Chaos Solitons Fractals 2007, 32, 149–1498. [Google Scholar] [CrossRef]
  22. Oppenheim, G.; Philippe, A.; de Rigal, J. The particle filters and their applications. Chemom. Intell. Lab. Syst. 2008, 91, 87–93. [Google Scholar] [CrossRef]
  23. Yang, N.; Tian, W.F.; Jin, Z.H.; Zhang, C.B. Particle filter for sensor fusion in a land vehicle navigation system. Meas. Sci. Technol. 2005, 16, 677–681. [Google Scholar] [CrossRef]
  24. Watzenig, D.; Brandner, M.; Steiner, G. A particle filter approach for tomographic imaging based on different state-space representations. Meas. Sci. Technol. 2007, 18, 30–40. [Google Scholar] [CrossRef]
  25. Yang, Y.; Cui, X. Adaptively robust filter with multi adaptive factors. Surv. Rev. 2008, 40, 260–270. [Google Scholar] [CrossRef]
  26. Tseng, C.H.; Lin, S.F.; Jwo, D.J. Fuzzy Adaptive Cubature Kalman Filter for Integrated Navigation Systems. Sensors 2016, 16, 1167. [Google Scholar] [CrossRef] [PubMed]
  27. Gao, Z.; Mu, D.; Gao, S.; Zhong, Y.; Gu, C. Robust adaptive filter allowing systematic model errors for transfer alignment. Aerosp. Sci. Technol. 2016, 59, 32–40. [Google Scholar] [CrossRef]
  28. Gao, Z.; Mu, D.; Wei, W.; Zhong, Y.; Gu, C. Adaptive unscented Kalman filter based on maximum posterior estimation and random weighting. Aerosp. Sci. Technol. 2017, 71, 12–24. [Google Scholar] [CrossRef]
  29. Gao, S.; Zhong, Y.; Zhang, X.; Shirinzadeh, B. Multi-Sensor optimal data fusion for INS/GPS/SAR integrated navigation system. Aerosp. Sci. Technol. 2009, 13, 232–237. [Google Scholar] [CrossRef]
  30. Gao, S.; Zhong, Y.; Li, W. Random weighting method for multisensor data fusion. IEEE Sens. J. 2011, 11, 1955–1961. [Google Scholar] [CrossRef]
  31. Fu, K.; Zhao, G.; Li, X.; Tang, Z.; He, W. Iterative spherical simplex unscented particle filter for CNS/Redshift integrated navigation system. Sci. China Inf. Sci. 2017, 60, 042201. [Google Scholar] [CrossRef]
Figure 1. The principle of the SINS/SRS/GNS autonomous integrated navigation system.
Figure 1. The principle of the SINS/SRS/GNS autonomous integrated navigation system.
Sensors 18 01145 g001
Figure 2. The information fusion principle of SINS/SRS/GNS autonomous integrated navigation system.
Figure 2. The information fusion principle of SINS/SRS/GNS autonomous integrated navigation system.
Sensors 18 01145 g002
Figure 3. Flight trajectory.
Figure 3. Flight trajectory.
Sensors 18 01145 g003
Figure 4. Velocity errors of the subsystems.
Figure 4. Velocity errors of the subsystems.
Sensors 18 01145 g004
Figure 5. Position errors of the subsystems.
Figure 5. Position errors of the subsystems.
Sensors 18 01145 g005
Figure 6. Velocity error of the autonomous integrated navigation system.
Figure 6. Velocity error of the autonomous integrated navigation system.
Sensors 18 01145 g006
Figure 7. Position error of the autonomous integrated navigation system.
Figure 7. Position error of the autonomous integrated navigation system.
Sensors 18 01145 g007
Figure 8. Velocity errors of the three filtering algorithms ( M = 200 ).
Figure 8. Velocity errors of the three filtering algorithms ( M = 200 ).
Sensors 18 01145 g008
Figure 9. Position errors of the three filtering algorithms ( M = 200 ).
Figure 9. Position errors of the three filtering algorithms ( M = 200 ).
Sensors 18 01145 g009
Table 1. Parameters of the sensors used in the simulation.
Table 1. Parameters of the sensors used in the simulation.
Parameter TypeValue
Gyro constant drift 0.02 ° / h
Gyro random drift 0.005 ° / h
Constant drift of accelerometer0.05 mg
Random drift of accelerometer 0.005   mg / h
Accuracy of geomagnetic matching20 m
Accuracy of velocity measurement with SRS [31]5.5 m/s
Table 2. The error statistics of the subsystems.
Table 2. The error statistics of the subsystems.
ErrorsThe Subsystems
SINS/GNSSINS/SRS
Velocity RMSE (m/s)East9.44915.3219
North8.75795.0692
Up8.66885.2851
Position RMSE (m)Longitude16.827465.6695
Latitude17.320568.7935
Height16.777570.1250
Table 3. The error statistics of the autonomous integrated navigation system.
Table 3. The error statistics of the autonomous integrated navigation system.
ErrorValue
SINS/SRS/GNS autonomous integrated navigation systemPosition RMSE(m)Longitude17.0375
Latitude16.3565
Height16.7865
Velocity RMSE(m/s)East5.3698
North5.1550
Up5.2546
Table 4. The error statistics of the three filtering algorithms ( M = 200 ).
Table 4. The error statistics of the three filtering algorithms ( M = 200 ).
Filtering AlgorithmVelocity Error/m/sPosition Error/m
UKF23.812256.7445
PF11.143433.4074
RACDPF5.152916.1745
Table 5. The error statistics of the three filtering algorithms ( M = 50 ).
Table 5. The error statistics of the three filtering algorithms ( M = 50 ).
Filtering AlgorithmVelocity Error/m/sPosition Error /m
UKF23.812256.7445
PF15.256739.9238
RACDPF6.174018.2510
Table 6. The error statistics of the three filtering algorithms ( M = 500 ).
Table 6. The error statistics of the three filtering algorithms ( M = 500 ).
Filtering AlgorithmVelocity Error/m/sPosition Error/m
UKF23.812256.7445
PF10.968228.4450
RACDPF4.542413.5341
Table 7. Real-time performance of the nonlinear filtering algorithms.
Table 7. Real-time performance of the nonlinear filtering algorithms.
Filtering AlgorithmComputational ComplexityRunning Time/s
M = 50 M = 200 M = 500
UKF O ( n 4 ) 0.67280.67280.6728
PF O ( M n 3 ) 1.07952.41414.8626
RACDPF O ( M n 3 ) 1.15892.69795.0032
Table 8. Robustness performance of the nonlinear filtering algorithms.
Table 8. Robustness performance of the nonlinear filtering algorithms.
Filtering AlgorithmPosition RMSE/m
Errors Are AddedNo Error Is AddedDifference
UKF61.034256.74458.2897
PF38.811033.40745.4036
RACDPF17.385216.17451.2107

Share and Cite

MDPI and ACS Style

Wei, W.; Gao, Z.; Gao, S.; Jia, K. A SINS/SRS/GNS Autonomous Integrated Navigation System Based on Spectral Redshift Velocity Measurements. Sensors 2018, 18, 1145. https://doi.org/10.3390/s18041145

AMA Style

Wei W, Gao Z, Gao S, Jia K. A SINS/SRS/GNS Autonomous Integrated Navigation System Based on Spectral Redshift Velocity Measurements. Sensors. 2018; 18(4):1145. https://doi.org/10.3390/s18041145

Chicago/Turabian Style

Wei, Wenhui, Zhaohui Gao, Shesheng Gao, and Ke Jia. 2018. "A SINS/SRS/GNS Autonomous Integrated Navigation System Based on Spectral Redshift Velocity Measurements" Sensors 18, no. 4: 1145. https://doi.org/10.3390/s18041145

Note that from the first issue of 2016, this journal uses article numbers instead of page numbers. See further details here.

Article Metrics

Back to TopTop