# Vehicle Pose Estimation Method Based on Maximum Correntropy Square Root Unscented Kalman Filter
## 3 The Proposed Algorithm
### 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(X,Y)=\mathrm{E}[\kappa(X,Y)]=\iint\kappa(x,y)d\mathbf{F}_{XY}(x,y), \tag{30}\]  
where \(E\left[\cdot\right]\) denotes the expectation operator, and \(\kappa\) (\(\cdot\)) 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 \(F_{XY}(x,y)\), 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).  
\[\hat{V}(X,Y)=\frac{1}{N}{\sum\limits_{i=1}^{N}\kappa(x_{i},y_{i})}, \tag{31}\]  
where \(\{(x_{i},y_{i})\}_{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(x_{i},y_{i})=G_{\sigma}(x_{i}-y_{i})=exp\left(-\frac{(x_{i}-y_{i})^{2}} {2\sigma^{2}}\right), \tag{32}\]  
where \(\sigma>0\) is the kernel bandwidth of the Correntropy. Substituting the Gaussian kernel function in Equation (32) into Equation (31), it can be expressed as  
\[\hat{V}(X,Y)=\frac{1}{N}{\sum\limits_{i=1}^{N}G_{\sigma}(x_{i}-y_{i})}, \tag{33}\]  
By Taylor series expansion of the Gaussian kernel function in Equation (33), we can obtain  
\[V(X,Y)=\sum\limits_{n=0}^{\infty}\frac{\left(-1\right)^{n}}{2^{n}\sigma^{2n}n!!}E\Big{[}(X-Y)^{2n}\Big{]}, \tag{34}\]  
In Equation (34), it can be observed that the Correntropy includes all even-order moments of the error variable \(X\ -\ 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  
\[\mathbf{J}_{\mathrm{MC}}=\max\left[\sum\limits_{i=1}^{N}G_{\sigma}(x_{i}-y_{i}) \right], \tag{35}\]In practical applications, the MC criterion is typically reformulated as an optimization problem that minimizes negative Correntropy. The MC criterion is formulated as  
\[\mathbf{J}_{\text{MC}}=\min\left[-\sum_{i=1}^{N}G_{\sigma}(x_{i}-y_{i})\right], \tag{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.