*Article* **Improved Robust High-Degree Cubature Kalman Filter Based on Novel Cubature Formula and Maximum Correntropy Criterion with Application to Surface Target Tracking**

**Tianjing Wang, Lanyong Zhang \* and Sheng Liu**

College of Intelligent Systems Science and Engineering, Harbin Engineering University, Harbin 150001, China **\*** Correspondence: zhanglanyong@hrbeu.edu.cn

**Abstract:** Robust nonlinear filtering is an important method for tracking maneuvering targets in non-Gaussian noise environments. Although there are many robust filters for nonlinear systems, few of them have ideal performance for mixed Gaussian noise and non-Gaussian noise (such as scattering noise) in practical applications. Therefore, a novel cubature formula and maximum correntropy criterion (MCC)-based robust cubature Kalman filter is proposed. First, the fully symmetric cubature criterion and high-order divided difference are used to construct a new fifth-degree cubature formula using fewer symmetric cubature points. Then, a new cost function is obtained by combining the weighted least-squares method and the MCC loss criterion to deal with the abnormal values of non-Gaussian noise, which enhances the robustness; and statistical linearization methods are used to calculate the approximate result of the measurement process. Thus, the final fifth-degree divided difference–maximum correntropy cubature Kalman filter (DD-MCCKF) framework is constructed. A typical surface-maneuvering target-tracking simulation example is used to verify the tracking accuracy and robustness of the proposed filter. Experimental results indicate that the proposed filter has a higher tracking accuracy and better numerical stability than other common nonlinear filters in non-Gaussian noise environments with fewer cubature points used.

**Keywords:** maximum correntropy criterion; fully symmetric cubature criterion; weighted least-squares method; cubature Kalman filter; surface target tracking

## **1. Introduction**

Accurate and robust state estimation is important for the stable target tracking of conventional ships and surface unmanned ships. It is one of the main target-tracking processes for realizing sensor data fusion and anti-interference performance via a filtering algorithm. For linear Gaussian state space models, the Kalman filter (KF) is a powerful optimal estimation algorithm based on minimum mean square error. It is the most widely used adaptive filter because of its analytical optimality, algorithm stability, and simplicity. However, most commonly used target-tracking models are nonlinear, and this limits the role of the traditional KF, which only applies to linear models in practical applications.

Therefore, a nonlinear filtering algorithm in the Gaussian filter framework is required for target tracking. The extended Kalman filter (EKF) [1–3] is a common filtering method that linearizes the nonlinear model by using the multivariate Taylor formula of the nonlinear function to perform local linear approximation for obtaining a linear model, which degrades the model to the general KF model. However, for functions with strong local nonlinearity, the fitting accuracy is poor, the effect of filtering is not ideal, and the calculation of the Jacobian matrices of complex multivariate functions is difficult. As a better alternative to the EKF, the unscented Kalman filter (UKF) [3–5] was proposed to deal with highly nonlinearstate estimation problems. While optimizing the model performance, the Jacobian matrix in the EKF need not be calculated, solving the problem of complex calculations. It is a widely used nonlinear filtering method, but has a disadvantage in that the weight may have a

**Citation:** Wang, T.; Zhang, L.; Liu, S. Improved Robust High-Degree Cubature Kalman Filter Based on Novel Cubature Formula and Maximum Correntropy Criterion with Application to Surface Target Tracking. *J. Mar. Sci. Eng.* **2022**, *10*, 1070. https://doi.org/10.3390/ jmse10081070

Academic Editor: Sergei Chernyi

Received: 24 July 2022 Accepted: 2 August 2022 Published: 4 August 2022

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

negative value in the process of untraced transformation, and the positive definiteness of the covariance matrix is difficult to maintain with an increase in the system dimension, eventually leading to filtering divergence [6]. Thus, filtering stability is poor for systems with strong nonlinearity.

To avoid the Jacobian matrix that must be calculated in the EKF, Norgarrd et al. proposed the central difference Kalman Filter (CDKF) [7]. The CDKF uses the Stirling polynomial interpolation formula to approximate the nonlinear system and inserts it into the nonlinear Bayesian filter framework for obtaining a new nonlinear KF. However, the accuracy and computational performance of the filter must be improved for a highly nonlinear system. The Gauss–Hermite quadrature filter (GHQF) [8] is a more accurate nonlinear filter method that uses the Gauss–Hermite numerical integration formula to estimate the parameters of the nonlinear KF and embed it into the framework of the KF to form a new nonlinear filter. Although the numerical integration formula of GHQF improves the parameter estimation accuracy of filtering obviously compared with CDKF, the number of points required in the integral formula grows exponentially as the system dimension increases, which increases the computational burden for parameter estimation, leading to the problem of "dimension explosion".

