Robust Adaptive Cubature Kalman Filter and Its Application to Ultra-Tightly Coupled SINS/GPS Navigation System

Abstract

In this paper, we propose a robust adaptive cubature Kalman filter (CKF) to deal with the problem of an inaccurately known system model and noise statistics. 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 dynamic disturbance error. Aiming at overcoming the abnormality error, we propose the robust estimation theory to adjust the CKF algorithm online. The proposed adaptive CKF can detect the degree of gross error and subsequently process it, so the influence produced by the abnormality error can be solved. The paper also studies a typical application system for the proposed method, which is the ultra-tightly coupled navigation system of a hypersonic vehicle. Highly dynamical scene experimental results show that the proposed method can effectively process errors aroused by the abnormality data and inaccurate model, and has better tracking performance than UKF and CKF tracking methods. Simultaneously, the proposed method is superior to the tracing method based on a single-modulating loop in the tracking performance. Thus, the stable and high-precision tracking for GPS satellite signals are preferably achieved and the applicability of the system is promoted under the circumstance of high dynamics and weak signals. The effectiveness of the proposed method is verified by a highly dynamical scene experiment.

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:

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

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

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

2. CKF Algorithm

2.1. Bayesian Estimation Rule

In this paper, we consider the nonlinear dynamic system, whose state space model is defined by the difference equation in discrete time [27]:

$$\begin{array}{l}{\mathit{x}}_k=f({\mathit{x}}_{k-1},{\mathit{u}}_{k-1})+{\mathit{v}}_{k-1}\\ z_k=h({\mathit{x}}_k,{\mathit{u}}_k)+{\mathit{w}}_k\end{array}$$ (1)

where ${\mathit{x}}_k$ and ${\mathit{z}}_k$ are the n-dimensional state and the m-dimensional measurement of the dynamic system, respectively; ${\mathit{u}}_k$ is the n-dimensional control input; $\left\{{\mathit{v}}_{k-1}\right\}$ is a n-dimensional system noise sequence with zero mean and covariance ${\mathit{Q}}_{k-1}$; $\left\{{\mathit{w}}_k\right\}$ is an m-dimensional measurement noise sequence with zero mean and covariance $R_k$; $f(•)$ and $h(•)$ are the nonlinear state function and measurement function of the system, respectively.

When ${\mathit{v}}_{k-1}$ and ${\mathit{w}}_k$ are independent, the posterior density of the state ${\mathit{x}}_k$ of the nonlinear dynamic model (1) can be obtained based on the Bayesian estimation rule as follows:

• (1)
  Time update, calculating the predictive density:

  $$p({\mathit{x}}_k|{\mathit{D}}_{k-1})={\displaystyle \int p({\mathit{x}}_k,{\mathit{x}}_{k-1}|{\mathit{D}}_{k-1})}d{\mathit{x}}_{k-1}={\displaystyle \int p({\mathit{x}}_{k-1}|{\mathit{D}}_{k-1})}\times p({\mathit{x}}_k|{\mathit{x}}_{k-1},{\mathit{u}}_{k-1}),d{\mathit{x}}_{k-1}$$ (2)

  where ${\mathit{D}}_{k-1}={\left\{u_i,z_i\right\}}_{i=1}^{(k-1)}$ describes the history data up to time $(k-1)$; $p({\mathit{x}}_{k-1}|{\mathit{D}}_{k-1})$ is the old posterior density at time $(k-1)$; the state transition density $p({\mathit{x}}_k|{\mathit{x}}_{k-1},{\mathit{u}}_{k-1})$ is calculated by Equation (1).
• (2)Measurement update
  The posterior density of the current state can be obtained by Bayesian rule:

  $$p({\mathit{x}}_k|{\mathit{D}}_k)=\frac{1}{c_k}p({\mathit{x}}_k|{\mathit{D}}_{k-1},u_{k-1})p({\mathit{z}}_k|{\mathit{x}}_k,{\mathit{u}}_k)$$ (3)

  $$c_k={\displaystyle \int p({\mathit{x}}_k|{\mathit{D}}_{k-1},{\mathit{u}}_k)}p({\mathit{z}}_k|{\mathit{x}}_k,{\mathit{u}}_k)d{\mathit{x}}_k$$

  where $c_k$ is a constant. However, for nonlinear/non-Gaussian cases, the computational complexity of $c_k$ is typically prohibitive. A computationally feasible approximation is provided by PF [28]. Since the denominator is a normalizing constant, Equation (3) can be regarded as an unnormalized density, the fact does not affect the following derivation.

If the $p({\mathit{x}}_k|{\mathit{D}}_k)$ can be calculated exactly, we obtain the state estimation and covariance matrix of $x_k$ as follows:

