Vehicle Pose Estimation Method Based on Maximum Correntropy Square Root Unscented Kalman Filter

Abstract

Simultaneous Localization and Mapping (SLAM) is an effective method for estimating and correcting the pose of the mobile robot. However, a large amount of external noise and observed outliers in complex unknown environments often lead to a decrease in the estimation accuracy and robustness of the SLAM algorithm. To improve the performance of the Square Root Unscented Kalman Filter SLAM (SRUKF-SLAM), this paper proposes the Maximum Correntropy Square Root Unscented Kalman Filter SLAM (MCSRUKF-SLAM) algorithm. The method first generates an estimate of the predicted state and covariance matrix through the Unscented Transform (UT), and then obtains the square root matrix of the covariance through Cholesky and QR decomposition to replace the original covariance, effectively preserving the positive definiteness of the covariance and improving the accuracy of the algorithm. Moreover, the proposed MCSRUKF-SLAM recharacterizes measurement information at the cost of the Maximum Correntropy (MC) instead of the Minimum Mean Square Error (MMSE), effectively improving the SLAM algorithm’s ability to suppress non-Gaussian noise. The simulation results show that compared with EKF-SLAM, UKF-SLAM, SRUKF-SLAM, and MCUKF-SLAM, the accuracy of the proposed MCSRUKF-SLAM in Gaussian mixture noise improves by 81.8%, 80.9%, 78.7%, and 63.6%, and the accuracy of the proposed MCSRUKF-SLAM in colored noise improves by 50.3%, 39.9%, 38.2%, and 36.3%.

1. Introduction

Since the 21st century, with the rapid development of microelectronics technology, artificial intelligence technology, new materials technology, and computer technology, simultaneous localization and mapping (SLAM) technology has shown great application potential in many real scenarios [1,2,3]. For example, in the field of automatic driving, the SLAM system constructs a high-precision environment map and locates vehicle position in real time by fusing lidar, vision, and inertial measurement unit (IMU) data, providing a key basis for path planning and obstacle avoidance decisions [4,5]. Similarly, for warehousing and the logistics robot, SLAM technology helps the robot accurately locate items in the dynamic shelf environment and improve the efficiency of sorting, handling, and inventory management [5,6]. In addition, SLAM is also widely used in UAV patrol, augmented reality (AR), and service robots [7,8,9,10]. Simultaneous Localization and Mapping (SLAM) technology [11,12] is a key technology for the mobile robot to achieve environmental perception and self-localization. SLAM technology can be described as the robot moving from the unknown position in the unknown environment, positioning itself based on pose estimation and sensor observation information during the movement process, and establishing an environmental map. Therefore, stable, reliable, and high-precision pose estimation technology is one of the key technologies for achieving autonomous robot movement and practical applications [13,14]. The pose estimation is the process of predicting the robot’s position and posture in the unknown environment based on the certain algorithm and combined with sensor observation information. According to its principles, the pose estimation technique can be divided into relative pose estimation and absolute pose estimation [15]. The relative pose estimation method requires the initial pose of the robot to be known, and then utilizes sensor information and observed relative motion states to indirectly estimate the current pose of the robot. Absolute pose estimation does not require the initial pose information of the robot, and can estimate the pose state of the mobile robot solely based on sensor information, with relatively stable estimation accuracy [16]. Therefore, this paper mainly studies the absolute pose estimation of the mobile robot based on map-matching positioning.

In practical applications, the motion model and observation model in SLAM is usually the nonlinear system, and the Extended Kalman Filter SLAM (EKF-SLAM) [13,17,18,19] is one of the commonly used nonlinear pose estimation algorithms in SLAM. EKF-SLAM applies Taylor series to linearly approximate nonlinear systems, resulting in significant linearization errors. Therefore, some scholars have proposed the Unscented Kalman Filter (UKF-SLAM) algorithm [20,21,22], which directly approximates the probability density distribution of nonlinear functions and uses the sigma point set to approximate the posterior probability density of the target state. However, the UKF-SLAM algorithm cannot guarantee the positive definiteness of the covariance matrix, which can lead to a decrease in the filtering accuracy of the UKF-SLAM algorithm. Researchers have proposed the Square Root Unscented Kalman Filter SLAM (SRUKF-SLAM) algorithm [23,24], which uses the square root matrix of covariance instead of the original covariance matrix to avoid filter divergence caused by non-positive definiteness of the covariance matrix. Furthermore, the SRUKF-SLAM algorithm has higher filtering efficiency than the UKF-SLAM algorithm, which is beneficial for real-time estimation of the robot pose.

The above-mentioned Kalman filter and its derivative algorithms are only applicable to robot systems under the assumption of Gaussian noise. In practical application scenarios, the noise in the sensor information of mobile robot SLAM has a certain correlation, and the noise model often follows the non-Gaussian distribution [25,26,27]. This non-Gaussian characteristic can cause second-order statistical divergence, resulting in a significant decrease or even divergence in the positioning accuracy of the traditional Kalman filter and its derivative algorithms based on the minimum mean square error (MMSE) criterion [28,29].

In response to the limitations of the aforementioned filtering algorithms, the Particle Filter SLAM (PF-SLAM) [30,31] algorithm has been proposed. The PF-SLAM algorithm uses random sample data to approximate the posterior probability density function, so it requires a large amount of sample data to obtain acceptable estimation accuracy. This increases the computational complexity of the PF-SLAM algorithm. Furthermore, this also increases the difficulty of the algorithm in practical applications.

In recent years, the Maximum Correntropy (MC) has been proven to solve the problem of low-pose estimation accuracy of traditional Kalman filtering and its derivative algorithms in non-Gaussian noise environments [32,33,34,35]. Qi et al. proposed vehicle state estimation algorithms based on the Maximum Correntropy Extended Kalman Filter (MC-EKF) [36] and the Maximum Correntropy Adaptive Extended Kalman Filter (MC-AEKF) [37]. The research results demonstrate that, under non-Gaussian noise environments (particularly heavy-tailed noise), MC-AEKF and MC-EKF exhibit stronger robustness and higher estimation accuracy compared to traditional AEKF/EKF. Subsequently, scholars have proposed the Maximum Correntropy Unscented Kalman Filter (MCUKF) [38] and an iterative Maximum Correntropy Unscented Kalman Filter algorithm [39]. These methods have demonstrated excellent performance in complex noise environments, though they were only validated through numerical simulations and not applied to practical problems. Wang et al. introduced an MCUKF algorithm for loosely coupled positioning in an inertial navigation system (INS) and ultra-wideband (UWB) integration [40], which significantly outperforms traditional UKF methods in terms of positioning accuracy and robustness. Zhang et al. proposed a Maximum Correntropy Adaptive Unscented Kalman Filter (MC-AUKF) algorithm [41] for estimating vehicle states and key parameters. This algorithm demonstrates superior performance under non-Gaussian noise and effectively improves estimation accuracy. However, in highly non-Gaussian noise environments or during rapid motion scenarios, the iterative process of MC-AUKF may suffer from loss of positive definiteness in the covariance matrix due to rounding errors or ill-conditioning, leading to inconsistent state estimation and numerical instability in filtering. In contrast, the Square-Root Unscented Kalman Filter (SRUKF) significantly enhances numerical stability, computational efficiency, and robustness by introducing square-root decomposition, making it more suitable for robot-pose estimation in non-Gaussian noise environments.

Based on the above research background, this paper combines the Maximum Correntropy with the square root unscented Kalman filter to propose the Maximum Correntropy Square Root Unscented Kalman Filter SLAM (MCSRUKF-SLAM) algorithm. The MCSRUKF-SLAM uses MC instead of MMSE, effectively solving the problem of low-pose estimation accuracy caused by non-Gaussian noise in the system. Furthermore, the MCSRUKF-SLAM algorithm adds the square root filter on the basis of the Unscented Kalman Filter. It ensures the positive definiteness of the covariance matrix and improves the stability of the filtering values by performing Cholesky decomposition on the covariance matrix.