Arasaratnam et al. [6] proposed a cubature Kalman filter (CKF) based on the thirddegree spherical–radial criterion, which uses the spherical–radial cubature criterion to solve the probability density integrals in the framework of the nonlinear KF, providing a systematic solution to the problem of high-dimensional nonlinear filtering. Furthermore, the CKF based on the square-root criterion was derived to improve the numerical stability in the calculation process [9]. The growth rate of the cubature points used in the spherical–radial numerical integration formula with an increase in the dimension is significantly lower than that for the GHQF, avoiding the problem of "dimension explosion" and bringing considerable advantages with regard to computational complexity and stability. However, the filter constructed by the third-degree spherical–radial criterion is not as accurate as the GHQF. Bin Jia et al. [10] proposed a high-degree spherical–radial criterion that can calculate an arbitrary order accuracy according to the third-degree spherical–radial criterion, and on this basis, they proposed the high-degree cubature Kalman filter (HDCKF). Because high-degree cubature formulas are used, the nonlinear KF has higher precision, and better numerical stability is achieved. According to the spherical–radial criterion, Dong Meng et al. [11] proposed a high-degree CKF calculation formula for the seventh-degree spherical–radial criterion. Table 1 shows the performance comparison of some commonly used filters.


**Table 1.** Performance comparison of some commonly used filters.

Xinchun Zhang et al. [12] used the fully symmetric cubature criterion of J. McNamee [13] to approximate the probability density function integral in the nonlinear KF and then combined it with the KF framework to obtain an embedded CKF (ECKF). Compared with the previous five-degree filter constructed according to the spherical–radial cubature criterion, the embedded cubature filter has fewer cubature points and can reduce the number of computations while maintaining the filtering accuracy. Additionally, because the coordinates of the cubature points do not increase with the system dimension *n*, compared with spherical–radial quadrature filter, the stability of the nonlinear filter is enhanced. However, even the embedded KF with fewer cubature points and a lower structural

complexity than the HDCKF has problems related to computational stability. It is necessary to develop a quadrature formula that can improve the stability of CKF iteration while maintaining the filtering accuracy and controlling the number of computations.

Conventional CKFs and high-order spherical–radial filtering algorithms have high performance under Gaussian noise conditions, but their accuracies decrease or even diverge under non-Gaussian noise or mixed Gaussian noise conditions because they are based on second-order information estimation via the KF framework. Unfortunately, in most practical applications, because the system is affected by the surrounding environment, e.g., unmanned equipment maneuvering extensively in a short time, process noise and measurement noise typically do not obey the simple Gaussian distribution, which degrades the performance of the conventional target-tracking algorithm of the cubature-based filter.

To solve this problem, scholars proposed some robust filters based on the conventional KF framework, which can adapt to the noise of non-Gaussian systems. For a general linear system containing non-Gaussian noise, Izanloo R et al. [14] developed a new optimization objective function based on the maximum correntropy criterion (MCC) and combined it with the weighted least squares (WLS) method. The fixed-point iteration method was used to obtain the optimal solution of the state estimation equation, which was inserted into the standard flow of the conventional KF to obtain the MCC-KF. The MCC-KF has the same structure as the KF and uses higher-order (>2) statistics to obtain state estimation parameters. Compared with the UKF and the Gaussian sum filter (GSF) [15], the MCC-KF has a smaller estimation error and does not require the use of multiple filters or sigma points; additionally, it has a lower computational complexity and less computations than the UKF and GSF. Guoqing Wang et al. [16] proposed the maximum correlation entropy unscented Kalman filter (MC-UKF) and unscented information filter (MC-UIF) based on the MCC combined with the framework of the UKF and information filtering to solve the filtering problem of nonlinear systems in non-Gaussian noise environments. Compared with the existing UKF algorithm, similar or better estimation results are obtained. When the core bandwidth is infinite, the proposed MC-UKF and MC-UIF converge to the UKF and UIF, respectively. Qingwen Meng et al. [17] proposed a robust KF based on the third-degree spherical–radial CKF and the smallest Cauchy kernel loss (CKL) function. Under the filtering framework of the third-degree CKF, a new optimization objective function was obtained by combining the WLS method with the smallest CKL function. The simulation results of typical nonlinear systems verify that the MCK-CKF has strong robustness and a high filtering efficiency against non-Gaussian noise. He et al. [18] proposed an adaptive and robust CKF based on the MCC of the variable decibel Bayesian (VB) method to solve the problems of unknown measurement noise covariance and outliers in a visual and dual inertial measurement unit integrated-attitude system.