$${\widehat{\mathit{x}}}_k={\displaystyle \int {\mathit{x}}_k}p({\mathit{x}}_k|{\mathit{D}}_k)d{x}_k$$ (4)

$${\mathit{P}}_{k+1}={\displaystyle \int ({\mathit{x}}_k-{\widehat{\mathit{x}}}_k)}{({\mathit{x}}_k-{\widehat{\mathit{x}}}_k)}^{\mathrm{T}}p({\mathit{x}}_k|D_k)d{\mathit{x}}_k$$ (5)

In fact, the analytic solutions for $p({\mathit{x}}_k|{\mathit{D}}_k)$ is difficult to be obtained in the nonlinear system. Therefore, we need to approximate the state estimation and covariance matrix.

2.2. CKF Algorithm Based on a Third-Order Spherical-Radial Cubature Rule

Here, we consider the Gaussian noise, and it is widely used in Bayesian filter theory. The key approximation is $p({\mathit{x}}_k|{\mathit{D}}_{k-1})$ and $p({\mathit{z}}_k|{\mathit{D}}_k)$ are both Gaussian in the Bayesian filter, which deduces a Gaussian $p({\mathit{x}}_k|{\mathit{D}}_k)$.

Suppose an n-dimensional random vector ${\mathit{x}}_k$ obeys the Gaussian distribution $N({\mathit{x}}_k;{\widehat{\mathit{x}}}_k,{\mathit{P}}_x)$, we can obtain:

$${\widehat{\mathit{x}}}_{k|k-1}=E\left[f({\mathit{x}}_{k-1},{\mathit{u}}_{k-1})|{\mathit{D}}_{k-1}\right]={\displaystyle \int f({\mathit{x}}_{k-1},{\mathit{u}}_{k-1})p({\mathit{x}}_{k-1}|{\mathit{D}}_{k-1})}d{\mathit{x}}_{k-1}={\displaystyle \int f({\mathit{x}}_{k-1},{\mathit{u}}_{k-1})N({\mathit{x}}_{k-1};{\widehat{\mathit{x}}}_{k-1|k-1},{\mathit{P}}_{k-1|k-1})}d{\mathit{x}}_{k-1}$$ (6)

The error covariance is:

$$\begin{array}{l}{\mathit{P}}_{k|k-1}=E\left[({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k-1}){({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k-1})}^{\mathrm{T}}|{\mathit{z}}_{1:k-1}\right]\\ ={\displaystyle \int f({\mathit{x}}_{k-1},{\mathit{u}}_{k-1})f^{\mathrm{T}}({\mathit{x}}_{k-1},{\mathit{u}}_{k-1})}\times N({\mathit{x}}_{k-1};{\widehat{\mathit{x}}}_{k-1|k-1},P_{k-1|k-1})d{\mathit{x}}_{k-1}-{\widehat{\mathit{x}}}_{k|k-1}{\widehat{\mathit{x}}}^{\mathrm{T}}{}_{k|k-1}+{\mathit{Q}}_{k-1}\end{array}$$ (7)

The predicted measurement can be obtained as follows:

$${\widehat{\mathit{z}}}_{k|k-1}={\displaystyle \int h({\mathit{x}}_k,{\mathit{u}}_k)N({\mathit{x}}_k;{\widehat{\mathit{x}}}_{k|k-1},{\mathit{P}}_{k|k-1})d{\mathit{x}}_{k-1}}$$ (8)

The associated covariance and the cross-covariance are:

$${\mathit{P}}_{zz,k|k-1}={\displaystyle \int h({\mathit{x}}_k,{\mathit{u}}_k)h^{\mathrm{T}}({\mathit{x}}_k,{\mathit{u}}_k)}\times N({\mathit{x}}_k;{\widehat{\mathit{x}}}_{k|k-1},{\mathit{P}}_{k|k-1})d{\mathit{x}}_k-{\widehat{\mathit{z}}}_{k|k-1}{\widehat{\mathit{z}}}^{\mathrm{T}}{}_{k|k-1}+{\mathit{R}}_{k-1}$$ (9)

$${\mathit{P}}_{xz,k|k-1}={\displaystyle \int {\mathit{x}}_k{h}^{\mathrm{T}}({\mathit{x}}_k,{\mathit{u}}_k)}\times N({\mathit{x}}_k;{\widehat{\mathit{x}}}_{k|k-1},{\mathit{P}}_{k|k-1})d{\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k-1}{\widehat{\mathit{z}}}_{k|k-1}^{\mathrm{T}}$$ (10)

We can see that Equations (6) to (10) have the unified integral form as:

$$\mathit{I}(g)={\displaystyle \int \mathit{g}(\mathit{x})N(\mathit{x};\widehat{\mathit{x}},{\mathit{P}}_x)d\mathit{x}}$$ (11)

It is difficult to obtain the analytic of the above multivariate integral, and the approximate calculation is needed to be adopted.

The CKF algorithm is derived from the idea of approximate calculation. CKF uses the third-order spherical-radial cubature rule to approximately compute the posterior mean and the covariance matrix for the nonlinear Gaussian system. The key step is to select a series of cubature points. Then we substitute these cubature points into a nonlinear function, calculate, and obtain some new cubature points which are used to approximately compute the integral values through the weighting method. The main steps of CKF are summarized as follows.

The CKF algorithm includes two processes: the time update and the measurement update [29]. The time updating is as follows:

• (1)
  The posteriori probability distribution of a given k − 1 moment is assumed to be:

  $$p({\mathit{x}}_{k-1}|{\left\{{\mathit{u}}_i,{\mathit{z}}_i\right\}}_{i=1}^{k-1})=\Im ({\mathit{x}}_{k-1};{\widehat{\mathit{x}}}_{k-1|k-1},{\mathit{P}}_{k-1|k-1})$$
  Let:

  $${\mathit{P}}_{k-1|k-1}={\mathit{S}}_{k-1|k-1}{\mathit{S}}_{k-1|k-1}^{\mathrm{T}}$$ (12)
• (2)
  Calculating state volume points:

  $${\mathit{X}}_{j,k-1|k-1}^{\hslash }={\mathit{S}}_{k-1|k-1}{\xi }_j+{\widehat{\mathit{x}}}_{k-1|k-1}$$ (13)
• (3)
  Volume points transformed based on the state equation:

  $${\mathit{X}}_{j,k-1|k-1}^{*}=f({\mathit{X}}_{j,k-1|k-1}^{\hslash },{\mathit{u}}_{k-1})$$ (14)
• (4)
  Weighted mean to compute the state quantity prediction value:

  $${\widehat{\mathit{x}}}_{k|k-1}=\frac{1}{2n}{\displaystyle \sum _{j=1}^{2n}{\mathit{X}}_{j,k-1|k-1}^{*}}$$ (15)
• (5)
  Calculate the covariance matrix of state prediction:

  $${\mathit{P}}_{k|k-1}=\frac{1}{2n}{\displaystyle \sum _{j=1}^{2n}{\mathit{X}}_{j,k-1|k-1}^{*}{\mathit{X}}_{j,k-1|k-1}^{*\mathrm{T}}}-{\widehat{\mathit{x}}}_{k|k-1}{\widehat{\mathit{x}}}^{\mathrm{T}}{}_{k|k-1}+{\mathit{Q}}_{k-1}$$ (16)

Measurement updating:

• (1)
  Matrix factorization:

  $${\mathit{P}}_{k|k-1}={\mathit{S}}_{k|k-1}{\mathit{S}}_{k|k-1}^{\mathrm{T}}$$ (17)
• (2)
  Calculate volume points:

  $${\mathit{X}}_{j,k|k-1}^{\hslash }={\mathit{S}}_{k|k-1}{\xi }_j+{\widehat{\mathit{x}}}_{k|k-1}$$ (18)
• (3)
  Transform volume points based on the measurement equation:

  $${\mathit{Z}}_{j,k-1|k-1}^{*}=h({\mathit{X}}_{j,k|k-1})$$ (19)
• (4)
  Forecast the measurement value:

  $${\widehat{\mathit{z}}}_{k|k-1}=\frac{1}{2n}{\displaystyle \sum _{j=1}^{2n}{\mathit{Z}}_{j,k|k-1}^{*}}$$ (20)
• (5)
  Calculate the prediction residual covariance matrix:

  $${\mathit{P}}_{zz,k|k-1}={\displaystyle \sum _{j=1}^{2n}{w}_j{\mathit{Z}}_{j,k|k-1}^{*}{\mathit{Z}}_{j,k|k-1}^{*\mathrm{T}}}-{\widehat{\mathit{z}}}_{k|k-1}{\widehat{\mathit{z}}}_{k|k-1}^{\mathrm{T}}+{\mathit{R}}_k$$ (21)
• (6)
  Calculate the cross covariance matrix:

  $${\mathit{P}}_{xz,k|k-1}={\displaystyle \sum _{j=1}^{2n}{w}_j{\mathit{X}}_{j,k|k-1}^{\hslash }{\mathit{Z}}_{j,k|k-1}^{*\mathrm{T}}}-{\widehat{\mathit{x}}}_{k|k-1}{\widehat{\mathit{z}}}_{k|k-1}^{\mathrm{T}}$$ (22)
