Adaptive MSSRCPF filtering based on constrained optimization and fading memory for SINS/GNSS integrated navigation

Abstract

To address particle degeneracy and the challenge of selecting importance density functions in traditional particle filters for complex nonlinear systems, we propose an adaptive mixed-order spherical simplex-radial cubature particle filter algorithm based on constrained optimization and fading memory. This algorithm integrates constrained optimization, an adaptive fading memory strategy, adaptive adjustment of particle numbers and weights, and the advantages of mixed-order spherical simplex-radial cubature Kalman filtering. By designing the importance density function using a mixed-order integration method, the algorithm significantly improves filtering accuracy over traditional cubature Kalman filters while maintaining lower computational complexity than high-order cubature Kalman filter methods. The adaptive fading memory strategy dynamically adjusts the covariance matrix, enhancing sensitivity to current measurement information and reducing the influence of historical data. By dynamically adjusting the noise covariance matrix and constraining the ratio between the error covariance and measurement noise covariance, the algorithm improves the convergence speed and accuracy of state estimation. An adaptive particle number and weight adjustment strategy based on effective sample size and entropy regularization dynamically adjusts the number of particles to reduce computational complexity while ensuring filtering accuracy, and employs entropy regularization to suppress weight over-concentration, thereby reducing the impact of outliers on the filtering results. Simulation results demonstrate that in complex SINS/GNSS integrated navigation systems, especially under high-noise and nonlinear conditions, the proposed algorithm significantly improves positioning accuracy compared to the CPF method, with the maximum latitude error reduced by approximately 54.8%, the maximum longitude error reduced by approximately 40.5%, and the maximum altitude error reduced by approximately 68.3%. Compared to the FMCPF method, the proposed algorithm also shows clear advantages in positioning accuracy, convergence speed, and computational efficiency, validating its effectiveness and practicality in complex environments.

Introduction

The Strapdown Inertial Navigation System (SINS) and the Global Navigation Satellite System (GNSS) have complementary advantages. The inertial navigation system offers comprehensive navigation information and strong autonomy, while satellite navigation provides all-weather capability and real-time performance. ¹ Therefore, SINS/GNSS integrated navigation is considered one of the most ideal navigation methods. ²

In SINS/GNSS integrated navigation systems, the Extended Kalman Filter (EKF) is often used to linearize nonlinear models and perform state estimation. Due to its simple structure and ease of implementation, it is widely applied. ³ However, EKF introduces significant truncation errors when dealing with highly nonlinear systems, and its update process requires the computation of complex Jacobian matrices, which limits its application. ⁴ The Unscented Kalman Filter (UKF), by using unscented transformation, improves filtering accuracy without the need to compute Jacobian matrices, ⁵ but its accuracy may degrade or even diverge in high-dimensional nonlinear systems. ⁶ To address these issues, Arasaratnam and Haykin ⁷ proposed the Cubature Kalman Filter (CKF), which approximates the probability density function using the third-order spherical-radial cubature rule. This approach offers higher filtering accuracy and numerical stability compared to UKF. ⁸ Additionally, Fagin and Sorenson’s fading memory filtering algorithm introduces a fading factor to limit the memory length of the Kalman filter, reducing the influence of historical information and enhancing the impact of current measurements in the filtering process, thereby improving accuracy. ⁹ To ensure the symmetry and positive semi-definiteness of the covariance matrix and further improve the filtering accuracy of CKF, Arasaratnam and Haykin ⁷ proposed the Square Root Cubature Kalman Filter (SCKF), which iterates based on the square root form of the error covariance matrix. Chandra et al. ¹⁰ points out that CKF has the highest accuracy among existing Gaussian filters.

Due to the superior performance of the Cubature Kalman Filter (CKF), it has garnered widespread attention in the field of integrated navigation in recent years. ¹¹ Although both UKF and CKF have contributed to improving filtering accuracy, they are still categorized as third-order filtering techniques, ¹² which face limitations when handling higher-order nonlinear systems. ¹³ To address this, researchers have proposed the High-Order Cubature Kalman Filter (HCKF), which achieves fifth-order estimation accuracy and demonstrates advantages over traditional methods. ¹⁴ However, its high computational complexity limits its practical applications. ¹⁵ In Shunyi et al., ¹⁶ HCKF was applied to integrated navigation systems, resulting in a significant improvement in positioning accuracy, but also substantially increasing computational load, which adversely affected the system’s real-time response capabilities. ¹⁷ To improve filtering accuracy while reducing computational complexity, Wang et al. ¹⁸ proposed an improved Cubature Kalman Filter based on the Spherical Simplex-Radial sampling rule (Spherical Simplex-Radial Cubature Kalman Filter, SSRCKF), which offers high filtering accuracy with reduced computational demands. ¹⁵ However, Xiaosu et al. ¹⁹ pointed out that in the SINS/DVL integrated navigation system, the application of the fifth-order SSRCKF caused issues with negative weights in the integration points when the system dimension exceeded seven, leading to a reduction in filtering performance. ²⁰ In response, Wang et al. ²¹ proposed the Mixed-Order Spherical Simplex-Radial Cubature Kalman Filter (MSSRCKF), and simulations demonstrated that this algorithm could achieve the performance improvements of the fifth-order SSRCKF while maintaining higher accuracy with less computational burden. ²² Additionally, to suppress filter divergence caused by model errors, Wangming ²³ introduced a Fading Memory Square Root Cubature Kalman Filter algorithm based on an exponentially weighted fading factor, which enhances the utilization of current measurement data. To address the issues of asymmetric or negative definite covariance matrices that may arise in the Unscented Particle Filter (UPF) algorithm during navigation filtering calculations, Gao et al. ²⁴ proposed a new Fading Memory Square Root UPF algorithm, which combines fading memory filtering with square root filtering to improve filtering stability and accuracy. Furthermore, Ning and Huang ²⁵ provided a comprehensive review of particle filter degradation issues and optimization strategies, particularly focusing on optimizing the importance density function through intelligent algorithms, which mitigates particle degradation and weight concentration problems. These strategies offer effective solutions to the challenges of particle degradation and the selection of importance density functions, significantly enhancing filtering accuracy and system robustness.

Building upon an in-depth study of adaptive fading memory filtering and square root filtering, this paper proposes a new Adaptive Mixed-Order Spherical Simplex-Radial Cubature Particle Filter based on Constrained Optimization and Fading Memory (COFM-AMSSRCPF), using the Mixed-Order Spherical Simplex-Radial Cubature Kalman Filter (MSSRCKF) as the framework. By introducing an adaptive fading factor, the algorithm reduces the influence of historical information on the filtering process, enhancing the weight of current measurement data in the filtering calculations. ²⁶ Additionally, the algorithm employs Cholesky decomposition to effectively maintain the positive definiteness of the covariance matrix, thus avoiding the issue of filter divergence. ²⁵ In practical applications, process noise and measurement noise are often influenced by dynamic external factors. To address this, the algorithm sets the covariance matrices of the process noise and measurement noise as dynamic variables and performs real-time adjustments. ²⁷ By constraining the ratio between the error covariance matrix and the measurement noise covariance matrix, the algorithm significantly improves the convergence speed of state estimation errors and enhances filtering accuracy. ²⁸ Furthermore, based on the mean and variance derived from this algorithm, an optimized design for the importance density function of the particle filter was developed, which significantly improves the positioning accuracy of the Cubature Particle Filter (CPF). ²⁹ To further enhance the algorithm’s performance in complex nonlinear systems, this paper introduces an adaptive particle number and weight adjustment strategy based on Effective Sample Size (ESS) and entropy regularization. ³⁰ This strategy dynamically adjusts the number of particles, effectively reducing computational complexity while ensuring filtering accuracy. By incorporating entropy regularization, the strategy suppresses excessive concentration of particle weights, reduces the influence of outliers on filtering results, and optimizes the weight distribution. This method effectively mitigates the particle degeneration problem, significantly improving the stability and robustness of the algorithm in complex systems.

Main contributions

• (1) Innovative Adaptive Filtering Algorithm: This paper proposes an Adaptive Mixed-Order Spherical Simplex-Radial Cubature Particle Filter based on Constrained Optimization and Fading Memory (COFM-AMSSRCPF). This algorithm effectively addresses the challenges of particle degradation and the selection of the importance density function in complex nonlinear systems. Compared to traditional particle filtering methods, the proposed algorithm significantly enhances filtering accuracy and system robustness in high-noise and strongly nonlinear environments.

• (2) Introduction of a Constrained Optimization Strategy: By dynamically adjusting the noise covariance matrix and constraining the ratio between the error covariance and measurement noise covariance matrices, the proposed algorithm improves the convergence speed and accuracy of state estimation. This optimization strategy enhances adaptability to different noise environments, particularly excelling in highly nonlinear systems.

