1. Introduction
The state estimation of nonlinear systems is widely used in engineering applications, such as radar target tracking, complex image processing, highly dynamical navigation and positioning, and signal processing. In order to obtain the optimal estimation of a nonlinear system, the posterior probability distribution of the system state needs to be obtained. However, a complete description of the posterior probability distribution can be accurately known only in a few specific cases. In recent years, driven by engineering applications, a large number of suboptimal filtering methods have been proposed, which can be classified into two main categories: The first one is the linearization method such as the extended Kalman filter (EKF). The second one is the sampling method, i.e., the unscented Kalman filter (UKF) and particle filter (PF). EKF is a traditional approach to solving nonlinear problems, such as navigation, target tracking, information fusion, monitoring state estimation [
1,
2], etc. However, in a system with a higher degree of nonlinearity, the EKF will have a larger truncation error under a low carrier to noise ratio, which can easily cause the filter to diverge. In addition, the EKF implementation needs to calculate the Jacobian matrix, which also limits its scope of applications. The second method directly uses nonlinear filtering algorithms to process observation information, which can improve the accuracy of state estimation. Among them, UKF approximates the distribution function of random variables through a set of determined weighted sampling points [
3]. When the sampling points are propagated through a nonlinear function, the statistical properties of the non-linear functions are captured, and the precision of UKF can reach the third-order [
4,
5]. However, UKF still has some limitations. In high-dimensional systems (usually higher than three-dimensions), in order to avoid the propagation of non-positive definite covariance matrices, it should choose the parameters of UKF very carefully [
6]. At this time, the UKF is prone to numerical instability, which can lead to dimensional disasters or divergence, and limited their application in complex systems, such as high dynamics, weak signals, and strong non-linearity. The PF is proposed based on the idea of the Monte Carlo method [
7,
8], which uses a large number of randomly-generated particles to approximate the posterior probability density. The PF is used to solve signal processing problems in nonlinear and non-Gaussian systems. In recent years, it has been widely used for target tracking, state estimation under low-dynamical conditions, and modality detection [
9]. However, as the number of iterations increases, there will be particle degradation and depletion when using PF [
10]. In order to solve the problem of particle degradation, many improved particle algorithms have emerged. However, the generation of a large number of particles will still increase the computational burden, and the reduction of degradation and depletion is at the cost of increasing the complexity of the PF algorithm. Therefore, it is difficult for PF to meet real-time requirements, especially for state estimation under highly dynamical conditions.
In [
11], the authors proposed a new non-linear filtering method based on the cubature transformation: the cubature Kalman filter (CKF). Once proposed, the method was used for navigation [
12,
13,
14], attitude estimation [
15], continuous system [
16], hybrid filtering [
17], and so on. The CKF is similar to the UKF filtering process, but its theoretical derivation is more rigorous [
18]. It generates new points by converting 2n equal-valued cubature points through nonlinear functions and is used to predict the system state at the next moment. The CKF also does not need to linearize the non-linear dynamics and has all the features of the UKF. CKF uses a symmetrical sampling strategy and has fewer sampling points than the UKF does. Compared with filtering algorithms, such as EKF, UKF, and PF, CKF has better nonlinear performance, higher numerical accuracy, and better filter stability, and it is relatively simple to implement.
However, when CKF is applied to a nonlinear system, it needs to know the mathematical model of the object to be studied and the prior knowledge of noise statistical properties. In practice, it is difficult to obtain the prior knowledge of the noises statistics. The inaccurate mathematical model and inaccurate noise statistics may lead to large state estimation error, or even divergence. To solve these problems, scholars further study the CKF technique combined with an adaptive filter. The typical research results are mainly divided into two types. The first types of typical results proposed are improved CKF algorithms based on the strong tracking filter (STF), which can be used to solve the filter divergence problem caused by inaccurate system models. Aiming at the problem of CKF precision degradation caused by model uncertainty, the literature [
19] established a strong tracking CKF algorithm (STCKF). The literature [
20] proposed a strong tracking adaptive CKF algorithm viewing STF as the basic theory framework and making D replace EKF, and the algorithm improved the filtering performance of the existing system model uncertainty. To overcome the outliers caused by the model uncertainties, the literature [
21] designed a robust strong tracking CKF and developed a noise statistic estimator based on the principle of maximum a posterior. The CKF used in the above method are all three-degree algorithms. Further, to improve the estimation accuracy, Cui and Zhang [
22] proposed an improved high-degree CKF combined with the STF algorithm, named as the adaptive high-degree CKF (AH-CKF). In the AH-CKF, by introducing the STF into the high-degree CKF and modifying the predicted states’ error covariance with a fading factor, the residual sequence is forced to be orthogonal so that the robustness of the filter and the capability to deal with uncertainty factors are improved. However, these results are only for research on low- and medium-dynamic applications.
The second types of typical research results are improved CKF methods based on the robust M-estimation technique. Huang [
23] proposed a class of robust CKF algorithms with statistical regression to solve the problem that the conventional CKF declines in accuracy and further diverges when the noise is not Gaussian noise. Wu [
24] proposed a robust CKF based on generalized M-estimation to reduce the influence of measurement outliers on a target tracking system. Similarly, in order to address the degradation of the standard CKF due to outliers in measurement, Li [
25] presented a robust version of CKF using Huber’s M-estimation methodology and square-root filtering framework. Essentially, the above research results are all effective improvements to the standard CKF or square-root CKF using he robust M-estimation technique. The solved problem is the degradation or divergence of the standard CKF caused by the inaccuracy of measurement noise. However, these robust M-estimation methods cannot adjust the noise covariance adaptively when it does not match with the truth. The problem will become more severe in the case of significant outliers [
26]. To overcome this deficiency, Zhang and Zhi [
26] came up with a new way and proposed an adaptive Huber’s M-estimation-based CKF (AHCKF) which can automatically adjust the measurement noise and gain adaptivity. The AHCKF has enhanced outlier robustness, reliability, and high estimate accuracy. This method, however, does not consider the problem of when the model uncertainties exist.
The above two types of improved CKF methods are developed respectively for the uncertainty of the model and the inaccuracy of measurement noise. However, these two kinds of problems often exist in the actual system synchronously, such as a highly dynamical navigation system or high maneuvering target tracking system, typically. To overcome this deficiency, we propose a novel, robust, adaptive CKF compromising the robust M estimation and the adaptive adjustment factor. The contribution of the paper can be summarized as follows:
The proposed algorithm could solve the problems of the model uncertainty and the measurement noise statistics inaccuracy. To handle abnormal measurement noise, we adopt robust M-estimation to automatically adjust the measurement noise covariance and gaining adaptivity. Compared with the literature [
23,
24,
25,
26], we directly use innovation to calculate the equivalent weight matrix, no-demand partial derivative operation, and iterative operation. To overcome model uncertainty, we derive an adaptive adjustment factor to modify the model as a whole. Compared with STF in the literature [
19,
20,
21,
22], the derived adaptive adjustment factor is simpler.
The proposed algorithm has lower computational complexity, and so it is suitable for dynamic systems with high real-time requirements. Our algorithm is not simply a cumulative combination of existing methods, but a derivation method considering the mathematical complexity and real-time, which ensures the computational efficiency of the algorithm.
In the application of the algorithm, we designed a stable and reliable ultra-tightly coupled structure based on a double-modulating loop. The effectiveness of the proposed method and the designed structure is verified by a highly dynamical scene experiment.
The rest of the paper is organized as follows:
Section 2 gives a brief introduction of the Bayesian filter theory in the Gaussian domain and the cubature Kalman filter. Then, the improved robust adaptive CKF, using robust estimation theory and adaptive filter techniques, is also derived in
Section 3. In
Section 4, the hypersonic vehicle state estimation problem is studied using a SINS-aided GPS ultra-tightly coupled system, and a SINS/GPS federated ultra-tightly coupled structure is designed based on a double-modulating loop. In
Section 4, we also give the nonlinear pre-filter tracking loop model of the satellite signal. In
Section 5, the proposed robust adaptive CKF is applied in the highly dynamical nonlinear state estimation experiment. The conclusion of the paper is given in
Section 6.
3. Novel Robust Adaptive CKF
When CKF is applied to a nonlinear system, it needs to know the mathematical model of the object to be studied and the prior knowledge of the noise statistics. However, if the filter is solved based on an inaccurate mathematical model and inaccurate noise statistics, it may result in a large state estimation error or even divergence. To solve this problem, it is necessary to study the adaptive filtering. For typical dynamic application systems, we focus on the study of adaptive filtering technology under the influence of two types of errors, that is, measuring abnormal errors and dynamic model errors.
It is inevitable to produce the gross error in the observations for a dynamic system. For example, when tracking a satellite signal, it is shown that the occurrence of gross errors accounts for 1% to 10% of the total number of observations [
30]. To a certain extent, the uncertainty factors are introduced into the statistical characteristics of the measurement noise. Therefore, it should be adjusted on line adaptively. In addition, in the condition of high dynamics and low SNR, the satellite signal tracking of the observed information is very easy to be influenced by a poor environment, and there is a large proportion of noise, so that the noise statistics deviate from the prior statistical characteristics. In serious cases, an abnormal perturbation error may exist. The above problem limits the application of CKF, so it is necessary to adjust the measurement noise in real-time to enhance the ability of the algorithm to resist gross and abnormal errors.
In view of the gross error, robust M estimation in robust estimation theory is applied to the CKF algorithm, which can detect the influence degree of gross error, and then the model is adaptively adjusted and corrected, so as to eliminate the influence of the abnormal observation error on the algorithm. The improved CKF algorithm is called the robust CKF algorithm. In what follows, we will derive the algorithm. Since the measurement information only affects the updating process of the measurement, compared with the standard CKF algorithm, the robust CKF algorithm only adjusts and modifies the relevant expressions in the measurement updating equation as follows:
where
is an equivalent measurement noise variance matrix corresponding to
, and it can be obtained by obtaining an equivalent weight matrix
in an anti-difference M estimation method. That is:
For the calculation of the equivalent weight matrix, the common methods are the IGGIII method, Andrew method, Tukey method, and Huber method [
31]. Considering that the first three methods can make the diagonal elements of the p matrix 0, the Huber method can guarantee that the diagonal elements of
matrices are positive. The expression of equivalent is as follows:
where
and
are diagonal elements and non-diagonal elements of
, respectively;
and
are diagonal elements and non-diagonal elements of the original
array;
is a residual component corresponding to the observation quantity
,
is a corresponding standard residual component, and
is a mean square error of
; and
c is a given constant, usually taken from 1.3 to 2.0.
The above operation involves that
and
are deterministic. In practice, because the covariance matrix of the measurement residuals is obtained from Equation (21), that is, the variable quantity
before being modified, the expressions
and
are:
Then, is substituted for in Equation (15), and the gain matrix is modified. Furthermore, the subsequent filtering solution is continued. The robust CKF algorithm is actually based on the standard CKF filter and modifies the noise covariance matrix , so as to adjust the filter gain matrix, and ultimately enhance the performance of algorithm against observation errors consisting in satellite signal to restrain the divergence of Kalman filter. Therefore, robust CKF algorithm is more suitable for the near space environment, in which satellite signals are unstable and measurement noise statistical characteristics deviate considerably from the prior information.
The robust CKF algorithm solves the influence of the observation abnormal errors in the system. In other words, aiming at the inaccurate measurement noise, the CKF algorithm was improved, and then completing the improvement of the CKF algorithm when there are errors in the dynamic model. It is notable that the inaccuracy of system noise statistical characteristics is also an inducement of dynamic model errors.
The dynamic equations established for the satellite signal link may deviate greatly from the real model of the system when a nearby space vehicle makes a highly dynamic maneuvering flight, that is to say, a large dynamic disturbance error may exist in the state equation of the system, and the CKF filter algorithm fixed by the model cannot estimate the state parameters of the system. Therefore, based on the above robust CKF algorithm, a robust adaptive CKF algorithm is proposed for dynamic model errors.
It is pointed out in [
13] that the error of the dynamic model usually destroys the effect of parameter estimation as a whole; in other words, the errors of the dynamic model will affect the estimation of all state parameter components. Therefore, we consider using an adaptive adjustment factor to modify the model as a whole, which also ensures the computational efficiency of the algorithm. The specific algorithm is that adaptive regulator
modify
, obtaining:
where the optimal value of
may be obtained from the residual covariance matrix after predicting and updating, where
is given by the following theorem.
Theorem 1. If is the residual covariance matrix estimated after the introduction of new measurement information, it is defined as the updating residual covariance matrix; is a theoretical residual covariance matrix obtained by adaptive filtering, and is a residual covariance matrix obtained by the covariance propagation law. The selection of the optimal adaptive factor shall ensure that the following equation holds: Then, the optimal adaptive factor is: Proof of Theorem 1. Let
a step-by-step prediction error, then:
Thus, the filtered residual is:
A first-order Taylor expansion of the second term on the right-hand side of the equation:
Bringing the above equation into the following equation:
Then the residual covariance matrix A is:
Note the equation using the measurement noise covariance matrix after being modified by robust CKF. Furthermore, in adaptive filtering,
is modified to
, and then the theoretical residual covariance matrix obtained by adaptive filtering is:
Multiply the two sides of the upper form by A and move the item to obtain
Take the traces of the matrices on both sides of the expressions. Then, the expression of the optimal adaptive factor can be obtained by the migration transformation. ☐
In practical application, the regulatory factor in adaptive filtering algorithm is usually not greater than 1 [
32,
33], so we further determine the optimal adaptive factor as follows:
Taking into account the fact that the numerator and the denominator in the upper equation all contain the variance term of measurement noises. The approximate optimal adaptive factor expression can also be obtained by using the common polynomial.
can be obtained by the residual error estimation of the observations. The sliding window method is adopted, namely:
In order to further clarify the robust adaptive CKF algorithm, the following steps are given:
- Step 1.
Set the initial condition.
- Step 2.
Forecast the update.
For a given A and B, according to Equations (12)–(26), the state prediction value C and its prediction covariance matrix D are obtained.
- Step 3.
Calculate the state volume points in the measurement updating.
The matrix decomposition of the state prediction covariance matrix is completed by using Equation (17), and replacing Equation (18) with the following equation, adaptive adjustment of the model will be complete:
- Step 4.
Calculate the predicted values and the residual covariance matrix.
Computational observational prediction and predictive residual covariance matrix are calculated by Equations (19)~(21).
- Step 5.
Robust correction.
The robust correction of the algorithm is completed according to the Equations (26)–(29).
- Step 6.
Adaptive factor regulation model.
Use the approximate expression Equation (45) to calculate the optimal adaptive factor, if the value is 1, then continue the following steps; if the value is less than 1, the value is substituted into Equation (48), and then by the Equations (19), (20) and (26) calculate the adaptive factor to adjust the revised prediction residual covariance matrix.
- Step 7.
Update the measurement.
According to Equations (22)–(25), the update calculation of the state variable and its corresponding state covariance matrix is completed.
Remark 1. The proposed robust adaptive cubature Kalman filter (CKF) could deal with the problem of inaccurately known system model and noises statistics simultaneously. The proposed method includes two steps of improvement for the basic CKF. First, in order to overcome the kinematic model error, we introduce an adaptive factor to adjust the covariance matrix of state prediction, and process the influence introduced by the dynamic disturbance error. Second, aiming at overcoming the abnormality error, we adopt the robust estimation theory to adjust the CKF algorithm online. Our algorithm is not simply a cumulative combination of existing methods, but a derivation method considering the mathematical complexity and real-time, which ensures the computational efficiency of the algorithm and is more suitable for highly dynamical application scenarios.
5. Experimental Analysis of a Highly Dynamical Scene
A near space vehicle often flies at high speed. This section of the experiment will focus on the carrier of high maneuverability factors on the impact of signal tracking, while considering that the lower the signal carrier noise ratio, the more difficult the stable tracking of the signal, and the more able to assess the performance of the research method. To this end, a lower signal to noise ratio is set to verify the tracking performance of the proposed robust adaptive CKF method under the condition of a highly dynamic, and low carrier, noise ratio.
5.1. Experimental Scheme
The NS300 multimode satellite signal simulator generates a highly dynamical GPS RF signal, and the RF signal is received by the GN0204 satellite signal receiving device, converted into an intermediate frequency signal and stored. Then we use the SINS/GPS ultra-tight coupling software to receive the intermediate frequency signal processed by the system platform so that it is convenient for further testing of the proposed coupling structure and the adaptive performance of the algorithm for highly dynamical conditions. In the experiment, the SINS information is generated by the digital emulator, and the information of SINS and GPS is generated according to the pre-established standard track, so as to ensure the information equivalence between them at the same time. The structure of the scheme is shown in
Figure 2.
The signal tracking experiment is compared based on the generated data with five algorithm combinations: The first method is the designed ultra-tightly coupled structure based on the double loops, and the proposed robust adaptive CKF algorithm is used to estimate and track parameters, which is abbreviated as DLB-RACKF. The structures of the second method and third method are the same as the first method. The difference is that the algorithms used in the filtering algorithms are CKF and UKF, respectively, similarly, the corresponding methods are abbreviated to DLB-CKF and DLB-UKF. To prove the superiority of the proposed algorithm, we have further completed the comparison between similar algorithms. The fourth and fifth algorithms are respectively selected from two typical improved CKFs mentioned in the introduction, namely the STCKF algorithm proposed by [
19] and the AHCKF algorithm proposed by [
26]. Similarly, the corresponding methods are abbreviated to DLB-STCKF and DLB-AHCKF.
5.2. Setup of the Experimental Conditions
Major parameter settings of flight path: the initial position is 102.0266 °E, 28.2460 °N, height: 50 km, and the initial velocity (in the geographical coordinate system) is 0 m/s in the east, 0 m/s in the north, and 0 m/s in the sky.
Select 60 s flight data of variable acceleration motion. The specific geographical coordinates are set as follows: the acceleration in the north direction and in the sky direction is constant between 0 s to 60 s, which is AN = 4 m/s2 in the north direction acceleration and AU = 0 m/s2 in the day direction acceleration, respectively. Eastward is variable acceleration motion, in which 0–20 s, 21–40 s, and 41–60 s are all constant acceleration motion, and eastward accelerations are AE = 100 m/s2, AE = 200 m/s2, and AE = −200 m/s2, respectively. 20–21 s and 40–41 s are the constant jerk motions, the numerical value of which are 100 m/s3 and −400 m/s3, respectively. As a result, the maximum absolute speed is about 6000 m/s, the maximum absolute acceleration is about 20 g, and the maximum jerk is about 40 g/s, which is fully in line with the highly dynamical characteristics.
The satellite signal simulator is used to generate the satellite signal which can be received by the highly dynamic moving carrier at the current time, and the carrier to noise ratio (C/N0) of all the satellite signals is set to 20 dB-Hz. There are currently 13 visible satellites in the sky. In addition, in order to verify the proposed method of adaptive adjusting performance, the measurement noise parameters are expanded by 20 times among 5–10 s, so that gross error of observations are simulated equivalently; on the other hand, the high maneuvering scenario set in this section can evaluate the adaptive performance of the algorithm when the dynamic model is imprecise, so it is not necessary to set the dynamic model error.
5.3. Experimental Results and Analysis
Since the performance of each tracking channel is the same and the satellite signal index is consistent, the results of one of the satellite channels is selected here in order to analyze the results.
The comparison results of DLB-RACKF, DLB-CKF, and DLB-UKF tracking errors are shown in
Figure 3 and
Figure 4.
Figure 5 and
Figure 6 show the comparison results of DLB-RACKF, DLB-AHCKF, and DLB-STCKF.
Table 1 shows the RMS statistics of the tracking error of the five methods in each period. The runtime performance of five filters is summarized in
Table 2.
Figure 3 and
Figure 4 are essentially based on the comparison results of tracking errors among three different algorithms with the same coupling structure. From
Figure 3 and
Figure 4 and
Table 1, we can see that the accuracy of DLB-CKF is slightly better than that of DLB-UKF in tracking error accuracy. The precision of DLB-RACKF is better than that of DLB-CKF and DLB-UKF. In the tracking stability of the algorithm, due to the lack of adaptive adjustment performance, both DLB-CKF and DLB-UKF show the condition of changing errors when the measurement noise increases and the vehicle moves with high maneuverability, such as when the 5th second measurement noise parameter continues to expand, the phase frequency and code tracking error of DLB-CKF and DLB-UKF immediately change. Eventually it will lead to a decline in the precision of navigation. When the noise of the 10th second measurement returns to normal, the PB-KF-UTC does not return to the initial accuracy immediately, but after about 1000 ms, it gradually converges to the normal precision range. In the 20th second, when the carrier performs variable acceleration motion in the range of about 10 g/s, the tracking error of PB-KF-UTC suddenly increases. This is due to the fact that the dynamic model no longer meets the basic situation of the actual motion, there is a large deviation, and when the variable acceleration motion is finished, the error of DLB-CKF and DLB-UKF is converged to the normal precision range again, with the process lasting for 2 s to 3 s. In the same way, in the 40th second, when the carrier performs variable acceleration motion at the range of about 10 g/s, the tracking error of DLB-CKF and DLB-UKF changes again, but because the maneuverability is too large, with the result that it is difficult for PB-KF-UTC to converge again when the motion of variable acceleration is finished, the phenomenon of divergence appears. However, in the whole tracking process, DLB-RACKF has maintained a good tracking accuracy and stability. The DLB-CKF converges gradually after the end of the variable acceleration motion, which ensures continuous tracking, which indicates that the stability of CKF is better than that of UKF. Compared with DLB-CKF and DLB-UKF, DLB-RACKF is relatively stable in the whole tracking process. When the measurement noise is abnormal and the dynamic model error increases due to the high maneuvering motion of the carrier, the DLB-RACKF can maintain better adaptive regulation performance. Its tracking stability is much better than that of DLB-CKF and DLB-UKF.
Figure 5 and
Figure 6 compare the tracking errors among the improved CKF algorithms with the same adaptive adjustment function. As shown in
Figure 5 and
Table 1, DLB-AHCKF and DLB-RACKF exhibit good adaptive performance when the kinematic model is accurate and the measurement error increases. Since the 5th second of measurement noise continues to increase, the DLB-AHCKF accurately estimates the noise changes after about 800 ms, and adjusts the algorithm to the normal range of precision. In the 20th and 40th seconds, when the carrier is highly maneuvering for two times, the dynamic model is no longer accurate, and the tracking error of DLB-AHCKF increases suddenly. The change rule is similar to that of DLB-CKF. After the first maneuver, the tracking error of the DLB-AHCKF gradually converges to the normal range. However, the tracking error of the second maneuver fails to converge normally, and the trend of the divergence of the algorithm appears. The result of this experiment is determined by the fundamental principles of the DLB-AHCKF algorithm, and it has its own specific application background. On the contrary, as shown in
Figure 6 and
Table 1, the DLB-CKF algorithm is essentially proposed for the motion state mutation of the carrier or the inaccurate model. Thus, when the highly dynamical variable acceleration takes place twice, the DLB-STCKF shows good tracking performance similar to that of the DLB-RACKF. However, when the system model is accurate and the measurement noise exists, the tracking error of DLB-STCKF is always greater than the normal mean for the duration of abnormal noise, which is similar to DLB-RACKF. When the noise returns to normal, there is a convergence delay of about 1300 ms. In general, compared with DLB-AHCKF and DLB-STCKF, the DLB-RACKF algorithm can deal with abnormal measurement noise and inaccurate motion models simultaneously. Within the normal range, the precision of the three algorithms is roughly equivalent, and the DLB-RACKF is slightly better than the other two algorithms.
As shown in
Table 2, the running time of the CKF algorithm is lower than that of the UKF under the same software operation conditions. The running time of the improved RACKF algorithm is about 1/2 of UKF. The computational efficiency of DLB-RACKF is better than that of DLB-AHCKF and DLB-STCKF, which is especially important for state estimation under highly dynamical conditions. Therefore, RACKF is an effective nonlinear state estimator.