# Vehicle Pose Estimation Method Based on Maximum Correntropy Square Root Unscented Kalman Filter
## 3 The Proposed Algorithm
### 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}\mathbf{x}_{k}=\mathbf{f}_{k-1}(\mathbf{x}_{k-1})+\mathbf{w}_{k-1}\\ \mathbf{z}_{k}=\mathbf{h}_{k}(\mathbf{x}_{k})+\mathbf{v}_{k}\end{array} \right., \tag{37}\]  
where \(\mathbf{x}_{k}\) is the state vector that describes the robot pose, \(\mathbf{z}_{k}\) is the observation vector from the current position of the robot to the feature, and \(\mathbf{w}_{k}\) and \(\mathbf{v}_{k}\) denote the process noise and measurement noise of the robot, respectively. The nonlinear functions \(\mathbf{f}_{k-1}(\mathbf{x}_{k-1})\) and \(\mathbf{h}_{k}(\mathbf{x}_{k})\) 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 \(\mathbf{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}\bigg{(}\left\|(\mathbf{z}_{k}-\mathbf{H}_{k}\mathbf{x}_{k})-\left(\mathbf{z}_ {k|k-1}-\mathbf{H}_{k}\mathbf{\hat{x}}_{k|k-1}\right)\right\|_{\mathbf{R}_{k}^{-1}}^{2} \bigg{)}, \tag{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  
\[\mathbf{J}_{D}=\left\|\mathbf{x}_{k}-\mathbf{\hat{x}}_{k|k-1}\right\|_{S^{-1}_{k|k-1}}^{2} -2\sigma^{2}G_{\sigma}\bigg{(}\left\|\mathbf{z}_{k}-\mathbf{H}_{k}\mathbf{x}_{k}-\mathbf{\hat {z}}_{k|k-1}+\mathbf{H}_{k}\mathbf{\hat{x}}_{k|k-1}\right\|_{\mathbf{R}_{k}^{-1}}^{2} \bigg{)}, \tag{39}\]  
where \(\left\|\mathbf{x}\right\|_{\mathbf{A}}^{2}=\mathbf{x}^{\mathbf{T}}\mathbf{A}\mathbf{x}\). 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. Whenthe 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/\left\|z_{k}-\hat{z}_{k|k-1}\right\|_{R_{k}^{-1}}, \tag{40}\]  
where \(R_{k}\) is the relative variance of measurement noise at each time instant \(k\).