To overcome the shortcomings of robust KFs based on the MCCKF, MC-UKF, and GSF algorithms with regard to the filtering accuracy and numerical stability, a new robust nonlinear KF, based on a novel cubature formula and MCC is proposed in this study. In contrast to the general spherical–radial criterion-based CKF, a new numerical integral quadrature formula was first constructed using a fully symmetric quadrature criterion and high-order divided difference formula to approximate the probability density of the Gaussian weighted integral form in the CKF state and measurement update. A cubature formula with good comprehensive performance is obtained, which considers the number of cubature points, numerical stability, and calculation accuracy. Then, a new optimization objective function and parameter estimation equation are defined by combining the WLS method and MCC. The solving process is combined with the filtering process of the constructed high-degree CKF framework to obtain a nonlinear KF, i.e., the fifth-degree divided difference-maximum correntropy cubature Kalman filter (DD-MCCKF). Finally, typical surface-target-tracking simulation examples were used to verify the performance of the filter. The experimental results indicate that the fifth-degree DD-MCCKF has high filtering accuracy and stability as compared to third-degree MCCKF, fifth-degree MCCKF, embedded MCCKF, and MC-UKF when there are two different types of non-Gaussian mixture noise.

#### **2. Construction of New High-Degree Cubature Formula**

*2.1. Nonlinear Filtering Problem and Gaussian Weighted Integral (GWI)*

Consider the following nonlinear systems that can be described by discrete nonlinear state–space models:

$$\begin{cases} \mathbf{x}\_{k+1} = f(\mathbf{x}\_k, \mathbf{u}\_k) + \mathbf{w}\_k\\ \mathbf{z}\_k = h(\mathbf{x}\_k, \mathbf{u}\_k) + \mathbf{v}\_k \end{cases} \tag{1}$$

where *f*(*x*, *u*) and *h*(*x*, *u*) are arbitrary nonlinear functions and *w<sup>k</sup>* and *v<sup>k</sup>* are the mutually independent system process noise and measurement noise with covariance matrices *<sup>Q</sup><sup>k</sup>* and *Rk*, respectively. Further, *u<sup>k</sup>* represents the control input, and *x<sup>k</sup>* and *z<sup>k</sup>* represent the system state and measurement, respectively, at time *k*.

The state posterior distribution *<sup>p</sup>*(*xk*|*Zk*) of the above discrete system at time *<sup>k</sup>* can be estimated using the measurement set *<sup>Z</sup><sup>k</sup>* <sup>=</sup> {*z*1, *<sup>z</sup>*2,..., *<sup>z</sup>k*} formulated in Equation (1), according to the Bayesian estimation theory. Using the Chapman–Kolmogorov equation, the posterior density can be estimated and updated as follows:

$$p(\mathbf{x}\_k|\mathbf{Z}\_{k-1}) = \int p(\mathbf{x}\_k|\mathbf{x}\_{k-1}) p(\mathbf{x}\_{k-1}|\mathbf{Z}\_{k-1}) d\mathbf{x}\_{k-1} \tag{2}$$

$$p(\mathbf{x}\_k|\mathbf{Z}\_k) = \frac{p(z\_k|\mathbf{Z}\_k)p(\mathbf{x}\_{k-1}|\mathbf{Z}\_{k-1})}{\int p(\mathbf{z}\_k|\mathbf{Z}\_k)p(\mathbf{x}\_k|\mathbf{Z}\_{k-1})d\mathbf{x}\_k} \tag{3}$$

For nonlinear systems, the posterior density cannot be directly calculated because the high-dimensional integral in the equation does not have an exact analytical solution. Therefore, approximate or suboptimal Bayesian algorithms must be used for nonlinear systems. There are some limitations to using the existing methods to filter nonlinear non-Gaussian systems.

