# Vehicle Pose Estimation Method Based on Maximum Correntropy Square Root Unscented Kalman Filter
## 2 Problem Description and Existing Methods  
### 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  
\[\mathbf{x}_{k}= \mathbf{f}(\mathbf{x}_{k-1},\mathbf{u}_{k})+\mathbf{w}_{k}, \tag{1}\] \[\mathbf{z}_{k}=\mathbf{h}(\mathbf{x}_{k},\mathbf{m}_{k})+\mathbf{v}_{k}, \tag{2}\]  
where \(\mathbf{x}_{k}\) denotes the state vector of the robot pose at time \(k\); it consists of the robot's position information \(\mathbf{x}_{k},\mathbf{y}_{k}\), and heading angle \(\phi_{k}\), i.e., \(\mathbf{x}_{k}=[\mathbf{x}_{k}\,\mathbf{y}_{k}\,\phi_{k}]^{\mathrm{T}}.\mathbf{u}_{k}\) is the control vector of the robot at time \(k\), \(\mathbf{f}(\cdot)\) is the nonlinear state transition function of robot motion, and \(\mathbf{w}_{k}\) is the process noise. \(\mathbf{z}_{k}\) denotes the observation vector of the robot at time \(k\), \(\mathbf{h}\)\((\cdot)\) is the nonlinear observation function of the robot, \(\mathbf{v}_{k}\) is the measurement noise, and \(\mathbf{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 \(\mathbf{m}_{k}=\{m_{1},\ m_{2},\ldots,\ m_{n}\}\).  
The SLAM problem essentially utilizes the observation information \(\mathbf{Z}_{1:\ k}\) and control variable \(\mathbf{U}_{1:\ k}\) before time \(k\) to calculate the joint posterior probability density function \(p\left(\mathbf{x}_{k},\mathbf{m}_{k}\ |\mathbf{Z}_{1:\ k},\mathbf{U}_{1:\ k}\right)\) of the robot state vector \(\mathbf{x}_{k}\) and the environmental map \(\mathbf{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\left(\mathbf{x}_{k},\mathbf{m}_{k}\ |\mathbf{Z}_{1:\ k},\mathbf{U}_{1:\ k}\right)\) can be expressed as  
\[\begin{array}{l}p(\mathbf{x}_{k},\mathbf{m}_{k}|\mathbf{Z}_{1:k},\mathbf{U}_{1:k})\propto p \left(\mathbf{x}_{k}|\mathbf{x}_{k-1},\mathbf{u}_{k}\right)\cdot\\ p(\mathbf{x}_{k-1},\mathbf{m}_{k-1}|\mathbf{Z}_{1:k-1},\mathbf{U}_{1:k-1})\mathbf{dx}_{k-1}\end{array}, \tag{3}\]  
where \(\mathbf{Z}_{1:\ k}=\{\mathbf{z}_{1},\ \mathbf{z}_{2},\ldots,\mathbf{z}_{k}\}\) denotes all observations from time \(1\) to \(k\), and \(\mathbf{U}_{1:\ k}=\{\mathbf{u}_{1},\ \mathbf{u}_{2},\ldots,\mathbf{u}_{k}\}\) denotes all control variables from time \(1\) to \(k\). \(p\left(\mathbf{x}_{k}\ |\mathbf{x}_{k-1},\mathbf{u}_{k}\right)\) is the probability model of the robot motion. \(p\left(\mathbf{z}_{k}\ |\mathbf{x}_{k},\mathbf{m}_{k}\right)\) is the probability model for the robot observation. \(p\left(\mathbf{x}_{k},\mathbf{m}_{k}\ |\mathbf{Z}_{1:\ k},\mathbf{U}_{1:\ k}\right)\), and \(p\left(\mathbf{x}_{k},\mathbf{m}_{k}\ |\mathbf{Z}_{1:\ k-1},\mathbf{U}_{1:\ k-1}\right)\) are posterior probability density functions at time \(k\) and time \(k-1\), respectively.