# Vehicle Pose Estimation Method Based on Maximum Correntropy Square Root Unscented Kalman Filter
## 3 The Proposed Algorithm
### The Proposed MCSRUKF-SLAM Algorithm
The optimal estimate of the state vector \(x_{k}\) is represented as  
\[\hat{x}_{k}=\underset{x_{k}}{argmax}\,J_{D}, \tag{41}\]  
In Equation (41), we observe that when \(\frac{\partial J_{D}}{\partial x_{k}}=0\), the optimal solution for the state, vector \(x_{k}\) can be obtained. Therefore, by making the partial derivative of the cost function \(J_{D}\) equal to 0, we can obtain  
\[ \begin{split}\frac{\partial J_{D}}{\partial x_{k}}& =\mathbf{s}_{k|k-1}^{-1}\Big{(}x_{k}-\hat{x}_{k|k-1}\Big{)}+2\sigma ^{2}\frac{L_{k}H_{k}^{T}R_{k}^{-1}}{-2\sigma^{2}},\\ &\Big{(}z_{k}-H_{k}x_{k}-\hat{z}_{k|k-1}+H_{k}\hat{x}_{k|k-1} \Big{)}=0\end{split} , \tag{42}\]  
Moving the term contains \(x_{k}\) to the left-hand side, we obtain  
\[ \begin{split}\Big{(}\mathbf{s}_{k|k-1}^{-1}+L_{k}H_{k}^{T}R_{k}^ {-1}H_{k}\Big{)}x_{k}&=L_{k}H_{k}^{T}R_{k}^{-1}\Big{(}z_{k}-\hat {z}_{k|k-1}\Big{)}+\\ &\Big{(}L_{k}H_{k}^{T}R_{k}^{-1}H_{k}+S_{k|k-1}^{-1}\Big{)}\hat{x }_{k|k-1}\end{split} , \tag{43}\]  
Equation (43) is a fixed-point equation [41] since \(L_{k}\) is related to \(x_{k}\), which can be solved using the fixed-point method. Consequently, Equation (43) can be reformulated using the fixed-point method as  
\[x_{k}=g(x_{k}), \tag{44}\]  
\[g(x_{k})=L_{k}H_{k}^{T}R_{k}^{-1}\Big{(}z_{k}-\hat{z}_{k|k-1}\Big{)}\Big{(} \mathbf{s}_{k|k-1}^{-1}+L_{k}H_{k}^{T}R_{k}^{-1}H_{k}\Big{)}^{-1}+\hat{x}_{k| k-1}, \tag{45}\]  
The solution to fixed-point Equations (44) and (45) fundamentally involves determining the fixed point that satisfies these equalities, which can be computed through iterative numerical methods. As established in Reference [50], these fixed-point equations satisfy single-step convergence conditions, meaning only one iteration is sufficient to achieve the required estimation performance. Consequently, after a single iteration of the fixed-point equations, we obtain  
\[x_{k}=\mathbf{K}_{k}\Big{(}z_{k}-\hat{z}_{k|k-1}\Big{)}+\hat{x}_{k|k-1}, \tag{46}\]  
\[\mathbf{K}_{k}=\Big{(}\mathbf{S}_{k|k-1}^{-1}+L_{k}\mathbf{H}_{k}^{T}\mathbf{R}_{k}^{-1} \mathbf{H}_{k}\Big{)}^{-1}L_{k}\mathbf{H}_{k}^{T}\mathbf{R}_{k}^{-1}, \tag{47}\]  
Equation (47) requires multiple matrix inversion operations, significantly increasing computational complexity. To address this, we introduce the matrix inversion lemma (All inversions exist) [51] as follows  
\[\Big{(}A+BC^{-1}D\Big{)}^{-1}=A^{-1}-A^{-1}B\Big{(}C+DA^{-1}B\Big{)}^{-1}DA^{ -1}, \tag{48}\]According to Equation (48), We can obtain another equivalent form of Equation (47), as shown below. \[ \begin{array}{ll}\mathbf{K}_{k}&=S_{k|k-1}L_{k}\mathbf{H}_{k}^{T}\Big{(}\mathbf{I}+\mathbf{R}_ {k}^{-1}\mathbf{H}_{k}S_{k\,|k-1}L_{k}\mathbf{H}_{k}^{T}\Big{)}^{-1}\mathbf{R}_{k}^{-1}\\ &=S_{k|k-1}L_{k}\mathbf{H}_{k}^{T}\big{(}\mathbf{R}_{k}+\mathbf{H}_{k}\mathbf{S}_{k\,|k-1}L_{k} \mathbf{H}_{k}^{T}\big{)}^{-1}\end{array} , \tag{49}\]  
In contrast to Equation (47), the formulation in Equation (49) requires only a single matrix inversion operation, thereby achieving significantly reduced computational complexity. Following the fixed-point methodology, during the iterative application of the fixed-point formula, \(\mathbf{H}_{k}\mathbf{x}_{k}\ \approx\ \mathbf{H}_{k}\mathbf{\hat{x}}_{k|k-1}\) in Equation (38). Consequently, Equation (38) can be reformulated as  
\[L_{k}=G_{\sigma}\bigg{(}\left\|\mathbf{z}_{k}-\mathbf{\tilde{z}}_{k|k-1}\right\|_{\mathbf{ R}_{k}^{-1}}\bigg{)}, \tag{50}\]  
The corresponding estimation error covariance is calculated by  
\[\mathbf{S}_{k}=(I-\mathbf{K}_{k}\mathbf{H}_{k})\mathbf{S}_{k|k-1}(I-\mathbf{K}_{k}\mathbf{H}_{k})^{T}+ \mathbf{K}_{k}\mathbf{R}_{k}\mathbf{K}_{k}^{T}, \tag{51}\]  
\[\mathbf{S}_{k}=\textit{cholupdate}\Big{\{}\Big{[}\mathbf{S}_{k|k-1},\mathbf{K}_{k}\mathbf{S}_{k }^{zz},-1\Big{]}\Big{\}}, \tag{52}\]  
The above is the specific derivation process of the proposed MCSRUKF-SLAM algorithm. To well understand the proposed algorithm, Algorithm 1 summarizes the pseudo-code of the MCSRUKF-SLAM algorithm, and Figure 1 shows the flowchart of the MCSRUKF-SLAM algorithm.