Because Equations (2) and (3) cannot be calculated accurately, and in consideration of the accuracy and computational complexity, CKF is typically used as a Gaussian approximation filtering algorithm. Before using it, the following key assumptions of the one-step posterior predictive PDF of the state *x<sup>k</sup>* and measurement *z<sup>k</sup>* conditioned by *Z<sup>k</sup>* must first be made:

$$p(\mathbf{x}\_k|\mathbf{Z}\_{k-1}) = N\left(\mathbf{x}\_{k'}\mathbf{\hat{x}}\_{k|k-1'}P\_{k|k-1}\right) \tag{4}$$

$$p(z\_k|\mathbf{Z}\_{k-1}) = N\left(z\_{k'}\mathbf{\hat{z}}\_{k|k-1'}\mathbf{P}\_{k|k-1}^{zz}\right) \tag{5}$$

By Equations (4) and (5) and the Bayesian rule, the posterior PDF of the state is also Gaussian, that is, *<sup>p</sup>*(*xk*|*Zk*) <sup>=</sup> *<sup>N</sup> <sup>x</sup>k*; ˆ*xk*|*k*,*Pk*|*<sup>k</sup>* . In this manner, we transform the general nonlinear filtering problem into a Kalman filtering problem under a Gaussian framework.

CKF is a suboptimal filtering algorithm that combines precision and computational performance. The difficulty in the CKF filtering process lies mainly in calculating the following Gaussian weighted integral (GWI):

$$G(f) = \int\_{\mathbb{R}^n} f(\mathbf{x}) N(\mathbf{x}; \boldsymbol{\mu}, \boldsymbol{\Sigma}) d\mathbf{x} \,. \tag{6}$$

where *f*(*x*) is a multivariate function, *x* = (*x*1, *x*2,..., *xn*), which does not yield exact results that conform to analytical expressions when *f*(*x*) is nonlinear and must be calculated by numerical integral approximation methods.

As we know the expression of normal distribution function *N*(*x*; *μ*,*P*) from Equation (7), this integral can be simplified by linear transformation of the integral variable.

$$N(\mathbf{x}; \boldsymbol{\mu}, \boldsymbol{\Sigma}) = \frac{1}{\left(2\pi\right)^{\mathbf{n}/2} |\boldsymbol{\Sigma}|^{1/2}} \exp\left(-\frac{(\mathbf{x} - \boldsymbol{\mu})^T \boldsymbol{\Sigma}^{-1} (\mathbf{x} - \boldsymbol{\mu})}{2}\right). \tag{7}$$

Then, let *<sup>x</sup>* <sup>=</sup> <sup>√</sup>2Σ*<sup>v</sup>* <sup>+</sup> *<sup>μ</sup>*. The specific integral form of the Gaussian weighted integral can be simplified as follows:

$$\begin{split} G(f) &= \int\_{\mathbb{R}^{\mathfrak{n}}} \frac{f(\mathbf{x})}{(2\pi)^{n/2} |\boldsymbol{\Sigma}|^{1/2}} \exp\left(-\frac{(\boldsymbol{x} - \boldsymbol{\mu})^{T} \boldsymbol{\Sigma}^{-1} (\boldsymbol{x} - \boldsymbol{\mu})}{2}\right) d\mathbf{x} \\ &= \frac{1}{(\boldsymbol{\pi})^{n/2}} \int\_{\mathbb{R}^{\mathfrak{n}}} f\left(\sqrt{2\boldsymbol{\Sigma}} \boldsymbol{\nu} + \boldsymbol{\mu}\right) \exp\left(-\boldsymbol{\nu}^{T} \boldsymbol{\nu}\right) d\boldsymbol{\nu} \end{split} \tag{8}$$

The integral can be approximated numerically via many proposed numerical approximation methods. A typical example is the use of unscented transform (UT) or the spherical–radial cubature criterion for approximation, which can be combined with the KF framework to obtain the UKF or CKF, respectively. The third-degree spherical–radial numerical integration formula is as follows:

$$\int\_{\mathbb{R}^{n}} f\left(\sqrt{2\Sigma}\boldsymbol{\nu} + \boldsymbol{\mu}\right) \exp\left(-\boldsymbol{\nu}^{T}\boldsymbol{\nu}\right) d\boldsymbol{\nu} \approx \frac{(\pi)^{n/2}}{2n} \sum\_{k=1}^{N} \left( f\left(\sqrt{2n\Sigma}\boldsymbol{\nu}\_{k} + \boldsymbol{\mu}\right) + f\left(-\sqrt{2n\Sigma}\boldsymbol{\nu}\_{k} + \boldsymbol{\mu}\right) \right) \tag{9}$$

