A Robust INS/SRS/CNS Integrated Navigation System with the Chi-Square Test-Based Robust Kalman Filter

In order to achieve a highly autonomous and reliable navigation system for aerial vehicles that involves the spectral redshift navigation system (SRS), the inertial navigation (INS)/spectral redshift navigation (SRS)/celestial navigation (CNS) integrated system is designed and the spectral-redshift-based velocity measurement equation in the INS/SRS/CNS system is derived. Furthermore, a new chi-square test-based robust Kalman filter (CSTRKF) is also proposed in order to improve the robustness of the INS/SRS/CNS navigation system. In the CSTRKF, the chi-square test (CST) not only detects measurements with outliers and in non-Gaussian distributions, but also estimates the statistical characteristics of measurement noise. Finally, the results of our simulations indicate that the INS/SRS/CNS integrated navigation system with the CSTRKF possesses strong robustness and high reliability.


Introduction
For hypersonic cruise vehicles (HCVs), a highly autonomous and reliable navigation system is needed [1,2]. The inertial navigation system (INS) is one of the most widely used navigation systems [3][4][5]. The INS is a self-contained system and can provide highly accurate positions, velocities, and attitudes for short-term navigation. However, the gyro drift and accelerometer bias lead to unbounded error growth in the INS [5]. In order to overcome this shortcoming, the inertial navigation system/global navigation satellite system(INS/GNSS) integrated navigation system has been investigated [6][7][8]. However, the GNSS relies on signals from artificial satellites, and therefore lacks autonomy and is susceptible to artificial interference [9]. The celestial navigation system (CNS) is an autonomous navigation system that has lower positioning accuracy than the GNSS but has the advantage of not accumulating navigation error and a strong ability to resist electromagnetic interference [10][11][12]. Thus, researchers have also investigated the INS/CNS integrated navigation system, which incorporates the measurement information from the CNS to correct the deviations in the INS. However, the CNS also has its defects, including the difficulty of star selection and outdated data.
The spectral redshift navigation system (SRS) is a novel application in the navigation field. In the SRS, velocity can be obtained from the spectral redshift information of celestial spectra. Compared with other navigation systems, the SRS has the advantage of simple navigation principles, easy star selection, and no time delay [13]. In order to improve the autonomy of the integrated navigation of spacecraft, the SRS is widely used as an auxiliary navigation system to assist in correcting the velocity error of the main navigation system. For example, the authors of [13] used both the CNS and the SRS to correct the divergence caused by model errors of the orbital dynamics equations used in deep space exploration. The authors of [14] investigated the INS/SRS/GNS integrated navigation system, which used geomagnetic navigation and spectral redshift navigation to correct for the error of the INS. Thus, for HCVs, the SRS can also correct for the bias of the INS, avoiding parameter divergence while maintaining system autonomy.
To keep the reliability and improve the accuracy of the output of an integrated navigation system, information fusion is also important. The conventional Kalman filter (KF) has been a primary algorithm for linear navigation system integration [15,16]. However, in order to achieve information fusion when using the traditional KF, the accuracy system model and exact noise statistics are required [17]. In reality, the system always involves uncertainties caused by outliers in measurements under highly dynamic conditions. Thus, the authors of [18] proposed a sigma-point-based receding horizon Kalman filter (SPRHKF) to improve robustness. However, since this filter is based on a finite impulse response structure, the filtering convergence is poor [19]. The Sage-Husa noise statistic estimator has also been used to develop an adaptive KF [20,21]. However, the forgetting factors used in these filters are determined empirically. In [22,23], an H-infinity strategy was used to handle the uncertainties in observation noise. However, this method may only work under the condition of randomly occurring outliers. Additionally, the Huber-based KF has been applied to resist the influences of measurement outliers through the statistical linear regression of nonlinear system functions [24]. However, this method achieves its robustness by sacrificing accuracy. The authors of [25,26] also estimated scaling factors for the covariance of measurement noise to further adjust the Kalman gain to maintain robustness. However, this method may lead to a suboptimal filtering solution because the scaling factors are determined empirically. Furthermore, the hypothesis test method is great at detecting the changes in observations with outliers; examples of such tests include the chi-square test (CST) [27] and the generalized likelihood ratio test (GLRT) [28,29]. However, many studies simply investigated the hypothesis test method as a fault detection and isolation method to remove all observations with outliers but did not utilize the useful information in those observations, leading to the loss of navigation accuracy [30,31].
Thus, based on the above research, this paper deduces the linear relationship equation based on velocity in the east-north-up frame and the redshift of the observed vehicle, and then establishes the INS/SRS/CNS integrated navigation model. Meanwhile, to improve robustness, the chi-square test-based robust Kalman filter (CSTRKF) is proposed. In the CSTRKF, the CST is used to detect the change in noise based on the innovation sequence. Furthermore, based on the judgment index of the CST, a robust noise estimator is also proposed. Finally, the results of our simulations indicate the CSTRKF has great robustness performance and the enhanced INS/SRS/CNS integrated navigation system with the CSTRKF has great reliability.

