# State Parameter Fusion Estimation for Intelligent Vehicles Based on IMM-MCCKF
## 3 Design of Vehicle State Estimator Based on IMM-MCCKF
### Derivation of the MCCKF  
Against non-Gaussian noise, correlation entropy performs very well as a suppressor. Due to the noise-sensitive characteristics of the CKF filtering algorithm, combining MCC and CKF facilitates the derivation of the MCCKF algorithm. Regarding the nonlinear system represented by Equation (13), the nonlinear recursive model is constructed in conjunction with Equation (25), as outlined below:  
\[ \begin{bmatrix}\hat{x}_{(k|k-1)}\\ z_{k}\end{bmatrix} = \begin{bmatrix}x_{k}\\ h(x_{k})\end{bmatrix} +\chi_{k} \tag{35}\]  
where  
\[\chi_{k}= \begin{bmatrix}-\Big{(}x_{k}-\hat{x}_{(k|k-1)}\Big{)}\\ v_{k}\end{bmatrix}  \tag{36}\]  
The following is a representation of the covariance. \[ \begin{split}&\mathrm{E}\big{[}\chi_{k}\chi_{k}^{T}\big{]}= \begin{bmatrix}P_{(k|k-1)}&0\\ 0&R_{k}\end{bmatrix}=V_{k}V_{k}^{T}\\ &=\begin{bmatrix}V_{(p,k|k-1)}&0\\ 0&V_{(R,k)}\end{bmatrix}\end{bmatrix}^{T}\end{split}  \tag{37}\]  
Multiplying Equation (36) from the left yields the solution \(V_{k}^{-1}\) to Equation (36). \[y_{k}=\mathrm{d}(x_{k})+f_{k} \tag{38}\]  
where \(y_{k}=V_{k}^{-1} \begin{bmatrix}\hat{x}_{(k|k-1)}\\ z_{k}\end{bmatrix} \), \(d(x_{k})=V_{k}^{-1} \begin{bmatrix}x_{k}\\ h(x_{k})\end{bmatrix} \). Subsequently, using MCC as a basis, define the cost function:  
\[J_{\mathrm{MCC}}(x_{k})=\frac{1}{N}\sum_{i=1}^{N}G_{\mathrm{T}}\Big{(}y_{(i,k) }-\mathrm{d}_{i}(x_{k})\Big{)} \tag{39}\]  
where the \(j\)th row of \(\mathrm{d}(x_{k})\) is represented by \(\mathrm{d}_{i}(x_{k})\), and \(x_{k}\) is the most effective solution obtained by substituting the relevant variables into the equation. \[\frac{\partial J_{\mathrm{MCC}}(x_{k})}{\partial x_{k}}=0 \tag{40}\]  
Then, this results in the following equation:  
\[ \begin{split}\bigg{(}\frac{\partial\mathrm{d}(x_{k})}{\partial x _{k}}\bigg{)}^{T}W_{k}(\;\mathrm{d}(x_{k})-y_{k})=0\end{split}  \tag{41}\]where the anticipated state error and the measurement error are quadratically weighted using the covariance matrix \(W_{k}= \begin{bmatrix}W_{(x,k)}&0\\ 0&W_{(x,k)}\end{bmatrix} \), \(W_{(x,k)}\) and \(W_{(x,z)}\) are defined as follows:  
\[ \begin{array}{l}f_{(i,k)}=y_{(i,k)}-d_{i}(x_{k}),\,w_{(x,k)}=\text{diag}(G_{ \tau}(f_{1,k}),\cdots,G_{\tau}(f_{n,k})),\\ W_{(z,k)}=\text{diag}\Big{(}G_{\tau}\Big{(}f_{(n+1,k)}\Big{)},\cdots,G_{\tau} \Big{(}f_{(n+p,k)}\Big{)}\Big{)}\end{array}  \tag{42}\]  
where the components of vector a are used to construct the diagonal matrix \(\text{diag}(\text{a})\). Since \(h(\text{x}_{k})\) is nonlinear, it can be further expanded at the current estimate \(\hat{x}_{(k|k)}^{t-1}\) as follows:  
\[\left(\frac{\partial\text{d}\Big{(}\hat{x}_{(k|k)}^{t-1}\Big{)}}{\partial\hat {x}_{(k|k)}^{t-1}}\right)^{T}W_{k}^{t-1}\Bigg{(}\,\text{d}\Big{(}\hat{x}_{(k| k)}^{t-1}\Big{)}+\frac{\partial\text{d}\Big{(}\hat{x}_{(k|k)}^{t-1}\Big{)}}{ \partial\hat{x}_{(k|k)}^{t-1}}\Big{(}x_{k}-\hat{x}_{(k|k)}^{t-1}\Big{)}-y_{k }\Bigg{)}=0 \tag{43}\]  
where \(W_{k}^{t-1}\) and \(\hat{x}_{(k|k)}^{t-1}\) are variables corresponding to the fixed-point algorithm at \(t-1\). Defining \(D_{k}^{t-1}=\partial\text{d}\Big{(}\hat{x}_{(k|k)}^{t-1}\Big{)}/\partial\hat{ x}_{(k|k)}^{t-1}\), the following results can be derived. \[\hat{x}_{(k|k)}^{t-1}=\hat{x}_{(k|k)}+K_{k}^{t-1}\Big{(}z_{k}-\text{h}\Big{(} \hat{x}_{(k|k)}^{t-1}\Big{)}-H_{k}^{t-1}\Big{(}\hat{x}_{(k|k-1)}-\hat{x}_{(k| k)}^{t-1}\Big{)}\Big{)} \tag{44}\]  
By applying the matrix inversion theorem, Equation (44) can be rewritten as follows:  
\[\hat{x}_{(k|k)}^{t}=\hat{x}_{(k|k)}+K_{k}^{t-1}\Big{(}z_{k}-\text{h}\Big{(} \hat{x}_{(k|k)}^{t-1}\Big{)}-H_{k}^{t-1}\Big{(}\hat{x}_{(k|k-1)}^{t}-\hat{x}_ {(k|k)}^{t-1}\Big{)}\Big{)} \tag{45}\]  
where \(H_{k}^{t-1}=\partial\text{h}\Big{(}\hat{x}_{(k|k)}^{t-1}\Big{)}/\partial\hat {x}_{(k|k)}^{t-1}\).