where *<sup>N</sup>* = *<sup>n</sup>* represents the system dimension, and *<sup>e</sup><sup>k</sup>* is the kth column in the *n*-order identity matrix *<sup>E</sup>*. The identity matrices *<sup>E</sup>* and <sup>−</sup>*<sup>E</sup>* form the first set of fully symmetric cubature points in the numerical approximation formula. This formula can stably approach the original integral with a minimum number of cubature points.

From the numerical analysis point of view, the formula of untraced transformation (UT) shows that when the dimension of the system, *n*, exceeds three, its stability decreases linearly with the increase in dimension *N*, thus causing a significant disturbance in the numerical estimation of the moment integral. Because there is no square root solution in the UKF, when the pseudo-square root operation is performed on the error covariance matrix, a non-positively determined updated matrix can be obtained owing to the existence of sigma points with negative weights in the UKF. Therefore, it is impossible to express the square root UKF with a numerical advantage similar to the square root-CKF by formula. The covariance matrix calculated by the UKF is not always guaranteed to be positive definite, and the unavailability of the square root covariance causes the UKF to stop running. However, the set of cubature points in the CKF does not have these problems. The cubature point method is mathematically more accurate and principled than the sigma point method [6].

#### *2.2. Commonly Used High-Degree Cubature Rules*

The accuracy of the numerical integration formula is determined primarily by the order of the fitting polynomial. Cubature formulas of the fifth degree can obtain higher numerical approximation accuracy at the cost of using more cubature points. In the third-degree cubature formula, only the GWI corresponding to polynomial - 1, *x*<sup>2</sup> 1 . is accurate, and its approximation error mainly comes from the fourth order or higher polynomial integration in the expansion of function *f*(*x*). In the formula of the fifth degree, the GWI corresponding to - 1, *x*<sup>2</sup> <sup>1</sup>, *<sup>x</sup>*<sup>4</sup> <sup>1</sup>, *<sup>x</sup>*<sup>2</sup> 1*x*2 2 . is accurate, and its approximation error mainly comes from the GWI corresponding to the sixth and higher order polynomials. To ensure numerical stability, the fifth degree can approximate GWI with higher numerical accuracy and obtain more accurate integral results in simple numerical calculation problems.

To increase the approximation accuracy of the numerical integration for the cubature formula, the third-degree cubature criterion in the CKF can be extended to higher degrees. For simplicity, we use *u*(*x*) instead of *f* <sup>√</sup>2Σ*<sup>x</sup>* <sup>+</sup> *<sup>μ</sup>* in this section.

#### 2.2.1. Basic Formulas and Theorems

We consider the fully symmetric numerical integration formula of the following form:

$$\mathbb{E}\left[G(\mathbf{u})\right] = \int\_{\mathcal{R}^n} \mathfrak{u}(\mathbf{x}) \exp\left(\mathbf{x}^T \mathbf{x}\right) d\mathbf{x} \approx \sum\_{k=1}^N \mathcal{W}\_k \sum\_{FS} \mathfrak{u}\left(r\_{1k}, r\_{2k}, \dots, r\_{pk}, \mathbf{0}\right)\_k \tag{10}$$

where *r*1*k*,*r*2*k*,...,*rpk*, **0** *k* represents the *k*th generator of cubature coordinate points and *rpk* represents the *p*th coefficients of the points. Further, *Wk* is the weight of the corresponding part of each generator, and *FS* is a set of fully symmetric cubature points. The integral region *R<sup>n</sup>* and the integrand weighted function exp <sup>−</sup>*xTx* are completely symmetric, with exp <sup>−</sup>*xTx* > 0. In *Rn*, if the set of evaluation points is fully symmetric and *<sup>S</sup>* is the union of the fully symmetric set *<sup>S</sup>i*, Equation (10)

is called a fully symmetric numerical integration formula. The cubature points are generated by different generators *r*1*k*,*r*2*k*,...,*rpk*, **0** *k* , and each generator corresponds to exactly one weight.