The rest of this paper is organized as follows: In Section 2, the SLAM problem is proposed, and then the SRUKF-SLAM algorithm is described. In Section 3, the MCSRUKF-SLAM for robot tracking is proposed. In Section 4, the robot system model is introduced, and then the simulation environment and parameters are given. In Section 5, the superiority of the proposed algorithm is verified by simulation and experiment. Finally, some conclusions are drawn in Section 6.

2. Problem Description and Existing Methods

2.1. SLAM Problem Description

The motion model and observation model of the mobile robot are the foundation for implementing SLAM. For SLAM, the motion model and observation model of the mobile robot can be described using the discrete nonlinear function. The motion model and observation model are expressed as

$$x_k=f\left(x_{k-1},u_k\right)+w_k,$$

(1)

$$z_k=h\left(x_k,m_k\right)+v_k,$$

(2)

where ${\mathit{x}}_k$ denotes the state vector of the robot pose at time k; it consists of the robot’s position information x_k, y_k, and heading angle ϕ_k, i.e., x_k = [x_k y_k ϕ_k]^T.${\mathit{u}}_k$ is the control vector of the robot at time k, $\mathit{f} (·)$ is the nonlinear state transition function of robot motion, and ${\mathit{w}}_k$ is the process noise. ${\mathit{z}}_k$ denotes the observation vector of the robot at time k, $\mathit{h} (·)$ is the nonlinear observation function of the robot, ${\mathit{v}}_k$ is the measurement noise, and ${\mathit{m}}_k$ denotes the environmental map at time k, which is composed of the spatial positions of n feature landmarks. It can be expressed as ${\mathit{m}}_k=\left\{m_1,{ m}_2,\dots ,{ m}_{\mathrm{n}}\right\}$.

The SLAM problem essentially utilizes the observation information ${\mathit{Z}}_{1: k}$ and control variable ${\mathit{U}}_{1: k}$ before time k to calculate the joint posterior probability density function $p ({\mathit{x}}_k,{\mathit{m}}_k |{\mathit{Z}}_{1: k},{\mathit{U}}_{1: k})$ of the robot state vector ${\mathit{x}}_k$ and the environmental map ${\mathit{m}}_k$ at the current time k. It is assumed that the motion model of the mobile robot is the Markov process, that is, the robot’s state is only related to the state of the previous moment. Therefore, according to Bayesian formula [42], $p ({\mathit{x}}_k,{\mathit{m}}_k |{\mathit{Z}}_{1: k},{\mathit{U}}_{1: k})$ can be expressed as

$$\begin{array}{r}p\left(x_k,m_k|Z_{1:k},U_{1:k}\right)\propto p\left(z_k|x_k,m_k\right){\displaystyle \int p\left(x_k|x_{k-1},u_k\right)}\cdot \\ p\left(x_{k-1},m_{k-1}|Z_{1:k-1},U_{1:k-1}\right)d{x}_{k-1}\end{array},$$

(3)

where ${\mathit{Z}}_{1: k}=\{{\mathit{z}}_1,{ \mathit{z}}_2,\dots ,{ \mathit{z}}_k\}$ denotes all observations from time 1 to k, and ${\mathit{U}}_{1: k}=\{{\mathit{u}}_1,{ \mathit{u}}_2,\dots ,{ \mathit{u}}_k\}$ denotes all control variables from time 1 to k. $p ({\mathit{x}}_k |{\mathit{x}}_{k-1},{\mathit{u}}_k)$ is the probability model of the robot motion. $p ({\mathit{z}}_k |{\mathit{x}}_k,{\mathit{m}}_k)$ is the probability model for the robot observation. $p ({\mathit{x}}_k, {\mathit{m}}_k |{\mathit{Z}}_{1: k},{\mathit{U}}_{1: k}),$ and $p ({\mathit{x}}_k,{\mathit{m}}_k |{\mathit{Z}}_{1: k-1},{\mathit{U}}_{1: k-1})$ are posterior probability density functions at time k and time k−1, respectively.

2.2. Square Root Unscented Kalman Filter SLAM

In general, it is difficult to directly integrate the probability expression in Equation (3). In SLAM, Bayesian filtering methods in probability statistics are usually used for the approximate calculation [13]. Furthermore, since the motion model and observation model of the mobile robot are the nonlinear system, nonlinear Bayesian filtering methods are usually used. Among them, the Square Root Unscented Kalman Filter SLAM (SRUKF-SLAM) algorithm [23] is a classic algorithm for solving the SLAM problem of the mobile robot. The algorithm uses Unscented Transform (UT) to approximate the posterior probability density of the system state and uses the square root form to ensure the positive definiteness of the covariance matrix, reduce the calculation error of the state estimation, and improve the calculation efficiency of the algorithm, so as to improve the real-time performance of the algorithm. The algorithm employs Unscented Transform (UT) to approximate the posterior probability density of the system state. By employing Cholesky decomposition to obtain the square-root matrix of the covariance, it ensures positive definiteness of the covariance matrix, reduces computational errors in state estimation, and enhances algorithmic efficiency.

The SRUKF-SLAM algorithm employs the Unscented Transform (UT) technique. The UT operates by strategically selecting sigma points that encapsulate the complete statistical properties (mean and covariance) of the state vector distribution. These sigma points are then propagated through the nonlinear system function, generating a transformed set of sigma points. The posterior statistics are subsequently computed through weighted averaging of these propagated sigma points. Furthermore, the algorithm utilizes the square-root matrix S of the covariance instead of the original covariance matrix P to guarantee the positive definiteness of the covariance matrix. The selection of sigma points in the SRUKF-SLAM algorithm can be determined by

$${\xi }_i=\left\{\begin{array}{l}\widehat{x},\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{1em}\hspace{0.25em} i=0\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\\ \widehat{x}+{\left(\sqrt{\left(n+\lambda \right)S}\right)}_i,\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}i=1,2,\cdot \cdot \cdot ,n\\ \widehat{x}-{\left(\sqrt{\left(n+\lambda \right)S}\right)}_{i-n},\hspace{0.33em}\hspace{0.33em}i=n+1,\cdot \cdot \cdot ,2n\end{array}\right.,$$

(4)

where $\widehat{\mathit{x}}$ is the mean vector of the known state vector x, and S is the square root matrix of the covariance matrix P of the state vector x, which can be obtained by decomposing P through Cholesky decomposition. n is the dimension of the state vector x, and ${\left(\sqrt{\left(n+\lambda \right)\mathit{S}}\right)}_i,$ is the i-th row of the square root matrix of $\left(n+\lambda \right)\mathit{S}$, which can be obtained by Cholesky decomposition.

The weight corresponding to each sigma point in Equation (4) is

$$\left\{\begin{array}{l}{\omega }_0^m={\displaystyle \frac{\lambda }{n+\lambda }}\\ {\omega }_0^c={\displaystyle \frac{\lambda }{n+\lambda }}+\left(1-{\alpha }^2+\beta \right)\\ {\omega }_i^m={\omega }_i^c={\displaystyle \frac{1}{2\left(n+\lambda \right)}},\hspace{0.33em}\hspace{0.33em}i=1,\cdot \cdot \cdot ,2n\end{array}\right.,$$

(5)

where ${\omega }_i^m$ denote the weights of the mean vector, and ${\omega }_i^c$ denotes the weights of the covariance matrix. The superscripts m and c represent the mean and covariance, respectively. The subscripts 0 and i represent the initial value and i-th sigma point, respectively. α determines the spread of the sigma points, the range of values for α is 0 ≤ α ≤ 1. β is a parameter related to the prior knowledge of the distribution of the state vector and is usually taken as 2. λ is a composite scaling factor, given by

$$\lambda ={\alpha }^2\left(n+\phi \right)-n,$$

(6)

where the value of parameter φ is usually 3n [43].

The following is the specific process of the SRUKF-SLAM algorithm.

(1) Initialize

$${\widehat{x}}_0=E\left[x_0\right],$$

(7)

$$S_0=\mathit{chol}\left\{E\left[\left(x_0-{\widehat{x}}_0\right){\left(x_0-{\widehat{x}}_0\right)}^T\right]\right\},$$

(8)

where ${\mathit{x}}_0$ denotes initial state vector, ${\widehat{\mathit{x}}}_0$ denotes the mean of the initial state vector, $E[·]$ denotes the expectation operator, $\mathrm{chol}\{·\}$ denotes the Cholesky decomposition of the matrix, and S₀ is the initial value of the square root matrix of covariance;

(2) Time Update

The selection of sigma points at time k−1 can be determined by

$${\xi }_{i,k-1}=\left\{\begin{array}{l}{\widehat{x}}_{k-1},\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}i=0\\ {\widehat{x}}_{k-1}+{\left(\sqrt{\left(n+\lambda \right)S_{k-1}}\right)}_i,\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}i=1,2,\cdot \cdot \cdot ,n\\ {\widehat{x}}_{k-1}-{\left(\sqrt{\left(n+\lambda \right)S_{k-1}}\right)}_{i-n},\hspace{0.33em}i=n+1,\cdot \cdot \cdot ,2n\end{array}\right.,$$

