# A Multiple-Step, Randomly Delayed, Robust Cubature Kalman Filter for Spacecraft-Relative Navigation
## 4 Multiple-Step, Randomly Delayed, Robust Cubature Kalman Filter
### Update  
The cubature points generated with \(\textbf{\hat{X}}_{k/k-1}\) and \(\textbf{P}_{k/k-1}\) is calculated as follows:  
\[\textbf{\chi}_{i,k|k-1}=\text{Trans}\Big{[}\textbf{\hat{X}}_{k/k-1},\textbf{P} _{k/k-1}\Big{]} \tag{32}\]  
We calculate the measurement prediction \(\textbf{\hat{y}}^{s}_{k/k-1}\), covariance \(\textbf{P}^{yy,s}_{k/k-1}\), and cross-covariance \(\textbf{P}^{xy,s}_{k/k-1}\) at the \(s\)-th \((0\leq s\leq d)\) step delay as shown in Equations (33)-(35). \[\textbf{\hat{y}}^{s}_{k/k-1}=\sum_{i=1}^{2n_{q}}w_{i}h\Big{(}\textbf{D}_{s} \textbf{\chi}_{i,k|k-1}\Big{)} \tag{33}\]  
\[\textbf{P}^{yy,s}_{k/k-1}=\sum_{i=1}^{2n_{q}}w_{i}\Big{(}h\Big{(}\textbf{D}_{s} \textbf{\chi}_{i,k|k-1}\Big{)}-\textbf{\hat{y}}^{s}_{k/k-1}\Big{)}\Big{(}h\Big{(} \textbf{D}_{s}\textbf{\chi}_{i,k|k-1}\Big{)}-\textbf{\hat{y}}^{s}_{k/k-1}\Big{)} ^{T} \tag{34}\]\[\mathbf{P}_{k/k-1}^{Xy,s}=\sum\limits_{i=1}^{2n_{\pi}}w_{i}\Big{(}\mathbf{x}_{i,k|k-1}- \mathbf{\hat{X}}_{k|k-1}\Big{)}\Big{(}h\Big{(}\mathbf{D}_{s}\mathbf{\chi}_{i,k|k-1}\Big{)}- \hat{y}_{k/k-1}^{s}\Big{)}^{T} \tag{35}\]  
The sub-update results of the multiple-step randomly delayed cubature Kalman filter at the \(s\)-th step are provided in Appendix A.2. In this study, the generalized maximum likelihood approach is adopted to complete the sub-update of MRD-DCSCKF for the purpose of suppressing outliers. The robust update calculation process is denoted by the function \(\Big{[}\mathbf{M},\widetilde{\mathbf{z}}\Big{]}=\text{Robust\_update}\Big{[}\mathbf{y}_{k },h(\cdot),\mathbf{P}_{k/k-1},\mathbf{P}^{xy},\mathbf{\hat{x}}_{k|k-1},\mathbf{R}\Big{]}\), according to Appendix A.3. The corresponding quantities generated at the \(s\)-th (\(0\leq s\leq d\)) step delay is expressed as follows:  
\[\Big{[}\mathbf{M}_{k}^{s},\widetilde{\mathbf{z}}_{k}^{s}\Big{]}=\text{ Robust\_update}\Big{[}\mathbf{y}_{k},h(\cdot),\mathbf{P}_{k/k-1},\mathbf{P}_{k/k-1}^{Xy,s}, \mathbf{\hat{X}}_{k|k-1},\mathbf{R}_{k-s}\Big{]} \tag{36}\]  
It is well known that the robustness of M-estimation in a non-Gaussian noise environment mainly depends on the robust kernel function, and this paper adopts the DCS robust kernel function introduced in Section 3. The residuals and weight matrix at \(s\)-th (\(0\leq s\leq d\)) step delay are as follows:  
\[\mathbf{\xi}_{k}^{s}=\mathbf{M}_{k}^{s}\mathbf{\hat{X}}_{k}^{s}-\widetilde{\mathbf{z}}_{k}^{s} \tag{37}\]  
\[\mathbf{\Psi}_{D}^{s}=diag\Big{[}\psi_{D}\Big{(}\mathbf{\xi}_{k}^{s,s}\Big{)}\Big{]} \tag{38}\]  
where \(\mathbf{\xi}_{k}^{s,s}\) is the \(l\)-th element of residuals \(\mathbf{\xi}_{k}^{s}\). In addition, \(\psi_{D}(\cdot)\) is the DCS weight function expressed in Equation (15), and \(\mathbf{\Psi}\) is the weight matrix, which denotes the proportion of residuals accounted for. The initial value of the iteration of \(\mathbf{\hat{X}}_{k}^{s}\) could be chosen based on Appendix A.2. After \(j\) iterations of Equations (37)-(38), we could obtain sub-update results at the \(s\)-th step delay. \[\mathbf{\hat{X}}_{k/k}^{(j+1),s}=\Big{(}(\mathbf{M}_{k}^{s})^{T}\mathbf{\Psi}_{D}^{j,s} \mathbf{M}_{k}^{s}\Big{)}^{-1}(\mathbf{M}_{k}^{s})^{T}\mathbf{\Psi}_{D}^{j,s}z_{k}^{s} \tag{39}\]  
\[\mathbf{P}_{k|k}^{(j+1),s}=\Big{(}(\mathbf{M}_{k}^{s})^{T}\mathbf{\Psi}_{D}^{j,s}\mathbf{M}_{ k}^{s}\Big{)}^{-1} \tag{40}\]  
The state estimate and corresponding covariance with sub-update results can be computed as:  
\[\mathbf{\hat{X}}_{k/k}=\sum\limits_{s=0}^{d}\mu_{k}^{s}\mathbf{\hat{X}}_{k/k}^{s} \tag{41}\]  
\[\textbf{\hat{y}}^{s}_{k/k-1}=\sum_{i=1}^{2n_{q}}w_{i}h\Big{(}\textbf{D}_{s} \textbf{\chi}_{i,k|k-1}\Big{)} \tag{33}\]0  
where \(\mu_{k}^{s}\) is the weight of the sub-update results, according to Equation (43), which is computed based on delay probabilities \(p^{s}\), measurement prediction \(\hat{\mathbf{y}}_{k/k-1}^{s}\), and covariance \(\mathbf{P}_{k/k-1}^{yy,s}\)[25].