In addition, another integration method, the spherical–radial quadrature method, was proposed. In Gaussian weighted integrals, the integrand weight function is of the form exp <sup>−</sup>*xTx* . Thus, Cartesian integration can be converted into a spherical integration via the *n*-dimensional spherical coordinate transformation *<sup>x</sup>* <sup>=</sup> *<sup>r</sup><sup>p</sup>* with *<sup>p</sup>* <sup>=</sup> (cos *<sup>θ</sup>*1, sin *<sup>θ</sup>*<sup>1</sup> cos *<sup>θ</sup>*2, . . . , sin *<sup>θ</sup>*<sup>1</sup> . . . sin *<sup>θ</sup>n*−<sup>2</sup> cos *<sup>θ</sup>n*−1, sin *<sup>θ</sup>*<sup>1</sup> . . . sin *<sup>θ</sup>n*−<sup>2</sup> sin *<sup>θ</sup>n*−1). Using the above coordinate transformation, variable substitution of the Gaussian weighted integral is performed as follows:

$$\begin{split} \mathbb{E}\left(\mathbf{G}(\boldsymbol{\mu}) = \int\_{\mathbf{R}^{\mathsf{u}}} \boldsymbol{\mu}(\mathbf{x}) \exp\left(-\mathbf{x}^{\mathsf{T}}\mathbf{x}\right) d\mathbf{x} \\ = \int\_{S\_{\mathsf{u}}} d\mathbf{S} \int\_{0}^{\infty} \boldsymbol{\mu}(\boldsymbol{r}\boldsymbol{p}) \boldsymbol{r}^{\mathsf{u}-1} \exp\left(-\boldsymbol{r}^{2}\right) d\boldsymbol{r} \approx \sum\_{k=1}^{N\_{\mathsf{r}}} \sum\_{l=1}^{N\_{\mathsf{p}}} w\_{\mathsf{r}k} w\_{\mathsf{p}l} \boldsymbol{u}(\boldsymbol{r}\_{k} \boldsymbol{s}\_{l}) \end{split} \tag{11}$$

This is the spherical radial integral. The integral region of the surface integral *Sn* = *<sup>p</sup>* <sup>∈</sup> *<sup>R</sup><sup>n</sup>* : *<sup>p</sup>*<sup>2</sup> <sup>1</sup> + *<sup>p</sup>*<sup>2</sup> <sup>2</sup> + ... + *<sup>p</sup>*<sup>2</sup> *<sup>n</sup>* = 1 . is an *n*-dimensional hypersphere with radius 1. This special surface integral can be approximated using the spherical isomorphic integration criterion and Stroud's integration formula. The radial integral can be calculated using the moment matching algorithm and the generalized Gauss–Laguerre quadrille criterion.

In the approximate expression of Equation (11), *rk* and *wrk* are the corresponding points of radial integrals and their weights respectively. *<sup>s</sup><sup>l</sup>* and *wpl* are vectors corresponding to spherical integrals and their weights, respectively. According to the cubature integration rules and the numerical method of radial integration, the Gaussian weighted integral can be approximated using a fifthdegree numerical integration formula with the radial integral formula and spherical integral formula. All the fifth-degree cubature formulas based on the spherical–radial criterion can be summarized by the form of Equation (11) [19].

To evaluate the computational complexity and numerical stability of the cubature formula, the following two theorems were introduced:

**Theorem 1.** *The cubature formula of degree 2s–1 has the minimum number of cubature points and given as follows [20]:*

$$P\_{min} = \begin{cases} \binom{n+s-1}{n} + \sum\_{k=1}^{n-1} 2^{k-n} \binom{k+s-1}{k}, s = 2p, p \in N^+\\ \binom{n+s-1}{n} + \sum\_{k=1}^{n-1} \binom{1-2^{k-n}}{k} \binom{k+s-2}{k}, s = 2p-1, p \in N^+ \end{cases} \tag{12}$$

**Theorem 2.** *When the system dimension n is so large that the signs of different weights of the cubature formula are not always positive, the stability of the cubature formula can be evaluated according to the stability coefficient discriminant, as follows:*

$$stb = \frac{\sum\_{\substack{\boldsymbol{\mu}=0 \\ \boldsymbol{\mu}=0}}^{M} \sum\_{k=1}^{N} |\mathcal{W}\_{\boldsymbol{\mu},k}|}{\sum\_{\substack{\boldsymbol{\mu}=0 \\ \boldsymbol{\mu}=0}}^{M} \sum\_{k=1}^{N} |\mathcal{W}\_{\boldsymbol{\mu},k}|} = \frac{\sum\_{\boldsymbol{\mu}=0}^{M} \sum\_{k=1}^{N} |\mathcal{W}\_{\boldsymbol{\mu},k}|}{G(1)} \ge 1 \,. \tag{13}$$

According to Theorem 1, for all third-degree cubature formulas (including the common thirddegree spherical–radial rule-based formula), the minimum number of cubature points is 2*n*. Next, we consider formulas of the fifth degree.

2.2.2. Fifth-Degree Cubature Formulas

• *Formula I*

Stroud et al. [21] provided a fully symmetric fifth-degree cubature formula based on the spherical–radial integration method, which is one of the most widely used cubature formulas in high-degree CKF robust algorithms. This formula uses Stroud's formula [22] to approximate the spherical integral, and the radial integral is approximated via the Gauss–Laguerre numerical integration method. It can be expressed as

$$G(\mathfrak{u}) \approx \frac{2(\pi)^{n/2}}{n+2}\mathfrak{u}(0) + \frac{(4-n)(\pi)^{n/2}}{2(n+2)^2} \sum\_{FS} \mathfrak{u}(s,0) + \frac{(\pi)^{n/2}}{(n+2)^2} \sum\_{FS} \mathfrak{u}(r,r,0) \,, \tag{14}$$

where *s* = /*n* <sup>2</sup> + 1 and *r* = /*n* <sup>4</sup> <sup>+</sup> <sup>1</sup> <sup>2</sup> . The generation method for fully symmetric cubature points is as follows:

$$\begin{aligned} (s, \mathbf{0}) &= \sqrt{2} \begin{bmatrix} s & 0 & 0 & -s & 0 & 0 \\ -\ddots & \cdots & \cdots & \cdots & \cdots & \cdots \\ 0 & 0 & s & 0 & 0 & -s \end{bmatrix} \\ \mathbf{U}\_1 &= \sqrt{2} r \underbrace{(e\_1 + e\_2, \dots, e\_1 + e\_{\text{H}}, e\_2 + e\_3, \dots, e\_2 + e\_{\text{H}}, \dots, e\_{\text{H}}, \dots, e\_{\text{H}-1} + e\_{\text{u}})}\_{n(n-1)/2} \\ \mathbf{U}\_2 &= \sqrt{2} r \underbrace{(e\_1 - e\_2, \dots, e\_1 - e\_{\text{H}}, e\_2 - e\_3, \dots, e\_2 - e\_{\text{H}}, \dots, e\_{\text{H}-1} - e\_{\text{u}})}\_{n(n-1)/2} \\ (r, r, \mathbf{0}) &= (\mathbf{U}\_1, -\mathbf{U}\_1, \mathbf{U}\_2, -\mathbf{U}\_2) \end{aligned} \tag{15}$$

Bin Jia et al. applied Stroud's cubature formula to the parameter estimation of a nonlinear KF and obtained the HDCKF. In the high-degree KF of [10], Equation (14) is rewritten as

$$\begin{split} G(\mathfrak{u}) &\approx \frac{2(\pi)^{n/2}}{n+2} \mathfrak{u}(0) + \frac{(4-n)(\pi)^{n/2}}{2(n+2)^2} \sum\_{k=1}^{n} \left( \mathfrak{u}\left(\sqrt{n+2}\mathfrak{e}\_k\right) + \mathfrak{u}\left(-\sqrt{n+2}\mathfrak{e}\_k\right) \right) \\ &+ \frac{(\pi)^{n/2}}{\left(n+2\right)^2} \sum\_{k=1}^{n\left(n-1\right)/2} \left( \begin{array}{c} \mathfrak{u}\left(\sqrt{n+2}I\_k^+\right) + \mathfrak{u}\left(-\sqrt{n+2}I\_k^+\right) \\ + \mathfrak{u}\left(\sqrt{n+2}I\_k^-\right) + \mathfrak{u}\left(-\sqrt{n+2}I\_k^-\right) \end{array} \right) \end{split} \tag{16}$$

where *l* + *<sup>k</sup>* = *ek* + *el*,*l* − *<sup>k</sup>* <sup>=</sup> *ek* <sup>−</sup> *el*, *<sup>k</sup>* <sup>&</sup>lt; *<sup>l</sup>* <sup>≤</sup> *<sup>n</sup>*. Equations (14) and (16) are essentially identical. The total number of cubature points is 2*n*<sup>2</sup> + 1. According to Theorem 2, when *n* is sufficiently large, its asymptotic stability coefficient converges to 3, resulting in a cubature formula with good numerical stability.
