Multi-Sensor Optimal Data Fusion Based on the Adaptive Fading Unscented Kalman Filter

Abstract

This paper presents a new optimal data fusion methodology based on the adaptive fading unscented Kalman filter for multi-sensor nonlinear stochastic systems. This methodology has a two-level fusion structure: at the bottom level, an adaptive fading unscented Kalman filter based on the Mahalanobis distance is developed and serves as local filters to improve the adaptability and robustness of local state estimations against process-modeling error; at the top level, an unscented transformation-based multi-sensor optimal data fusion for the case of N local filters is established according to the principle of linear minimum variance to calculate globally optimal state estimation by fusion of local estimations. The proposed methodology effectively refrains from the influence of process-modeling error on the fusion solution, leading to improved adaptability and robustness of data fusion for multi-sensor nonlinear stochastic systems. It also achieves globally optimal fusion results based on the principle of linear minimum variance. Simulation and experimental results demonstrate the efficacy of the proposed methodology for INS/GNSS/CNS (inertial navigation system/global navigation satellite system/celestial navigation system) integrated navigation.

1. Introduction

With the rapid development of electronics technologies, various sensors have been developed and applied to many engineering fields such as integrated navigation, target tracking, signal processing, and networked communications [1,2,3,4]. Since the use of multiple sensors provides more accurate and reliable information than that of a single sensor alone, multi-sensor data fusion has received considerable attention in recent years. The basic idea of multi-sensor data fusion is to combine the data obtained from multiple sensors and further associate them with database information to achieve improved estimation accuracy compared to the use of a single sensor [5,6].

The existing studies on multi-sensor data fusion can be divided into two categories [4,6]. One is the centralized fusion method, which can achieve a globally optimal state estimation by directly combining local measurement models to form an augmented measurement model [7,8]. This method has minimal information loss. However, it causes large computation and communication burdens in the fusion center due to high-dimension computations and large data memories. Further, it has a poor robustness and reliability when one sensor is faulty [1,9]. The other is the decentralized fusion method, which can yield globally optimal or suboptimal state estimations according to certain criteria [9,10]. This method has small computation and communication burdens in the fusion center due to the decentralized structure. Furthermore, the decentralized structure also enables easy fault detection and isolation. Due to its superiority over the centralized fusion method, the decentralized fusion method has been extensively used in engineering fields.

The federated Kalman filter (FKF) is a typical example of the decentralized fusion method. It employs the principle of information sharing for local and global filters, and eliminates the correlation between local estimations by using the technique of covariance upper bound [11,12]. However, FKF is only suitable for multi-sensor linear stochastic systems [13,14]. Its fusion accuracy is sharply deteriorated when applied to multi-sensor nonlinear stochastic systems. In addition, due to the use of process noise covariance’s upper bound rather than process noise covariance itself, the resultant local state estimations are at suboptimal level, making the global state estimation also at suboptimal level. Furthermore, FKF also requires the local estimations resulting from local filters to be independent initially, which is difficult to implement in practical engineering [4,14,15].

The unscented Kalman filter (UKF) is a promising filtering method to estimate the state of a nonlinear stochastic system [16,17]. This method can approximate the posterior mean and covariance of any Gaussian random variable in third-order accuracy by using unscented transformation (UT). It has advantages of high estimation accuracy, high convergence rate and simple implementation compared to other nonlinear filtering methods [17,18]. Due to these merits, Hu and Huang [13] presented an UKF-based FKF (UKF-FKF) by combining UKF with FKF to address the data fusion problem in multi-sensor nonlinear stochastic systems. This method adopts UKFs as local filters to calculate local state estimations, avoiding the linearization error of the system model involved in FKF and improving the estimation accuracy of local filters. However, it still suffers from the problem due to the use of covariance upper bound in FKF. Further, since UKF is designed based on the condition that the nonlinear system can be exactly modeled [19,20,21], UKF-FKF also requires the system model to be accurate. If the system model of a multi-sensor nonlinear stochastic system involves modeling error, the performances of local filters in UKF-FKF will be degraded or even divergent, leading to the poor fusion accuracy.

Currently, the existing multi-sensor data fusion methods are mainly dominated by inhibiting the influence of covariance upper bound on the fusion accuracy. Chang et al. [22] studied an information matrix-based distributed fusion Kalman filter. The authors’ group also reported a random weighting estimation-based multi-sensor optimal data fusion approach and a matrix weighted multi-sensor data fusion method [23,24]. However, the above studies are mainly focused on multi-sensor linear stochastic systems. Just recently, the authors’ group reported a UKF-based multi-sensor optimal data fusion method (UKF-MODF) for multi-sensor nonlinear stochastic systems to inhibit the influence of covariance upper bound on the fusion solution [14]. This method develops an optimal data fusion scheme according to the principle of linear minimum variance to overcome the problem due to the use of covariance upper bound in FKF. It can achieve the globally optimal state estimation for multi-sensor nonlinear stochastic systems. However, since UKF still serves as local filters to calculate local state estimations, the estimation accuracy of UKF-MODF also relies on the accuracy of the system model. Furthermore, the global state estimation process of UKF-MODF is only suitable for the case with two local filters. It is necessary to provide a process of global state estimation for general cases with N local filters.

The system model of a nonlinear stochastic system consists of process and measurement models. In practical applications, the measurement model’s accuracy can be guaranteed by using high-precision measurement equipment together with plenty of available measurement data [25,26]. However, since the process model is a theoretical approximation to the real-world dynamic system [26], it inevitably involves error. In some applications with the preference of computational simplicity and real-time performance, such an approximation is intended given the complexity of the dynamic environment [25]. Therefore, the process-modeling error is generally considered to be the main source of model error for a nonlinear stochastic system [25,26,27,28].

Adaptive UKF is a strategy to improve the UKF adaptability and robustness against process-modeling error by adjusting the Kalman gain matrix for state estimation. Song and Han presented an adaptive UKF to inhibit the process-modeling error by updating the process noise covariance via minimization of the difference between computed and actual innovation covariances [29]. However, this method requires the calculation of partial derivatives, causing a relatively large computation burden. Meng et al. reported a covariance matching-based adaptive UKF to make online estimates of and adjust process noise covariance using innovation and residual sequences [30]. However, this covariance matching technique yields a steady-state estimation error, leading to a limited improvement in the filtering accuracy [20]. Compared to the above two adaptive UKFs, the adaptive fading UKF (AFUKF) can effectively inhibit the influence of process-modeling error on the filtering solution under a modest computation load [27,28]. In this method, when process-modeling error is detected, an adaptive fading factor is constructed based on innovation vector and its corresponding statistical information. By using the adaptive fading factor to scale process noise covariance or predicted state covariance, historical information is less weighted while current measurement information is more weighted to tune UKF recursively to alleviate the influence of process-modeling error on the estimation solution [19,28]. Therefore, the introduction of AFUKF into multi-sensor optimal data fusion provides a solution to improve the adaptability and robustness of UKF-MODF.

This paper focuses on improvement of our previous work (UKF-MODF) for multi-sensor nonlinear stochastic systems. It presents a novel multi-sensor optimal data fusion methodology based on AFUKF to address the UKF-MODF problems, leading to improved adaptability and robustness of data fusion for multi-sensor nonlinear stochastic systems. This methodology is in a two-level fusion structure: at the bottom level, an AFUKF based on the Mahalanobis distance is developed and serves as local filters to improve the adaptability and robustness of local state estimations against process-modeling error; at the top level, an UT-based multi-sensor optimal data fusion for the case of N local filters is established according to the principle of linear minimum variance to calculate globally optimal state estimation via fusion of local estimations. The proposed methodology not only improves the robustness of multi-sensor data fusion against process-modeling error, but it also achieves globally optimal fusion results based on the principle of linear minimum variance. Simulations and practical experiments as well as comparison analysis with FKF, UKF-FKF and UKF-MODF have been conducted to comprehensively evaluate the performance of the proposed multi-sensor optimal fusion methodology for INS/GNSS/CNS (inertial navigation system/global navigation satellite system/celestial navigation system) integrated navigation system.

2. Adaptive Fading UKF Based Multi-Sensor Optimal Data Fusion

The framework of the proposed AFUKF-based multi-sensor optimal data fusion methodology (AFUKF-MODF) is shown in Figure 1. It has a two-level fusion structure: at the bottom level, an AFUKF based on the Mahalanobis distance is developed and serves as local filters to improve the robustness of local state estimations against process-modeling error; at the top level, according to the principle of linear minimum variance, the UT-based multi-sensor optimal data fusion for the case of N local filters is established to calculate the globally optimal state estimation by fusion of local estimations.

2.1. Local State Estimation

Consider the $i\mathrm{th}$ ($i=1,2,\cdots ,N$) local filter with the following dynamic model

