# Vehicle Pose Estimation Method Based on Maximum Correntropy Square Root Unscented Kalman Filter
## 2 Problem Description and Existing Methods
### Square Root Unscented Kalman Filter SLAM
(1) Initialize  
\[\hat{\mathbf{x}}_{0}=E[\mathbf{x}_{0}], \tag{7}\]  
\[S_{0}=chol\Big{\{}E\Big{[}(\mathbf{x}_{0}-\hat{\mathbf{x}}_{0})(\mathbf{x}_{0}-\hat{\mathbf{x} }_{0})^{T}\Big{]}\Big{\}}, \tag{8}\]  
where \(\mathbf{x}_{0}\) denotes initial state vector, \(\hat{\mathbf{x}}_{0}\) denotes the mean of the initial state vector, \(E[\cdot]\) denotes the expectation operator, \(chol\{\cdot\}\) denotes the Cholesky decomposition of the matrix, and \(S_{0}\) 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  
\[\hat{\mathbf{x}}_{i,k-1}=\left\{ \begin{array}{ll}\hat{\mathbf{x}}_{k-1},&i=0\\ \hat{\mathbf{x}}_{k-1}+\left(\sqrt{(n+\lambda)S_{k-1}}\right)_{i^{\prime}}&i=1,2, \cdot\cdot\cdot,n\\ \hat{\mathbf{x}}_{k-1}-\left(\sqrt{(n+\lambda)S_{k-1}}\right)_{i-n^{\prime}},&i=n+ 1,\cdot\cdot\cdot,2n\end{array} \right., \tag{9}\]  
We substitute the sigma points \(\xi_{i,\;k-1}\) in Equation (9) into the nonlinear state function \(\mathbf{f}(\cdot)\) in Equation (1) and obtain the predicted value of the state vector at time \(k\) as  
\[\mathbf{x}_{i,k|k-1}=\mathbf{f}(\xi_{i,k-1},\mathbf{u}_{k}), \tag{10}\]  
The mean prediction of the state vector at time \(k\) can be determined by  
\[\hat{\mathbf{x}}_{k|k-1}=\sum_{i=0}^{2n}\omega_{i}^{m}\mathbf{x}_{i,k|k-1}, \tag{11}\]  
The covariance prediction of the state vector at time \(k\) can be determined by  
\[\mathbf{S}_{k|k-1}=\text{QR}\Big{\{}\Big{[}\sqrt{\omega_{i}^{c}}\Big{(}\mathbf{x}_{i, k|k-1}-\hat{\mathbf{x}}_{k|k-1}\Big{)}\;\sqrt{\mathbf{Q}_{\text{k}}}\Big{]}\Big{\}}, \tag{12}\]  
\[\mathbf{S}_{k|k-1}=cholupdate\Big{\{}\Big{[}\mathbf{S}_{k|k-1},\Big{(}\mathbf{x}_{0,k|k-1}- \hat{\mathbf{x}}_{k|k-1}\Big{)},\omega_{0}^{c}\Big{]}\Big{\}}, \tag{13}\]where QR \(\{\cdot\}\) denotes the process of QR decomposition, \(\mathbf{Q}_{k}\) represents the covariance matrix of process noise, and the operation cholupdate \(\{\cdot\}\) 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  
\[\boldsymbol{\chi}_{i,k|k-1}=\left\{ \begin{array}{ll}\hat{\boldsymbol{x}}_{k|k -1},&i=0\\ \hat{\boldsymbol{x}}_{k|k-1}+\left(\sqrt{(n+\lambda)\boldsymbol{S}_{k|k-1}} \right)_{i},&i=1,2,\cdots,n\\ \hat{\boldsymbol{x}}_{k|k-1}-\left(\sqrt{(n+\lambda)\boldsymbol{S}_{k|k-1}} \right)_{i-n},&i=n+1,\cdots,2n\end{array} \right., \tag{14}\]  
We substitute the sigma points \(\boldsymbol{\chi}_{i,\ k|k-1}\) in Equation (14) into the nonlinear observation function \(\boldsymbol{h}\left(\cdot\right)\) in Equation (2), and obtain the predicted value of the measurement vector at time \(k\) as  
\[\boldsymbol{z}_{i,\ k|k-1}=\boldsymbol{h}\Big{(}\boldsymbol{\chi}_{i,k|k-1}, \boldsymbol{m}_{k}\Big{)}, \tag{15}\]  
The mean prediction of the measurement vector at time \(k\) can be determined by  
\[\boldsymbol{\hat{z}}_{k|k-1}=\sum_{i=0}^{2n}\omega_{i}^{m}\boldsymbol{z}_{i,\ k|k-1}, \tag{16}\]  
The covariance prediction of the measurement vector at time \(k\) can be determined by  
\[\boldsymbol{S}_{k}^{zz}=QR\Big{\{}\Big{[}\sqrt{\omega_{i}^{\prime}}\Big{(} \boldsymbol{z}_{i,\ k|k-1}-\boldsymbol{\hat{z}}_{k|k-1}\Big{)}\ \sqrt{\boldsymbol{R}_{k}}\Big{]}\Big{\}}, \tag{17}\]  
\[\boldsymbol{S}_{k}^{zz}=cholupdate\Big{\{}\Big{[}\boldsymbol{S}_{k}^{zz}, \Big{(}\boldsymbol{z}_{0,k|k-1}-\boldsymbol{\hat{z}}_{k|k-1}\Big{)},\omega_{ 0}^{z}\Big{]}\Big{\}}, \tag{18}\]  
where \(\boldsymbol{R}_{k}\) denotes the covariance matrix of the measurement noise.