(9)

We substitute the sigma points ${\xi }_{i, k-1}$ in Equation (9) into the nonlinear state function f$(·)$ in Equation (1) and obtain the predicted value of the state vector at time k as

$$x_{i,k|k-1}=f\left({\xi }_{i,k-1},u_k\right),$$

(10)

The mean prediction of the state vector at time k can be determined by

$${\widehat{x}}_{k|k-1}={\displaystyle \sum _{i=0}^{2n}{\omega }_i^m{x}_{i,k|k-1}},$$

(11)

The covariance prediction of the state vector at time k can be determined by

$$S_{k|k-1}=\mathrm{QR}\left\{\left[\sqrt{{\omega }_i^c}\left(x_{i,k|k-1}-{\widehat{x}}_{k|k-1}\right)\hspace{1em}\sqrt{Q_{\mathrm{k}}}\right]\right\},$$

(12)

$$S_{k|k-1}=\mathit{cholupdate}\left\{\left[S_{k|k-1},\left(x_{0,k|k-1}-{\widehat{x}}_{k|k-1}\right),{\omega }_0^c\right]\right\},$$

(13)

where $\mathrm{QR} \{·\}$ denotes the process of QR decomposition, ${\mathit{Q}}_k$ represents the covariance matrix of process noise, and the operation cholupdate {·} implements a rank-1 update to the Cholesky factorization, yielding the corresponding updated Cholesky factor without requiring complete recomputation of the decomposition;

(3) Measurement Update

The selection of sigma points in the measurement update at time k−1 can be determined by

$${\chi }_{i,k|k-1}=\left\{\begin{array}{l}{\widehat{x}}_{k|k-1},\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}i=0\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\\ {\widehat{x}}_{k|k-1}+{\left(\sqrt{\left(n+\lambda \right)S_{k|k-1}}\right)}_i,\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}i=1,2,\cdot \cdot \cdot ,n\\ {\widehat{x}}_{k|k-1}-{\left(\sqrt{\left(n+\lambda \right)S_{k|k-1}}\right)}_{i-n},\hspace{0.33em}i=n+1,\cdot \cdot \cdot ,2n\end{array}\right.,$$

(14)

We substitute the sigma points ${\mathit{\chi }}_{i, k|k-1}$ in Equation (14) into the nonlinear observation function $\mathit{h} (·)$ in Equation (2), and obtain the predicted value of the measurement vector at time k as

$$z_{i,\hspace{0.33em}k|k-1}=h\left({\chi }_{i,k|k-1},m_k\right),$$

(15)

The mean prediction of the measurement vector at time k can be determined by

$${\widehat{z}}_{k|k-1}={\displaystyle \sum _{i=0}^{2n}{\omega }_i^m}{z}_{i,\hspace{0.33em}k|k-1},$$

(16)

The covariance prediction of the measurement vector at time k can be determined by

$$S_k^{zz}=\mathit{QR}\left\{\left[\sqrt{{\omega }_i^c}\left(z_{i,\hspace{0.33em}k|k-1}-{\widehat{z}}_{k|k-1}\right)\hspace{1em}\sqrt{R_k}\right]\right\},$$

(17)

$$S_k^{zz}=\mathit{cholupdate}\left\{\left[S_k^{zz},\left(z_{0,k|k-1}-{\widehat{z}}_{k|k-1}\right),{\omega }_0^c\right]\right\},$$

(18)

where ${\mathit{R}}_k$ denotes the covariance matrix of the measurement noise. The cross-covariance between the state vector and the measurement vector at time k can be determined by

$$P_k^{xz}={\displaystyle \sum _{i=0}^{2n}{\omega }_i^c}\left[\left({\chi }_{i,k|k-1}-{\widehat{x}}_{k|k-1}\right){\left(z_{i,k|k-1}-{\widehat{z}}_{k|k-1}\right)}^T\right],$$

(19)

(4) States Enhancement

The SRUKF-SAM algorithm is based on the MMSE criterion, and its cost function can be expressed as the problem of minimizing the error between state estimation and observation data, which can be written as

$$J\left(x_k\right)={\displaystyle \frac{1}{2}}{‖\left(z_k-H_k{x}_k\right)-\left({\widehat{z}}_{k|k-1}-H_k{\widehat{x}}_{k|k-1}\right)‖}_{R_k^{-1}}^2,$$

(20)

where ${‖\mathit{x}‖}_{\mathit{A}}^2={\mathit{x}}^{\mathrm{T}}\mathit{Ax}$, H_k is the Jacobian matrix of the measurement function h (·), and R_k is the covariance matrix of the measurement noise. To minimize the cost function J(x_k), we take the derivative of the state vector x_k in Equation (20) and set it as equal to zero, which can be expressed as

$${\displaystyle \frac{\partial J\left(x_k\right)}{\partial x_k}}=0,$$

(21)

We substitute the cost function of Equation (20) into Equation (21), and further write it as

$${\displaystyle \frac{\partial J\left(x_k\right)}{\partial x_k}}=-H_k^T{R}_k^{-1}\left[\left(z_k-H_k{x}_k\right)-\left({\widehat{z}}_{k|k-1}-H_k{\widehat{x}}_{k|k-1}\right)\right]=0,$$

(22)

We move the x_k item contained in Equation (22) to the left, which can be written as

$$x_k={\widehat{x}}_{k|k-1}+{\left(H_k{R}_k^{-1}{H}_k\right)}^{-1}{H}_k^T{R}_k^{-1}\left(z_k-{\widehat{z}}_{k|k-1}\right),$$

(23)

According to Equation (23), the Kalman gain K_k is

$$K_k={\left(H_k{R}_k^{-1}{H}_k\right)}^{-1}{H}_k^T{R}_k^{-1},$$

(24)

where

$$H_k=P_k^{xz}{\left(S_{k|k-1}{S}_{k|k-1}^T\right)}^{-1},$$

(25)

$$R_k=S_k^{zz}{\left(S_k^{zz}\right)}^T-H_k{S}_{k|k-1}{S}_{k|k-1}^T{H}_k^T,$$

(26)

According to Equations (24)–(26), the Kalman gain can also be expressed by the covariance matrix as

$$K_k=P_k^{xz}{\left[S_k^{zz}{\left(S_k^{zz}\right)}^T\right]}^{-1},$$

(27)

The state estimation can be determined by

$${\widehat{x}}_k={\widehat{x}}_{k|k-1}+K_k\left(z_k-{\widehat{z}}_{k|k-1}\right),$$

(28)

The covariance matrix of the state estimation error can be determined by

$$S_k=\mathit{cholupdate}\left\{\left[S_{k|k-1},K_k{S}_k^{zz},-1\right]\right\},$$

(29)

In summary, the SRUKF-SLAM algorithm employs the UT technique to address system nonlinearities. During both the time update and measurement update phases, it utilizes QR decomposition [44] and Cholesky decomposition to factorize matrix S into the orthogonal matrix and right matrix. By substituting this right matrix for the original covariance matrix P in iterative updates, the algorithm effectively resolves the issue of non-positive-definite covariance matrices.