$$\{\begin{array}{c}\mathit{x}(k)=f\left(\mathit{x}(k-1)\right)+\mathit{w}(k)\\ {\mathit{z}}_i(k)=h_i\left(\mathit{x}(k)\right)+{\mathit{v}}_i(k)\end{array}$$

(1)

where $\mathit{x}(k)\in {\mathbf{R}}^n$ is the system state vector; $f(\cdot )$ is the nonlinear state function; $\mathit{w}(k)$ is the process noise which is commonly assumed as a zero-mean Gaussian white noise with covariance $\mathit{Q}\ge \mathbf{0}$; ${\mathit{z}}_i(k)\in {\mathbf{R}}^{m_i}$ is the measurement of the $i\mathrm{th}$ local filter; $h_i(\cdot )$ is the nonlinear measurement function of the $i\mathrm{th}$ local filter; and ${\mathit{v}}_i(k)$ is the measurement noise of the $i\mathrm{th}$ local filter and is commonly assumed as a zero-mean Gaussian white noise with covariance ${\mathit{R}}_i>\mathbf{0}$.

2.1.1. Classical UKF

In order to describe the Mahalanobis distance-based AFUKF clearly, let us briefly review the classical UKF at first. For the nonlinear system model given by (1), the procedure of the classical UKF can be summarized as:

Step 1: Initialization

$$\{\begin{array}{c}{\widehat{\mathit{x}}}_i(0)=E[{\mathit{x}}_i(0)]\\ {\mathit{P}}_i(0)=E\left[\left({\mathit{x}}_i(0)-{\widehat{\mathit{x}}}_i(0)\right){\left({\mathit{x}}_i(0)-{\widehat{\mathit{x}}}_i(0)\right)}^{\mathrm{T}}\right]\end{array}$$

(2)

Step 2: Time Update

Assume that the state estimate ${\widehat{\mathit{x}}}_i(k-1)$ and the corresponding error covariance matrix ${\mathit{P}}_i(k-1)$ are given, the sigma points are selected as

$$\{\begin{array}{ll}{\mathit{\chi }}_{j,k-1}^{(i)}={\widehat{\mathit{x}}}_i(k-1)& j=0\\ {\mathit{\chi }}_{j,k-1}^{(i)}={\widehat{\mathit{x}}}_i(k-1)+a{\left(\sqrt{n{\mathit{P}}_i(k-1)}\right)}_j& j=1,2,\dots ,n\\ {\mathit{\chi }}_{j,k-1}^{(i)}={\widehat{\mathit{x}}}_i(k-1)-a{\left(\sqrt{n{\mathit{P}}_i(k-1)}\right)}_{j-n}& j=n+1,n+2,\dots ,2n\end{array}$$

(3)

where $a$ is a tuning parameter to determine the spread of the sigma points around ${\widehat{\mathit{x}}}_i(k-1)$ and it is commonly set to a small positive value; and ${\left(\sqrt{n{\mathit{P}}_i(k-1)}\right)}_j$ denotes the jth column of the square root of the matrix $n{\mathit{P}}_i(k-1)$.

Calculate the predicted state mean and covariance as

$${\mathit{\chi }}_{j,k/k-1}^{(i)}=f({\mathit{\chi }}_{j,k-1}^{(i)})(j=0,1,\dots ,2n)$$

(4)

$${\widehat{\mathit{x}}}_i(k/k-1)={\displaystyle \sum _{j=0}^{2n}{\omega }_j}{\mathit{\chi }}_{j,k/k-1}^{(i)}$$

(5)

$${\mathit{P}}_i(k/k-1)={\displaystyle \sum _{j=0}^{2n}{\omega }_j}({\mathit{\chi }}_{j,k/k-1}^{(i)}-{\widehat{\mathit{x}}}_i(k/k-1)){({\mathit{\chi }}_{j,k/k-1}^{(i)}-{\widehat{\mathit{x}}}_i(k/k-1))}^{\mathrm{T}}+\mathit{Q}$$

(6)

where $\{\begin{array}{ll}{\omega }_j=1-\frac{1}{a^2},& j=0\\ {\omega }_j=\frac{1}{2n{a}^2},& j=1,2,\cdots ,2n\end{array}$.

Step 3: Sigma Point Update

Select a set of new sigma points with the mean ${\widehat{\mathit{x}}}_i(k/k-1)$ and the covariance ${\mathit{P}}_i(k/k-1)$

$$\{\begin{array}{ll}{{\mathit{\chi }}^{\prime }}_{j,k/k-1}^{(i)}={\widehat{\mathit{x}}}_i(k/k-1),& j=0\\ {{\mathit{\chi }}^{\prime }}_{j,k/k-1}^{(i)}={\widehat{\mathit{x}}}_i(k/k-1)+{\left(a\sqrt{n{\mathit{P}}_i(k/k-1)}\right)}_j,& j=1,2,\dots ,n\\ {{\mathit{\chi }}^{\prime }}_{j,k/k-1}^{(i)}={\widehat{\mathit{x}}}_i(k/k-1)-{\left(a\sqrt{n{\mathit{P}}_i(k/k-1)}\right)}_{j-n},& j=n+1,n+2,\dots ,2n\end{array}$$

(7)

Step 4: Measurement Update

The weighted mean and covariance of the predicted measurement are computed by

$${\mathit{\gamma }}_{j,k/k-1}^{\left(i\right)}=h({{\mathit{\chi }}^{\prime }}_{j,k/k-1}^{(i)})$$

(8)

$${\widehat{\mathit{z}}}_i(k/k-1)={\displaystyle \sum _{j=0}^{2n}{\omega }_j}{\mathit{\gamma }}_{j,k/k-1}^{\left(i\right)}={\displaystyle \sum _{j=0}^{2n}{\omega }_j}h({{\mathit{\chi }}^{\prime }}_{j,k/k-1}^{(i)})$$

(9)

$${\mathit{P}}_i({\widehat{\mathit{z}}}_i(k/k-1))={\displaystyle \sum _{j=0}^{2n}{\omega }_j\left({\mathit{\gamma }}_{j,k/k-1}^{\left(i\right)}-{\widehat{\mathit{z}}}_i(k/k-1)\right)}{\left({\mathit{\gamma }}_{j,k/k-1}^{\left(i\right)}-{\widehat{\mathit{z}}}_i(k/k-1)\right)}^{\mathrm{T}}+{\mathit{R}}_i$$

(10)

Subsequently, the state estimate and associated error covariance matrix are updated as

$${\mathit{P}}_i({\widehat{\mathit{x}}}_i(k/k-1){\widehat{\mathit{z}}}_i(k/k-1))={\displaystyle \sum _{j=0}^{2n}{\omega }_j\left({\mathit{\chi }}_{j,k/k-1}^{(i)}-{\widehat{\mathit{x}}}_i(k/k-1)\right)}{\left({\mathit{\gamma }}_{j,k/k-1}^{\left(i\right)}-{\widehat{\mathit{z}}}_i(k/k-1)\right)}^{\mathrm{T}}$$

(11)

$${\mathit{K}}_i(k)={\mathit{P}}_i({\widehat{\mathit{x}}}_i(k/k-1){\widehat{\mathit{z}}}_i(k/k-1)){\mathit{P}}_i^{-1}({\widehat{\mathit{z}}}_i(k/k-1))$$

(12)

$${\widehat{\mathit{x}}}_i(k)={\widehat{\mathit{x}}}_i(k/k-1)+{\mathit{K}}_i(k)({\mathit{z}}_i(k)-{\widehat{\mathit{z}}}_i(k/k-1))$$

(13)

$${\mathit{P}}_i(k)={\mathit{P}}_i(k/k-1)-{\mathit{K}}_i(k){\mathit{P}}_i({\widehat{\mathit{z}}}_i(k/k-1)){\mathit{K}}_i^{\mathrm{T}}(k)$$

(14)

Step 5: Get back to Step 2 for the next sample until all samples are processed.

2.1.2. Mahalanobis Distance Based Adaptive Fading UKF

The Mahalanobis distance of innovation vector, which is based on hypothesis testing theory, is commonly used as a measure to identify system modeling error for Gaussian systems [27,31]. This paper adopts the Mahalanobis distance into AFUKF and further develop a Mahalanobis distance-based AFUKF to improve the adaptability and robustness of the classical UKF against process-modeling error for multi-sensor nonlinear stochastic systems.

Define the innovation vector of the ith local filter as

$${\tilde{\mathit{z}}}_i(k)={\mathit{z}}_i(k)-{\widehat{\mathit{z}}}_i(k/k-1)$$

(15)

For the nonlinear Gaussian system given by (1), ${\tilde{\mathit{z}}}_i(k)$ should obey the zero-mean Gaussian distribution with the covariance

$${\mathit{P}}_i({\widehat{\mathit{z}}}_i(k/k-1))={\displaystyle \sum _{j=0}^{2n}{\omega }_j\left({\mathit{\gamma }}_{j,k/k-1}^{\left(i\right)}-{\widehat{\mathit{z}}}_i(k/k-1)\right)}{\left({\mathit{\gamma }}_{j,k/k-1}^{\left(i\right)}-{\widehat{\mathit{z}}}_i(k/k-1)\right)}^{\mathrm{T}}+{\mathit{R}}_i$$

(16)

Thus, the square of the Mahalanobis Distance of the innovation vector should obey the chi-square distribution with $m_i$ degrees of freedom [27,31], i.e.,

$$M_i^2(k)={\tilde{\mathit{z}}}_i^{\mathrm{T}}(k){\left({\mathit{P}}_i({\widehat{\mathit{z}}}_i(k/k-1))\right)}^{-1}{\tilde{\mathit{z}}}_i(k)\sim {\chi }_{m_i}^2$$

(17)

where $m_i$ is the dimension of the measurement in the $i\mathrm{th}$ local filter.

According to the hypothesis testing theory, for a given significance level $\alpha$, we have

$$P(M_i^2(k)\le {\chi }_{m_i,\alpha }^2)=1-\alpha (0<\alpha \le 1)$$

(18)

where $P(\cdot )$ represents the probability of a random event.

If (18) holds, this means the system works under the optimal condition, i.e., the nonlinear multi-sensor system described by (1) does not have process-modeling error. However, if (18) does not hold, it can be concluded with high probability that there exists process-modeling error in the multi-sensor system (1). In this case, AFUKF incorporates a time-varying adaptive fading factor into the predicted state covariance matrix to refrain from the influence of prior knowledge of the current state estimate. Thus, the predicted state covariance matrix in AFUKF is modified as

$${\mathit{P}}_i^{\ast }(k/k-1)={\lambda }_k^i\left({\displaystyle \sum _{j=0}^{2n}{\omega }_j}({\mathit{\chi }}_{j,k/k-1}^{(i)}-{\widehat{\mathit{x}}}_i(k/k-1)){({\mathit{\chi }}_{j,k/k-1}^{(i)}-{\widehat{\mathit{x}}}_i(k/k-1))}^{\mathrm{T}}+\mathit{Q}\right)$$

(19)

Introduce the adaptive fading factor ${\lambda }_k^i$ to the predicted state covariance matrix such that (18) holds. Thus, we have the following equation

$$g_i({\lambda }_k^i)={\tilde{\mathit{z}}}_i^{\mathrm{T}}(k){\left({\mathit{P}}_i^{\ast }({\widehat{\mathit{z}}}_i(k/k-1))\right)}^{-1}{\tilde{\mathit{z}}}_i(k)-{\chi }_{m_i,\alpha }^2=\mathbf{0}$$

(20)

where ${\mathit{P}}_i^{\ast }({\widehat{\mathit{z}}}_i(k/k-1))$ is the measurement prediction covariance matrix calculated by using ${\mathit{P}}_i^{\ast }(k/k-1)$.

It can be seen from (20) that the determination of the adaptive fading factor ${\lambda }_k^i$ is a problem of solving the nonlinear equation. Since the derivative of $g_i({\lambda }_k^i)$ with respect to ${\lambda }_k^i$ is difficult to calculate, the traditional Newton’s method cannot be used to solve the nonlinear Equation (20). In this paper, a chord secant method [32,33] is adopted to iteratively determine the adaptive fading factor ${\lambda }_k^i$ by solving the nonlinear Equation (20). Thus, we have

$${\lambda }_k^i(t+1)={\lambda }_k^i(t)-\frac{g_i({\lambda }_k^i(t))[{\lambda }_k^i(t)-{\lambda }_k^i(t-1)]}{g_i({\lambda }_k^i(t))-g_i[{\lambda }_k^i(t-1)]},(t=1,2,\cdots )$$

(21)

where $t$ represents the iteration time.

Substituting (20) into (21) yields

$${\lambda }_k^i(t+1)={\lambda }_k^i(t)-\frac{[{\overline{M}}_k^i({\lambda }_k^i(t))-{\chi }_{m_i,\alpha }^2][{\lambda }_k^i(t)-{\lambda }_k^i(t-1)]}{{\overline{M}}_k^i({\lambda }_k^i(t))-{\overline{M}}_k^i({\lambda }_k^i(t-1))},(t=1,2,\cdots )$$

(22)

where ${\overline{M}}_k^i({\lambda }_k^i(t))={\tilde{\mathit{z}}}_i^{\mathrm{T}}(k){\left({\mathit{P}}_i^{\ast }({\widehat{\mathit{z}}}_i(k/k-1))\right)}^{-1}{\tilde{\mathit{z}}}_i(k)$.

The above iterative process to determine the adaptive fading factor ${\lambda }_k^i$ is initialized as ${\lambda }_k^i(1)=1$ and ${\lambda }_k^i(0)=0$. It will be terminated when the criterion ${\overline{M}}_k^i[{\lambda }_k^i(t)]\le {\chi }_{m_i,\alpha }^2$ is satisfied. The calculation process of the Mahalanobis distance-based AFUKF can be summarized as:

Step 1:

Initialization

Initiate the Local filters by presetting the ${\widehat{\mathit{x}}}_i(0)$ and ${\mathit{P}}_i(0)$ as (2).

Step 2:

Local Filtering

(i)

Conduct the classical UKF procedures (3)–(10) and calculate the ${\overline{M}}_k^i[{\lambda }_k^i(t)]$.

(ii)

If ${\overline{M}}_k^i[{\lambda }_k^i(t)]\le {\chi }_{m_i,\alpha }^2$,

• Perform the classical UKF procedures (11)–(14) to compute the local state estimations.

Else,

• Determine the adaptive fading factor ${\lambda }_k^i$ by iteratively computing (22) until the criterion ${\overline{M}}_k^i[{\lambda }_k^i(t)]\le {\chi }_{m_i,\alpha }^2$ is satisfied.
• Then, replace the predicted state covariance matrix ${\mathit{P}}_i(k/k-1)$ of the classical UKF with the modified type ${\mathit{P}}_i^{\ast }(k/k-1)$ as described by (19).
• Complete the classical UKF procedures (7)–(14) to update the local state estimations.

(iii)

Repeat Steps (i) and (ii) for the next sample.

Step 3:

Complete the local state estimations until all samples are processed.

Each local filter is performed in a parallel manner to generate the local state estimation ${\widehat{\mathit{x}}}_i(k)$ and the corresponding error covariance ${\mathit{P}}_i(k)$ ($i=1,2,\cdots ,N$) by using the Mahalanobis distance-based AFUKF. After this, these local state estimations will be further fused by the UT-based data fusion method, which is to be described in the next section, to obtain the globally optimal state estimation.

2.2. Global State Estimation

Suppose that the local state estimation achieved by the ith ($i=1,2,\cdots ,N$) local filter is ${\widehat{\mathit{x}}}_i(k)$. According to the linear weighting fusion criterion, the globally optimal state estimation can be described as the following form

$${\widehat{\mathit{x}}}^{\ast }(k)={\displaystyle \sum _{i=1}^N{\mathit{\alpha }}_i{\widehat{\mathit{x}}}_i(k)}$$

(23)

$${\displaystyle \sum _{i=1}^N{\mathit{\alpha }}_i}={\mathit{I}}_n$$

(24)

where ${\mathit{\alpha }}_i$ ($i=1,2,\cdots ,N$) is the weighting matrix to be determined; and ${\mathit{I}}_n$ represents an n-dimensional identity matrix.

Based on the principle of linear minimum variance, the globally optimal state estimation ${\widehat{\mathit{x}}}^{\ast }(k)$ should satisfy the following conditions [14,24]:

Condition 1.

${\widehat{\mathit{x}}}^{\ast }(k)$ must be the unbiased estimation of $\mathit{x}(k)$, i.e., $E\left\{{\widehat{\mathit{x}}}^{\ast }(k)-\mathit{x}(k)\right\}=0$.

Condition 2.

${\widehat{\mathit{x}}}^{\ast }(k)$ makes $tr\left\{{\mathit{P}}^{\ast }(k)\right\}$ minimum, where ${\mathit{P}}^{\ast }(k)$ is the error covariance matrix of ${\widehat{\mathit{x}}}^{\ast }(k)$, and $tr\{\cdot \}$ is the trace operator.

Theorem 1.

Denote the local state estimation achieved by the ith ($i=1,2,\cdots ,N$) local filter by ${\widehat{\mathit{x}}}_i(k)$, and its corresponding error covariance matrix by ${\mathit{P}}_{ii}(k)$ (i.e., ${\mathit{P}}_i(k)$, $i=1,2,\dots ,N$ in the local filters). The cross-covariance matrix between the state estimation errors of ${\widehat{\mathit{x}}}_i(k)$ and ${\widehat{\mathit{x}}}_j(k)$ is ${\mathit{P}}_{ij}(k)$ ($i,j=1,2,\dots ,N$; $i\ne j$). Define ${\left(\mathit{\Sigma }\left(k\right)\right)}_{ij}={\mathit{P}}_{ij}(k)\in {\mathbf{R}}^{n\times n}$ ($i,j=1,2,\dots ,N$), $\overline{\mathit{E}}={\left[{\mathit{I}}_n,{\mathit{I}}_n,\dots ,{\mathit{I}}_n\right]}^{\mathrm{T}}\in {\mathbf{R}}^{nN\times n}$ and $\overline{\mathit{\alpha }}={\left[{\mathit{\alpha }}_1,{\mathit{\alpha }}_2,\dots ,{\mathit{\alpha }}_N\right]}^{\mathrm{T}}\in {\mathbf{R}}^{nN\times n}$. Based on the principle of linear minimum variance, the globally optimal state estimation ${\widehat{\mathit{x}}}^{\ast }(k)$ is obtained as

$${\widehat{\mathit{x}}}^{\ast }(k)={\displaystyle \sum _{i=1}^N{\mathit{\alpha }}_i{\widehat{\mathit{x}}}_i(k)}$$

(25)

where ${\mathit{\alpha }}_i$ ($i=1,2,\cdots ,N$) is given by

$$\overline{\mathit{\alpha }}={\mathit{\Sigma }}^{-1}\left(k\right)\overline{\mathit{E}}{\left[{\overline{\mathit{E}}}^{\mathrm{T}}{\mathit{\Sigma }}^{-1}\left(k\right)\overline{\mathit{E}}\right]}^{-1}$$

(26)

Proof. 

As $E\left\{{\widehat{\mathit{x}}}_i(k)-\mathit{x}(k)\right\}=\mathbf{0}$, taking the expectation of (25) and considering the relation ${\displaystyle \sum _{i=1}^N{\mathit{\alpha }}_i}={\mathit{I}}_n$, we can obtain

$$E\left({\widehat{\mathit{x}}}^{\ast }(k)\right)={\displaystyle \sum _{i=1}^N{\mathit{\alpha }}_{i}E\left({\widehat{\mathit{x}}}_i(k)\right)}=\mathit{x}(k)$$

(27)

It can be seen from (27), ${\widehat{\mathit{x}}}^{\ast }(k)$ obtained by (25) is the unbiased estimation of $\mathit{x}(k)$.

Now let us consider Condition 2 to derive the globally optimal state estimation ${\widehat{\mathit{x}}}^{\ast }(k)$. Define the estimation error of the local state estimation ${\widehat{\mathit{x}}}_i(k)$ ($i=1,2,\dots ,N$) as

$${\tilde{\mathit{x}}}_i(k)=\mathit{x}(k)-{\widehat{\mathit{x}}}_i(k)$$

(28)

Thus, the estimation error of ${\widehat{\mathit{x}}}^{\ast }(k)$ is described as

$${\tilde{\mathit{x}}}^{\ast }(k)=\mathit{x}(k)-{\widehat{\mathit{x}}}^{\ast }(k)={\displaystyle \sum _{i=1}^N{\mathit{\alpha }}_i}{\tilde{\mathit{x}}}_i(k)$$

(29)

According to (29), the error covariance matrix of ${\widehat{\mathit{x}}}^{\ast }(k)$ is

$${\mathit{P}}^{\ast }(k)={\overline{\mathit{\alpha }}}^{\mathrm{T}}\mathit{\Sigma }\left(k\right)\overline{\mathit{\alpha }}$$

(30)

Applying the Lagrange multiplier to solve the minimum of $tr\left\{{\mathit{P}}^{\ast }(k)\right\}$, the Lagrange function is defined as

$$\mathit{𝕃}=tr\left\{{\mathit{P}}^{\ast }\left(k\right)\right\}+tr\left\{\mathit{\Lambda }\right[{\overline{\mathit{\alpha }}}^{\mathrm{T}}\overline{\mathit{E}}-{\mathit{I}}_n\left]\right\}$$

(31)

where $\mathit{\Lambda }\in {\mathbf{R}}^{n\times n}$ is the Lagrange multiplier.

Taking the partial derivative of (31) with respect to $\overline{\mathit{\alpha }}$ and letting it to be zero, we have

$$\mathit{\Sigma }\left(k\right)\overline{\mathit{\alpha }}+\frac{1}{2}\overline{\mathit{E}}\mathit{\Lambda }=\mathbf{0}$$

(32)

Combining ${\displaystyle \sum _{i=1}^N{\mathit{\alpha }}_i}={\mathit{I}}_n$ with (32), we readily have

$$\left[\begin{array}{cc}\mathit{\Sigma }\left(k\right)& \overline{\mathit{E}}\\ {\overline{\mathit{E}}}^{\mathrm{T}}& \mathbf{0}\end{array}\right]\left[\begin{array}{c}\overline{\mathit{\alpha }}\\ \frac{1}{2}\mathit{\Lambda }\end{array}\right]=\left[\begin{array}{c}\mathbf{0}\\ {\mathit{I}}_n\end{array}\right]$$

(33)

By solving (33), it is verified that

$$\left[\begin{array}{c}\overline{\mathit{\alpha }}\\ \frac{1}{2}\mathit{\Lambda }\end{array}\right]={\left[\begin{array}{cc}\mathit{\Sigma }\left(k\right)& \overline{\mathit{E}}\\ {\overline{\mathit{E}}}^{\mathrm{T}}& \mathbf{0}\end{array}\right]}^{-1}\left[\begin{array}{c}\mathbf{0}\\ {\mathit{I}}_n\end{array}\right]=\left[\begin{array}{c}{\mathit{\Sigma }}^{-1}\left(k\right)\overline{\mathit{E}}{\left[{\overline{\mathit{E}}}^{\mathrm{T}}{\mathit{\Sigma }}^{-1}\left(k\right)\overline{\mathit{E}}\right]}^{-1}\\ -{\left[{\overline{\mathit{E}}}^{\mathrm{T}}{\mathit{\Sigma }}^{-1}\left(k\right)\overline{\mathit{E}}\right]}^{-1}\end{array}\right]$$

(34)

Form (34), we have

$$\overline{\mathit{\alpha }}={\mathit{\Sigma }}^{-1}\left(k\right)\overline{\mathit{E}}{\left[{\overline{\mathit{E}}}^{\mathrm{T}}{\mathit{\Sigma }}^{-1}\left(k\right)\overline{\mathit{E}}\right]}^{-1}$$

(35)

The proof of Theorem 1 is completed. ☐

It can be seen from (25) and (26), in order to calculate the global optimal state estimation ${\widehat{\mathit{x}}}^{\ast }(k)$, it is necessary to predetermine the matrix $\mathit{\Sigma }\left(k\right)$. In the matrix $\mathit{\Sigma }\left(k\right)$, ${\mathit{P}}_{ii}(k)$($i=1,2,\dots ,N$) can be directly determined by the error covariance matrix of the state estimation in the ith local filter. However, it is difficult to calculate the cross-covariance matrix ${\mathit{P}}_{ij}(k)$ between the state estimation errors of ${\widehat{\mathit{x}}}_i(k)$ and ${\widehat{\mathit{x}}}_j(k)$. This paper adopts the UT concept to approximate ${\mathit{P}}_{ij}(k)$, and the derivation process is shown in Theorem 2.

Theorem 2.

For the multi-sensor system given by (1), the cross-covariance matrix ${\mathit{P}}_{ij}(k)$ between the estimation errors of local state estimations ${\widehat{\mathit{x}}}_i(k)$ and ${\widehat{\mathit{x}}}_j(k)$ ($i,j=1,2,\dots ,N$; $i\ne j$) is computed as

$$\begin{array}{cc}{\mathit{P}}_{ij}(k)\hfill & ={\displaystyle \sum _{s=0}^{2n}{\omega }_s}({\mathit{\chi }}_{s,k/k-1}^{(i)}-{\widehat{\mathit{x}}}_i(k/k-1)){({\mathit{\chi }}_{s,k/k-1}^{(j)}-{\widehat{\mathit{x}}}_j(k/k-1))}^{\mathrm{T}}\hfill \\ & -\left[{\displaystyle \sum _{s=0}^{2n}{\omega }_s}({\mathit{\chi }}_{s,k/k-1}^{(i)}-{\widehat{\mathit{x}}}_i(k/k-1)){({\mathit{\gamma }}_{s,k/k-1}^{(j)}-{\widehat{\mathit{z}}}_j(k/k-1))}^{\mathrm{T}}\right]{\mathit{K}}_j^{\mathrm{T}}(k)\hfill \\ & -{\mathit{K}}_i(k)\left[{\displaystyle \sum _{s=0}^{2n}{\omega }_s}({\mathit{\gamma }}_{s,k/k-1}^{(i)}-{\widehat{\mathit{z}}}_i(k/k-1)){({\mathit{\chi }}_{s,k/k-1}^{(j)}-{\widehat{\mathit{x}}}_j(k/k-1))}^{\mathrm{T}}\right]\hfill \\ & +{\mathit{K}}_i(k)\left[{\displaystyle \sum _{s=0}^{2n}{\omega }_s}({\mathit{\gamma }}_{s,k/k-1}^{(i)}-{\widehat{\mathit{z}}}_i(k/k-1)){({\mathit{\gamma }}_{s,k/k-1}^{(j)}-{\widehat{\mathit{z}}}_j(k/k-1))}^{\mathrm{T}}\right]K_j^{\mathrm{T}}(k)\hfill \end{array}$$

(36)

where ${\mathit{\chi }}_{s,k/k-1}^{(i)}$ denotes the sigma point transformed by the nonlinear function $f(\cdot )$ in (4) for the ith local filter; ${\mathit{\gamma }}_{s,k/k-1}^{(i)}$ denotes the sigma point transformed by the nonlinear function $h_i(\cdot )$ in (8) for the ith local filter; and $s=0,1,\cdots ,2n$ represents the order of the transformed sigma points.

Proof. 

Define the estimation error of the ith local filter as

$$\begin{array}{cc}\mathit{x}(k)\hfill & -{\widehat{\mathit{x}}}_i(k)\hfill \\ & =\mathit{x}(k)-\left[{\widehat{\mathit{x}}}_i(k/k-1)+{\mathit{K}}_i(k)\left({\mathit{z}}_i(k)-{\widehat{\mathit{z}}}_i(k/k-1)\right)\right]\hfill \\ & =\left(\mathit{x}(k)-{\widehat{\mathit{x}}}_i(k/k-1)\right)-{\mathit{K}}_i(k)\left({\mathit{z}}_i(k)-{\widehat{\mathit{z}}}_i(k/k-1)\right)\hfill \end{array}$$

(37)

Then, it is verified from (37)

$$\begin{array}{cc}\hfill {\mathit{P}}_{ij}(k)& =E\left\{\left[\mathit{x}(k)-{\widehat{\mathit{x}}}_i(k)\right]{\left[\mathit{x}(k)-{\widehat{\mathit{x}}}_j(k)\right]}^{\mathrm{T}}\right\}\hfill \\ & =E\{\left[\left(\mathit{x}(k)-{\widehat{\mathit{x}}}_i(k/k-1)\right)-{\mathit{K}}_i(k)\left({\mathit{z}}_i(k)-{\widehat{\mathit{z}}}_i(k/k-1)\right)\right]\hfill \\ & {\left[\left(\mathit{x}(k)-{\widehat{\mathit{x}}}_j(k/k-1)\right)-{\mathit{K}}_j(k)\left({\mathit{z}}_j(k)-{\widehat{\mathit{z}}}_j(k/k-1)\right)\right]}^{\mathrm{T}}\}\hfill \\ & =E\left\{\left[\left(\mathit{x}(k)-{\widehat{\mathit{x}}}_i(k/k-1)\right)\right]{\left[\left(\mathit{x}(k)-{\widehat{\mathit{x}}}_j(k/k-1)\right)\right]}^{\mathrm{T}}\right\}\hfill \\ & -E\left\{\left[\left(\mathit{x}(k)-{\widehat{\mathit{x}}}_i(k/k-1)\right)\right]{\left[\left({\mathit{z}}_j(k)-{\widehat{\mathit{z}}}_j(k/k-1)\right)\right]}^{\mathrm{T}}\right\}{\mathit{K}}_j^{\mathrm{T}}(k)\hfill \\ & -{\mathit{K}}_i(k)E\left\{\left[\left({\mathit{z}}_i(k)-{\widehat{\mathit{z}}}_i(k/k-1)\right)\right]{\left[\left(\mathit{x}(k)-{\widehat{\mathit{x}}}_j(k/k-1)\right)\right]}^{\mathrm{T}}\right\}\hfill \\ & +{\mathit{K}}_i(k)E\left\{\left[\left({\mathit{z}}_i(k)-{\widehat{\mathit{z}}}_i(k/k-1)\right)\right]{\left[\left({\mathit{z}}_j(k)-{\widehat{\mathit{z}}}_j(k/k-1)\right)\right]}^{\mathrm{T}}\right\}{\mathit{K}}_j^{\mathrm{T}}(k)\hfill \end{array}$$

(38)

Based on the tranformated sigma points ${\mathit{\chi }}_{s,k/k-1}^{(i)}$ and ${\mathit{\gamma }}_{s,k/k-1}^{(i)}$ ($i=1,2,\cdots ,N$), ${\mathit{P}}_{ij}(k)$ can be approximated by using UT

$$\begin{array}{cc}{\mathit{P}}_{ij}(k)=\hfill & {\displaystyle \sum _{s=0}^{2n}{\omega }_s}({\mathit{\chi }}_{s,k/k-1}^{(i)}-{\widehat{\mathit{x}}}_i(k/k-1)){({\mathit{\chi }}_{s,k/k-1}^{(j)}-{\widehat{\mathit{x}}}_j(k/k-1))}^{\mathrm{T}}\hfill \\ & -\left[{\displaystyle \sum _{s=0}^{2n}{\omega }_s}({\mathit{\chi }}_{s,k/k-1}^{(i)}-{\widehat{\mathit{x}}}_i(k/k-1)){({\mathit{\gamma }}_{s,k/k-1}^{(j)}-{\widehat{\mathit{z}}}_j(k/k-1))}^{\mathrm{T}}\right]{\mathit{K}}_j^{\mathrm{T}}(k)\hfill \\ & -{\mathit{K}}_i(k)\left[{\displaystyle \sum _{s=0}^{2n}{\omega }_s}({\mathit{\gamma }}_{s,k/k-1}^{(i)}-{\widehat{\mathit{z}}}_i(k/k-1)){({\mathit{\chi }}_{s,k/k-1}^{(j)}-{\widehat{\mathit{x}}}_j(k/k-1))}^{\mathrm{T}}\right]\hfill \\ & +{\mathit{K}}_i(k)\left[{\displaystyle \sum _{s=0}^{2n}{\omega }_s}({\mathit{\gamma }}_{s,k/k-1}^{(i)}-{\widehat{\mathit{z}}}_i(k/k-1)){({\mathit{\gamma }}_{s,k/k-1}^{(j)}-{\widehat{\mathit{z}}}_j(k/k-1))}^{\mathrm{T}}\right]{\mathit{K}}_j^{\mathrm{T}}(k)\hfill \end{array}$$

(39)

The proof of Theorem 2 is completed. ☐

3. Performance Evaluation and Discussion

A prototype system of INS/GNSS/CNS integration was implemented using the proposed AFUKF-MODF. Simulations and experiments as well as comparison analysis with FKF, UKF-FKF [13], and UKF-MODF [14] were conducted to comprehensively evaluate the performance of the proposed AFUKF-MODF.

3.1. System Model of INS/GNSS/CNS Integration

Fundamentally, INS/GNSS/CNS integration generates navigation solutions by utilizing the high-precision GNSS position and velocity to correct the INS velocity and position errors and utilizing the high-precision CNS attitude to correct the INS attitude error.

3.1.1. Process Model

The process model of the INS/GNSS/CNS integrated navigation system is established by combining the INS error equations with the inertial measurement unit (IMU) error equations.

The navigation frame (n-frame) is selected as the E-N-U (East-North-Up) geography frame (g-frame). Denote the inertial frame by i, the earth frame e, the body frame b and the INS simulated actual platform frame $n^{\prime }$. The system state vector is defined as

$$\mathit{x}(t)={[{\varphi }_E,{\varphi }_N,{\varphi }_U,\delta v_E,\delta v_N,\delta v_U,\delta L,\delta \lambda ,\delta h,{\epsilon }_x^b,{\epsilon }_y^b,{\epsilon }_z^b,{\nabla }_x^b,{\nabla }_y^b,{\nabla }_z^b]}^{\mathrm{T}}$$

(40)

where $\left({\varphi }_E,{\varphi }_N,{\varphi }_U\right)$ is the attitude error, $\left(\delta v_E,\delta v_N,\delta v_U\right)$ the velocity error, $\left(\delta L,\delta \lambda ,\delta h\right)$ the position error, $\left({\epsilon }_x^b,{\epsilon }_y^b,{\epsilon }_z^b\right)$ the gyro constant drift, and $\left({\nabla }_x^b,{\nabla }_y^b,{\nabla }_z^b\right)$ the accelerometer zero-bias.

The nonlinear attitude and velocity error equations are given as [34,35]

$$\{\begin{array}{c}\dot{\mathit{\varphi }}={\mathit{C}}_{\omega }^{-1}[(\mathit{I}-{\mathit{C}}_n^{n^{\prime }}){\widehat{\mathit{\omega }}}_{in}^n+{\mathit{C}}_n^{n^{\prime }}\delta {\mathit{\omega }}_{in}^n-{\mathit{C}}_b^{n^{\prime }}\delta {\mathit{\omega }}_{ib}^b]\\ \delta {\dot{\mathit{v}}}^n=\delta {\mathit{g}}^n+[\mathit{I}-{({\mathit{C}}_n^{n^{\prime }})}^{\mathrm{T}}]{\mathit{C}}_b^{n^{\prime }}{\widehat{f}}^b+{\mathit{C}}_b^n\delta {\mathit{f}}^b-\\ (2{\widehat{\mathit{\omega }}}_{ie}^n+{\widehat{\mathit{\omega }}}_{en}^n)\times \delta {\mathit{v}}^n-(2\delta {\mathit{\omega }}_{ie}^n+\delta {\mathit{\omega }}_{en}^n)\times {\mathit{v}}^n\end{array}$$

(41)

where $\mathit{\varphi }={\left({\varphi }_E,{\varphi }_N,{\varphi }_U\right)}^{\mathrm{T}}$ and $\delta {\mathit{v}}^n={(\delta v_E,\delta v_N,\delta v_U)}^{\mathrm{T}}$; ${\mathit{v}}^n={(v_E,v_N,v_U)}^{\mathrm{T}}$ is the velocity of the vehicle in n-frame; ${\mathit{C}}_n^{n^{\prime }}$, ${\mathit{C}}_b^{n^{\prime }}$ and ${\mathit{C}}_b^n$ are the rotation matrices; $\delta {\mathit{g}}^n$ is the gravity error; ${\widehat{\mathit{f}}}^b$ is the measured specific force in the b-frame, and it is composed of accelerometer zero-bias ${\nabla }^b$ and its white noise ${\mathit{\omega }}_a^b$; $\delta {\mathit{f}}^b$ is the corresponding error; $\delta {\mathit{\omega }}_{ib}^b$ is the measurement error of the gyro, which is composed of gyro constant drift ${\mathit{\epsilon }}^b$ and its white noise ${\mathit{\omega }}_g^b$; ${\mathit{\omega }}_{ie}^n$ is the rotational angular velocity of the earth; ${\mathit{\omega }}_{en}^n$ is the angular velocity of the vehicle relative to the earth; ${\mathit{\omega }}_{in}^n={\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n$ is the relative rotational angular velocity between the n-frame and i-frame; ${\widehat{\mathit{\omega }}}_{ie}^n$, ${\widehat{\mathit{\omega }}}_{en}^n$ and ${\widehat{\mathit{\omega }}}_{in}^n$ are the actual values of ${\mathit{\omega }}_{ie}^n$, ${\mathit{\omega }}_{en}^n$ and ${\mathit{\omega }}_{in}^n$ in the $n^{\prime }$-frame; and $\delta {\mathit{\omega }}_{ie}^n$, $\delta {\mathit{\omega }}_{en}^n$ and $\delta {\mathit{\omega }}_{in}^n$ represent the corresponding errors. The above parameters can be calculated as

$$\{\begin{array}{l}{\mathit{\omega }}_{ie}^n={\left[\begin{array}{ccc}0& {\omega }_{ie}\cos L& {\omega }_{ie}\sin L\end{array}\right]}^{\mathrm{T}}\\ \delta {\mathit{\omega }}_{ie}^n={\left[\begin{array}{ccc}0& -\delta L{\omega }_{ie}\sin L& \delta L{\omega }_{ie}\cos L\end{array}\right]}^{\mathrm{T}}\\ {\mathit{\omega }}_{en}^n={\left[\begin{array}{ccc}-\frac{v_N}{R_M+h}& \frac{v_E}{R_N+h}& \frac{v_E\tan L}{R_N+h}\end{array}\right]}^{\mathrm{T}}\\ \delta {\mathit{\omega }}_{en}^n=\left[\begin{array}{c}-\frac{\delta v_N}{R_M+h}+\delta h\frac{v_N}{{(R_M+h)}^2}\\ \frac{\delta v_E}{R_N+h}-\delta h\frac{v_E}{{(R_N+h)}^2}\\ \frac{\delta v_E\tan L}{R_N+h}+\frac{\delta L{v}_x\sec L^2}{R_N+h}-\delta h\frac{v_E\sec L}{{(R_N+h)}^2}\end{array}\right]\end{array}$$

(42)

where $L$ and $h$ represent the latitude and altitude of the vehicle; and $R_M$ and $R_N$ are the median radius and normal radius.

${\mathit{C}}_{\omega }^{-1}$ is computed as

$${\mathit{C}}_{\omega }^{-1}=\frac{1}{\cos {\varphi }_E}\left[\begin{array}{ccc}\cos {\varphi }_N\cos {\varphi }_E& 0& \sin {\varphi }_N\cos {\varphi }_E\\ \sin {\varphi }_N\sin {\varphi }_E& \cos {\varphi }_E& -\cos {\varphi }_N\sin {\varphi }_E\\ -\sin {\varphi }_N& 0& \cos {\varphi }_N\end{array}\right]$$

(43)

The position error equation of INS is described by [36]

$$\{\begin{array}{l}\delta \dot{L}=\frac{\delta v_N}{R_M+h}-\delta h\frac{v_N}{{(R_M+h)}^2}\hfill \\ \delta \dot{\lambda }=\frac{\delta v_E\sec L}{R_N+h}+\delta L\frac{v_E\tan L\sec L}{R_N+h}-\delta h\frac{v_E\sec L}{{(R_N+h)}^2}\hfill \\ \delta \dot{h}=\delta v_U\hfill \end{array}$$

(44)

The gyro constant drift ${\mathit{\epsilon }}^b$ and accelerometer zero-bias ${\nabla }^b$ are commonly described as random constants [14,30], i.e.,

$${\dot{\epsilon }}_i^b=0(i=x,y,z)$$

(45)

$${\dot{\mathbf{\nabla }}}_i^b=0(i=x,y,z)$$

(46)

According to the selected system state vector, the process model of the INS/GNSS/CNS integration can be established by combining (41)–(46)

$$\dot{\mathit{x}}(t)=\overline{f}\left(\mathit{x}(t)\right)+\mathit{w}\left(t\right)$$

(47)

where $\overline{f}\left(\cdot \right)$ is a nonlinear function describing the system state equation in continuous form; and $\mathit{w}(t)={\left[{\left(-{\mathit{C}}_{\omega }^{-1}{\mathit{C}}_b^{n^{\prime }}{\mathit{\omega }}_a^b\right)}^{\mathrm{T}},{\left({\mathit{C}}_b^n{\mathit{\omega }}_g^b\right)}^{\mathrm{T}},{\mathbf{0}}_{1\times 9}\right]}^{\mathrm{T}}$ is the process noise vector.

By discretizing (47) with the improved Euler formulation [37], the discrete-time process model of the INS/GNSS/CNS integration is obtained as

$$\mathit{x}(k)=f\left(\mathit{x}(k-1)\right)+\mathit{w}(k)$$

(48)

where $f(\cdot )$ is a nonlinear function describing the system state equation in discrete form; and $\mathit{w}(k)$ is the discrete-time process noise vector.

3.1.2. Measurement Model of INS/GNSS Subsystem

Take the difference between INS and GNSS in terms of velocity and position as the measurement of the INS/GNSS subsystem, i.e.,

$${\mathit{z}}_1(k)={\left[\begin{array}{llllll}{v}_{EI}-v_{EG}\hfill & v_{NI}-v_{NG}\hfill & v_{UI}-v_{UG}\hfill & L_I-L_G\hfill & {\lambda }_I-{\lambda }_G\hfill & h_I-h_G\hfill \end{array}\right]}^{\mathrm{T}}$$

(49)

where ${(v_{EI},v_{NI},v_{UI})}^{\mathrm{T}}$ and ${(L_I,{\lambda }_I,h_I)}^{\mathrm{T}}$ are the velocity and position output by INS; and ${(v_{EG},v_{NG},v_{UG})}^{\mathrm{T}}$ and ${(L_G,{\lambda }_G,h_G)}^{\mathrm{T}}$ are the velocity and position achieved by GNSS.

Then, the measurement model of the INS/GNSS subsystem can be described as [11,24]

$$\begin{array}{cc}\hfill {\mathit{z}}_1(k)& ={\mathit{H}}_1(k)\mathit{x}(k)+{\mathit{v}}_1(k)\hfill \\ & =\left[\begin{array}{c}{\mathit{H}}_v(k)\\ {\mathit{H}}_{\mathit{P}}(k)\end{array}\right]\mathit{x}(k)+\left[\begin{array}{c}{\mathit{v}}_v(k)\\ {\mathit{v}}_P(k)\end{array}\right]\hfill \end{array}$$

(50)

where ${\mathit{H}}_v(k)=[{\mathbf{0}}_{3\times 3},{\mathit{I}}_{3\times 3},{\mathbf{0}}_{3\times 9}]$, ${\mathit{H}}_P(k)=[{\mathbf{0}}_{3\times 6},diag(R_M,R_N\cos L,1),{\mathbf{0}}_{3\times 6}]$; and ${\mathit{v}}_v(k)$ and ${\mathit{v}}_P(k)$ are the measurement noises, which correspond to the velocity and position errors of GNSS, respectively.

3.1.3. Measurement Model of INS/CNS Subsystem

CNS provides high-precise attitude information for a vehicle through the star sensor. Take the difference between INS and CNS in terms of attitude as the measurement of the INS/CNS subsystem, i.e.,

$${\mathit{z}}_2(k)=\left[\begin{array}{ccc}{\varphi }_{EI}-{\varphi }_{EC}& {\varphi }_{NI}-{\varphi }_{NC}& {\varphi }_{UI}-{\varphi }_{UC}\end{array}\right]$$

(51)

where ${\left({\varphi }_{EI},{\varphi }_{NI},{\varphi }_{UI}\right)}^{\mathrm{T}}$ and ${\left({\varphi }_{EC},{\varphi }_{NC},{\varphi }_{UC}\right)}^{\mathrm{T}}$ are the attitude information obtained by INS and CNS, respectively.

Accordingly, the measurement model of the INS/CNS subsystem is constructed as [11,24]

$${\mathit{z}}_2(k)={\mathit{H}}_2(k)\mathit{x}(k)+{\mathit{v}}_2(k)$$

(52)

where ${\mathit{H}}_2(k)=[{\mathit{I}}_{3\times 3},{\mathbf{0}}_{3\times 12}]$, and ${\mathit{v}}_2(k)$ is the measurement noise corresponding to the measurement error of the star sensor.

The INS/GNSS/CNS integrated navigation system is a multi-sensor nonlinear system with 2 local filters (N = 2). The nonlinear system model, which is generally described by (1), is now specified by (48), (50) and (52) for the specific INS/GNSS/CNS integrated navigation system. The system models of the INS/GNSS and INS/CNS subsystems are described by (48) and (50), and (48) and (52), respectively. Based on these, the AFUKF based on Mahalanobis distance described in Section 2.1.2 serves as the local filters to calculate the local state estimations of the INS/GNSS and INS/CNS subsystems. Subsequently, both local state estimations of the INS/GNSS and INS/CNS subsystems are fused by the UT-based multi-sensor optimal data fusion described in Section 2.2 to calculate the globally optimal state estimation for the INS/GNSS/CNS integration.

3.2. Simulations and Analysis

Monte Carlo simulations were conducted to comprehensively evaluate the performance of the proposed AFUKF-MODF for the dynamic flight of an unmanned aerial vehicle (UAV) using the INS/GNSS/CNS integration for navigation and positioning. The flight trajectory, which involves various maneuvers such as climbing, pitching, rolling and turning, is shown in Figure 2. The simulation parameters are listed in

Table 1. Simulation parameters.

Parameters			Values
Initial parameters	Initial position (longitude-latitude-altitude)		(108.997°, 34.246°, 5000 m)
	Initial velocity (east-north-up)		(0 m/s, 150 m/s, 0 m/s)
	Initial attitude (pitch-roll-yaw)		(0°, 0°, 0°)
Initial parameter errors	Initial position error (longitude-latitude-altitude)		(10 m, 10 m, 15 m)
	Initial velocity error (east-north-up)		(0.4 m/s, 0.4 m/s, 0.4 m/s)
	Initial attitude error (pitch-roll-yaw)		(1′, 1′, 1.5′)
INS parameters	Gyro parameters	Constant drift	0.1 °/h
		Random walk coefficient	0.05 °/$\surd \mathrm{h}$
		Sampling frequency	20 Hz
	Accelerometer parameters	Zero-bias	1 × 10−3 g
		Random walk coefficient	1 × 10−4 g$\times \surd \mathrm{s}$
		Sampling frequency	20 Hz
GNSS parameters	Horizontal position error (RMS)		5 m
	Altitude error (RMS)		8 m
	Velocity error (RMS)		0.05 m/s
	Data update rate		1 Hz
CNS parameters	Attitude error (RMS)		5″
	Data update rate		1 Hz
Simulation time	1000 s

. Both filtering period and fusion period were 1 s. The Monte Carlo simulations were carried out 100 times.

The initial state error covariances and process noise covariance for the local filters are set as

$${\mathit{P}}_i(0)=diag\left[{\left(1^{\prime }\right)}^2,{\left(1^{\prime }\right)}^2,{\left({1.5}^{\prime }\right)}^2,{\left(0.4\mathrm{m}/\mathrm{s}\right)}^2{\mathit{I}}_{3\times 3},{\left(10\mathrm{m}\right)}^2,{\left(10\mathrm{m}\right)}^2,{\left(15\mathrm{m}\right)}^2,{\left({0.1}^{\circ }/\mathrm{h}\right)}^2{\mathit{I}}_{3\times 3},{\left(1\times {10}^{-3}\mathrm{g}\right)}^2{\mathit{I}}_{3\times 3}\right](i=1,2)$$

(53)

$$\mathit{Q}=diag\left[{\left({0.05}^{\circ }/\sqrt{\mathrm{h}}\right)}^2{\mathit{I}}_{3\times 3},{\left(1\times {10}^{-4}\mathrm{g}\cdot \sqrt{\mathrm{s}}\right)}^2{\mathit{I}}_{3\times 3},{\mathbf{0}}_{9\times 9}\right]$$

(54)

However, ${\mathit{P}}_i(0)$ ($i=1,2$) and $\mathit{Q}$ were enlarged to the twice of their initial values for both FKF and UKF-FKF to eliminate the correlation between the two local state estimations by the use of covariance upper bound. Moreover, the process model of the INS/GNSS/CNS integration should be linearized in local fitering processes for the FKF.

The measurement noise covariances are set as

$${\mathit{R}}_1=diag\left[{\left(0.05\mathrm{m}/\mathrm{s}\right)}^2{\mathit{I}}_{3\times 3},{\left(5\mathrm{m}\right)}^2,{\left(5\mathrm{m}\right)}^2,{\left(8\mathrm{m}\right)}^2\right]$$

(55)

$${\mathit{R}}_2={\left(5^{″}\right)}^2{\mathit{I}}_{3\times 3}$$

(56)

In order to evaluate the performance of the proposed AFUKF-MODF in terms of process-modeling error, the following process-modeling error is introduced to the process model of the INS/GNSS/CNS integration during the time interval from 400 s to 600 s

$$\Delta \mathit{x}={[{\mathbf{0}}_{1\times 3},0.02\mathrm{m}/\mathrm{s},0.02\mathrm{m}/\mathrm{s},0.02\mathrm{m}/\mathrm{s},(2\times {10}^{-6}{)}^{\prime },(2\times {10}^{-6}{)}^{\prime },5\mathrm{m},{\mathbf{0}}_{1\times 6}]}^{\mathrm{T}}$$

(57)

Accordingly, the process model used in the local filters is

$$\{\begin{array}{cc}\mathit{x}(k)=f\left(\mathit{x}(k-1)\right)+\mathit{w}(k)\hfill & ,\mathrm{other}\mathrm{time}\mathrm{intervals}\hfill \\ \mathit{x}(k)=f(\mathit{x}(k-1))+\Delta \mathit{x}+\mathit{w}(k)\hfill & ,(400\mathrm{s},600\mathrm{s})\hfill \end{array}$$

(58)

To identify the above process-modeling error, ${\chi }_{m_1,\alpha }^2$ and ${\chi }_{m_2,\alpha }^2$ used in the proposed AFUKF-MODF for the INS/GNSS subsystem and INS/CNS subsystem were chosen as 12.592 and 7.815, respectively. These values are resulted from the ${\chi }^2$ distribution when the reliability level is 95% ($\alpha =0.05$) and the degrees of freedom are 6 and 3, respectively.

For comparison analysis, simulation trials were conducted at the same conditions by using FKF, UKF-FKF, UKF-MODF and AFUKF-MODF, respectively. Moreover, the overall estimation error is adopted to evaluate the navigation accuracy in the simulation analysis for the four fusion methods. It is defined as the norm of the navigation parameters estimation error [38]

$$\Vert \Delta x\Vert =\sqrt{\Delta x_E^2+\Delta x_N^2+\Delta x_U^2}$$

(59)

where $\Delta x_E$, $\Delta x_N$ and $\Delta x_U$ are the components of $\Delta x$ in East, North and Up, respectively.

Figure 3, Figure 4 and Figure 5 depict the root mean squared errors (RMSEs) of overall attitude errors, velocity errors and position errors by the above four fusion methods, respectively. During the time intervals (0 s, 400 s) and (600 s, 1000 s) without process-modeling error involved, FKF has poor navigation accuracy compared to UKF-FKF, UKF-MODF and AFUKF-MODF. Its estimation RMSEs in attitude, velocity and position are around ${0.2010}^{\prime }$, 0.2084 m/s and 15.1129 m. This is because the use of covariance upper bound and the linearization of the system model cause suboptimal fusion performance. UKF-FKF adopts UKF to calculate the local state estimations, thus effectively refraining from the error caused by the linearization of system model and leading to the improved fusion accuracy. Its estimation RMSEs in attitude, velocity and position are around ${0.1844}^{\prime }$, 0.1647 m/s and 13.1980 m. However, due to the use of covariance upper bound, the fusion results of UKF-FKF are still suboptimal. Different to FKF and UKF-FKF, both UKF-MODF and AFUKF-MODF are directly derived based on the principle of linear minimum variance, avoiding the use of covariance upper bound in the local filters. Thus, the fusion results obtained by both UKF-MODF and AFUKF-MODF are globally optimal, which are much more accurate than those by FKF and UKF-FKF. The estimation RMSEs in attitude, velocity and position are around ${0.1612}^{\prime }$, 0.1155 m/s and 10.6099 m for UKF-MODF, and ${0.1643}^{\prime }$, 0.1176 m/s and 10.7212 m for AFUKF-MODF. Moreover, since there is no process-modeling error identified, the proposed AFUKF-MODF does not incorporate the adaptive fading factor into the local estimation processes. Accordingly, its navigation accuracy is close to that of UKF-MODF during these time intervals.

During the time interval (400 s, 600 s), due to the influence of the introduced process-modeling error, the navigation accuracy of FKF, UKF-FKF and UKF-MODF degrades seriously. This is because these three methods do not have the ability to resist the influence of process-modeling error on the fusion solution. The estimation RMSEs in attitude, velocity and position are around ${0.2319}^{\prime }$, 0.2471 m/s and 17.8467 m for FKF; ${0.2078}^{\prime }$, 0.1917 m/s and 15.2703 m for UKF-FKF; and ${0.1887}^{\prime }$, 0.1472 m/s and 13.1225 m for UKF-MODF. In contrast, the proposed AFUKF-MODF has higher navigation accuracy than FKF, UKF-FKF and UKF-MODF during this time interval. Its estimation RMSEs in attitude, velocity and position are around ${0.1702}^{\prime }$, 0.1280 m/s and 11.5798 m. This is because the proposed AFUKF-MODF can identify the process-modeling error according to the hypothesis testing theory and effectively refrain from the influence of process-modeling error on the fusion solution by adjusting the time-varying adaptive fading factor incorporated in the predicted state covariance matrix, leading to a strong adaptability and robustness.

The mean RMSEs of the over attitude errors, velocity errors and position errors obtained by FKF, UKF-FKF, UKF-MODF and AFUKF-MODF for the time interval (400 s, 600 s) and the other time intervals are listed in

Table 2. Mean RMSEs of the overall estimation errors obtained by FKF, UKF-FKF, UKF-MODF and AFUKF-MODF for the simulation case.

Data fusion Methods	Navigation Errors	Mean RMSE
		(400 s, 600 s)	The Other Time Intervals
FKF	Attitude error (′)	0.2319	0.2010
	Velocity error (m/s)	0.2471	0.2084
	Position error (m)	17.8467	15.1129
UKF-FKF	Attitude error (′)	0.2078	0.1844
	Velocity error (m/s)	0.1917	0.1647
	Position error (m)	15.2703	13.1980
UKF-MODF	Attitude error (′)	0.1887	0.1612
	Velocity error (m/s)	0.1472	0.1155
	Position error (m)	13.1225	10.6099
AFUKF-MODF	Attitude error (′)	0.1702	0.1643
	Velocity error (m/s)	0.1280	0.1176
	Position error (m)	11.5798	10.7212

. The results in Table 2 also verify that the proposed AFUKF-MODF has a better adaptability and robustness than the other methods, thus leading to improved navigation accuracy for the INS/GNSS/CNS integration.

The above simulations and analysis demonstrate that the proposed AFUKF-MODF can overcome the limitation of using covariance upper bound and provide globally optimal fusion results. It can also enhance the adaptability and robustness against process-modeling error to overcome the limitation of UKF-MODF, thus leading to higher navigation accuracy than FKF, UKF-FKF and UKF-MODF in the presence of process-modeling error.

3.3. Experiments and Analysis

Practical experiments were also conducted to evaluate the performance of the proposed AFUKF-MODF by observing the flight of an UAV. As shown in Figure 6, the UAV uses an INS/GNSS/CNS integration system for navigation. This navigation system includes a MTi-100 IMU, a Hemisphere P307 BDS/GNSS receiver and a SODERN SED26 star sensor. The main parameters of the above three devices are listed in

Table 3. Noise parameters of MTi-100 IMU.

Noise Parameters		Values
Gyro	Constant drift	$10°/\mathrm{h}$
	Random walk coefficient	${0.6}^{\circ }/\sqrt{h}$
Accelerometer	Zero-bias	$40\mathsf{\mu }\mathrm{g}$
	Random walk coefficient	$80\mathsf{\mu }\mathrm{g}\times \sqrt{\mathrm{s}}$

,

Table 4. Main parameters of Hemisphere P307 BDS/GNSS receiver.

Feature Parameters	Values
Satellite signals	BDS(B1, B2, B3), GPS(L1, L2), GLONASS(G1, G2)
Horizontal position error (RMS)	1.2 m
Altitude error (RMS)	3 m
Velocity error (RMS)	0.02 m/s
Data update rate	20 Hz

and

Table 5. Main parameters of SODERN SED26 star sensor.

Feature Parameters	Values
Field of view (FOV)	15.437° × 15.437° (Wide FOV)
Observable star number	$\ge$6
Attitude error (RMS)	$5^{″}$
Data update rate	10 Hz

. Moreover, another Hemisphere P370 BDS/GNSS receiver, which was placed at a local reference station (around 1km from the initial UAV position), was used along with the one mounted on the UAV to provide the differential GPS (DGPS) data. The maximal distance between the UAV and local reference station was less than 60 km to achieve the position accuracy of less than 0.1 m from the DGPS via post difference processing. The DGPS data were used as the reference values to evaluate the positioning error of the INS/GNSS/CNS integrated system.

The UAV flight test was conducted at the City of Yanliang in Shaanxi, China. The experimental navigation data were selected from the UAV flight test within a continuous time period of 1000 s, where different maneuvers were involved. Within the selected UAV flight test, the UAV initial position was at East longitude $109.217°$, North latitude $34.647°$ and altitude 3525 m. The UAV initial velocity was 180 m/s, 150 m/s and 50 m/s in East, North and Up, respectively. The other initial parameters were set identically to those in the simulation case. The filtering periods of the local filters, i.e., the INS/GNSS and INS/CNS subsystems, were 1 s. The period for fusion of local state estimations was also 1 s.

Due to the complexity of the dynamic flight environment, the process model of the INS/GNSS/CNS integration involves modeling error during the UAV flight process. Figure 7 illustrates the UAV position errors obtained by FKF, UKF-FKF, UKF-MODF and AFUKF-MODF, respectively. As shown in Figure 7, the position error achieved by FKF is relatively large due to the influence of process-modeling error, the use of the covariance upper bound and the linearization of the process model. During the time period (100 s, 1000 s), the position errors in longitude, latitude and altitude for FKF are within (−23.0174 m, 20.7557 m), (−21.7712 m, 20.6464 m) and (−26.7975 m, 27.7306 m), respectively. UKF-FKF improves the fusion accuracy of FKF by adopting UKF to calculate the local state estimations, leading to the longitude, latitude and altitude errors within (−15.1817 m, 16.5516 m), (−16.5740 m, 15.9417 m) and (−21.8423 m, 24.0462 m). UKF-MODF can provide globally optimal fusion results based on the principle of linear minimum variance. Thus, it has higher navigation accuracy than FKF and UKF-FKF, leading to the longitude, latitude and altitude errors within (−12.0942 m, 11.8042 m), (−12.0311 m, 12.2480 m) and (−16.9458 m, 15.9384 m). However, its position error curve still has pronounced oscillations due to the influence of the process-modeling error. In contrast, the position errors in longitude, latitude and altitude for the proposed AFUKF-MODF are within (−7.1247 m, 7.8715 m), (−7.0576 m, 7.5495 m) and (−11.8677 m, 11.2610 m), which are much smaller than those by FKF, UKF-FKF and UKF-MODF. This is because the proposed AFUKF-MODF has the capability to inhibit the disturbances of process-modeling error.

Table 6. MAEs and STDs of the position errors obtained by FKF, UKF-FKF, UKF-MODF and AFUKF-MODF for the UAV experiment test.

Data Fusion Methods		Position
		Longitude	Latitude	Altitude
FKF	MAE (m)	7.2719	7.3707	9.0803
	STD (m)	8.8325	8.9150	11.1273
UKF-FKF	MAE (m)	5.4530	5.5924	8.0972
	STD (m)	6.5879	6.8447	9.8784
UKF-MODF	MAE (m)	3.8872	3.8996	6.1051
	STD (m)	4.8051	4.8142	7.3485
AFUKF-MODF	MAE (m)	2.3503	2.3610	4.0656
	STD (m)	2.9015	2.9225	4.9706

lists the mean absolute errors (MAEs) and standard deviations (STDs) of the position errors achieved by FKF, UKF-FKF, UKF-MODF and AFUKF-MODF. It can be seen that the MAE and STD of the position errors by the proposed AFUKF-MODF are also much smaller than those of the other three methods.

The above experimental results and analysis demonstrate that the proposed AFUKF-MODF enhances the adaptability and robustness of multi-sensor data fusion in presence of process-modeling errors, leading to improved navigation accuracy for INS/GNSS/CNS integration in comparison with FKF, UKF-FKF and UKF-MODF.

4. Conclusions

This paper presents a novel AFUKF-MODF to improve the fusion adaptability and robustness for multi-sensor nonlinear stochastic systems against process-modeling error. The contributions of this paper are: (i) an AFUKF based on the Mahalanobis distance is developed to improve the adaptability and robustness of local state estimations against process-modeling error; and (ii) an UT-based multi-sensor optimal data fusion for the case of N local filters is also established to achieve the globally optimal state estimation via fusion of local state estimations. Simulations and practical experiments as well as comparison analysis demonstrate that the proposed AFUKF-MODF not only resists the disturbance of process-modeling error on the fusion solution, leading to improved adaptability and robustness for multi-sensor data fusion, but it also provides globally optimal data fusion results in the sense of linear minimum variance. The achieved fusion accuracy is much higher than that of FKF, UKF-FKF and UKF-MODF for INS/GNSS/CNS integration.

Future research work will focus on two aspects. One is the improvement of the proposed AFUKF-MODF. It is expected to combine the proposed AFUKF-MODF with artificial intelligence technologies such as machine learning, pattern recognition and neural networking to intelligently identify and compensate for system modeling error. The other is on the applications of the proposed AFUKF-MODF to address the multi-sensor data fusion problems in other fields such as target tracking, fault diagnosis and signal processing.