Relationship between Velocity and Redshift in the East-North-Up Geographical Frame
According to the redshift principle of a spectrum and the Doppler frequency shift formula, we can obtain the following equation [13]: where z denotes the spectral redshift value of the celestial body calculated in the target vehicle; v p denotes the velocity vector of the vehicle in the inertial frame (I-frame); v c denotes the velocity vector of the celestial body in the I-frame, which can be obtained by querying the celestial ephemeris; c is the velocity of light; and v r denotes the radial velocity along the direction from the target vehicle to the observed celestial body.
Assuming that the aircraft can obtain the spectral redshift values of three noncollinear observed celestial bodies, the three-vector operation relationship can be written as where (v c1 , v c2 , v c3 ) represents the velocity vectors of three reference celestial bodies in the I-frame, which can be obtained by querying the ephemeris of related celestial bodies; and (u 1 , u 2 , u 3 ) represents the unit vector of the position vector of each celestial body pointing to the aircraft in the inertial coordinate system, which can be measured by the star sensor.
Then, substituting (2) into (1) produces Because (3) is a nonlinear equation, it should be linearized with the Taylor expansion in order to solve for v p .
According to (3), set the function as where i represents different observed objects. Then, the first-order Taylor expansion of (4) yields where ∆ Z represents the higher-order term and (v px , v py , v pz ) T represents the components of v p in the I-frame. After omitting the higher-order terms in Equation (5), the equation can be rewritten as Then, a nonhomogenous equation can be obtained and written as Because the three observed celestial bodies are noncollinear, L is a full rank matrix. Thus, it has Therefore, the velocity of aircraft calculated by the SRS in the ENU-frame can be obtained and written as where, v SRS is the velocity calculated by the SRS in the east-north-up geographical frame (ENU-frame); C e g is the conversion matrix from the Earth-frame to the ENU-frame; and C i e is the conversion matrix from the I-frame to the Earth-frame.

Model of the INS/SRS/CNS Integrated Navigation System
The structure of the INS/SRS/CNS integrated navigation system is shown in Figure 1. In the INS/SRS/CNS integrated navigation system, the INS is the main system, and the SRS, the CNS, and the barometric altimeter provide the velocity and position measurements to help correct the deviation of the INS. In addition, a closed loop system is set in the INS/SRS/CNS integrated system, which can further improve the system's accuracy.

Kinematic Model of the INS/SRS/CNS Integrated Navigation System
According to the error model of the INS, we can represent the kinematic model of the integrated navigation system as [14] · X(t)=F(t)X(t)+W(t) (10) where X(t) is the system state vector, specifically represented as (φ E , φ N , φ U ) denotes the platform angle error in the ENU-frame; (δv E , δv N , δv U ) denotes the velocity error in the ENU-frame; (δL, δλ, δh) denotes the position error in the ENU-frame; respectively denote the gyro random drift and the accelerometer random bias. F(t) is the system matrix, which is specifically represented as [17]: (12) where F N is the attitude, velocity, and position-related system submatrix and F S is the gyro and accelerometer-related system submatrix. W(t) is the system noise matrix, specifically indicates the random error vector of gyroscopes and (w a E , w a N , w a U ) indicates the accelerometer drift vector.

