# 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
The cross-covariance between the state vector and the measurement vector at time \(k\) can be determined by  
\[\boldsymbol{P}_{k}^{xz}=\sum_{i=0}^{2n}\omega_{i}^{z}\Big{[}\Big{(}\boldsymbol {\chi}_{i,k|k-1}-\boldsymbol{\hat{x}}_{k|k-1}\Big{)}\big{(}\boldsymbol{z}_{i, k|k-1}-\boldsymbol{\hat{z}}_{k|k-1}\big{)}^{T}\Big{]}, \tag{19}\]  
(4) States Enhancement  
The SRUKF-SAM algorithm is based on the MMSE criterion, and its cost function can be expressed as the problem of minimizing the error between state estimation and observation data, which can be written as  
\[\boldsymbol{J}(\boldsymbol{x}_{k})=\frac{1}{2}\Big{\|}\big{(}\boldsymbol{z}_{ k}-\boldsymbol{H}_{k}\boldsymbol{x}_{k}\big{)}-\Big{(}\boldsymbol{\hat{z}}_{k|k-1}- \boldsymbol{H}_{k}\boldsymbol{\hat{x}}_{k|k-1}\Big{)}\Big{\|}_{\boldsymbol{R} _{k}^{-1}}^{2}, \tag{20}\]  
where \(\|\boldsymbol{x}\|_{\boldsymbol{A}}^{2}=\boldsymbol{x}^{T}\boldsymbol{A} \boldsymbol{x},\boldsymbol{H}_{k}\) is the Jacobian matrix of the measurement function \(\boldsymbol{h}\left(\cdot\right)\), and \(\boldsymbol{R}_{k}\) is the covariance matrix of the measurement noise. To minimize the cost function \(\boldsymbol{J}(\boldsymbol{x}_{k})\), we take the derivative of the state vector \(\boldsymbol{x}_{k}\) in Equation (20) and set it as equal to zero, which can be expressed as  
\[\frac{\partial\boldsymbol{J}(\boldsymbol{x}_{k})}{\partial\boldsymbol{x}_{k}} =0, \tag{21}\]  
We substitute the cost function of Equation (20) into Equation (21), and further write it as  
\[\frac{\partial\boldsymbol{J}(\boldsymbol{x}_{k})}{\partial\boldsymbol{x}_{k}} =-\boldsymbol{H}_{k}^{T}\boldsymbol{R}_{k}^{-1}\Big{[}\big{(}\boldsymbol{z}_{ k}-\boldsymbol{H}_{k}\boldsymbol{x}_{k}\big{)}-\Big{(}\boldsymbol{\hat{z}}_{k|k-1}- \boldsymbol{H}_{k}\boldsymbol{\hat{x}}_{k|k-1}\Big{)}\Big{]}=0, \tag{22}\]We move the \(\mathbf{x}_{k}\) item contained in Equation (22) to the left, which can be written as  
\[\mathbf{x}_{k}=\hat{\mathbf{x}}_{k|k-1}+\left(\mathbf{H}_{k}\mathbf{R}_{k}^{-1}\mathbf{H}_{k}\right) ^{-1}\mathbf{H}_{k}^{T}\mathbf{R}_{k}^{-1}\Big{(}z_{k}-\hat{\mathbf{z}}_{k|k-1}\Big{)}, \tag{23}\]  
According to Equation (23), the Kalman gain \(\mathbf{K}_{k}\) is  
\[\mathbf{K}_{k}=\left(\mathbf{H}_{k}\mathbf{R}_{k}^{-1}\mathbf{H}_{k}\right)^{-1}\mathbf{H}_{k}^{T} \mathbf{R}_{k}^{-1}, \tag{24}\]  
where  
\[\boldsymbol{\hat{z}}_{k|k-1}=\sum_{i=0}^{2n}\omega_{i}^{m}\boldsymbol{z}_{i,\ k|k-1}, \tag{16}\]0  
\[\boldsymbol{\hat{z}}_{k|k-1}=\sum_{i=0}^{2n}\omega_{i}^{m}\boldsymbol{z}_{i,\ k|k-1}, \tag{16}\]1  
According to Equations (24)-(26), the Kalman gain can also be expressed by the covariance matrix as  
\[\boldsymbol{\hat{z}}_{k|k-1}=\sum_{i=0}^{2n}\omega_{i}^{m}\boldsymbol{z}_{i,\ k|k-1}, \tag{16}\]2  
The state estimation 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}\]3  
The covariance matrix of the state estimation error 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}\]4  
In summary, the SRUKF-SLAM algorithm employs the UT technique to address system nonlinearities. During both the time update and measurement update phases, it utilizes QR decomposition [44] and Cholesky decomposition to factorize matrix \(\mathbf{S}\) into the orthogonal matrix and right matrix. By substituting this right matrix for the original covariance matrix \(\mathbf{P}\) in iterative updates, the algorithm effectively resolves the issue of non-positive-definite covariance matrices.