Abstract
As an emerging means of transportation for the intelligent transportation system (ITS) in aviation and aerospace, hypersonic cruise vehicles (HCVs) have received numerous research interests during the past several decades. However, the navigation and positioning strictly limit the progress and application of HCVs due to their special characteristics on dynamics and environments. To improve the stability of navigation in HCVs, a chi-square test-based adaptive federated cubature Kalman filter (CAFCKF) is proposed in this paper. In the proposed approach, the chi-square test is adopted for the estimation of the measurement noise statistics firstly. Subsequently, a new adaptive information fusion factor is designed for the federated filter to adjust the contribution of each subsystem. Finally, the information sharing factor, which is used for the amendment of the state covariance of each subsystem, is refined based on the judging index of the chi-square test accordingly. Simulation results show that the proposed CAFCKF can be used to improve the accuracy and stability of the navigation system.
1. Introduction
The intelligent transportation system (ITS) has received numerous research interests to improve the consumption of vehicles using advanced technologies during the past several decades. Recently, the concept of ITS has been expanded from the land traffic to the fields of aviation and aerospace. Hypersonic cruise vehicle (HCV) is an emerging means of transportation for the ITS in aviation and aerospace. However, the navigation and positioning for the HCVs are still a challenging problem due to their special characteristics on dynamics and environments, leading to the limited progress and application of the HCVs. Currently, the strapdown inertial navigation system (SINS) has been widely used for autonomous navigation in HCVs [1–3]. However, the error of SINS often increases boundlessly in time series due to the gyro drift and accelerometer bias [3]. To overcome the shortcoming using a single SINS, the Global Navigation Satellite System (GNSS) was proposed to provide highly accurate position information [4–6]. But the GNSS usually lacks autonomy, which makes it susceptible to artificial interference. To reduce the accumulated navigation error and to resist electromagnetic interference, the celestial navigation system (CNS) was proposed in [7, 8]. The SINS, GNSS, and CNS were then combined to keep the system reliable when the GNSS was out of service [9]. However, the problem with the CNS is that it has difficulty in star selection and velocity measurement [10]. As a new supplement for the navigation technique, the spectral redshift navigation system (SRS) was then studied, in which the velocity can be calculated from the spectral redshift information of celestial spectrum [11]. Compared with other navigation systems, the SRS has the advantages such as simple navigation principle, easy star selection, and zero time delay, which makes it suitable as an auxiliary navigation system to assist in correcting the velocity error of SINS, avoiding parameter divergence while maintaining system autonomy [12, 13]. Due to these reasons, the integration of INS/GNSS/CNS/SRS will be a promising navigation strategy for the HCVs to address the limitations of INS/GNSS integration in lack of autonomy and INS/GNSS/CNS integration in star selection and velocity measurement.
The Kalman filter has been proposed as an important information fusion algorithm for the linear integrated navigation system [14]. Recently, the nonlinear information fusion algorithms, such as extended Kalman filter (EKF) [15], unscented Kalman filter (UKF) [16], and cubature Kalman filter (CKF) [17], were proposed to handle the data fusion problem involved in the nonlinear system. The CKF applies the cubature rule to obtain the consequently fixed cubature points with a constant weight, which is more stable than UKF [18]. A comparison between CKF and UKF was made in [19], and it was shown that the CKF was more accurate than the sigma points approach-based UKF. When the Kalman filter was used to achieve information fusion in the navigation system, an accurate system model and exact noise statistics were required [20]. However, the navigation system always involves uncertainties and faults caused by the outlier in measurement under highly dynamic conditions. To avoid the influence of outlier in measurement, measurement noise was often handled by the H-infinity strategy [21, 22]. However, this method may break down in the presence of randomly occurred outliers since it assumes that the estimation to be achieved is a linear combination of state vectors which is very difficult to satisfy. Through statistical linear regression of a nonlinear system function, the Huber-based KF method was proposed in [23] to curb the effect of outlier in measurement, which can achieve the robustness but sacrifices the estimation accuracy. Moreover, a scaling factor of measurement noise covariance was calculated to further adjust the Kalman gain to deal with the outlier in measurement [24, 25]. However, the scaling factor in this method is often selected by experience. To improve the accuracy of noise estimation, the maximum likelihood principle (MLP) was also studied in [26, 27]. However, this method involves large computational burden, which makes it unable to achieve the real-time navigation performance [25].
Considering redundant and homologous measurement information in the multisensor navigation system, the federated Kalman filter (FKF) was proposed for the state fusion estimation of integrated navigation information [28, 29]. With a distributed filter structure, the FKF is more convenient for fault isolation and more efficient than the centralized filter. In the FKF, the information sharing factor has great effects on the accuracy of information fusion and correlation elimination among the local estimations. However, the value of information sharing factor does not always reflect the differences between each state variable. The chi-square test is a statistical method for the real-time detection of abnormal changes in a dynamic system [30, 31]. Thus, it can be used to detect changes of statistical characteristics of measurement noise and to optimize the information sharing factor in the FKF for the application of integrated navigation.
In this paper, a SINS/GNSS/SRS/CNS integrated navigation system and a chi-square test-based adaptive federated cubature Kalman filter (CAFCKF) are proposed to improve the accuracy and reliability of the navigation system in HCVs. In CAFCKF algorithm, the chi-square test is used for the estimation of the statistical characteristics of measurement noise. Then, a new adaptive information fusion factor is designed to adjust the contribution of each subsystem. Further, the information sharing factor is also refined based on the judge index of the chi-square test to amend the state covariance of each subsystem for the optimization of the global state estimation. Finally, the proposed CAFCKF algorithm is verified in the SINS/GNSS/SRS/CNS integrated navigation system.
2. SINS/GNSS/SRS/CNS Integrated Navigation System
The structure of SINS/GNSS/SRS/CNS integrated navigation system is shown in Figure 1, in which the SINS is used as the main navigation system and the GNSS, SRS, and CNS are applied to correct the error of SINS. In this integrated system, the SRS and CNS can be regarded as the supplement of SINS/GNSS integration when GNSS is out of service, since they can provide the velocity observation and position measurement to correct the error of SINS.
2.1. Kinematic Model
The state of the kinematic model is chosen aswhere denotes the attitude error, is the velocity error, denotes the position error, and denote the gyro constant drift and accelerometer zero bias, respectively, and n and b represent the n-frame and b-frame, which will be explained in the next paragraph.
The navigation frame (n-frame) is selected as the E-N-U (East-North-Up) frame. Denote the inertial frame as i-frame, the body frame as b-frame, the Earth frame as e-frame, and the SINS simulated navigation frame as n’-frame. Based on the E-N-U frame, the error equations of SINS in terms of velocity and attitude are written aswhere is the measured specific force in the b-frame, is the gyro’s measurement error, is the angular velocity of the Earth, is the angular velocity of the vehicle relative to the Earth, is the relative angular velocity between n-frame and i-frame, , , and are the corresponding values in the n’-frame, and is the velocity obtained by SINS in n-frame.
The error equation of SINS in terms of position is given bywhere and are the median radius and normal radius and , , and are the altitude, longitude, and latitude output using SINS in n-frame.
The equation of and can be described as
Thus, from (2)–(4), the discrete kinematic model of SINS/GNSS/SRS/CNS integration system can be presented bywhere is the sample time, is the discrete system’s state vector, is the discrete system’s state transformation matrix, and is the discrete system’s kinematic noise matrix.
2.2. Measurement Equation of SINS/GNSS Subsystem
The pseudorange and pseudorange rate from a GNSS receiver can be represented as [10]where and represent the pseudorange and pseudorange rate, respectively, and represents the measurement noise.where is the actual position of the vehicle in the e-frame; is the position of the th satellite in the e-frame; is the actual velocity of vehicle in the e-frame; is the velocity of the th satellite in the e-frame; and represents the geometric range from the th satellite to the receiver, which can be calculated as
It is known thatwhere is the corresponding position of the vehicle in n-frame, is the radius of curvature in the prime vertical, is the eccentricity of the ellipsoid, is the rotation matrix from n-frame to e-frame, and is the corresponding velocity of vehicle in n-frame.
The position and velocity estimated by SINS are satisfied such thatwhere is the real position.
By substituting (7)–(11) into (6), the nonlinear measurement equation is obtained aswhere .
2.3. Measurement Equation of SINS/SRS Subsystem
Considering the redshift principle of spectrum and the Doppler frequency shift formula, the relationship between redshift of celestial spectrum observed in vehicle and the velocity of vehicle is [12]where denotes the observed objects which is not less than 3, denotes spectral redshift value of celestial body calculated in the target vehicle, denotes the velocity vector of the vehicle in the i-frame, denotes the velocity vector of the celestial body in the inertial coordinate system, which can be obtained by querying the celestial ephemeris, denotes the velocity of light, and denotes the position unit vector of the celestial object in the i-frame, which can be measured by the star sensors.
Assume that
Through the first order Taylor expansion at and omitting the higher-order terms, (14) can be transformed towhere represents the components of in i-frame.
A nonhomogenous equation can be obtained as
Due to the fact that the three observed celestial bodies are noncollinear, is a full-rank matrix:
The velocity of vehicle calculated by SRS in ENU-frame can be obtained aswhere is the velocity of SRS in ENU-frame, is the rotation matrix from Earth frame to ENU-frame, and is the rotation matrix from i-frame to the e-frame.
The velocity error measurement equation of SINS/SRS subsystem can be expressed aswhere is the measurement noise.
2.4. Measurement Equation of SINS/CNS Subsystem
The measurement equation of SINS/CNS subsystem is chosen as the difference of longitude and latitude information between SINS and CNS:where denote the longitude and latitude measurements of CNS.
In order to prevent the divergence of altitude channel in SINS, a barometric altimeter is introduced to the integrated navigation system. The difference in altitude between the barometric altimeter and SINS is taken as the measurement. Thus, the measurement equation is written aswhere denotes the altitude output of barometric altimeter.
Then, the measurement equation of SINS/CNS subsystem can be further written aswhere is the measurement matrix of the SINS/CNS subsystem and is the measurement noise.
3. Chi-Square Test-Based Adaptive Federated Cubature Kalman Filter
3.1. Federated Cubature Kalman Filter
To improve the computation efficiency and reliability, a federated cubature Kalman filter (FCKF) is established and used for the multisensor nonlinear system. Its procedure is shown below.
Step 1. State prediction:where, .where denotes the state equation and denotes the ith element of .
can be expressed aswhere d is the dimension of system state.where .
Step 2. Calculate the gain matrix of the subfilter:The prediction innovation covariance matrix and cross-covariance matrix of subfilter s can be written aswhere denotes the measurement prediction of subsystem s.
The gain of subfilter s can be written as
Step 3. Update state of the subfilter aswhere is the covariance matrix of subfilter s and is the state estimation of subfilter s.
Step 4. Calculate the global fusion through the main filter:where is the covariance matrix of the global filter and is the state estimation of the global filter.
3.2. Establishment of the Chi-Square Test-Based Adaptive FCKF (CAFCKF)
In reality, the statistic characteristics of measurement noise are always unknown or changed with time. Under this situation, the estimator has great difficulty in ensuring the system accuracy. To address this problem, a noise estimator for the measurement noise statistics based on the chi-square test (CST) is firstly proposed in this paper.
The innovation of sth subsystem is defined as
The hypothesis test based on innovation can be constructed as
Then, calculate
According to the principle of CST, the judge index can be expressed aswhere .
According to the hypothesis test, by setting the significance level α (0 < α < 1), there will be a threshold Ts, which makes α follow [28]
When the statistical characteristics of measurement noise are unchanged, will be smaller than Ts. Otherwise, the judge index will exceed the threshold, and the covariance of predicted measurement should be adjusted according to
Based on Newton’s method, we havewhere i denotes the times of iteration in Newton’s method.
Setting , the estimation of measurement noise covariance can be obtained by running the iteration of (44) until is less than 0. To limit the times of iteration, a cutoff time C is set to 5, which means if is not less than the threshold when the times of iteration reach C, the iteration will end.
Subsequently, to improve the performance of the information procedure, a new adaptive information fusion factor is defined in FCKF. With the update of state estimation through the subfilter, a judge index of the chi-square test can be calculated according to the state estimation and the state prediction from the subfilter:where denotes the state judge index of subfilter s.
Since state estimation error of a subsystem increases as increases, the information fusion factor under the chi-square test can be defined aswhere
Then, the global fusion of FCKF can be modified as
After the global fusion process, the information sharing factor for each subfilter is needed to refine the estimation of each subsystem. The judge index of the chi-square test calculated by the global state estimation and the local state estimation can be written as
When is larger than those of other subsystems, the real contribution of the sth subsystem to the integration system is smaller than that of other subsystems, which means this subsystem should have a bigger state covariance than others. So, we refine the information sharing factor by calculating it aswhere
It is known that
Thus, the refined state covariance of the subfilter can be expressed as
3.3. Procedure of CAFCKF
The structure of CAFCKF is shown in Figure 2. The procedure is shown below. Step 1.Initialization of . Step 2.State prediction through equations (23)–(27). Step 3.Estimation of predicted measurement covariance through equations (38)–(44). Step 4.Calculation of the gain matrix of the subfilter through equations (28)–(34). Step 5.State updation of the subfilter through equations (35) and (36). Step 6.Calculation of the defined information fusion factor through equations (45)–(47). Step 7.Global fusion through equations (48) and (50). Step 8.Calculation of the information sharing factor through equations (50)–(53). Step 9.Information sharing through equation (54). Step 10.Repeat Steps 2–9 until the fusion ends.
4. Results and Discussion
A simulation case study is conducted to verify the performance of the proposed CAFCKF for the SINS/GNSS/SRS/CNS integrated navigation system. In the simulation, a comparison between the proposed CAFCKF, FCKF, and cubature rule-based distributed optimal fusion (CRDOF) [28] is made and discussed. The dynamic flight trajectory of HCVs is shown in Figure 3. The parameters set in the simulation are listed in Table 1. The entire simulation time is set to 1000 s, and the filtering period is set to 1 s.
The initial value of , , , , , and is selected as
With the aforementioned simulated parameters, the effects of the proposed CAFCKF, FCKF, and CRDOF on the SINS/GNSS/SRS/CNS integrated navigation system are analyzed. The GNSS, SRS, and CNS are given with changed measurement noise and described as condition 1:where , , and ; these are also the initial values of R0 for each subfilter.
Under condition 1, the estimation error curve of the velocity and position using CAFCKF, FCKF, and CRDOF is shown in Figure 4. The mean absolute error of the velocity and position is listed in Table 2.
As shown in Figure 4 and Table 2, during the time interval of (200 s, 300 s), with the changed covariance in the measurement noise of SINS/GNSS subsystem, the biggest fluctuation occurs for both the error curve of velocity and position by the FCKF. Compared to FCKF, smaller position and velocity errors are obtained by the CRDOF. The mean absolute error of velocity and position reaches 0.133 m/s and 6.64 m. For the chi-square-based noise estimation algorithm and the adaptive information fusion factors, smaller mean absolute errors are obtained by the CAFCKF than those of FCKF, which are about 0.121 m/s and 3.57 m. During the time interval of (500 s, 600 s), the error curve of velocity by the FCKF fluctuates abruptly, and the mean absolute error of velocity is about 0.189 m/s. On the contrary, compared to the FCKF, a smaller mean absolute error of velocity is obtained by the CRDOF, which is 0.149 m/s. Compared to the CRDOF, a smaller mean absolute error of velocity is obtained by the CAFCKF, which is about 0.1 m/s. During the interval of (800 s,900 s), the covariance of the measurement noise is not adjusted and the contribution of each subsystem is not ensured for the FCKF, leading to the biggest fluctuation in the position error curve. Compared to the FCKF, a more smooth error curve is obtained by the CAFCKF. Meanwhile, a smaller mean absolute error of position, which is 3.43 m, is obtained by the CAFCKF.
To verify the universality under condition 1, the ARMSE of velocity and position is calculated after 50 Monte Carlo runs and shown in Figure 5.
As shown in Figure 5, the FCKF results in the biggest ARMSE during all time intervals. During the time interval of (200 s, 300 s), the ARMSE of velocity and position is 0.14 m/s and 4.57 m for the CAFCKF, which is 22% and 39% smaller than that of CRDOF. During the time interval of (500 s, 600 s), the ARMSE of velocity and position is 0.11 m/s and 4.34 m for the CAFCKF, which is 36% and 37% smaller than that of CRDOF. During the interval of (800 s, 900 s), the ARMSE of velocity and position is 0.12 m/s and 4.68 m/s for the CAFCKF, which is 20% and 77% smaller than that of CRDOF.
The GNSS, SRS, and CNS are given with another changed measurement noise and described as condition 2:where , , and .
Under condition 2, the estimation error curve of the velocity and position using CAFCKF, FCKF, and CRDOF is shown in Figure 6. The mean absolute error of the velocity and position is listed in Table 3.
As shown in Figure 6 and Table 3, during the time interval of (200 s, 300 s), the error of velocity and position by the FCKF obviously increases. Its mean absolute errors in velocity and position are 1.73 m/s and 71m, which are the biggest among these methods. The mean absolute errors of velocity and position by the CAFCKF, which are 0.33 m/s and 6.57 m, are smaller than those of CRDOF. During the time interval of (500 s, 600 s), the mean absolute errors of velocity and position by the FCKF are about 1.84 m/s and 21.36 m. The CAFCKF and CRDOF present better performance of information fusion than FCKF. The mean absolute errors by the CAFCKF are smaller than those of CRDOF. During the time interval of (800 s, 900 s), the FCKF presents the biggest error in both velocity and position. The mean absolute errors are 0.69 m/s and 64.14 m. In contrast, the smallest mean absolute errors of velocity and position are obtained by the CAFCKF, which are 0.1093 m/s and 4.58 m.
Under condition 2, the ARMSE of velocity and positions is also calculated after 50 Monte Carlo runs and shown in Figure 7.
As shown in Figure 7, during the time interval of (200 s, 300 s), the ARMSE of velocity and position is 0.43 m/s and 7.67 m by the CAFCKF, which is 62% and 40% smaller than that of CRDOF. During the time interval of (500 s, 600 s), the ARMSE of velocity and position is 0.31 m/s and 4.33 m by the CAFCKF, which is 56% and 21% smaller than that of CRDOF. During the time interval of (800 s, 900 s), the ARMSE of velocity and position by the CAFCKF is 0.11 m/s and 5.58 m, which is 22% and 62% smaller than that of CRDOF. Furthermore, without adaptive information fusion factor, the biggest ARMSE is obtained by the FCKF.
5. Conclusions
In this paper, a SINS/GNSS/SRS/CNS integrated navigation system and chi-square test-based adaptive federated cubature Kalman filter (CAFCKF) are proposed to improve the accuracy and reliability of the navigation system in HCVs. In the proposed CAFCKF algorithm, the chi-square test is used to estimate the statistical characteristics of measurement noise firstly. An adaptive information fusion factor and the information sharing factor are also designed by using the judge index of the chi-square test, which optimize the contribution of each subsystem and lead to a more accurate global state estimation. Finally, the proposed CAFCKF algorithm is verified through simulation. The result shows that, compared with other filtering methods, the CAFCKF leads to the smallest mean absolute errors in velocity and position, demonstrating that it can be used to improve the accuracy and stability of the navigation system.
Data Availability
The data used to support the findings of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This research was funded by the Natural Science Basic Research Plan in Shaanxi Province of China (project nos. 2020JQ-234 and 2020JQ-150), the National Natural Science Foundation of China(project no. 52102443), and a project entitled Funding for School-Level Research Projects of Yancheng Institute of Technology (project no. xjr2019036).