• (7)
  Calculate the Kalman filter gain:

  $${\mathit{W}}_k={\mathit{P}}_{xz,k|k-1}{\mathit{P}}_{zz,k|k-1}^{-1}$$ (23)
• (8)
  Update the state:

  $${\widehat{\mathit{x}}}_{k|k}={\widehat{\mathit{x}}}_{k|k-1}+{\mathit{W}}_k({\mathit{z}}_k-{\widehat{\mathit{z}}}_{k|k-1})$$ (24)
• (9)
  Update the state covariance matrix:

  $${\mathit{P}}_{k|k}={\mathit{P}}_{k|k-1}-{\mathit{W}}_k{\mathit{P}}_{zz,k|k-1}{\mathit{W}}_k^{\mathrm{T}}$$ (25)

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:

$${\overline{\mathit{P}}}_{zz,k|k-1}={\displaystyle \sum _{j=1}^{2n}{w}_j{\mathit{Z}}_{j,k|k-1}^{*}{\mathit{Z}}_{j,k|k-1}^{*\mathrm{T}}}-{\widehat{\mathit{z}}}_{k|k-1}{\widehat{\mathit{z}}}_{k|k-1}^{\mathrm{T}}+{\overline{\mathit{R}}}_k$$ (26)

where ${\overline{\mathit{R}}}_k$ is an equivalent measurement noise variance matrix corresponding to ${\mathit{R}}_k$, and it can be obtained by obtaining an equivalent weight matrix $\overline{\mathit{P}}$ in an anti-difference M estimation method. That is:

$${\overline{\mathit{R}}}_k={\overline{\mathit{P}}}^{-1}$$ (27)

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 $\overline{\mathit{P}}$ matrices are positive. The expression of equivalent is as follows:

$${\overline{p}}_{t_{ii}}=\{\begin{array}{c}\begin{array}{cc}\begin{array}{c}\begin{array}{cc}1/{\sigma }_{ii}& \end{array}\end{array}& \left|r_i/{\sigma }_{r_i}\right|=\left|r_i^{\prime }\right|\le c\end{array}\\ \begin{array}{cc}\begin{array}{c}\begin{array}{cc}c/{\sigma }_{ii}\left|r_i^{\prime }\right|& \end{array}\end{array}& \left|r_i^{\prime }\right|>c\end{array}\end{array}$$ (28)

$${\overline{p}}_{t_{ij}}=\{\begin{array}{c}\begin{array}{cc}\begin{array}{c}\begin{array}{cc}1/{\sigma }_{ij}& \end{array}\end{array}& \left|r_i^{\prime }\right|\le c \mathrm{and} \left|r_j^{\prime }\right|\le c\end{array}\\ \frac{c}{{\sigma }_{ij}\max \left\{\left|r_i^{\prime }\right|,\left|r_j^{\prime }\right|\right\}}\begin{array}{cc}\begin{array}{c}\begin{array}{cc}& \end{array}\end{array}& \left|r_i^{\prime }\right|>c \mathrm{or} \left|r_j^{\prime }\right|>c\end{array}\end{array}$$ (29)

where ${\overline{p}}_{t_{ii}}$ and ${\overline{p}}_{t_{ij}}$ are diagonal elements and non-diagonal elements of $\overline{\mathit{P}}$, respectively; ${\sigma }_{ii}$ and ${\sigma }_{ij}$ are diagonal elements and non-diagonal elements of the original ${\mathit{R}}_k$ array; $r_i$ is a residual component corresponding to the observation quantity $Z_i$, $r_i^{\prime }$ is a corresponding standard residual component, and ${\sigma }_{r_i}$ is a mean square error of $r_i$; and c is a given constant, usually taken from 1.3 to 2.0.

The above operation involves that ${\sigma }_{r_i}$ and $r_i$ are deterministic. In practice, because the covariance matrix of the measurement residuals is obtained from Equation (21), that is, the variable quantity ${\mathit{P}}_{zz,k|k-1}$ before being modified, the expressions ${\sigma }_{r_i}$ and $r_i$ are:

$${\sigma }_{r_i}={({\mathit{P}}_{zz,k|k-1})}_{ii}$$ (30)

$$r_i={({\mathit{z}}_k-{{\displaystyle \widehat{\mathit{z}}}}_{k|k-1})}_i$$ (31)

Then, ${\overline{\mathit{P}}}_{zz,k|k-1}$ is substituted for ${\mathit{P}}_{zz,k|k-1}$ 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 ${\mathit{R}}_k$, 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 ${\vartheta }_k$ modify ${\mathit{P}}_{k|k-1}$, obtaining:

$${\stackrel{⌢}{\mathit{P}}}_{k|k-1}={\vartheta }_k^{-1}{\mathit{P}}_{k|k-1}={\vartheta }_k^{-1}\left[\frac{1}{2n}{\displaystyle \sum _{j=1}^{2n}{\mathit{X}}_{j,k-1|k-1}^{*}{\mathit{X}}_{j,k-1|k-1}^{*\mathrm{T}}}-{\widehat{\mathit{x}}}_{k|k-1}{\widehat{\mathit{x}}}_{k|k-1}^{\mathrm{T}}+{\mathit{Q}}_{k-1}\right]$$ (32)

where the optimal value of ${\vartheta }_k$ may be obtained from the residual covariance matrix after predicting and updating, where ${\vartheta }_k$ is given by the following theorem.

Theorem 1.

If ${\widehat{\mathit{P}}}_{zz,k|k-1}$ is the residual covariance matrix estimated after the introduction of new measurement information, it is defined as the updating residual covariance matrix; ${\tilde{\mathit{P}}}_{zz,k|k-1}$ is a theoretical residual covariance matrix obtained by adaptive filtering, and ${\mathit{P}}_{zz,k|k-1}$ 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:

$${\tilde{\mathit{P}}}_{zz,k|k-1}={\widehat{\mathit{P}}}_{zz,k|k-1}$$ (33)

Then, the optimal adaptive factor is:

$${\vartheta }_k=\frac{tr({\mathit{P}}_{zz,k|k-1}-{\overline{\mathit{R}}}_k)}{tr({\widehat{\mathit{P}}}_{zz,k|k-1}-{\overline{\mathit{R}}}_k)}$$ (34)

Proof of Theorem 1.

Let ${\tilde{\mathit{x}}}_{k|k-1}$ a step-by-step prediction error, then:

$${\widehat{\mathit{x}}}_{k|k-1}={\mathit{x}}_k-{\tilde{\mathit{x}}}_{k|k-1}$$ (35)

Thus, the filtered residual is:

$${\mathit{r}}_k={\mathit{z}}_k-h({\widehat{\mathit{x}}}_{k|k-1})={\mathit{z}}_k-h({\mathit{x}}_k-{\tilde{\mathit{x}}}_{k|k-1})$$ (36)

A first-order Taylor expansion of the second term on the right-hand side of the equation:

$$h({\mathit{x}}_k-{\tilde{\mathit{x}}}_{k|k-1})\approx h({\mathit{x}}_k)-\frac{\partial h}{\partial \mathit{x}}|{}_{x={\widehat{x}}_{k|k-1}}\times {\tilde{\mathit{x}}}_{k|k-1}=h({\mathit{x}}_k)-{\mathit{D}}_k\times {\tilde{\mathit{x}}}_{k|k-1}$$ (37)

Bringing the above equation into the following equation:

$${\mathit{r}}_k={\mathit{z}}_k-h({\mathit{x}}_k)+{\mathit{D}}_k{\tilde{\mathit{x}}}_{k|k-1}={\mathit{v}}_k+{\mathit{D}}_k{\tilde{\mathit{x}}}_{k|k-1}$$ (38)

Then the residual covariance matrix A is:

$${\mathit{P}}_{zz,k|k-1}=E\left\{{\mathit{r}}_k{\mathit{r}}_k^{\mathrm{T}}\right\}=E\left\{\left({\mathit{v}}_k+{\mathit{D}}_k{\tilde{\mathit{x}}}_{k|k-1}\right){\left({\mathit{v}}_k+{\mathit{D}}_k{\tilde{\mathit{x}}}_{k|k-1}\right)}^{\mathrm{T}}\right\}=E\left({\mathit{D}}_k{\tilde{\mathit{x}}}_{k|k-1}{\tilde{\mathit{x}}}_{k|k-1}^{\mathrm{T}}{\mathit{D}}_k^{\mathrm{T}}\right)+E\left({\mathit{v}}_k{\mathit{v}}_k^{\mathrm{T}}\right)={\mathit{D}}_k{\mathit{P}}_{k|k-1}{\mathit{D}}_k^{\mathrm{T}}+{\overline{R}}_k$$ (39)

Note the equation using the measurement noise covariance matrix after being modified by robust CKF. Furthermore, in adaptive filtering, ${\mathit{P}}_{k|k-1}$ is modified to ${\vartheta }_k^{-1}{\mathit{P}}_{k|k-1}$, and then the theoretical residual covariance matrix obtained by adaptive filtering is:

$${\tilde{\mathit{P}}}_{zz,k|k-1}={\vartheta }_k^{-1}{\mathit{D}}_k{\mathit{P}}_{k|k-1}{\mathit{D}}_k^{\mathrm{T}}+{\mathit{R}}_k$$ (40)

From the equation:

$${\widehat{\mathit{P}}}_{zz,k|k-1}={\tilde{\mathit{P}}}_{zz,k|k-1}={\vartheta }_k^{-1}{\mathit{D}}_k{\mathit{P}}_{k|k-1}{\mathit{D}}_k^{\mathrm{T}}+{\mathit{R}}_k$$ (41)

Multiply the two sides of the upper form by A and move the item to obtain

$${\vartheta }_k({\widehat{\mathit{P}}}_{zz,k|k-1}-{\mathit{R}}_k)={\mathit{D}}_k{\mathit{P}}_{k|k-1}{\mathit{D}}_k^{\mathrm{T}}$$ (42)

Further derivation:

$${\vartheta }_k({\widehat{\mathit{P}}}_{zz,k|k-1}-{\mathit{R}}_k)={\mathit{D}}_k{\mathit{P}}_{k|k-1}{\mathit{D}}_k^{\mathrm{T}}={\mathit{P}}_{zz,k|k-1}-{\overline{\mathit{R}}}_k$$ (43)

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:

$${\vartheta }_k=\{\begin{array}{c}1\begin{array}{cc}\begin{array}{cc}\begin{array}{cc}\begin{array}{cc}& \end{array}& \end{array}& \end{array}& \end{array}\begin{array}{cc}& tr({\widehat{\mathit{P}}}_{zz,k|k-1})\le tr({\mathit{P}}_{zz,k|k-1})\end{array}\\ \frac{tr({\mathit{P}}_{zz,k|k-1}-{\overline{\mathit{R}}}_k)}{tr({\widehat{\mathit{P}}}_{zz,k|k-1}-{\overline{\mathit{R}}}_k)}\begin{array}{cc}& tr({\widehat{\mathit{P}}}_{zz,k|k-1})>tr({\mathit{P}}_{zz,k|k-1})\end{array}\end{array}$$ (44)

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.

$${\vartheta }_k\approx \{\begin{array}{c}1\begin{array}{cc}\begin{array}{cc}\begin{array}{cc}\begin{array}{cc}& \end{array}& \end{array}& \end{array}& \end{array}\begin{array}{cc}& tr({\widehat{\mathit{P}}}_{zz,k|k-1})\le tr({\mathit{P}}_{zz,k|k-1})\end{array}\\ \frac{tr({\mathit{P}}_{zz,k|k-1})}{tr({\widehat{\mathit{P}}}_{zz,k|k-1})}\begin{array}{cc}& tr({\widehat{\mathit{P}}}_{zz,k|k-1})>tr({\mathit{P}}_{zz,k|k-1})\end{array}\end{array}$$ (45)

${\widehat{\mathit{P}}}_{zz,k|k-1}$ can be obtained by the residual error estimation of the observations. The sliding window method is adopted, namely:

$$tr({\widehat{\mathit{P}}}_{zz,k|k-1})=tr({\mathit{r}}_k{\mathit{r}}_k^{\mathrm{T}})={\mathit{r}}_k^{\mathrm{T}}{\mathit{r}}_k={\displaystyle \sum _{i=1}^m{({\mathit{z}}_k-{\widehat{\mathit{z}}}_{k|k-1})}_i}$$ (46)

In order to further clarify the robust adaptive CKF algorithm, the following steps are given:

• Step 1.
  Set the initial condition.

  $${\widehat{\mathit{x}}}_0=E({\mathit{x}}_0)\begin{array}{cc}& \end{array}{\mathit{P}}_0=E{\left[({\mathit{x}}_0-{\widehat{\mathit{x}}}_0)({\mathit{x}}_0-{\widehat{\mathit{x}}}_0)\right]}^{\mathrm{T}}$$ (47)
• 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:

  $${\mathit{X}}_{j,k|k-1}^{\hslash }=\sqrt{{\vartheta }_k^{-1}}{\mathit{S}}_{k|k-1}{\xi }_j+{\widehat{\mathit{x}}}_{k|k-1}$$ (48)
• Step 4.

  Calculate the predicted values and the residual covariance matrix.

  Computational observational prediction ${\widehat{\mathit{z}}}_{k|k-1}$ and predictive residual covariance matrix ${\mathit{P}}_{zz,k|k-1}$ 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.

4. SINS/GPS Ultra-Tightly Coupling Structure Based on a Double Loop

4.1. Design of SINS/GPS Ultra-Tightly Coupling Structure Based on a Double Loop

In this section, we present a typical application of the proposed method, that is, the application to an ultra-tightly coupled SINS/GPS navigation system of a hypersonic vehicle.

In the literature [34], the federated ultra-tightly coupled structure is divided into three types. In essence, three kinds of structures are implemented only in signal tracking error estimation, which use three different ways to estimate code and carrier correlation error. Other parts are essentially identical. The realization of satellite signal tracking uses a closed loop mode of an external loop, which is a typical vector tracking structure, and this kind of structure enhances the mutual assistance of satellite signal tracking between channels. However, at the same time, there is a problem that when the number of unstable channels is large, if the carrier to noise ratio of multiple satellite signals is reduced due to strong interference, the unstable channel dominates in the signal tracking process. The accuracy of the integrated navigation filter will be reduced, and using this single information to adjust the local signal generator of all channels, the tracking performance of a few normal channels will be affected, and the performance of integrated navigation system will further deteriorate. Therefore, the design of this kind of single regulating loop has a certain “inverse” effect on the tracking process of the satellite signal under special circumstances. In addition, the GPS tracking loop of this kind of structure is an open loop [35,36]. If the integrated navigation filter, for some reason, such as failure, cannot work normally, the entire system will be paralyzed, and the reliability of the system will reduce.

In order to reduce the influence of the “inverse” action mentioned above, we take the advantages of the conventional scalar tracking structure and design the ultra-tightly coupled structure based on the double loop adjustment method shown in Figure 1. One path is aimed at the internal loop of each channel, which is like the channel tracking of the conventional receiver. We use the method of mutual auxiliary tracking between the code and carrier closed loop, and the tracking error that is estimated by the pre-filter of each channel to adjust the code/carrier NCO in real-time, in which the pre-filter adopts a non-linear filtering model. The other path is the external loop described above, which uses the position and velocity information output by the corrected SINS and per the ephemeris information to update and correct, so as to adjust the code phase and the carrier frequency information of the NCO. The structure design of the double loop determines the code/carrier NCO adjustment in two ways, one of which achieves a single internal loop adjusting mode through the pre-filter, and the other way is a combined adjustment mode of external loop supporting internal loop with SINS participating. The strategy of adjustment is to read the corrected inertial navigation information in the program design. If the information meets the accuracy level requirement of NCO adjustment, the combination adjustment method will be adopted. If not, it will depend on the internal loop to adjust. This strategy can reduce the influence of the “inverse” function existing in the common vector tracking method and improve the reliability of the integrated navigation system.

Figure 1.

SINS/GPS federated ultra-tightly coupled structure based on double loop.

4.2. Pre-Filter Tracking Modeling

The above pointed out that the pre-filter tracking algorithm is one of the key points of ultra-tightly coupled systems. Thus, this section gives the nonlinear pre-filter tracking loop model of the satellite signal.

• (1)Pre-filter state equation establishment
  State variables take:

  $$\mathit{X}={[\begin{array}{ccccc}A& \delta \tau & \delta \varphi & \delta f& \delta a\end{array}]}^{\mathrm{T}}$$ (49)

  where $A$ denotes the normalized amplitude of the received signal, the code phase error; $\delta f$ the carrier wave frequency error, and $\delta a$ the carrier phase acceleration error. The equation of state of the system is:

  $$\begin{array}{ll}\dot{\mathit{X}}& ={[\begin{array}{ccccc}\dot{A}& \delta \dot{\tau }& \delta {\dot{\varphi }}_0& \delta {\dot{f}}_0& \delta {\dot{a}}_0\end{array}]}^{\mathrm{T}}=\mathit{F}\mathit{X}+\mathit{G}\mathit{w}\\ & =\left(\begin{array}{ccccc}0& 0& 0& 0& 0\\ 0& 0& 0& \beta & 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0\end{array}\right)\left(\begin{array}{c}A\\ \delta \tau \\ \delta {\varphi }_0\\ \delta f_0\\ \delta a_0\end{array}\right)+\left(\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& \beta & 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\end{array}\right)\left(\begin{array}{c}{w}_A\\ w_{\tau }\\ w_{{\varphi }_0}\\ w_{f_0}\\ w_{a_0}\end{array}\right)\end{array}$$ (50)

  where $\beta$ is the transformation coefficient that converts the radian to the chip, and the components of $\mathit{w}$ are the amount of the process noise corresponding to the state variables.
• (2)Pre-filter measurement equation establishment
  The system’s observation is obtained by in-phase signal I and quadrature signal Q outputting by the correlator. I and Q include the early (E) signal, the present (P) signal and the later (L) signal. The expression of the measurement equation is as follows:

  $$\left[\begin{array}{l}{I}_P\\ I_E\\ I_L\\ Q_P\\ Q_E\\ Q_L\end{array}\right]=\left[\begin{array}{c}ATD(t_k-\tau )R(\delta \tau )\sin c(\delta fT/2)\cos (\delta fT/2+\delta \varphi )\\ ATD(t_k-\tau )R(\delta \tau -0.5\Delta )\sin c(\delta fT/2)\cos (\delta fT/2+\delta \varphi )\\ ATD(t_k-\tau )R(\delta \tau +0.5\Delta )\sin c(\delta fT/2)\cos (\delta fT/2+\delta \varphi )\\ ATD(t_k-\tau )R(\delta \tau )\sin c(\delta fT/2)\sin (\delta fT/2+\delta \varphi )\\ ATD(t_k-\tau )R(\delta \tau -0.5\Delta )\sin c(\delta fT/2)\sin (\delta fT/2+\delta \varphi )\\ ATD(t_k-\tau )R(\delta \tau +0.5\Delta )\sin c(\delta fT/2)\sin (\delta fT/2+\delta \varphi )\end{array}\right]+\left[\begin{array}{c}{n}_{I_P}\\ n_{I_E}\\ n_{I_L}\\ n_{Q_P}\\ n_{Q_E}\\ n_{Q_L}\end{array}\right]$$ (51)

  where T is the cumulative sum of time; $D(t_k-\tau )$ is the navigation message modulation; $\Delta$ is the difference number of code between the early and the later codes, $R(*)$ respectively correspond with the results of the received C/A code sequence and the locally-generated code in E code, P code, and L code after being relative to each other.

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.

Figure 2.

Structure of the highly-dynamical scene experiment scheme.

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/s² in the north direction acceleration and AU = 0 m/s² 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/s², AE = 200 m/s², and AE = −200 m/s², respectively. 20–21 s and 40–41 s are the constant jerk motions, the numerical value of which are 100 m/s³ and −400 m/s³, 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/N₀) 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.

Comparison of the Doppler frequency tracking error among DLB-UKF, DLB-CKF, and DLB-RACKF.

Figure 4.

Comparison of the code phase tracking error among DLB-UKF, DLB-CKF, and DLB-RACKF.

Figure 5.

Comparison of code phase tracking error among DLB-AHCKF and DLB-RACKF.

Figure 6.

Comparison of code phase tracking error among DLB-STCKF and DLB-RACKF.

Table 1.

Satellite Doppler frequency shift and code phase tracking error RMS.

Satellite Signal Tracking Error Terms Corresponded to Four Methods		0–20 s	20–40 s	40–60 s
Doppler shift errors (Hz)	DLB-UKF	0.9153	1.0574
	DLB-CKF	0.5345	0.7375	1.5053
	DLB-AHCKF	0.2483	0.6547	1.3267
	DLB-STCKF	0.4755	0.3003	0.5004
	DLB-RACKF	0.1125	0.1220	0.2455
Code phase errors (chips)	DLB-UKF	0.0128	0.0277	0.2413
	DLB-CKF	0.0123	0.0245	0.0474
	DLB-RACKF	0.0091	0.0060	0.0077

Table 2.

Runtime of five tracking algorithms.

Algorithms	DLB-UKF	DLB-CKF	DLB-AHCKF	DLB-STCKF	DLB-RACKF
Runtime (s)	1.832600	0.7332	1.2109	1.2973	0.9368

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.

6. Conclusions

The design of an ultra-tight coupling structure realized by a single regulating loop has a certain “inverse” effect on tracking satellite signals under special circumstances. In this paper, a cascade super-tight coupling structure based on double loop adjustment was designed, which reduces the “inverse” effect of the common vector tracking method and improves the reliability of the integrated navigation system.

At the same time, in order to improve tracking accuracy, a new nonlinear filtering algorithm called CKF was studied. The robust M estimation in robust estimation theory is used to improve the CKF, which solves the influence of the abnormal error of the observed quantity. On the other hand, an adaptive adjustment factor was used to modify the algorithm, which can effectively deal with the influence of the dynamic disturbance error. The simulation results of highly dynamical scenes based on the satellite signal simulator show that the proposed structure and the proposed robust adaptive CKF algorithm effectively improve the tracking accuracy and tracking stability of the GPS satellite signal so that the navigation system has better dynamic adaptability.

Author Contributions

Data curation, X.Z. and X.Y.; Investigation, X.Z.; Methodology, X.Z.; Project administration, J.L.; Software, S.J.; Validation, X.Y.; Writing—original draft, X.Z.; Writing—review & editing, X.Z. and J.L.; Funding Acquisition, X.Z. and X.Y.

Funding

The work was partially supported by National Natural Science Foundation of China (No. 61503393 and No. 11602296.).

Conflicts of Interest

The authors declare no conflict of interest. The funders had no role in the design of the study; in the collection, analyses, or interpretation of data; in the writing of the manuscript, and in the decision to publish the results.