• (3) Adaptive Fading Memory Strategy: The adaptive fading memory strategy introduced in this paper allows for dynamic adjustment of the covariance matrix, increasing sensitivity to current measurement data while reducing the negative impact of historical data on estimation results. This strategy not only improves system adaptability and robustness but also further optimizes filtering accuracy.

• (4) Mixed-Order Sampling Method: For the first time, this paper introduces the Mixed-Order Spherical Simplex-Radial Cubature Kalman Filter (MSSRCKF) sampling strategy into particle filtering. Compared to traditional CKF, the proposed method significantly improves accuracy, while offering lower computational complexity than high-order CKF, effectively balancing filtering accuracy and computational cost.

• (5) Ensuring Positive Definiteness of the Covariance Matrix: By incorporating Cholesky decomposition, the proposed algorithm guarantees the positive definiteness of the covariance matrix, preventing divergence in the filtering process and further enhancing the stability and reliability of the algorithm.

• (6) Adaptive Particle Number and Weight Adjustment Mechanism: Based on the effective sample size (ESS) and entropy regularization strategy, the algorithm dynamically adjusts the particle number and weights. While ensuring filtering accuracy, the optimized particle number reduces computational complexity, and entropy regularization suppresses excessive concentration of particle weights, improving algorithm stability and computational efficiency.

• (7) Extensive Simulation Validation: The proposed algorithm has undergone extensive simulation testing in complex SINS/GNSS integrated navigation systems. The results demonstrate that under high-noise and strongly nonlinear conditions, the filtering accuracy and stability of the algorithm significantly outperform existing methods, validating its effectiveness and robustness in practical applications.

Structure of the paper

This paper is structured as follows:

• Section 1: Introduction: This section briefly reviews the fundamental concepts of the SINS/GNSS integrated navigation system and existing technologies, highlighting the limitations of current filtering methods (EKF, UKF, CKF, and PF), particularly the challenges of particle degradation and the selection of the importance density function. The motivation for this research is clearly defined, namely to improve filtering accuracy and efficiency in high-noise and nonlinear environments.

• Section 2: SINS/GNSS Integrated Navigation Filtering Model: This section introduces the state and measurement equations for tightly coupled SINS/GNSS integrated navigation. It discusses the principles of filter design under conditions of high noise and nonlinearity.

• Section 3: Mixed-Order Spherical Simplex-Radial Cubature Kalman Filtering Algorithm Based on Constrained Optimization and Fading Memory: A novel filtering algorithm is proposed in this section, combining third-order spherical and fifth-order radial integration rules. By incorporating constrained optimization and adaptive fading memory strategies, the algorithm enhances filtering accuracy and robustness.

• Section 4: Design of the Importance Density Function in Particle Filtering and Adaptive Optimization Strategy: This section discusses the application of the COFM-MSSRCKF algorithm in optimizing particle filtering and proposes an adaptive particle number and weight adjustment strategy based on Effective Sample Size (ESS) and entropy regularization. This strategy effectively mitigates particle degradation and reduces computational complexity.

• Section 5: Performance Analysis: The performance of the COFM-AMSSRCPF algorithm is analyzed, comparing its computational complexity and filtering accuracy with that of traditional CPF. The focus is on the superiority of MSSRCKF over SSRCKF and SSRHCKF. The algorithm combines positive weights, constrained optimization, and adaptive adjustment strategies, showing clear advantages under high-noise and nonlinear conditions.

• Section 6: Simulation and Analysis: This section presents simulation experiments within the SINS/GNSS integrated navigation system to validate the robustness and effectiveness of the COFM-AMSSRCPF algorithm. The results show that the algorithm significantly improves filtering accuracy, convergence speed, and computational efficiency under high-noise and nonlinear conditions.

• Section 7: Conclusion and Outlook: This section summarizes the main contributions of the COFM-AMSSRCPF algorithm, highlighting its adaptability in complex dynamic scenarios and the relative complexity of parameter tuning. Future research will focus on optimizing the adaptive particle number and weight adjustment strategies and exploring the application of the algorithm in multi-sensor fusion systems.

SINS/GNSS integrated navigation filtering model

This study adopts the tightly coupled SINS/GNSS integrated navigation approach. The space equations for tightly coupled SINS/GNSS navigation include the state equation and the measurement equation. Compared with loosely coupled navigation, tightly coupled navigation uses more raw GNSS outputs (pseudorange and pseudorange rate) as the observations. This approach allows the system to maintain high navigation accuracy even when the number of visible GNSS satellites is fewer than four. ³¹ The detailed process is shown in Figure 1.

Figure 1.

Tightly coupled navigation mode.

SINS/GNSS tightly coupled state equation

The establishment of the tightly coupled navigation state equation first requires selecting an appropriate state vector. The state vector chosen in this paper is:

$$X={\left[\begin{array}{c}\delta {\phi }_x,\delta {\phi }_y,\delta {\phi }_z,\delta V_E,\delta V_N,\delta V_U,\delta L,\delta \lambda ,\delta h,\hfill \\ \multicolumn{1}{c}{{ϵ}_{\mathrm{bx}},{ϵ}_{\mathrm{by}},{ϵ}_{\mathrm{bz}},{\nabla }_{\mathrm{bx}},{\nabla }_{\mathrm{by}},{\nabla }_{\mathrm{bz}},\delta t,\delta \stackrel{·}{t}}\end{array}\right]}^{\top }$$

(1)

Where: $\delta {\phi }_x,\delta {\phi }_y,\delta {\phi }_z$ represent the attitude error angles in the three axes; $\delta V_E,\delta V_N,\delta V_U$ represent the velocity errors in the east, north, and up directions; $\delta L,\delta \lambda ,\delta h$ represent the latitude, longitude, and altitude errors; ${ϵ}_{\mathrm{bx}},{ϵ}_{\mathrm{by}},{ϵ}_{\mathrm{bz}}$ represent the gyro biases along the three axes in the body frame; ${\nabla }_{\mathrm{bx}},{\nabla }_{\mathrm{by}},{\nabla }_{\mathrm{bz}}$ represent the accelerometer biases along the three axes in the body frame; $\delta t$ represents the clock bias equivalent distance; $\delta \stackrel{·}{t}$ represents the clock drift rate equivalent distance variation. ³¹

The tightly coupled state equation is:

$$\stackrel{·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$$

(2)

Where $F\left(t\right)$ is the state transition matrix, $G\left(t\right)$ is the system noise input matrix, and $W\left(t\right)$ is the system noise matrix. By establishing the attitude error equation, velocity error equation, position error equation, gyroscope error equation, accelerometer error equation, and the GNSS clock bias and clock drift error models, the specific expressions for $F\left(t\right)$ and $G\left(t\right)$ can be derived.

The specific expression for the system noise matrix $W\left(t\right)$ is:

$$W\left(t\right)={[{\omega }_{\mathrm{gx}},{\omega }_{\mathrm{gy}},{\omega }_{\mathrm{gz}},{\omega }_{\mathrm{ax}},{\omega }_{\mathrm{ay}},{\omega }_{\mathrm{az}},{\omega }_{\mathrm{tu}},{\omega }_{\mathrm{tru}}]}^{\mathrm{T}}$$

Where ${\omega }_{\mathrm{gi}}(i=x,y,z)$ represents the white noise of the gyroscopes along the three axes in the body frame, ${\omega }_{\mathrm{ai}}(i=x,y,z)$ represents the white noise of the accelerometers along the three axes in the body frame; and ${\omega }_{\mathrm{tu}}$ and ${\omega }_{\mathrm{tru}}$ represent the white noise of the clock bias and clock drift, respectively.

SINS/GNSS tightly coupled measurement equation

The tightly coupled measurement equation selects the pseudorange difference and pseudorange rate difference as the measurement vector:

$$Z=\left[\begin{array}{c}\delta \rho \hfill \\ \multicolumn{1}{c}{\delta \stackrel{·}{\rho }}\end{array}\right]=\left[\begin{array}{c}{\rho }_G-{\rho }_{\mathrm{SINS}}\hfill \\ \multicolumn{1}{c}{{\stackrel{·}{\rho }}_G-{\stackrel{·}{\rho }}_{\mathrm{SINS}}}\end{array}\right]$$

(3)

Where ${\rho }_{\mathrm{G}}$ and ${\stackrel{·}{\rho }}_{\mathrm{G}}$ are the GNSS-measured pseudorange and pseudorange rate, respectively; ${\rho }_{\mathrm{SINS}}$ and ${\stackrel{·}{\rho }}_{\mathrm{SINS}}$ are the equivalent pseudorange and pseudorange rate from the SINS system, respectively.

By establishing the measurement equations for the pseudorange and pseudorange rate, the measurement equation for the SINS/GNSS tightly coupled system can be expressed as:

$$\mathbf{Z}=\left[\begin{array}{c}{H}_{\rho }\left(t\right)\hfill \\ \multicolumn{1}{c}{H_{\stackrel{·}{\rho }}\left(t\right)}\end{array}\right]X+\left[\begin{array}{c}{V}_{\rho }\left(t\right)\hfill \\ \multicolumn{1}{c}{V_{\stackrel{·}{\rho }}\left(t\right)}\end{array}\right]$$

(4)

Where $H_{\rho }\left(t\right)$ and $H_{\stackrel{·}{\rho }}\left(t\right)$ are the observation matrices for the pseudorange and pseudorange rate, respectively; $V_{\rho }\left(t\right)$ and $V_{\stackrel{·}{\rho }}\left(t\right)$ are the white noise terms for the pseudorange and pseudorange rate, respectively.

MSSRCKF filtering algorithm based on constrained optimization and fading memory

In recent years, the Cubature Kalman Filter (CKF) and its variants have shown significant application potential in integrated navigation systems. The divergence of CKF is mainly caused by computational errors and model inaccuracies. ²⁵ To address the issue of filter divergence due to model errors, this study introduces an adaptive fading factor to enhance the utilization of current measurement data while reducing the weight of historical data, effectively suppressing issues related to covariance matrix asymmetry, negative definiteness, and filter divergence. ²⁶ Considering that process noise and measurement noise are often subject to dynamic external disturbances in practical applications, the algorithm treats the covariance matrices of these noises as dynamic variables and adjusts them in real time. By constraining the ratio between the error covariance matrix and the measurement noise covariance matrix, the convergence speed of state estimation errors and filtering accuracy are significantly improved. ²⁸ Against this backdrop, the Mixed-Order Spherical Simplex-Radial Cubature Kalman Filter (MSSRCKF) has garnered attention due to its higher sampling accuracy and lower computational cost compared to CKF. This method significantly enhances the performance of the fifth-order Spherical Simplex-Radial Cubature Kalman Filter (SSRCKF). MSSRCKF filtering, based on constrained optimization and fading memory, not only provides higher filtering accuracy but also demonstrates superior computational efficiency and numerical stability, making it a promising algorithm for further research and application in navigation systems. The detailed process is shown in Figure 2.

Figure 2.

COFM-MSSRCKF filtering algorithm flowchart.

MSSR cubature sampling rule

To enhance the accuracy of the filtering process and reduce the computational complexity, the MSSR cubature sampling rule integrates third-order spherical integrals and fifth-order radial integrals. The detailed sampling rules are as follows:

Select a set of vectors $a_i={[a_{i,1},a_{i,2},\cdots ,a_{i,n}]}^T$ , for $i=1,2,\cdots ,n+1$ , where n represents the dimension of the state.

$$\begin{array}{c}{a}_{i,j}=\{\begin{array}{c}-{\left[\frac{n+1}{n(n-j+2)(n-j+1)}\right]}^{\frac{1}{2}},j<i\hfill \\ \multicolumn{1}{c}{\begin{array}{c}\multicolumn{1}{c}{{\left[\frac{(n+1)(n-i+1)}{n(n-i+2)}\right]}^{\frac{1}{2}},j=i}\end{array}}\\ \multicolumn{1}{c}{\begin{array}{c}\multicolumn{1}{c}{0,j>i}\end{array}}\end{array}\hfill \end{array}$$

(5)

The definition of the cubature points ${\xi }_i$ of the filter is as follows:

$$\{\begin{array}{c}{\xi }_i=0,i=0\hfill \\ {\xi }_i={[\sqrt{(n+2)}a]}_i,i=1,2,\cdots ,n+1\hfill \\ {\xi }_i=-{[\sqrt{(n+2)}a]}_i,i=n+2,\cdots ,2n+2\hfill \end{array}$$

(6)

Where []i is the set of n-dimensional points calculated using equation (5), with the number of points being n + 1. The weights corresponding to the cubature points are defined as follows:

$$\{\begin{array}{c}{\omega }_i=\frac{2}{n+2},i=0\hfill \\ {\omega }_i=\frac{n}{2(n+1)(n+2)},i=1,2,\cdots ,2n+2\hfill \end{array}$$

(7)

Based on the aforementioned MSSR cubature sampling rule, the Gaussian summation integral with a mean of 0 and variance $I$ can be expressed as follows:

$$\begin{array}{c}{\int }_{R^n}f(x)\mathrm{N}(x;0,I)\mathrm{d}x=\frac{2}{n+2}f(0)+\frac{n}{2(n+1)(n+2)}·\hfill \\ \sum _{i=1}^{n+1}(f\left(\sqrt{(n+2)}{a}_i\right)+f(-\sqrt{(n+2)}{a}_i))\hfill \end{array}$$

(8)

For a Gaussian distribution with a mean of $\overline{x}$ and a variance of P, the cubature points are $\sqrt{P}{\xi }_i+\overline{x}$ . Thus, the multidimensional integral can be expressed as follows:

$$\begin{array}{c}{\int }_{R^n}f(x)N(x;\overline{x},P)\mathrm{d}x=\frac{2}{n+2}f(\overline{x})+\frac{n}{2(n+1)(n+2)}.\hfill \\ \sum _{i=1}^{n+1}(f(\sqrt{(n+2)P}{a}_i+\overline{x})+f(-\sqrt{(n+2)P}{a}_i+\overline{x}))\hfill \end{array}$$

(9)

Where, $P=\sqrt{P}·(\sqrt{P}{)}^T$ .

MSSRCKF filtering algorithm

To improve filtering accuracy and reduce computational complexity, this paper proposes a Mixed-Degree Spherical Simplex-Radial Cubature Kalman Filtering algorithm (MSSRCKF). This algorithm combines the advantages of third-degree spherical integration and fifth-degree radial integration, utilizing a mixed strategy of spherical simplex and radial integration. This approach ensures high accuracy while effectively reducing computational costs. Compared to the traditional Cubature Kalman Filtering (CKF) algorithm, MSSRCKF significantly enhances accuracy in nonlinear system state estimation and demonstrates higher computational efficiency. The specific steps of the filtering algorithm are as follows:

• (1) Calculate the $\xi$ points and their corresponding weights:

$$\{\begin{array}{c}{\xi }_i=0,i=0\hfill \\ {\xi }_i={[\sqrt{(n+2)}a]}_i,i=1,2,\cdots ,n+1\hfill \\ {\xi }_i=-{[\sqrt{(n+2)}a]}_i,i=n+2,\cdots ,2n+2\hfill \end{array}$$

(10)

$$\{\begin{array}{c}{\omega }_i=\frac{2}{n+2},i=0\hfill \\ {\omega }_i=\frac{n}{2(n+1)(n+2)},i=1,2,\cdots ,2n+2\hfill \end{array}$$

(11)

• (2) Time update:

Utilize Cholesky decomposition on the error covariance ${\mathit{P}}_{k-1/k-1}$ :

$${\mathbf{P}}_{k-1/k-1}={\mathbf{S}}_{k-1/k-1}{\mathbf{S}}_{k-1/k-1}^T$$

(12)

Calculate the corresponding cubature points:

$$X_{i,k-1/k-1}=S_{k-1/k-1}{\xi }_i+{\hat{\mathbf{x}}}_{k-1/k-1},i=0,1,\cdots ,2n+2$$

(13)

Propagate the cubature points through the state equation:

$${\mathbf{X}}_{i,k/k-1}^{*}=f\left({\mathbf{X}}_{i,k-1/k-1}\right),i=0,1,\cdots ,2n+2$$

(14)

Estimate the predicted state at time step k:

$${\hat{\mathbf{x}}}_{k/k-1}=\sum _{i=0}^{2n+2}{\omega }_i{\mathbf{X}}_{i,k/k-1}^{*},i=0,1,\cdots ,2n+2$$

(15)

Estimate the predicted covariance of the state error at time step k:

$$\begin{array}{c}\multicolumn{1}{c}{{\mathbf{P}}_{k/k-1}=\sum _{i=0}^{2n+2}{\omega }_i({\mathbf{X}}_{i,k/k-1}^{*}-{\hat{\mathbf{x}}}_{k/k-1})·({\mathbf{X}}_{i,k/k-1}^{*}-{\hat{\mathbf{x}}}_{k/k-1}{)}^T+{\stackrel{\wedge }{\mathbf{Q}}}_{k-1}}\end{array}$$

(16)

In fact, equations (10) and (11) yield:

$$\begin{array}{c}\multicolumn{1}{c}{P_{k/k-1}=(\frac{2}{n+2}({{\mathbf{X}}^{*}}_{0,k/k-1}-{\hat{x}}_{k/k-1}){({{\mathbf{X}}^{*}}_{0,k/k-1}-{\hat{x}}_{k/k-1})}^{\mathrm{T}}}\\ +\sum _{i=1}^{2n+2}{\omega }_i({{\mathbf{X}}^{*}}_{i,k/k-1}-{\hat{x}}_{k/k-1}){({{\mathbf{X}}^{*}}_{i,k/k-1}-{\hat{x}}_{k/k-1})}^{\mathrm{T}})+{\stackrel{\wedge }{Q}}_{k-1}\hfill \end{array}$$

$$=\sum _{i=1}^{2n+2}{\omega }_i({{\mathbf{X}}^{*}}_{i,k/k-1}-{\hat{x}}_{k/k-1}){({{\mathbf{X}}^{*}}_{i,k/k-1}-{\hat{x}}_{k/k-1})}^{\mathrm{T}}$$

(17)

Where,

$$\begin{array}{c}\multicolumn{1}{c}{{{\mathbf{X}}^{*}}_{0,k/k-1}-{\hat{\mathbf{x}}}_{k/k-1}=f\left({{\mathbf{X}}^{*}}_{0,k-1}\right)-{\hat{\mathbf{x}}}_{k/k-1}}\\ =f(S_{0/k-1}{\xi }_0+{\hat{x}}_{k-1/k-1})-{\hat{x}}_{k/k-1}\hfill \\ =f\left({\hat{x}}_{k-1/k-1}\right)-{\hat{x}}_{k/k-1}=0\hfill \end{array}$$

(18)

• (3) Measurement update:

Utilizing Cholesky decomposition to factorize the error covariance ${\mathit{P}}_{k/k-1}$ :

$${\mathbf{P}}_{k/k-1}={\mathbf{S}}_{k/k-1}{\mathbf{S}}_{k/k-1}^T$$

(19)

Compute the corresponding cubature points:

$$X_{i,k/k-1}=S_{k/k-1}{\xi }_i+{\hat{x}}_{k/k-1}$$

(20)

Propagate the cubature points through the observation equation:

$${\mathbf{Z}}_{i,k/k-1}=h\left({\mathbf{X}}_{i,k/k-1}\right)$$

(21)

Estimate the predicted observation at time step k:

$${\hat{z}}_{k/k-1}=\sum _{i=0}^{2n+2}{\omega }_i{Z}_{i,k/k-1}$$

(22)

Estimation of the autocovariance matrix:

$${\mathbf{P}}_{\mathrm{zz},k/k-1}=\sum _{i=0}^{2n+2}{\omega }_i{Z}_{i,k/k-1}{Z}_{i,k/k-1}^T-{\hat{\mathbf{z}}}_{k/k-1}{\hat{\mathbf{z}}}_{k/k-1}^T+{\mathbf{R}}_k$$

(23)

Estimation of the cross-covariance matrix:

$${\mathbf{P}}_{\mathrm{xz},k/k-1}=\sum _{i=0}^{2n+2}{\omega }_i{\mathbf{X}}_{i,k/k-1}{Z}_{i,k/k-1}^T-{\hat{\mathbf{x}}}_{k/k-1}{\hat{\mathbf{z}}}_{k/k-1}^T$$

(24)

Estimation of the Kalman filter gain:

$${\mathbf{K}}_k={\mathbf{P}}_{\mathrm{xz},k/k-1}{\mathbf{P}}_{\mathrm{zz},k/k-1}^{-1}$$

(25)

The state estimate at time step k:

$${\hat{\mathbf{x}}}_{k/k}={\hat{\mathbf{x}}}_{k/k-1}+K_k(z_k-{\hat{\mathbf{z}}}_{k/k-1})$$

(26)

The estimated covariance of state error at time step k:

$${\mathbf{P}}_{k/k}={\mathbf{P}}_{k/k-1}-{\mathbf{K}}_k{\mathbf{P}}_{\mathrm{zz},k/k-1}{\mathbf{K}}_k^T$$

(27)

Adaptive fading memory MSSRCKF filtering algorithm

To improve filtering accuracy, enhance computational stability, and prevent divergence issues that may arise due to the accumulation of iterations, this paper introduces a fading factor based on the MSSRCKF algorithm and proposes an Adaptive Fading Memory Mixed-Order Spherical Simplex-Radial Cubature Kalman Filter (FM-MSSRCKF). This method adopts a simple fading factor introduction technique proposed by Tarn and Zaborsky, aiming to effectively suppress accumulated errors and accelerate the convergence of state estimation errors. ²⁶ The main difference between FM-MSSRCKF and MSSRCKF is the introduction of a fading factor SSS in equation (16), which transforms equation (16) into:

$$\begin{array}{c}{\mathbf{P}}_{k/k-1}=\hfill \\ S[\sum _{i=0}^{2n+2}{\omega }_i({\mathbf{X}}_{i,k/k-1}^{*}-{\hat{\mathbf{x}}}_{k/k-1})·({\mathbf{X}}_{i,k/k-1}^{*}-{\hat{\mathbf{x}}}_{k/k-1}{)}^T+{\stackrel{\wedge }{\mathbf{Q}}}_{k-1}\hfill \end{array}]$$

(28)

In fact, from equations (10) and (11), we can derive:

$$\begin{array}{c}\multicolumn{1}{c}{P_{k/k-1}=S(\frac{2}{n+2}({{\mathbf{X}}^{*}}_{0,k/k-1}-{\hat{x}}_{k/k-1}){({{\mathbf{X}}^{*}}_{0,k/k-1}-{\hat{x}}_{k/k-1})}^{\mathrm{T}}}\\ +\sum _{i=1}^{2n+2}{\omega }_i({{\mathbf{X}}^{*}}_{i,k/k-1}-{\hat{x}}_{k/k-1}){({{\mathbf{X}}^{*}}_{i,k/k-1}-{\hat{x}}_{k/k-1})}^{\mathrm{T}})+S{\stackrel{\wedge }{Q}}_{k-1}\hfill \\ =S\sum _{i=1}^{2n+2}{\omega }_i({{\mathbf{X}}^{*}}_{i,k/k-1}-{\hat{x}}_{k/k-1}){({{\mathbf{X}}^{*}}_{i,k/k-1}-{\hat{x}}_{k/k-1})}^{\mathrm{T}}\hfill \end{array}$$

(29)

By replacing equation (16) in the MSSRCKF computation steps with equation (29), the FM-MSSRCKF algorithm can be obtained. However, simulations have shown that the choice of the fading factor has a significant impact on the filtering results. After introducing the fading factor, the variance of the estimated values may increase, and the filtering accuracy tends to decrease as the fading factor SSS increases. In some cases, anomalies may occur, potentially leading to filter divergence. To address this issue, we introduced an adaptive factor in addition to the fading factor. Equations (23), (24), and (27) are modified as follows:

$${\mathbf{P}}_{\mathrm{zz},k/k-1}=\frac{1}{{\alpha }_{\mathrm{k}}}[\sum _{i=0}^{2n+2}{\omega }_i{Z}_{i,k/k-1}{Z}_{i,k/k-1}^T-{\hat{\mathbf{z}}}_{k/k-1}{\hat{\mathbf{z}}}_{k/k-1}^T+{\mathbf{R}}_k]$$

(30)

$${\mathbf{P}}_{\mathrm{xz},k/k-1}=\frac{1}{{\alpha }_{\mathrm{k}}}[\sum _{i=0}^{2n+2}{\omega }_i{\mathbf{X}}_{i,k/k-1}{Z}_{i,k/k-1}^T-{\hat{\mathbf{x}}}_{k/k-1}{\hat{\mathbf{z}}}_{k/k-1}^T]$$

(31)

$$P_k=\frac{1}{{\alpha }_k}{P}_{k|k-1}^{-}{K}_k{\mathbf{P}}_{\mathrm{zz},k/k-1}{K}_k^{\mathrm{T}}$$

(32)

Where ${\alpha }_k$ is the adaptive factor, with a range of 0 <  ${\alpha }_k$  ≤ 1. A reasonable adaptive factor can dynamically balance the weights of the state equation and observation information, effectively controlling the influence of state model disturbances on the filtering results. ³² Ge et al. ³³ and Zhang et al. ³⁴ indicate that the adaptive factor, by adjusting the weights of prediction and observation information, significantly improves filtering accuracy and system robustness in high-noise and nonlinear environments. Setting ${\alpha }_k$ within this range ensures that the system does not rely on a single information source, maintaining the stability and accuracy of the filter. The specific construction of the adaptive factor is as follows ³³ :

$$\begin{array}{c}\multicolumn{1}{c}{{\alpha }_k=\{\begin{array}{cc}1\hfill & \mathrm{tr}\left(V_k{V}_k^{\mathrm{T}}\right)\le \mathrm{tr}\left({\mathbf{P}}_{\mathrm{zz},k/k-1}\right)\hfill \\ \multicolumn{1}{c}{\frac{\mathrm{tr}\left({\mathbf{P}}_{\mathrm{zz},k/k-1}\right)}{\mathrm{tr}\left(V_k{V}_k^{\mathrm{T}}\right)}}& \mathrm{tr}\left(V_k{V}_k^{\mathrm{T}}\right)>\mathrm{tr}\left({\mathbf{P}}_{\mathrm{zz},k/k-1}\right)\hfill \end{array}}\end{array}$$

(33)

Where $V_k$ is the prediction residual, and its expression is:

$$V_k=z-\sum _{i=0}^{2n+2}{\omega }_i{Z}_{i,k/k-1}$$

(34)

From the above equation, it can be seen that as the value of $V_k$ increases, ${\alpha }_k$ correspondingly decreases, indicating that the error in the state estimation model has increased. In such cases, the weight of the predictive information in the filtering solution will be reduced, thereby balancing the weights of the predictive information and the observation information. Additionally, a well-designed adaptive factor can effectively mitigate the impact of anomalies in the state model. ³⁴

MSSRCKF filtering algorithm based on constrained optimization and fading memory

Building upon the adaptive fading memory MSSRCKF filtering algorithm, this paper proposes a MSSRCKF filtering method based on constrained optimization and fading memory (COFM-MSSRCKF) to address error issues caused by the non-constant characteristics of process noise and measurement noise in practical applications. This method ensures the convergence speed of state estimation errors by constructing a constraint on the ratio between the error covariance matrix and the measurement noise covariance matrix, thereby improving filtering accuracy. ²⁸

Specifically, this method dynamically adjusts and corrects the process noise covariance matrix and the measurement noise covariance matrix, and it includes the following steps:

• (1) Update the process noise covariance matrix $Q_k$ and the measurement noise covariance matrix $R_k$ :

$$\begin{array}{l}{\mathrm{Q}}_{\mathrm{k}}=\mathrm{E}\left[{\mathrm{w}}_{\mathrm{k}}{\mathrm{w}}_{\mathrm{k}}^{\mathrm{T}}\right]=\mathrm{E}[\left({\mathrm{x}}_{\mathrm{k}}-{\mathrm{F}}_{\mathrm{k}}{\mathrm{x}}_{\mathrm{k}-1}\right){({\mathrm{x}}_{\mathrm{k}}-F_{\mathrm{k}}{\mathrm{x}}_{\mathrm{k}-1})}^{\mathrm{T}}]\\ =\mathrm{E}[\left({\widehat{\mathrm{x}}}_{\mathrm{k}}^{-}+{\mathrm{K}}_{\mathrm{k}}\left({\mathrm{z}}_{\mathrm{k}}-{\mathrm{H}}_{\mathrm{k}}{\widehat{\mathrm{x}}}_{\mathrm{k}}^{-}\right)\right)-{\mathrm{F}}_k{\widehat{\mathrm{x}}}_{\mathrm{k}-1})\\ \left({\widehat{\mathrm{x}}}_{\mathrm{k}}^{-}+{\mathrm{K}}_{\mathrm{k}}\left({\mathrm{z}}_{\mathrm{k}}-{\mathrm{H}}_{\mathrm{k}}{\widehat{\mathrm{x}}}_{\mathrm{k}}^{-}\right)\right)-{\mathrm{F}}_{\mathrm{k}}{\widehat{x}}_{\mathrm{k}-1}){)}^{\mathrm{T}}]\\ =\mathrm{E}\left[\left({\widehat{\mathrm{x}}}_{\mathrm{k}}^{-}-{\mathrm{F}}_{\mathrm{k}}{\mathrm{x}}_{\mathrm{k}-1}\right){\left({\widehat{\mathrm{x}}}_{\mathrm{k}}^{-}-{\mathrm{F}}_{\mathrm{k}}{\mathrm{x}}_{\mathrm{k}-1}\right)}^T\right]+{\mathrm{K}}_{\mathrm{k}}{\mathrm{R}}_{\mathrm{k}}{\mathrm{K}}_{\mathrm{k}}^{\mathrm{T}}\\ +2{\mathrm{K}}_{\mathrm{k}}\mathrm{E}\left[\left({\widehat{\mathrm{x}}}_{\mathrm{k}}^{-}-{\mathrm{F}}_{\mathrm{k}}{\mathrm{x}}_{\mathrm{k}-1}\right)\right({{\mathrm{z}}_k-{\mathrm{H}}_{\mathrm{k}}{\widehat{\mathrm{x}}}_{\mathrm{k}}^{-})}^{\mathrm{T}}]\end{array}$$

(35)

$$\begin{array}{c}\mathrm{E}[{\hat{\mathrm{x}}}_{\mathrm{k}}-{\mathrm{F}}_{\mathrm{k}}{\mathrm{x}}_{\mathrm{k}-1}{({\mathrm{z}}_{\mathrm{k}}-{\mathrm{H}}_{\mathrm{k}}{\hat{\mathrm{x}}}_{\mathrm{k}}^{-})}^{\mathrm{T}}]\hfill \\ =\mathrm{E}({\hat{\mathrm{x}}}_{\mathrm{k}}^{-}-{\mathrm{F}}_{\mathrm{k}}{\mathrm{x}}_{\mathrm{k}-1}{\mathrm{z}}_{\mathrm{k}}^{\mathrm{T}})+\mathrm{E}({\hat{\mathrm{x}}}_{\mathrm{k}}^{-}+{\mathrm{F}}_{\mathrm{k}}{\mathrm{x}}_{\mathrm{k}-1}{\left({\mathrm{H}}_{\mathrm{k}}{\hat{\mathrm{x}}}_{\mathrm{k}}^{-}\right)}^{\mathrm{T}})\hfill \\ ={\mathrm{P}}_{\mathrm{k}}{\mathrm{H}}_{\mathrm{k}}^{\mathrm{T}}{\mathrm{P}}_{\mathrm{k}}\hfill \end{array}$$

(36)

Where $P_k^{-}$ is the a priori estimation error covariance.

$${\mathrm{R}}_{\mathrm{k}}=\frac{\mathrm{E}\left[{({\mathrm{z}}_{\mathrm{k}}-{\mathrm{H}}_{\mathrm{k}}{\hat{\mathrm{x}}}_{\mathrm{k}}^{-})}^2\right]{\mathrm{H}}_{\mathrm{k}}{\mathrm{H}}_{\mathrm{k}}^{\mathrm{T}}}{\mathrm{E}\left[{({\mathrm{z}}_{\mathrm{k}}-{\mathrm{H}}_{\mathrm{k}}{\hat{\mathrm{x}}}_{\mathrm{k}}^{-})}^2\right]-{\mathrm{H}}_{\mathrm{k}}{\mathrm{P}}_{\mathrm{k}}^{-}{\mathrm{H}}_{\mathrm{k}}^{\mathrm{T}}/({\mathrm{H}}_{\mathrm{k}}{\mathrm{P}}_{\mathrm{k}}^{-}{\mathrm{H}}_{\mathrm{k}}^{\mathrm{T}}+{\mathrm{R}}_{\mathrm{k}})}$$

(37)

• (2) Adjust and correct the dynamic noise covariance matrix:The dynamic adjustment formula for the process noise covariance matrix  $Q_k$ is:

$${\mathrm{Q}}_k=(\mathrm{I}-{\mathrm{K}}_k{\mathrm{H}}_k){\mathrm{P}}_k^{-}{\mathrm{Q}}_{k-1}{\left({\mathrm{P}}_k^{-}\right)}^T{(\mathrm{I}-{\mathrm{K}}_k{\mathrm{H}}_k)}^T+{\mathrm{K}}_k{\mathrm{R}}_k{\mathrm{K}}_k^T$$

(38)

The dynamic adjustment formula for the measurement noise covariance matrix $R_k$ is:

$$\begin{array}{c}{\mathrm{R}}_{\mathrm{k}}=\hfill \\ \frac{\max \{\mathrm{E}\left[{({\mathrm{z}}_{\mathrm{k}}-{\mathrm{H}}_{\mathrm{k}}{\hat{\mathrm{x}}}_{\mathrm{k}}^{-})}^2\right]{\mathrm{H}}_{\mathrm{k}}{\mathrm{H}}_{\mathrm{k}}^{\mathrm{T}},\mathrm{\lambda }\}}{\max \{\mathrm{E}\left[{({\mathrm{z}}_{\mathrm{k}}-{\mathrm{H}}_{\mathrm{k}}{\hat{\mathrm{x}}}_{\mathrm{k}}^{-})}^2\right]{\mathrm{H}}_{\mathrm{k}}{\mathrm{H}}_{\mathrm{k}}^{\mathrm{T}},\mathrm{\lambda }\}-{\mathrm{H}}_{\mathrm{k}}{\mathrm{P}}_{\mathrm{k}}^{-}{\mathrm{H}}_{\mathrm{k}}^{\mathrm{T}}/({\mathrm{H}}_{\mathrm{k}}{\mathrm{P}}_{\mathrm{k}}^{-}{\mathrm{H}}_{\mathrm{k}}^{\mathrm{T}}+{\mathrm{R}}_{\mathrm{k}})}\hfill \end{array}$$

(39)

Where $\lambda$ is the threshold parameter used to control the minimum value of the measurement noise covariance matrix $R_k$ , preventing it from becoming too small and causing filter divergence. ³³ During the Kalman gain update and constraint process, the gain matrix is adjusted to minimize the final estimation error variance. The ratio $\alpha$ between the error covariance matrix $P_k^{-}$ and the measurement noise covariance matrix $R_k$ is defined as:

$$\alpha =\frac{\mathrm{tr}\left(R_{\mathrm{k}}\right)}{\mathrm{tr}\left(P_k^{-}\right)}$$

(40)

As shown in Figure 3, the performance of state estimation errors under different values of $\alpha$ is as follows: When $\alpha$ is small, meaning that tr( $P_k^{-}$ ) is relatively larger than tr( $R_k$ ), the system primarily relies on the prediction model, resulting in a smaller Kalman gain $K_k$ , and the state estimation error converges slowly. When $\alpha$ is large, meaning that tr( $R_k$ ) is relatively larger than tr( $P_k^{-}$ ), the system becomes overly dependent on the measurement data, leading to a larger Kalman gain $K_k$ , making the state estimation sensitive to measurement noise, which may cause filter instability. To prevent filter instability or divergence due to an overly large or small $K_k$ during the dynamic adjustment of the noise covariance matrix, a ratio constraint must be established:

$${\alpha }_{\min }\le \alpha \le {\alpha }_{\max }$$

(41)

Figure 3.

State estimation errors under different α values.

During the Kalman gain constraint process, it is critical to reasonably determine the upper and lower bounds of the ratio constraint parameter $\alpha$ , based on prior knowledge, experimental results, and practical application requirements. Wang and Zhang ³⁵ and Gao and Zhang ³⁶ indicate that the ratio between the noise covariance matrix and the error covariance matrix significantly affects the Kalman gain. Limiting this ratio helps maintain the stability and accuracy of the filter in complex noise environments. When $\alpha$ exceeds the set threshold, dynamic adjustments to the measurement noise and error covariance matrices are made to bring the ratio back within a reasonable range. Typically, $\alpha$ is set within the range of 0.1 ≤ $\alpha$ ≤ 10 to ensure the stability and accuracy of the filter. The experiments in Gao and Zhang ³⁶ confirmed that this range effectively enhances the robustness and accuracy of the filter under high noise and nonlinear conditions.

The COFM-MSSRCKF filtering algorithm proposed in this study adopts an improved MSSR cubature sampling rule, combining third-order spherical integration with fifth-order radial integration methods. This combination not only improves filtering accuracy but also effectively reduces the computational complexity of the algorithm. ²¹ By using the adaptive fading memory method to predict and update the covariance matrix, the weight of historical information is reduced, thereby suppressing the issue of filter divergence. Additionally, the introduction of a ratio constraint between the error covariance matrix and the measurement noise covariance matrix further ensures a faster convergence of state estimation errors and significantly improves filtering accuracy. This method provides greater stability and reliability for filtering in complex noise environments.

Design of importance density function and adaptive optimization strategy for particle filtering

To effectively mitigate the particle degradation problem, choosing an appropriate importance density function is critical. This study proposes the COFM-AMSSRCPF filtering algorithm, which is based on the Mixed-Order Spherical Simplex-Radial Cubature Kalman Filter (MSSRCKF) combined with an adaptive fading memory strategy using constrained optimization. ²⁸ By optimizing the mean and variance, this method designs a more accurate particle importance density function, ensuring that the newly generated particles can better adapt to state changes in complex environments. Additionally, the algorithm employs an adaptive particle number and weight adjustment mechanism based on Effective Sample Size (ESS) and entropy regularization. This mechanism dynamically adjusts the number of particles, reducing computational complexity while maintaining filtering accuracy. ³⁸ It also suppresses excessive weight concentration, reducing the impact of outliers on filtering results, thereby improving the overall performance and stability of the system. ³⁹ The process flow of the COFM-AMSSRCPF filtering algorithm is shown in Figure 4.

Figure 4.

COFM-AMSSRCPF algorithm flowchart.

The specific process of the COFM-AMSSRCPF algorithm is as follows:

• (1) Initialization:

• ① Particle state initialization:

$$x_0^i~p\left(x_0\right),w_0^i=\frac{1}{N},i=1,2,\cdots ,N$$

(42)

Where $N$ is the initial number of particles, $x_0^i$ is the initial state of the $i$ -th particle, and $w_0^i$ is the initial weight.

• ② Mean and covariance matrix initialization:

$${\hat{x}}_0^i=E\left[x_0^i\right],P_0^i=E\left[(x_0^i-{\hat{x}}_0^i){(x_0^i-{\hat{x}}_0^i)}^T\right]$$

(43)

Where ${\hat{x}}_0^i$ is the expected value of the initial state, and $P_0^i$ is the initial covariance matrix.

• ③ Parameter setting:

$\alpha$ : The threshold ratio for the effective sample size, used to determine whether particle number adjustment or resampling is necessary to prevent particle degeneracy. Based on the recommendations in Li et al. ³⁷ and Zhao et al., ³⁸ $\alpha$ is typically set to 0.5, which effectively detects particle degeneracy and maintains filter robustness. In this study, $\alpha$ is set to 0.5.

$\lambda$ : The entropy regularization coefficient, controlling the balance of particle weights to prevent degeneration caused by excessive weight concentration. According to the findings in Li and Zhang ³⁹ and Xiong and Zhu, ⁴⁰ the typical range for $\lambda$ is 0.1 ≤  $\lambda$  ≤ 1. With λ set to 0.5, the filter can effectively suppress the impact of sudden noise changes on the filtering results. In this study, $\lambda$ is set to 0.5.

$N_{\min }$ and $N_{\max }$ : The lower and upper limits of particle numbers. Here, we set $N_{\min }$  = 110 and $N_{\max }$  = 200 to restrict the range of particle number adjustments, balancing filtering accuracy with computational complexity.

ΔN: The particle number adjustment step size, set to ΔN = 10 in this study. This step size allows for a gradual increase or decrease in the number of particles during adaptive adjustment, achieving smooth particle number adjustments and avoiding filter instability caused by abrupt particle count changes.

$x_k^i$ : The state vector of the $i$ -th particle at time step k.

$w_k^i$ : The weight of the $i$ -th particle at time step k.

$N_{\mathrm{eff}}$ : Effective sample size (ESS), which measures the degree of concentration in the particle weight distribution.

• (2) State Prediction:

Predict the state using the system state transition equation:

$$x_k^i~q(x_k^i\mid x_{k-1}^i,z_k)$$

(44)

Where $q(x_k^i\mid x_{k-1}^i,z_k)$ is the importance density function optimized by the COFM-MSSRCKF algorithm. Design the importance density function using the COFM-MSSRCKF:

$$[{\hat{x}}_k^i,P_k^i]=\mathrm{COFM}-\mathrm{MSSRCKF}[{\hat{x}}_{k-1}^i,P_{k-1}^i,z_k]$$

(45)

Generate particles based on the new distribution:

$$x_k^i~N({\hat{x}}_k^i,P_k^i)$$

(46)

• (3) Adaptive Particle Number Adjustment:

Calculate the effective sample size $N_{\mathrm{eff}}$ :

$$N_{\mathrm{eff}}=\frac{1}{\sum _{i=1}^N{\left(w_k^i\right)}^2}$$

(47)

Where $w_k^i$ is the weight of the $i$ -th particle. Determine whether to adjust the particle number based on $N_{\mathrm{eff}}$ : If $N_{\mathrm{eff}}<\alpha \times N$ , reduce the particle number:

$$N_{t+1}=\max (N_{\min },N_t-\mathrm{\Delta }N)$$

(48)

If $N_{\mathrm{eff}}\ge \alpha \times N$ , increase the particle number:

$$N_{t+1}=\min (N_{\max },N_t+\mathrm{\Delta }N)$$

(49)

Where $N_{\min }$ and $N_{\max }$ are the lower and upper limits of the particle number, respectively.

• (4) Particle Weight Calculation and Adjustment:

The weight calculation formula is:

$$w_k^i=\frac{p(z_k\mid x_k^i)P(x_k^i\mid x_{k-1}^i)}{q(x_k^i\mid x_{k-1}^i,z_k)}$$

(50)

To prevent the weight distribution from becoming too concentrated, apply entropy regularization to adjust the weights:

$$w_k^i=w_k^i·\exp (-\lambda H\left(w_k\right))$$

(51)

Where $H(w_k)=-\sum _{i=1}^N{w}_k^i\log (w_k^i)$ represents the entropy of the particle weights. $\lambda$ is the entropy regularization coefficient, which controls the balance of the particle weight distribution.

• (5) Particle Weight Normalization:

Weight normalization:

$$w_k^i=\frac{w_k^i}{\sum _{j=1}^N{w}_k^j}$$

(52)

• (6) Resampling:

Resample based on the normalized particle weights:

$${\{x_k^j,w_k^j\}}_{j=1}^N=\mathrm{Resample}\left({\{x_k^i,w_k^i\}}_{i=1}^N\right)$$

(53)

• (7) State Estimation and Covariance Calculation:

$${\hat{x}}_k=\sum _{i=1}^N{w}_k^i{x}_k^i$$

(54)

$$P_k=\sum _{i=1}^N{w}_k^i(x_k^i-{\hat{x}}_k){(x_k^i-{\hat{x}}_k)}^T$$

(55)

• (8) Iteration:

Let $k=k+1$ , and repeat steps 2–7 until the filtering process is completed.

In the COFM-AMSSRCPF algorithm, using α = 0.5 as the threshold for the effective sample size (ESS) effectively identifies and mitigates the issue of particle degeneracy. Li et al. ³⁷ and Zhao et al., ³⁸ emphasize the effectiveness of this threshold in complex noise environments, contributing to the robustness of the filter’s performance. Li and Zhang ³⁹ and Xiong and Zhu, ⁴⁰ provide an in-depth discussion on the rationality of the entropy regularization technique, noting that the typical range for λ is between 0.1 and 1, ensuring the balance of particle weight distribution and system stability. Based on experimental tuning results, this study selects λ = 0.5, and according to the experimental validation in Li and Zhang ³⁹ and Xiong and Zhu, ⁴⁰ this value significantly enhances the robustness and accuracy of the filter when dealing with sudden noise changes. Additionally, to achieve smooth particle number adjustments, this study introduces a particle number adjustment step size of ΔN = 10, allowing the particle count to dynamically vary between $N_{\min }$  = 110 and $N_{\max }$  = 200, further improving the filter’s stability and adaptability.

In summary, the COFM-AMSSRCPF algorithm effectively addresses the issue of particle degeneracy through the optimization of the importance density function, an adaptive particle number adjustment strategy (including the introduction of the particle number adjustment step ΔN), and an entropy regularization strategy. This algorithm not only significantly improves the filtering accuracy and computational efficiency in nonlinear systems but also reduces computational complexity by optimizing the sampling rules and dynamically adjusting the particle number. Moreover, the introduction of constraint conditions and fading factors effectively reduces the impact of noise on the particle weight distribution, making it more suitable for state estimation problems in nonlinear, non-Gaussian integrated navigation systems. ⁴¹

Performance analysis

The COFM-AMSSRCPF filtering algorithm proposed in this paper adopts the MSSR cubature sampling rule, combining third-order spherical integration with fifth-order radial integration techniques. Table 1 shows that MSSR sampling adds only three more sampling points compared to traditional CKF, and only one more sampling point compared to SSR sampling, which uses 2n + 2 sampling points. ²¹ In high-dimensional systems, the computational complexity of these three sampling rules is nearly identical, and all are significantly lower than that of HCKF. The average simulation times shown in Table 1 support this conclusion. Compared to the fifth-order SSR sampling in, ⁴² the MSSR sampling rule has positive weights, aligning with the “good integration rules” described in, ⁷ thus giving MSSRCKF superior accuracy over fifth-order SSRCKF. Comprehensive analysis of the filtering results and processing speed reveals that MSSRCKF provides higher filtering accuracy than SSRCKF, while demonstrating faster computational speed compared to SSRHCKF.

Table 1.

Number of sampling points and simulation time for three filtering algorithms.

Algorithm (n = 17)	Number of sampling points	Average simulation time (s)
CKF	2n	77.36
HCKF	2n 2  + 1	992.15
MSSRCKF	2n + 3	79.68

The COFM-AMSSRCPF algorithm, through constrained optimization and adaptive fading memory strategies, dynamically adjusts the noise covariance matrix, improving filtering accuracy and convergence speed. The introduction of the fading factor reduces the influence of historical information and enhances the weight of current measurement data, significantly improving system stability. The importance density function designed using MSSRCKF enhances filtering accuracy while maintaining low computational complexity. Additionally, the Cholesky decomposition ensures the positive definiteness of the covariance matrix, preventing filter divergence. The adaptive particle number and weight adjustment mechanism based on effective sample size and entropy regularization mitigates particle degradation and reduces the impact of outliers on the results. This performance analysis shows that the COFM-AMSSRCPF algorithm proposed in this paper achieves significant performance improvements in optimizing the importance density function of particle filtering and mitigating particle degradation.

Additionally, this paper explores another novel combination method, which integrates fifth-order spherical simplex integration with third-order radial integration. This method requires $n^2+3n+2$ sampling points. However, similar to fifth-order SSRCKF, this combination mode may introduce negative weights in high-dimensional systems, potentially leading to filter divergence. ²¹ The combination strategy of third-order spherical simplex integration and fifth-order radial integration adopted in this paper effectively avoids this problem. This unique integration strategy not only adheres to the “good integration rules” but also significantly improves filtering accuracy while reducing the computational complexity of the algorithm and enhancing its robustness.

Simulation and analysis

To verify the effectiveness and superiority of the proposed filtering algorithm, two MATLAB simulation experiments were conducted. Simulation 1 compares MSSRCKF, CKF, and HCKF, while Simulation 2 compares COFM-AMSSRCPF, FMCPF, and CPF. The experimental data were collected from a flight test of a specific model UAV near Anshun Huangguoshu Airport in 2022. The UAV utilized a SINS/GPS integrated navigation system for navigation. The starting position of the aircraft was set at a longitude of 105.89°E, latitude of 26.26°N, and an altitude of 320 m. The initial heading was southwest, with a starting velocity of zero. During the flight, the UAV performed various maneuvers, including acceleration, right turns, steady flight, right turns, left turns, climbing, descending, deceleration, and left turns. The final position was at a longitude of 108.63°E, latitude of 23.68°N, and an altitude of 200 m, with a total flight time of 1400 s. The trajectory is shown in Figure 5. The constant drift of the gyroscope was set to 0.03(°)/h, and the random walk was set to 0.001(°)/ $\sqrt{\mathrm{h}}$ . The accelerometer’s constant bias was set to $1\times {10}^{-4}g$ , and the random walk was set to $1\times {10}^{-5}g·\sqrt{s}$ . The SINS sampling period was set to 0.01 s, and the GPS sampling period was set to 0.1 s. The filtering period was set to 0.2 s, with the overall simulation duration set to 1400 s. The initial alignment error of the SINS was set to 0.01°, the initial velocity error to 0.2 m/s, and the initial position error to 10 m. The number of particles was 200, and 50 Monte Carlo simulations were conducted.

Figure 5.

Aircraft trajectory.

Simulation 1 employs three algorithms: standard third-order CKF, HCKF, and MSSRCKF. These algorithms are tested under the same initial conditions and system model settings to verify their positioning accuracy in the integrated navigation system. The key evaluation metric of the experiment is the estimation accuracy of the SINS device errors and the latitude and longitude errors of the navigation position, which directly reflect the filtering accuracy. The simulation results are shown in Figures 6–11.

Figure 6.

Uncorrected gyroscope bias in SINS.

Figure 7.

Uncorrected accelerometer bias in SINS.

Figure 8.

Gyroscope bias in SINS after MSSRCKF filter correction.

Figure 9.

Accelerometer bias in SINS after MSSRCKF filter correction.

Figure 10.

Latitude position errors of simulation experiment 1.

Figure 11.

Longitude position errors of simulation experiment 1.

Before delving into the analysis of the positioning errors, we first compare the gyro and accelerometer bias errors of the SINS system before and after MSSRCKF filtering correction. Figures 6 and 7 show the bias errors of the gyroscope and accelerometer in the uncorrected SINS system, revealing the trend of error accumulation over time. In contrast, Figures 8 and 9 demonstrate that after MSSRCKF filtering correction, the bias errors of the gyroscope and accelerometer are significantly reduced, showing the effective suppression of error accumulation by the filtering correction. This comparison clearly highlights the superiority of the MSSRCKF filter in error correction, providing a solid foundation for the subsequent analysis of positioning accuracy.

From Figure 10, it can be observed that the positioning error of the MSSRCKF algorithm is smaller than that of CKF and is comparable to HCKF in terms of positioning accuracy, with MSSRCKF even showing higher accuracy at certain times. At around 550 s, the latitude positioning error of MSSRCKF remains within 1.8 m, which is better than CKF’s 2.4 m error. Figure 11 shows that HCKF exhibits the fastest convergence speed, but this comes at the cost of significantly higher computational complexity. In contrast, MSSRCKF demonstrates faster speed in achieving similar convergence accuracy. For example, at around 350 s, the longitude position error of MSSRCKF drops to within 1.2 m, whereas CKF takes until 550 s to reduce the error to within 2.9 m. Based on the data shown in Table 1 and the analysis of the simulation results, it is clear that MSSRCKF achieves significant performance improvements in positioning accuracy and convergence speed with only three additional sampling points compared to the CKF algorithm. This observation not only confirms the superiority of the MSSRCKF algorithm design but also highlights its efficiency in practical applications. As shown in Figure 11, although MSSRCKF’s convergence speed is slightly lower than HCKF, its computation time is significantly reduced from HCKF’s 992.15–79.68 s, indicating that MSSRCKF greatly reduces computational resource consumption while maintaining similar filtering accuracy. The comparison of sampling points, average simulation time, and positioning errors for the three filtering algorithms is shown in Tables 1 and 2. This improvement offers an important optimization direction for navigation systems with limited computational resources. Table 2 provides a detailed comparison of the positioning errors in latitude, longitude, and altitude.

Table 2.

Comparison of latitude, longitude, and altitude positioning errors.

Filtering algorithm	Simulation time range (s)	Maximum latitude error (m)	Maximum longitude error (m)
MSSRCKF	0–1400	6.42	3.98
HCKF	0–1400	2.86	2.52
CKF	0–1400	12.0	7.80

Simulation 2 aims to evaluate the performance of the filters in a complex noise environment. For this purpose, we amplified the errors in both the system noise and measurement noise to create a challenging simulation environment. Noise perturbations were introduced at 500 and 900 s during the operation of the integrated navigation system. Specifically, at 500 s, the system noise matrix Q and the measurement noise matrix R were artificially increased to 10 and 9.5 times their original values, respectively, that is, Q′ = 10Q, R′ = 9.5R. At 900 s, Q and R were increased to 7.5 and 8 times their original values, that is, Q′ = 7.5Q, R′ = 8R. This setup is intended to simulate extreme situations that navigation systems may encounter, such as abrupt accelerations, signal interference, sensor malfunctions, or decreased accuracy – especially in scenarios involving signal interruptions or GPS blockages in dynamic environments. The simulation focuses on testing the filters’ robustness and accuracy under such adverse conditions. By introducing noise amplification, the key evaluation was the filters’ responsiveness and stability when faced with sudden state changes. Under this setting, the performance of the proposed COFM-AMSSRCPF algorithm, as well as FMCPF and CPF algorithms, was assessed. The filtering accuracy evaluation metrics included latitude error, longitude error, and altitude error. The simulation results are shown in Figures 12–14.

Figure 12.

Latitude position errors.

Figure 13.

Longitude position errors.

Figure 14.

Altitude position errors.

From Figures 12 to 14, it can be observed that after the filter stabilizes, the system is affected by sudden noise perturbations at 500 and 900 s. The traditional CPF algorithm fails to effectively handle these disturbances, leading to a decline in filtering performance and even divergence. Specifically, the CPF algorithm’s latitude errors at 500 and 900 s reach 8.6 and 8.9 m, respectively; longitude errors are 7.4 and 6.6 m; and altitude errors reach 16.4 and 10.4 m. These results indicate the instability of the CPF algorithm when dealing with abrupt state changes.

Comparing the data from Figures 12 to 14, it is evident that the COFM-AMSSRCPF algorithm, by incorporating constrained optimization, fading memory, and adaptive strategies, can effectively suppress noise anomalies and prevent filter divergence. Specifically, after disturbances, the COFM-AMSSRCPF algorithm maintains latitude and longitude positioning errors within the range of [–3.5 m, 3.5 m], and altitude errors within [–5.2 m, 5.2 m]. In contrast, the FMCPF algorithm shows latitude and longitude errors reaching [–6.8 m, 6.8 m], and altitude errors within [–10.5 m, 10.5 m].

To quantify the performance improvement of the algorithm, we conducted a percentage analysis of the positioning errors. Compared to the CPF algorithm, the COFM-AMSSRCPF algorithm significantly enhances positioning accuracy. Specifically, the maximum latitude error is reduced by approximately 54.8% (from 8.9 to 4.02 m), the maximum longitude error is reduced by approximately 40.5% (from 7.4 to 4.4 m), and the maximum altitude error is reduced by approximately 68.3% (from 16.4 to 5.2 m). This demonstrates that our algorithm possesses higher robustness and accuracy when handling sudden state changes and high-noise environments.

Compared to the FMCPF algorithm, the COFM-AMSSRCPF algorithm also exhibits clear advantages. The maximum latitude error is reduced by approximately 41.2% (from 6.8 to 4.0 m), the maximum longitude error is reduced by approximately 23.5% (from 5.75 to 4.4 m), and the maximum altitude error is reduced by approximately 50.5% (from 10.5 to 5.2 m). This further validates the superior performance of our algorithm under complex dynamic conditions.

To further validate the performance of the algorithm in terms of effective particle number and weight distribution, the simulation results demonstrate that the COAFM-MSSRCPF algorithm significantly improves the range of effective particle numbers (maintained between 110 and 180) through adaptive particle number adjustment. Additionally, the weight distribution is more uniform, reducing the issue of weight concentration and preventing particle degeneration. In contrast, the traditional CPF and FMCPF algorithms failed to adaptively adjust the effective particle numbers, leading to highly concentrated weights, with some particle weights approaching zero, ultimately resulting in degraded filtering performance and instability. Table 3 further presents a comparison of different filtering algorithms in terms of positioning errors, particle numbers, and weight distributions. In summary, the simulation results demonstrate that the COFM-AMSSRCPF algorithm exhibits significant advantages in positioning accuracy, convergence speed, and computational efficiency, providing a more effective solution for navigation state estimation problems under complex dynamic conditions.

Table 3.

Comparison of positioning errors, particle numbers, and weight distributions for different filtering algorithms.

Filtering algorithm	Simulation time range (s)	Maximum latitude error (m)	Maximum longitude error (m)	Maximum altitude error (m)	Initial particle number	Effective particle number	Weight distribution trend
COAFM—MSSRCPF	0–1400	4.02	4.40	5.20	200	110–180	Even distribution of weights
FMCPF	0–1400	6.80	5.75	10.50	200	200	Concentrated weights, some particle weights near zero
CPF	0–1400	8.90	7.40	16.40	200	200	Highly concentrated, some particle weights near zero

Conclusion

This paper proposes an adaptive mixed-order spherical simplex-radial cubature particle filter algorithm based on constrained optimization and fading memory (COFM-AMSSRCPF), aimed at addressing the issues of particle degradation and importance density function selection in complex nonlinear systems. The algorithm combines constrained optimization with an adaptive fading memory mechanism, dynamically adjusting the noise covariance matrix and optimizing the ratio between the error covariance and measurement noise covariance, thereby significantly improving the convergence speed and filtering accuracy of state estimation. COFM-AMSSRCPF employs the mixed-order spherical simplex-radial cubature Kalman filter (MSSRCKF) sampling strategy, demonstrating excellent performance in balancing computational complexity and filtering accuracy. Through the use of adaptive particle number adjustment based on effective sample size and an entropy regularization strategy, the algorithm effectively mitigates particle degradation, reduces the impact of outliers on weight distribution, and enhances robustness under high-noise and nonlinear conditions.

Although the COFM-AMSSRCPF algorithm exhibits significant advantages in terms of accuracy and robustness, there is still room for optimization. First, as the number of particles increases, it may introduce additional computational costs; however, in most application scenarios, the computational efficiency remains within an acceptable range. Second, the parameter tuning of entropy regularization and adaptive particle number adjustment can be relatively complex, especially in different noise environments, and requires further optimization for specific applications. Nevertheless, the algorithm shows high stability when dealing with sudden noise perturbations and exhibits outstanding overall performance.

Simulation results demonstrate that COFM-AMSSRCPF achieves notable improvements in robustness and accuracy in complex SINS/GNSS integrated navigation systems, particularly when dealing with strong nonlinearity, noise surges, and measurement errors in challenging environments. Compared to traditional cubature particle filtering and fading memory particle filtering algorithms, this algorithm significantly enhances filtering accuracy, convergence speed, and computational efficiency through adaptive particle number adjustment and entropy regularization, validating its efficiency and stability.

In conclusion, the COFM-AMSSRCPF algorithm not only offers theoretical innovations but also shows high practicality in real-world applications, especially in complex navigation scenarios. Future research will focus on optimizing its real-time performance and computational efficiency to meet the stringent real-time requirements of industrial and commercial applications, as well as further exploring its potential in multi-sensor fusion navigation systems.