3. The Proposed Algorithm

In SLAM, accumulated sensor errors, measurement noise, complex environments, and abrupt changes in robot motion states not only introduce system nonlinearities but also often result in non-Gaussian noise distributions. Conventional nonlinear optimization algorithms, including the aforementioned SRUKF-SLAM approach, universally adopt the MMSE optimization criterion. As evident from Equation (20), their cost functions incorporate only first-order (mean) and second-order (variance) statistics of the error signals, failing to exploit the higher-order statistical characteristics (e.g., skewness and kurtosis) of non-Gaussian noise. Consequently, MMSE-based estimators exhibit heightened sensitivity to outliers under non-Gaussian noise conditions, leading to significant performance degradation in state estimation. Research has demonstrated that replacing the MMSE criterion with the Maximum Correntropy (MC) Criterion [45,46] in conventional nonlinear filtering algorithms can effectively suppress non-Gaussian noise interference. In order to improve the estimation accuracy of the SRUKF-SLAM algorithm in non-Gaussian noise, this paper proposes the Maximum Correntropy Square Root Unscented Kalman Filter SLAM (MCSRUKF-SLAM) algorithm. The algorithm introduces the MC criterion instead of the MMSE criterion to effectively improve its robustness in non-Gaussian noise. Combined with the SRUKF-SLAM algorithm, it constructs a new cost function, and uses Cholesky decomposition and orthogonal triangular decomposition to better ensure the positive definiteness of the covariance matrix, so as to ensure the stability of the filtering value.

3.1. Maximum Correntropy Criterion

Rooted in information theory, Correntropy is particularly suitable for non-Gaussian noise environments encountered in communication systems, image processing, and robotic systems [47]. This measure demonstrates superior capability in handling nonlinear signals compared to conventional approaches, delivering enhanced performance. Correntropy quantifies the similarity between two random variables by incorporating the kernel function [48]. Considering two given random variables X and Y with joint density function F_XY (x, y), their Correntropy is defined by

$$V\left(X,Y\right)=\mathrm{E}\left[\kappa \left(X,Y\right)\right]={\displaystyle \iint \kappa \left(x,y\right)d{F}_{XY}\left(x,y\right)},$$

(30)

where $E [·]$ denotes the expectation operator, and $\kappa (·)$ is a real-valued continuous, symmetric, and positive definite kernel function. In practical problems, the infinite number of sample data results in an unknown joint distribution function ${\mathit{F}}_{\mathit{XY}}\left(x, y\right)$, making it impossible to directly calculate the Correntropy. Therefore, the Correntropy can be estimated using a sample estimator with N data points, as shown in Equation (31).

$$\widehat{V}\left(X,Y\right)={\displaystyle \frac{1}{N}}{\displaystyle \sum _{i=1}^N\kappa \left(x_i,y_i\right)},$$

(31)

where ${\left\{\left(x_i, y_i\right)\right\}}_{i=1}^N$ is the sample data. For the kernel function selection, we adopt the Gaussian kernel due to its inherent smoothness and positive definiteness properties, which can be mathematically expressed as

$$\kappa \left(x_i,y_i\right)=G_{\sigma }\left(x_i-y_i\right)=\mathit{exp}\left(-{\displaystyle \frac{{\left(x_i-y_i\right)}^2}{2{\sigma }^2}}\right),$$

(32)

where σ > 0 is the kernel bandwidth of the Correntropy. Substituting the Gaussian kernel function in Equation (32) into Equation (31), it can be expressed as

$$\widehat{V}\left(X,Y\right)={\displaystyle \frac{1}{N}}{\displaystyle \sum _{i=1}^N{G}_{\sigma }\left(x_i-y_i\right)},$$

(33)

By Taylor series expansion of the Gaussian kernel function in Equation (33), we can obtain

$$V\left(X,Y\right)={\displaystyle \sum _{n=0}^{\infty }\frac{{\left(-1\right)}^n}{2^n{\sigma }^{2n}n!}}E\left[{\left(X-Y\right)}^{2n}\right],$$

(34)

In Equation (34), it can be observed that the Correntropy includes all even-order moments of the error variable $\mathrm{X} - \mathrm{Y}$. From Equations (32) and (33), it can be observed that when the error (x_i − y_i) is small, the kernel function value approaches 1, whereas for large errors, it rapidly decays to 0. This analysis demonstrates that Correntropy can capture higher-order statistical moments of the error signal, thereby accurately characterizing the higher-order properties of non-Gaussian noise distributions. Moreover, by assigning smaller weights to large errors through the kernel function, Correntropy effectively suppresses the influence of non-Gaussian noise.

The Maximum Correntropy criterion is an optimization criterion to maximize the Correntropy between the estimated value and the real value. Therefore, the MC criterion can be expressed as

$$J_{\mathrm{MC}}=\max \left[{\displaystyle \sum _{i=1}^N{G}_{\sigma }\left(x_i-y_i\right)}\right],$$

(35)

In practical applications, the MC criterion is typically reformulated as an optimization problem that minimizes negative Correntropy. The MC criterion is formulated as

$$J_{\mathrm{MC}}=\min \left[-{\displaystyle \sum _{i=1}^N{G}_{\sigma }\left(x_i-y_i\right)}\right],$$

(36)

By leveraging the significant advantages of the MC criterion in suppressing non-Gaussian noise, we develop the Maximum Correntropy Square Root Unscented Kalman Filter SLAM (MCSRUKF-SLAM). The detailed derivation of the algorithm is presented in the following sections.

3.2. The Proposed MCSRUKF-SLAM Algorithm

For the convenience of analysis and research, the nonlinear system model of the proposed MCSRUKF-SLAM algorithm can be written as

