# Maximum Correntropy Unscented Kalman Filter for Spacecraft Relative State Estimation
## 3 Unscented Kalman Filter under MCC  
To improve the robustness of the UKF, we present the idea to combine the MCC cost function with the UKF framework to derive a novel UKF, which may perform much better in non-Gaussian noise environments, since correntropy contains second and higher order moments of the error. First, we consider a nonlinear model given by Equation (7) and Equation (8), combine expression Equations (13) and (14) and recast the nonlinear regression model as  
\[\left[ \begin{array}{c}\widehat{\mathbf{x}}(k|k-1)\\ \mathbf{y}(k)\end{array} \right]=\left[ \begin{array}{c}\mathbf{x}(k)\\ \mathbf{h}(k,\mathbf{x}(k))\end{array} \right]+\psi(k), \tag{24}\]  
where \(\psi(k)\) denotes by  
\[\psi(k)=\left[ \begin{array}{c}\eta(\mathbf{x}(k))\\ \mathbf{r}(k)\end{array} \right], \tag{25}\]  
with  
\[ \begin{split}&\Psi(k)\\ &=\mathbb{E}\left[\psi(k)\psi^{T}(k)\right]\\ &=\left[\begin{array}{cc}\mathbf{P}(k|k-1)&0\\ 0&\mathbf{R}(k)\end{array}\right]\\ &=\left[\begin{array}{cc}\mathbf{T}_{p}(k|k-1)\mathbf{T}_{p}^{T}(k|k-1)&0\\ 0&\mathbf{T}_{r}(k)\mathbf{T}_{r}^{T}(k)\end{array}\right]\\ &=\mathbf{T}(k)\mathbf{T}^{T}(k),\end{split}  \tag{26}\]  
where \(\mathbf{T}(k)\) can be obtained by Cholesky decomposition of \(\Psi(k)\). Left multiplying both sides of Equation (24) by \(\mathbf{T}^{-1}(k)\), we have the following equation  
\[\mathbf{D}(k)=\mathbf{g}(k,\mathbf{x}(k))+\mathbf{e}(k), \tag{27}\]where \(\mathbf{D}(k)=\mathbf{T}^{-1}(k)\left[ \begin{array}{c}\widehat{\mathbf{x}}(k|k-1) \\ \mathbf{y}(k)\end{array} \right],\mathbf{g}(k,\mathbf{x}(k))=\mathbf{T}^{-1}(k) \left[ \begin{array}{c}\mathbf{x}(k)\\ \mathrm{h}(k,\mathbf{x}(k))\end{array} \right]\), \(\mathbf{e}(k)=\mathbf{T}^{-1}(k)\psi(k)\) and the covariance of \(\mathbf{e}(k)\) is the identity matrix. Then, we define the following cost function \(J_{L}\left(\mathbf{x}(k)\right)\) based MCC:  
\[J_{L}\left(\mathbf{x}(k)\right)=\sum_{i=1}^{L}\mathrm{G}_{\sigma}\left(d_{i}(k )-\mathbf{g}_{i}(k,\mathbf{x}(k))\right), \tag{28}\]  
where \(d_{i}(k)\) is the \(i\)-th element of \(\mathbf{D}(k)\), \(\mathbf{g}_{i}(k,\mathbf{x}(k))\) is the \(i\)-th row of \(\mathbf{g}(k,\mathbf{x}(k))\), and \(L=n+m\) is the dimension of \(\mathbf{D}(k)\). Under the MCC, the optimal estimate of \(\mathbf{x}(k)\) can be found by maximizing the cost function  
\[\widehat{\mathbf{x}}(k)=\arg\max_{\mathbf{x}(k)}J_{L}\left(\mathbf{x}(k) \right)=\arg\max_{\mathbf{x}(k)}\sum_{i=1}^{L}G_{\sigma}\left(e_{i}(k)\right), \tag{29}\]  
where \(e_{i}(k)\) is the \(i\)-th element of \(\mathbf{e}(k)\) and  
\[e_{i}(k)=d_{i}(k)-\mathbf{g}_{i}\left(k,\mathbf{x}(k)\right). \tag{30}\]  
Hence, the optimal solution for \(\mathbf{x}(k)\) can be found from the following equation  
\[\frac{\partial J_{L}\left(\mathbf{x}(k)\right)}{\partial\mathbf{x}(k)}=0, \tag{31}\]  
which can also be expressed as  
\[\sum_{i=1}^{L}\varphi(e_{i}(k))\frac{\partial e_{i}(k)}{\partial x_{i}(k)}=0, \tag{32}\]  
when \(x_{i}(k)\) is the \(i\)-th element of \(\mathbf{x}(k)\), and \(\varphi(e_{i}(k))=\mathrm{G}_{\sigma}(e_{i}(k))\cdot e_{i}(k)\). Then, we define \(C(k,i)=\varphi(e_{i}(k))/e_{i}(k)\) and have  
\[C(k,i)=\mathrm{G}_{\sigma}(e_{i}(k)). \tag{33}\]  
Equation (32) further writes in matrix form as  
\[\left(\frac{\partial\mathbf{g}(k,\mathbf{x}(k))}{\partial\mathbf{x}(k)} \right)^{T}\mathbf{C}(k)\left[\mathbf{D}(k)-\mathbf{g}(k,\mathbf{x}(k)) \right]=0, \tag{34}\]  
where  
\[\mathbf{C}(k)=\left[ \begin{array}{cc}\mathbf{C}_{x}(k)&\mathbf{0}\\ \mathbf{0}&\mathbf{C}_{y}(k)\end{array} \right],\]  
with  
\[\mathbf{C}_{x}(k)=diag\left(\mathrm{G}_{\sigma}\left(e_{1}(k)\right),..., \mathrm{G}_{\sigma}\left(e_{n}(k)\right)\right),\mathbf{C}_{y}(k)=diag\left( \mathrm{G}_{\sigma}\left(e_{n+1}(k)\right),...,\mathrm{G}_{n+m}\left(e_{n+m}( k)\right)\right).\]  
Based on Equation (27) with only one iteration in [32], we could obtain similar results in the UKF framework by using \(\mathbf{C}(k)\) to reformulate the measurement information. Similar to [16], there are two ways to do it.