# Vehicle Pose Estimation Method Based on Maximum Correntropy Square Root Unscented Kalman Filter
## 5 Results
### Simulation Results and Analysis
#### 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. \(\hat{\mathbf{x}}_{k}(s)\) denotes the estimation pose of the robot at time \(k\) in the \(s\)-th simulation, and \(\mathbf{x}_{k}(s)\) denotes the true pose of the robot at time \(k\) in the \(s\)-th simulation. \(\hat{\mathbf{x}}_{k}(s)\) and \(\mathbf{x}_{k}(s)\) are composed of the robot's position information and heading angle, which can be written as  
\[\hat{\mathbf{x}}_{k}(s)=\big{[}\hat{x}_{k}(s),\hat{y}_{k}(s),\hat{\mathbf{\phi}}_{k}(s) \big{]}^{T}, \tag{55}\]  
\[\mathbf{x}_{k}(s)=[x_{k}(s),y_{k}(s),\hat{\mathbf{\phi}}_{k}(s)]^{T}, \tag{56}\]  
Therefore, the RMSE and Average RMSE (ARMSE) can be expressed as  
\[\text{RMSE}_{k}=\sqrt{\frac{1}{m}\sum_{s=1}^{m}\lVert[\hat{x}_{k}(s),\hat{y}_ {k}(s)]-[x_{k}(s),y_{k}(s)]\rVert_{2}^{2}}, \tag{57}\]  
\[\text{ARMSE}=\frac{1}{n}\sum_{k=1}^{n}\text{RMSE}_{k}, \tag{58}\]  
where m denotes the number of Monte Carlo runs, \(m=50\), \(n\) denotes the total time step, and \(n=1133\). \begin{table}
\begin{tabular}{c c c} \hline \hline
**Parameters** & **Symbols** & **Value** \\ \hline Movement speed (m/s) & \(V\) & 8 \\ Maximum observation distance (m) & mx & 30 \\ Wheel-steering angle (rad) & \(\Omega\) & 30 \\ Maximum turning speed of wheels (rad/s) & \(Vt\) & 20 \\ Control frequency (Hz) & \(f\hat{c}\) & 40 \\ Observed frequency (Hz) & \(f\hat{b}\) & 5 \\ Adjusting weights & \(\delta\) & 8 \\ Damping parameters & \(\mu\) & 0.001 \\ Robot wheelbase (m) & L & 4 \\ \hline \hline \end{tabular}
\end{table} 
Table 1: Main parameters of the robot. Similarly, the state estimation error and average state estimation error can be expressed as  
\[\text{error}_{x,k}=\sqrt{\frac{1}{m}\sum\limits_{s=1}^{m}\bigl{[}\hat{x}_{k}(s)-x _{k}(s)\bigr{]}^{2}}, \tag{59}\]  
\[\text{Aerror}_{x}=\frac{1}{n}\sum\limits_{k=1}^{n}\text{error}_{x,k}, \tag{60}\]  
\[\text{error}_{y,k}=\sqrt{\frac{1}{m}\sum\limits_{s=1}^{m}\bigl{[}\hat{y}_{k}(s )-y_{k}(s)\bigr{]}^{2}}, \tag{61}\]  
\[\text{Aerror}_{y}=\frac{1}{n}\sum\limits_{k=1}^{n}\text{error}_{y,k}, \tag{62}\]  
\[\text{error}_{\phi,k}=\sqrt{\frac{1}{m}\sum\limits_{s=1}^{m}\bigl{[}\hat{y}_{ k}(s)-\phi_{k}(s)\bigr{]}^{2}}, \tag{63}\]  
\[\text{Aerror}_{\phi}=\frac{1}{n}\sum\limits_{k=1}^{n}\text{error}_{\phi,k}, \tag{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), \tag{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 Figures 4-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. Figure 4: Estimation results of five algorithms in Gaussian noise. 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\Big{(}\mu,\sigma_{1}^{2}\Big{)}+(1-\lambda)N\Big{(}\mu,(2\sigma_ {1})^{2}\Big{)}, \tag{66}\]  
where \(\sigma_{1}^{2}\) is determined by \(\sigma_{V}^{2}\) and \(\sigma_{\gamma}^{2}\).