$$\left\{\begin{array}{l}{x}_k=f_{k-1}\left(x_{k-1}\right)+w_{k-1}\\ z_k=h_k\left(x_k\right)+v_k\end{array}\right.,$$

(37)

where ${\mathit{x}}_k$ is the state vector that describes the robot pose, ${\mathit{z}}_k$ is the observation vector from the current position of the robot to the feature, and ${\mathit{w}}_k$ and ${\mathit{v}}_k$ denote the process noise and measurement noise of the robot, respectively. The nonlinear functions ${\mathit{f}}_{k-1}\left({\mathit{x}}_{k-1}\right)$ and ${\mathit{h}}_k\left({\mathit{x}}_k\right)$ are known. The traditional SLAM algorithm usually assumes the Gaussian white noise sequence with zero mean values for both process noise and measurement noise. However, the proposed MCSRUKF-SLAM algorithm assumes the measurement noise ${\mathit{v}}_k$ follows the non-Gaussian distribution. The proposed MCSRUKF-SLAM algorithm has the same (1) initialization, (2) time update, and (3) measurement update as the above SRUKF-SLAM algorithm.

In Equation (4) States Enhancement, the proposed MCSRUKF-SLAM algorithm reconstructs the cost function by employing Weighted Least Squares (WLS) [38] for Gaussian process noise while adopting the MC instead of MMSE to handle non-Gaussian measurement noise. We substitute the measurement error vector into Equation (32), and the Correntropy of the measurement error vector is expressed as

$$L_k=G_{\sigma }\left({‖\left(z_k-H_k{x}_k\right)-\left({\widehat{z}}_{k|k-1}-H_k{\widehat{x}}_{k|k-1}\right)‖}_{R_k^{-1}}^2\right),$$

(38)

where L_k is a scalar, which is easily separated in subsequent calculations, avoiding numerical problems. By minimizing the negative Correntropy according to [45] and by incorporating the measurement error’s Correntropy from Equation (38) into the cost function, we define the new cost function as

$$J_D={‖x_k-{\widehat{x}}_{k|k-1}‖}_{S_{k|k-1}^{-1}}^2-2{\sigma }^2{G}_{\sigma }\left({‖z_k-H_k{x}_k-{\widehat{z}}_{k|k-1}+H_k{\widehat{x}}_{k|k-1}‖}_{R_k^{-1}}^2\right),$$

(39)

where ${‖\mathit{x}‖}_{\mathit{A}}^2={\mathit{x}}^{\mathbf{T}}\mathit{Ax}$. We hope the Correntropy will be able to capture more information when non-Gaussian measurement noise appears in the system. Therefore, the Gaussian kernel is applied to every element of the estimation error matrices following the ideas of M-estimation [49], The expression of Gaussian kernel function is shown in Equation (32). The value of the kernel bandwidth of Gaussian kernel function has a significant impact on the performance of the algorithm, and the mismatched kernel bandwidth will lead to state prediction bias or covariance estimation distortion. When the bandwidth is too large, local dynamic changes will be ignored in state estimation, resulting in underfitting. When the bandwidth is too small, the state estimation will be interfered by outliers, resulting in overfitting, and the weight distribution of sigma points of the proposed algorithm will be unbalanced. In general, the empirical method is usually used to select the kernel bandwidth [38], but the fixed kernel bandwidth cannot flexibly adapt to the local characteristics of the data, which leads to the performance degradation of the algorithm in complex scenes. Adaptive kernel bandwidth solves the limitation of fixed bandwidth through data-driven flexibility [39], which is formulated as

$$\sigma =1/{‖z_k-{\widehat{z}}_{k|k-1}‖}_{R_k^{-1}},$$

(40)

where R_k is the relative variance of measurement noise at each time instant k.

The optimal estimate of the state vector ${\mathit{x}}_k$ is represented as

$${\widehat{x}}_k=\underset{x_k}{\mathit{argmax}} J_D,$$

(41)

In Equation (41), we observe that when ${\displaystyle \frac{\partial {\mathit{J}}_D}{\partial {\mathit{x}}_k }}=0$, the optimal solution for the state, vector ${\mathit{x}}_k$ can be obtained. Therefore, by making the partial derivative of the cost function ${\mathit{J}}_D$ equal to 0, we can obtain

$$\begin{array}{l}{\displaystyle \frac{\partial J_D}{\partial x_{\mathrm{k}}}}=S_{k|k-1}^{-1}\left(x_k-{\widehat{x}}_{k|k-1}\right)+2{\sigma }^2{\displaystyle \frac{L_k{H}_k^T{R}_k^{-1}}{-2{\sigma }^2}}\cdot \\ \hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\left(z_k-H_k{x}_k-{\widehat{z}}_{k|k-1}+H_k{\widehat{x}}_{k|k-1}\right)=0\end{array},$$

(42)

Moving the term contains ${\mathit{x}}_k$ to the left-hand side, we obtain

$$\begin{array}{r}\left(S_{k|k-1}^{-1}+L_k{H}_k^T{R}_k^{-1}{H}_k\right)x_k={\mathrm{L}}_k{H}_k^T{R}_k^{-1}\left(z_k-{\widehat{z}}_{k|k-1}\right)+\hspace{1em}\\ \left(L_k{H}_k^T{R}_k^{-1}{H}_k+S_{k|k-1}^{-1}\right){\widehat{x}}_{k|k-1}\end{array},$$

(43)

Equation (43) is a fixed-point equation [41] since L_k is related to ${\mathit{x}}_k$, which can be solved using the fixed-point method. Consequently, Equation (43) can be reformulated using the fixed-point method as

$$x_k=g\left(x_k\right),$$

(44)

$$g\left(x_k\right)=L_k{H}_k^T{R}_k^{-1}\left(z_k-{\widehat{z}}_{k|k-1}\right){\left(S_{k|k-1}^{-1}+L_k{H}_k^T{R}_k^{-1}{H}_k\right)}^{-1}+{\widehat{x}}_{k|k-1},$$

(45)

The solution to fixed-point Equations (44) and (45) fundamentally involves determining the fixed point that satisfies these equalities, which can be computed through iterative numerical methods. As established in Reference [50], these fixed-point equations satisfy single-step convergence conditions, meaning only one iteration is sufficient to achieve the required estimation performance. Consequently, after a single iteration of the fixed-point equations, we obtain

$$x_k=K_k\left(z_k-{\widehat{z}}_{k|k-1}\right)+{\widehat{x}}_{k|k-1},$$

(46)

$$K_k={\left(S_{k|k-1}^{-1}+L_k{H}_k^T{R}_k^{-1}{H}_k\right)}^{-1}{L}_k{H}_k^T{R}_k^{-1},$$

(47)

Equation (47) requires multiple matrix inversion operations, significantly increasing computational complexity. To address this, we introduce the matrix inversion lemma (All inversions exist) [51] as follows

$${\left(A+B{C}^{-1}D\right)}^{-1}=A^{-1}-A^{-1}B{\left(C+D{A}^{-1}B\right)}^{-1}D{A}^{-1},$$

(48)

According to Equation (48), We can obtain another equivalent form of Equation (47), as shown below.

$$\begin{array}{cc}{K}_k\hfill & =S_{k|k-1}{L}_k{H}_k^T{\left(I+R_k^{-1}{H}_k{S}_{k|k-1}{L}_k{H}_k^T\right)}^{-1}{R}_k^{-1}\hfill \\ & =S_{k|k-1}{L}_k{H}_k^T{\left(R_k+H_k{S}_{k|k-1}{L}_k{H}_k^T\right)}^{-1}\hfill \end{array},$$

(49)

In contrast to Equation (47), the formulation in Equation (49) requires only a single matrix inversion operation, thereby achieving significantly reduced computational complexity. Following the fixed-point methodology, during the iterative application of the fixed-point formula, ${\mathit{H}}_k{\mathit{x}}_k \approx {\mathit{H}}_k{\widehat{\mathbf{x}}}_{k|k-1}$ in Equation (38). Consequently, Equation (38) can be reformulated as

$$L_k=G_{\sigma }\left({‖z_k-{\widehat{z}}_{k|k-1}‖}_{R_k^{-1}}\right),$$

(50)

The corresponding estimation error covariance is calculated by

$$S_k=\left(I-K_k{H}_k\right)S_{k|k-1}{\left(I-K_k{H}_k\right)}^T+K_k{R}_k{K}_k^T,$$

(51)

$$S_k=\mathit{cholupdate}\left\{\left[S_{k|k-1},K_k{S}_k^{zz},-1\right]\right\},$$

(52)

The above is the specific derivation process of the proposed MCSRUKF-SLAM algorithm. To well understand the proposed algorithm, Algorithm 1 summarizes the pseudo-code of the MCSRUKF-SLAM algorithm, and Figure 1 shows the flowchart of the MCSRUKF-SLAM algorithm.

Algorithm 1: MCSRUKF-SLAM

1: Initialize: ${\widehat{\mathit{x}}}_0$, S0

2: for k = 1: n;   3: Generate sigma points ${\mathit{\xi }}_{i, k-1}$ using ${\widehat{\mathit{x}}}_{k-1}$ and Sk−1 by (9);   4: Obtain the mean ${\widehat{\mathit{x}}}_{k|k-1}$ and covariance of the one step predicted state Sk|k−1 by (10)–(13);   5: Calculate sigma points ${\mathit{\chi }}_{i, k|k-1}$ using ${\widehat{\mathit{x}}}_{k|k-1}$ and Sk|k−1 by (14);   6: Compute the mean ${\widehat{\mathit{z}}}_{k|k-1}$, the covariance matrix ${\mathit{S}}_k^{\mathit{zz}}$ and the cross-covariance matrix ${\mathit{P}}_k^{\mathit{zz}}$ of the predicted measurement by (15)–(19);   7: Obtain the pseudo measurement matrix Hk by (25);   8: Obtain Rk by (26);

9: Calculate Lk and Kk by (49) and (50);   10: Calculate xk and Sk by (46), (51), and (52);

11: end.

4. Material and Methods

4.1. System Model

The system model of the mobile robot generally consists of the motion model and the observation model. For SLAM, the motion model and the observation model can be described using discrete nonlinear functions. Therefore, the more complex the nonlinear function, the greater the computational complexity in solving the SLAM problem. Relatively simple motion models and observation models are generally used in SLAM. In this paper, we adopt the wheeled robot model shown in Figure 2.

The state vector ${\mathit{x}}_k$ of the wheeled robot is usually composed of the robot’s position information $x_k$, $y_k$, and course angle ${\varphi }_k$, i.e., ${\mathit{x}}_k={[x_k{y}_k{ \varphi }_k ]}^{\mathrm{T}}$. The observation vector ${\mathit{z}}_{j, k}$ of the wheeled robot is usually composed of the distance ${\mathit{r}}_{j, k}$ between the j-th feature $m_j$ of the map and the sensor, and the direction angle ${\mathit{\theta }}_{j, k}$ between feature $m_j$ and the sensor, i.e., ${\mathit{z}}_{j, k}={[{\mathit{r}}_{j, k} {\mathit{\theta }}_{j, k}]}^{\mathrm{T}}$. Therefore, the motion model and observation model of the wheeled robot are expressed as

$$x_k=\left[\begin{array}{l}{x}_k\\ y_k\\ {\varphi }_k\end{array}\right]=\left[\begin{array}{c}{x}_{k-1}+\Delta T\cdot V_k\mathit{cos}\left({\varphi }_{k-1}+{\Omega }_k\right)\\ y_{k-1}+\Delta T\cdot V_k\mathit{sin}\left({\varphi }_{k-1}+{\Omega }_k\right)\\ {\varphi }_{k-1}+{\displaystyle \frac{\Delta T\cdot V_k}{L}}\mathit{sin}\left({\Omega }_k\right)\end{array}\right]+w_k,$$

(53)

$$z_{j,k}=\left[\begin{array}{l}{r}_{j,k}\\ {\theta }_{j,k}\end{array}\right]=\left[\begin{array}{c}\sqrt{{\left(x_{j,k}-x_k\right)}^2+{\left(y_{j,k}-y_k\right)}^2}\\ \mathit{arctan}\left({\displaystyle \frac{y_{j,k}-y_k}{x_{j,k}-x_k}}\right)-{\varphi }_k\end{array}\right]+v_k,$$

(54)

where $V_k$ denotes the speed control amount of the robot, ${\Omega }_k$ denotes the wheel steering angle control amount of the robot, $\Delta T$ denotes the sampling interval, L denotes the wheelbase of the front and rear wheels, $(x_{j, k}, y_{j, k})$ is the coordinate of the j-th feature in the world coordinate system, ${\mathit{w}}_k$ is the process noise, and ${\mathit{v}}_k$ is the observation noise.

4.2. Simulation Environment

We used the MATLAB R2018a platform to establish an experimental environment with the size of 170 m * 140 m, randomly placed 100 uniformly distributed road markers, and set the true path of the mobile robot, as shown in Figure 3.

In Figure 3, the blue (*) denote the landmarks, the red dots 1–8 denote the key points on the path, the black dashed line denotes the true path of the robot, the black circle denotes the robot, the blue dot denotes the current position of the robot, the landmarks connected to the blue dot denote the landmarks observed by the robot sensor, and the rose red circle denotes the area composed of the sensor’s observation radius. The robot started moving from red dot 1 and moved along the black dashed line until it stopped at red dot 8. In order to conform to reality, we programed it so that the robot only observed the landmarks within the 180 degrees range directly ahead during movement. The key parameters in the simulation [25,26] are shown in

Table 1. Main parameters of the robot.

Parameters	Symbols	Value
Movement speed (m/s)	V	8
Maximum observation distance (m)	mx	30
Wheel-steering angle (rad)	Ω	30
Maximum turning speed of wheels (rad/s)	Vt	20
Control frequency (Hz)	fc	40
Observed frequency (Hz)	fb	5
Adjusting weights	δ	8
Damping parameters	μ	0.001
Robot wheelbase (m)	L	4

.

5. Results

5.1. Simulation Results and Analysis

Non-Gaussian noise is ubiquitous in practical robot systems, such as the radar with Gaussian mixture measurement noise and the visual sensor with colored measurement noise. Therefore, we set up simulation experiments to prove the effectiveness of the proposed MCSRUKF-SLAM algorithm in Gaussian noise, Gaussian mixture noise, and colored noise environments. Furthermore, we analyzed the experimental results from the accuracy and running time of different algorithms.

5.1.1. The Accuracy Comparison of Different Algorithms

In order to study the accuracy performance of the algorithm, the Root Mean Square Error (RMSE) and state estimation error were used to evaluate the SLAM algorithm in the experiment. ${\widehat{\mathit{x}}}_k\left(s\right)$ denotes the estimation pose of the robot at time k in the s-th simulation, and ${\mathit{x}}_k\left(s\right)$ denotes the true pose of the robot at time k in the s-th simulation. ${\widehat{\mathit{x}}}_k\left(s\right)$ and ${\mathit{x}}_k\left(s\right)$ are composed of the robot’s position information and heading angle, which can be written as

$${\widehat{x}}_k\left(s\right)={\left[{\widehat{x}}_k\left(s\right),{\widehat{y}}_k\left(s\right),{\widehat{\varphi }}_k\left(s\right)\right]}^T,$$

(55)

$$x_k\left(s\right)={\left[x_k\left(s\right),y_k\left(s\right),{\varphi }_k\left(s\right)\right]}^T,$$

(56)

Therefore, the RMSE and Average RMSE (ARMSE) can be expressed as

$${\mathrm{RMSE}}_k=\sqrt{{\displaystyle \frac{1}{m}}{\displaystyle \sum _{s=1}^m{‖\left[{\widehat{x}}_k\left(s\right),{\widehat{y}}_k\left(s\right)\right]-\left[x_k\left(s\right),y_k\left(s\right)\right]‖}_2^2}},$$

(57)

$$\mathrm{ARMSE}={\displaystyle \frac{1}{n}}{\displaystyle \sum _{k=1}^n{\mathrm{RMSE}}_k},$$

(58)

where m denotes the number of Monte Carlo runs, m = 50, n denotes the total time step, and n = 1133.

Similarly, the state estimation error and average state estimation error can be expressed as

$${\mathrm{error}}_{x,k}=\sqrt{{\displaystyle \frac{1}{m}}{\displaystyle \sum _{s=1}^m{\left[{\widehat{x}}_k\left(s\right)-x_k\left(s\right)\right]}^2}},$$

(59)

$${\mathrm{Aerror}}_x={\displaystyle \frac{1}{n}}{\displaystyle \sum _{k=1}^n{\mathrm{error}}_{x,k}},$$

(60)

$${\mathrm{error}}_{y,k}=\sqrt{{\displaystyle \frac{1}{m}}{\displaystyle \sum _{s=1}^m{\left[{\widehat{y}}_k\left(s\right)-y_k\left(s\right)\right]}^2}},$$

(61)

$${\mathrm{Aerror}}_y={\displaystyle \frac{1}{n}}{\displaystyle \sum _{k=1}^n{\mathrm{error}}_{y,k}},$$

(62)

$${\mathrm{error}}_{\varphi ,k}=\sqrt{{\displaystyle \frac{1}{m}}{\displaystyle \sum _{s=1}^m{\left[{\widehat{\varphi }}_k\left(s\right)-{\varphi }_k\left(s\right)\right]}^2}},$$

(63)

$${\mathrm{Aerror}}_{\varphi }={\displaystyle \frac{1}{n}}{\displaystyle \sum _{k=1}^n{\mathrm{error}}_{\varphi ,k}},$$

(64)

• Gaussian noise

In this simulation, we set both state noise and measurement noise as Gaussian noise. The distribution function of Gaussian noise is expressed as

$$f_g=N\left(\mu ,{\sigma }^2\right),$$

(65)

where $\mu =0$, ${\sigma }^2=4$, $N \left(\mu , {\sigma }^2\right)$ denotes normal distribution with mean $\mu$ and variance ${\sigma }^2$.

The simulation results are given in Figure 4, Figure 5 and Figure 6. Figure 4 shows the comparison of simulation results of five algorithms in Gaussian noise. Figure 5 shows the RMSE comparison of five algorithms in Gaussian noise. Figure 6 shows the state estimation errors comparison of five algorithms in Gaussian noise. As we can see, EKF-SLAM, UKF-SLAM, SRUKF-SLAM, MCUKF-SLAM, and the proposed MCSRUKF-SLAM algorithm can obtain similar estimation performance. Therefore, the simulation results demonstrate that the proposed MCSRUKF-SLAM algorithm performs effectively in a Gaussian noise environment.

• Gaussian mixture noise;

In this simulation, we set the state noise as Gaussian noise, as shown in Equation (65). The measurement noise was set as Gaussian mixture noise [50], and its distribution function was defined as

$$f_m=\lambda N\left(\mu ,{\sigma }_1^2\right)+\left(1-\lambda \right)\mathrm{N}\left(\mu ,{\left(2{\sigma }_1\right)}^2\right),$$

(66)

where ${\sigma }_1^2$ is determined by ${\sigma }_V^2$ and ${\sigma }_{\gamma }^2$. When adding noise to the robot speed V, ${\sigma }_1^2={\sigma }_{\mathrm{V}}^2=4 \mathrm{m}$. When adding noise to the robot rudder angle γ, ${\sigma }_1^2={\sigma }_{\gamma }^2= 100°$.

The simulation results are given in Figure 7, Figure 8 and Figure 9, and

Table 2. Comparison of average RMAE and average state estimation error for some SLAM algorithms in Gaussian mixture noise.

Algorithms	ARMSE (m)	Aerrorx (m)	Aerrory (m)	Aerrorϕ (rad)
EKF-SLAM [13]	1.0039	0.4128	0.6310	0.0168
UKF-SLAM [20]	0.8756	0.2956	0.4927	0.0138
SRUKF-SLAM [23]	0.8348	0.2549	0.4776	0.0136
MCUKF-SLAM [39]	0.8023	0.2335	0.4511	0.0129
The proposed MCSRUKF-SLAM	0.4737	0.1905	0.2708	0.0079

. Figure 7 shows the comparison of the simulation results of five algorithms in Gaussian mixture noise. Figure 8 shows the RMSE comparison of five algorithms in Gaussian mixture noise. Figure 9 shows the state estimation errors comparison of five algorithms in Gaussian mixture noise. Table 2 shows the Average RMSE and average state estimation errors of five algorithms in Gaussian mixture noise.

In Figure 7, we observe that the simulation results obtained by EKF-SLAM, UKF-SLAM, and SRUKF-SLAM have little difference, and there is a large error compared to the true path. Because the above three algorithms are affected by non-Gaussian noise and exhibit divergence, it increases the difficulty of control. The simulation results obtained by the MCUKF-SLAM algorithm perform better than the above three algorithms, which also proves the effectiveness of the Maximum Correntropy in the non-Gaussian noise environment. The simulation results of the proposed MCSRUKF-SLAM algorithm are optimal, and its pose estimation results are the closest to the true path. Therefore, compared to other algorithms, the proposed MCSRUKF-SLAM algorithm has higher pose estimation accuracy and better anti-interference ability in Gaussian mixture noise.

In Figure 8 and Figure 9, and Table 2, we observe that the accuracy of MCUKF-SLAM is higher than that of the EKF-SLAM, UKF-SLAM, and SRUKF-SLAM algorithms. The accuracy of the EKF-SLAM algorithm is the worst, and UKF-SLAM and SRUKF-SLAM show almost the same accuracy performance. The proposed MCSRUKF-SLAM algorithm is significantly better than other algorithms in pose estimation accuracy. Furthermore, the accuracy of the proposed MCSRUKF-SLAM algorithm improved by 81.8%, 80.9%, 78.7%, and 63.6% compared to EKF-SLAM, UKF-SLAM, SRUKF-SLAM, and MCUKF-SLAM, respectively.

• Colored noise

In this simulation, we set the state noise as Gaussian noise, as shown in Equation (65). The measurement noise was set as colored noise, and its distribution function [38] can be defined as

$$f_c\left(k\right)=f_1\left(k\right)+c_1{f}_1\left(k-1\right)+c_2{f}_1\left(k-2\right),$$

(67)

where $f_1=N \left(\mu , {\sigma }_1^2\right)$, $c_1$ and $c_2$ are weight coefficients, and $c_1=0.8$, $c_2=-0.6$. The value of ${\sigma }_1^2$ in ${\mathrm{f}}_1$ refers to Equation (66).

The simulation results are given in Figure 10, Figure 11 and Figure 12 and

Table 3. Comparison of average RMAE and average state estimation error for some SLAM algorithms in colored noise.

Algorithms	ARMSE (m)	Aerrorx (m)	Aerrory (m)	Aerrorϕ (rad)
EKF-SLAM [13]	1.3783	0.6039	0.8841	0.0171
UKF-SLAM [20]	1.1340	0.4336	0.6119	0.0138
SRUKF-SLAM [23]	1.1091	0.4257	0.6005	0.0137
MCUKF-SLAM [39]	1.0767	0.4018	0.5903	0.0135
The proposed MCSRUKF-SLAM	0.6851	0.2734	0.3849	0.0123

. Figure 10 shows the comparison of the simulation results of five algorithms in colored noise. Figure 11 shows the RMSE comparison of five algorithms in colored noise. Figure 12 shows the state estimation errors comparison of five algorithms in colored noise. Table 3 shows the ARMSE and average state estimation errors of different SLAM algorithms in colored noise.

In Figure 10, we observe that there is a certain error between the estimated path obtained by the EKF-SLAM, UKF-SALM, and SRUKF-SLAM algorithms in colored noise and the true path of the robot, indicating that the three algorithms have poor resistance to non-Gaussian noise interference. The simulation results of the MCUKF-SLAM algorithm are slightly better than the above three algorithms. The proposed MCSRUKF-SLAM algorithm is optimal, and the estimated path obtained is also closer to the true path of the robot. Therefore, the proposed MCSRUKF-SLAM algorithm has better anti-interference ability and robustness in colored noise.

In Figure 11 and Figure 12, and Table 3, we observe that UKF-SLAM and SRUKF-SLAM have better accuracy performance than the EKF-SLAM algorithm. UKF-SLAM and SRUKF-SLAM have almost the same accuracy performance, and MCUKF-SLAM has better accuracy than the EKF-SLAM, UKF-SLAM, and SRUKF-SLAM algorithms. The proposed MCSRUKF-SLAM algorithm is significantly better than other algorithms in pose estimation accuracy. Furthermore, the accuracy of the proposed MCSRUKF-SLAM algorithm improves by 50.3%, 39.9%, 38.2%, and 36.3% compared to EKF-SLAM, UKF-SLAM, SRUKF-SLAM, and MCUKF-SLAM, respectively.

In summary, the experimental results show that the proposed MCSRUKF-SLAM algorithm has better pose estimation accuracy compared to other algorithms in Gaussian mixture noise and colored noise. The simulation results confirm the proposed MCSRUKF-SLAM algorithm’s robustness and effectiveness under non-Gaussian noise conditions.

5.1.2. The Stability Analysis of Different Algorithms

In the fields of target tracking, navigation, and autonomous driving, the stability of filtering algorithms directly affects the accuracy and reliability of state estimation. Due to variations in motion speed, the system nonlinearity significantly increases, which may lead to substantial errors or even divergence in SLAM algorithms. Therefore, analyzing the stability of different algorithms under varying speeds holds significant theoretical and practical value.

We conducted 100 Monte Carlo simulation experiments in low-speed (8 m/s), medium-speed (15 m/s), and high-speed (30 m/s) scenarios. The measurement noise was set as Gaussian mixture noise, while the state noise followed a Gaussian distribution. The Average Root Mean Square Error (ARMSE) and divergence count per 100 runs were used as evaluation metrics for algorithm stability. The experimental results are presented in

Table 4. Comparison of average RMSE for various algorithms at different speeds (Unit: m).

Algorithms	ARMSE (8 m/s)	ARMSE (15 m/s)	ARMSE (30 m/s)	Error growth rate (8→30 m/s)
EKF-SLAM [13]	0.9858	1.8040	3.1131	216%
UKF-SLAM [20]	0.8607	1.3159	2.3353	171%
SRUKF-SLAM [23]	0.8208	1.0231	1.4518	77%
MCUKF-SLAM [39]	0.7891	1.1233	1.8623	136%
The proposed MCSRUKF-SLAM	0.4655	0.5633	0.8039	73%

and

Table 5. Comparison of divergence count per 100 runs for various algorithms at different speeds.

Algorithms	Divergence Count per 100 Runs (8 m/s)	Divergence Count per 100 Runs (15 m/s)	Divergence Count per 100 Runs (30 m/s)
EKF-SLAM [13]	3	13	25
UKF-SLAM [20]	0	6	13
SRUKF-SLAM [23]	0	0	0
MCUKF-SLAM [39]	0	4	8
The proposed MCSRUKF-SLAM	0	0	0

.

As shown in Table 4 and Table 5, the EKF-SLAM algorithm exhibits significant limitations in high-speed scenarios (30 m/s), with an error of 3.1131 m, an error growth rate of 216%, and a divergence rate of 25%. These empirical results conclusively validate the inherent shortcomings of linearization approximations when applied to highly nonlinear systems operating at elevated velocities. The UKF-SLAM algorithm demonstrates notable improvements through unscented transformation, reducing the error by 25% compared to EKF at 30 m/s. However, its 171% error growth rate and 13% divergence rate still indicate insufficient adaptability to high-speed conditions. In contrast, SRUKF-SLAM exhibits excellent error control capabilities, achieving zero divergence across all speed ranges and maintaining a low error growth rate of 77%. The square-root decomposition effectively enhances its numerical stability. The MCUKF-SLAM algorithm demonstrates notable limitations in high-speed scenarios (30 m/s), where maintaining positive definiteness of the covariance matrix becomes challenging. This instability leads to significant estimation errors, reflected in a 136% error growth rate and an 8% divergence rate. The proposed MCSRUKF-SLAM algorithm achieves zero divergence across all speed ranges and delivers the best error growth rate (73%). These results confirm its superior numerical stability, as the square-root decomposition effectively addresses numerical ill-conditioning issues.

5.1.3. The Running Time Comparison of Different Algorithms

In order to investigate the computational efficiency of different SLAM algorithms, the running time of different SLAM algorithms was compared in Gaussian mixture noise and colored noise. The experimental results are shown in Table 4. In this experiment, the processor model of the laptop is 11th Gen Intel (R) Core (TM) i7-1165G7 @ 2.80GHz, the memory is 16G, and the software used in the experiment is MATLAB 2018a.

Table 6. Running time comparison of five algorithms under Gaussian mixture noise conditions.

Algorithms	Total Running Time (s)	Single Step Time (s)
EKF-SLAM [13]	223.201	0.197
UKF-SLAM [20]	244.718	0.216
SRUKF-SLAM [23]	235.664	0.208
MCUKF-SLAM [39]	319.506	0.282
The proposed MCSRUKF-SLAM	311.575	0.275

and

Table 7. Running time comparison of five algorithms under colored noise conditions.

Algorithms	Total Running Time (s)	Single Step Time (s)
EKF-SLAM [13]	232.265	0.206
UKF-SLAM [20]	250.393	0.222
SRUKF-SLAM [23]	240.196	0.215
MCUKF-SLAM [39]	329.703	0.293
The proposed MCSRUKF-SLAM	324.038	0.285

demonstrate that the computational time of identical SLAM algorithms varies slightly between Gaussian mixture noise and colored noise environments. This variation primarily results from noise-induced differences in matrix operation complexity, though the observed time differences remain negligible relative to the total running time. The experimental results obtained in both non-Gaussian noise environments exhibit remarkable consistency. Comparative analysis reveals that the EKF-SLAM algorithm achieves the shortest execution time. SRUKF-SLAM and UKF-SLAM exhibit similar performance in terms of computational speed. The proposed MCSRUKF-SLAM algorithm achieves comparable computational efficiency to MCUKF-SLAM. While the square-root formulation of MCSRUKF-SLAM provides marginal improvements in numerical stability, statistical analysis confirms this advantage does not reach significance. Experimental results show that while MCSRUKF-SLAM requires marginally longer runtime, its superior system performance and robustness fully compensate for this minor drawback.

5.2. Experimental Results and Analysis

To further validate the performance of the proposed SLAM algorithm, we evaluated EKF-SLAM, UKF-SLAM, SRUKF-SLAM, MCUKF-SLAM, and our MCSRUKF-SLAM using a laser SLAM dataset collected in an outdoor parking environment by the Australian Centre for Robotics at the University of Sydney [52].

This parking lot represents a complex semi-structured outdoor environment prone to generating non-Gaussian noise and significant outliers due to environmental variations and electromagnetic interference. Such realistic conditions provide an ideal testbed for validating both the robustness and precision performance of SLAM algorithms. The geometric parameters and sensor configuration of the data collection vehicle are shown in Figure 13. As illustrated, the experimental platform is a four-wheeled intelligent vehicle equipped with wheel encoders (ROD-430), a LiDAR sensor (SICK LMS 221), and a GPS receiver (Ashtech GG24). The wheel encoder was employed to measure the vehicle speed and steering angle, with standard deviations of 2 m/s and π/30 rad, respectively. A lidar system with a 180° frontal field of view (FOV) was utilized to estimate the range and heading angle of environmental features, exhibiting standard deviations of 1 m and π/60 rad, respectively. GPS was used to obtain the ground truth of the vehicle state for validating the performance of the SLAM algorithm. In this experiment, the steel pole wrapped with reflective material served as an artificial landmark to enhance the detection of feature-based fiducials by the lidar system.

EKF, UKF, SRUKF, and MCUKF are widely adopted algorithms in SLAM systems. Therefore, in our experiments, the proposed MCSRUKF-SLAM algorithm was comprehensively compared against these baseline methods. The experimental results in Figure 14 were obtained by averaging 30 Monte Carlo runs for each SLAM algorithm. In the figure, crosses (+) and squares (□) denote the estimated landmark positions and ground truth landmark positions, respectively, while the dashed lines in different colors and solid black lines represent the robot’s estimated trajectory and ground truth trajectory, respectively.

Table 8. Comparison of average RMSE for various SLAM algorithm.

Algorithm	EKF-SLAM	UKF-SLAM	SRUKF-SLAM	MCUKF-SLAM	The Proposed MCSRUKF-SLAM
ARMSE(m)	0.71	0.558	0.537	0.493	0.284

shows the ARMSE of different algorithms in the car park dataset. As shown in Figure 14 and Table 8, the estimation performance of EKF-SLAM is inferior to that of UKF-SLAM and SRUKF-SLAM, with UKF-SLAM and SRUKF-SLAM exhibiting comparable performance. MCUKF-SLAM outperformed SRUKF-SLAM, while the proposed MCSRUKF-SLAM demonstrates significantly higher accuracy than all other SLAM algorithms. Therefore, the experimental results in Figure 14 and Table 8 indicate that the proposed MCSRUKF-SLAM algorithm achieved remarkable localization precision in the real-world parking lot datasets.

6. Conclusions

The contribution of this paper is to address the problems of the SLAM algorithm in mobile robot applications, with a focus on researching robot-pose estimation algorithms under non-Gaussian noise. Furthermore, in response to the problem of low estimation accuracy of the traditional Kalman filtering algorithm in non-Gaussian noise, we proposed the MCSRUKF-SLAM algorithm to improve the accuracy of the robot pose estimation. The proposed MCSRUKF-SLAM algorithm incorporates the square root form of sigma points into the Unscented Kalman Filter, and uses relatively simple QR decomposition and Cholesky decomposition to perform square root operations on the error covariance matrix. Moreover, the proposed MCSRUKF-SLAM algorithm which combines with the Maximum Correntropy criterion, effectively improves the problems of poor pose estimation accuracy in non-Gaussian noise.