Measurement Model of the INS/SRS/CNS Integrated Navigation System
The velocity measurement equation based on the SRS can be expressed as where v INS is the velocity obtained by the INS in the ENU-frame and V SRS is the noise matrix of the SRS. The longitude and latitude observation equation of the INS/SRS/CNS is the difference of the longitude and latitude information between the INS and the CNS, which is shown as where (λ CNS , L CNS ) denotes the longitude and latitude measurement of the CNS in the ENU-frame; (λ INS , L INS ) denotes the longitude and latitude outputs of the INS in the ENU-frame; and V CNS denotes the measurement noise matrix of the CNS. In order to prevent the divergence of the altitude channel of the INS, the barometric altimeter is introduced into the integrated navigation system. Then, the measurement equation of altitude is shown as where h INS and h BA denote the altitude output by the INS and the barometric altimeter in the ENU-frame, respectively, and V BA denotes the measurement noise matrix of the barometric altimeter. Finally, the whole measurement equation of the INS/SRS/CNS system can be written as where H k = 0 6×3 I 6×6 0 6×6 is the measurement matrix of the INS/SRS/CNS system; X k is the discrete state vector; and V k = [V k,SRS ; V k,CNS ; V k,BA is the measurement noise matrix of the whole system.

The Traditional Kalman Filter
The denotation of the noise matrices is as follows: where Q k is the non-negative matrix, R k is the positive matrix, and δ k j is the Kronecker-δ function. Then, the procedure of the KF can be written as follows: First, the state prediction is shown as where X k|k−1 ∈ R n denotes the state prediction; P k|k−1 ∈ R n×n denotes the state prediction covariance matrix; Z k|k−1 ∈ R m denotes the measurement prediction; and P zz k|k−1 ∈ R m×m denotes the predicted measurement covariance matrix.
Second, the state estimation is shown as whereX k denotes the state estimation and P k denotes the estimation covariance matrix of the state.

CST-Based Noise Estimator for Measurement
In reality, the measurement noise is unknown and changes with time; thus, it needs to be estimated and adjusted to maintain the robustness and accuracy of the estimation obtained from the Kalman filter. In this paper, a new noise estimator based on the CST is proposed: Assuming ν j j = k − M + 1, · · · , k is the selected independent innovation sequence at time k under a limited window of size M, the innovation-based measurement is calculated as The hypothesis test based on the innovation sequence can be set as H 0 : E[νν T ] = P zz k|k−1 , charactoristic of noise is unchanged H 1 : E[νν T ] = Σ k P zz k|k−1 , charactoristic of noise is changed (27) where P zz k|k−1 represents the covariance of the prior innovation estimation by the Kalman filter and Σ k denotes the covariance of the posterior innovation estimation, which can be calculated under the limited innovation sequence as As (27) shows, under the accurate system model, if a measurement is without an outlier, Σ k is near the value of P zz k|k−1 . Otherwise, the statistical noise can be considered to have changed. Then, according to the principle of the CST, the judgment index can be expressed as where λ(k) ∼ χ(m) 2 .
When the statistical characteristics of measurement noise are unchanged compared to the last time, λ(k) will be small and under threshold T. Otherwise, the judgment index will be over the threshold, and in that time the covariance of measurement noise should be adjusted AssumingR where β k is the adjust matrix of the measurement noise matrix. Additionally, substituting (32) with (30), one obtains Then, set the equation as follows According to Newton's method, one then obtains Thus, we can obtain where i denotes the time of iterations in Newton's method. Finally, set the initial β k (1) = 1 and keep the iterations of (36) until λ(β k (i)) is under threshold T. Accordingly, β k can be written as Remark 1: Further, to avoid an unlimited number of iterations, a cut off time of C = 20 is set in the CSTRKF. If the λ(β k (i)) is not under the threshold when the number of iterations is over C, the iterations end and the measurement update in the CSTRKF stops at this time.
In order to avoid the element ofR k being negative and keepR k as a diagonal matrix, β k is modified as where ε is smaller than 1. Therefore, the covariance estimation of the observation noise can be written aŝ

Procedure of the CSTRKF
By involving the CST-based noise estimator in the KF, the CSTRKF can be obtained, as illustrated in Figure 2. As Figure 2 shows, the procedure is as follows: Step 1. Initialize the matrix of X 0 , P 0 , R 0 , and Q 0 ; Step 2. Achieve the prediction of X k|k−1 , P k|k−1 , Z k|k−1 , and P zz k|k−1 by using (19) through (22); Step 3. Compute the innovations sequence using (26); Step 4. Set β k (1) = 1 and calculate the judgment index from (30); Step 5. Then judge whetherR k is changed by CST: if λ(k) ≤ T,R k−1 is considered as accurate and β k is adjusted to 1. Otherwise,R k−1 is considered as changed and β k needs to be iterated by (36) until λ(β k (i)) is under threshold T; Step 6. CalculateR k using (40); Step 7. Incorporate the newR k and estimateX k andP k using (23)-(25); and Step 8. Repeat steps 2-7 until the navigation ends.

Simulation and Results
In this section, the superiority of the INS/SRS/CNS integrated system with the proposed CSTRKF algorithm is verified through simulations. Figure 3 shows the dynamic flight trajectory of HCVs. The parameters of the simulations are shown in Table 1. The total simulation time was set to 30 min (1800 s) and the filtering period was 0.1 s. In the CSTRKF, the significance level α was 0.05.

Evaluation of CSTRKF under the Condition of Measurements with Outliers
In this part, the CSTRKF is compared with the H-infinity-based robust filter (HI-RF) [22] and the traditional KF under the condition of measurements with outliers in the INS/SRS/CNS integrated navigation system.
The observation errors were enlarged to 4 times their normal error for observations at 400s, 800s, 1200s, and 1600s. Under those observations with outliers, the curves of the velocity error and position error under the traditional KF, the HI-RF, and the CSTRKF are compared in Figures 4 and 5.  From Figures 4 and 5, it can be seen that under the KF, the velocity errors and position errors have the largest fluctuations and values at times 400s, 800s, 1200s, and 1600s when compared with those under the other two filters, indicating the poor robustness of the KF, which does not involve the measurement noise estimation method. Additionally, due to the utilization of the H-infinity strategy to resist outliers, the velocity errors and position errors under the HI-RF are smaller than those under the KF. However, this method still has pronounced errors. Furthermore, it can be seen in Figures 4 and 5 that by using the CST to judge the change in measurement noise and estimate the noise simultaneously, the velocity and position errors have the smallest values among the three filters, which shows the great robust performance of the CSTRKF.
The root-mean-square error (RMSE) and mean absolute error (MAE) are defined as where k denotes the simulation times and ∆x denotes the ∆V or ∆P, which is calculated as The MAEs of velocity and position at times with outliers and at times without outliers are shown in Table 2. When measurements have outliers, under the KF, the system has the greatest MAEs for both velocity and position, approximately 0.5443 m/s and 25.0624 m, respectively. By utilizing the H-infinity strategy, the MAEs of both velocity and position under the HI-RF are smaller than those under the KF by 13.1% and 22%, respectively. Furthermore, thanks to the estimation noise based on the CST, the MAEs of velocity and position under the CSTRKF are smaller than those under the HI-RF by 10.2% and 28.2%, respectively, which shows the superiority of the CSTRKF.

Evaluation of CSTRKF under a Contaminated Gaussian Measurement Noise Condition
To continue to evaluate the performance of the proposed CSTRKF in terms of the non-Gaussian characteristics of noise statistics, the measurement noise was set to change as a contaminated Gaussian distribution, which is as follows where η is set to follow a uniform distribution between 0 and 1. Figures 8 and 9 show the velocity error and position error under the KF, HI-RF, and CSTRKF. The MAE of velocity and position is shown in Table 3. From Figures 8 and 9, it can be seen that without the relative method to deal with the contaminated Gaussian noise, the velocity error and position error under the KF reach their highest values across the entire set of simulations. Under the HI-RF, the system has a smaller velocity error and position error. Additionally, the MAE of velocity and position under the HI-RF are 37.5% and 27.2% smaller, respectively, than those under the KF. However, the velocity error and position error under the CSTRKF are the smallest, with MAEs 29.9% and 25.6% smaller, respectively, than those under the HI-RF, showing the superior performance of the CSTRKF under the condition of contaminated Gaussian noise.

Conclusions
In this paper, the linear relationship equation between the velocity in the ENU-frame and the redshift of an observed vehicle was deduced and the INS/SRS/CNS integrated navigation model was established based on this relationship for the purposes of improving the autonomy and reliability in the navigation of HCVs. Furthermore, to improve robustness, the CSTRKF algorithm was also proposed in the INS/SRS/CNS integrated navigation model. In the CSTRKF, based on the posterior innovation covariance estimation the CST has been added to the KF in order to detect the change in noise and estimate the statistical characteristics of measurement noise. The simulation results indicate that the CSTRKF has great robustness performance and, under the CSTRKF, the robustness of the INS/SRS/CNS integrated navigation system also improved.