A Multiple-Step, Randomly Delayed, Robust Cubature Kalman Filter for Spacecraft-Relative Navigation

Abstract

This study is focused on addressing the problem of delayed measurements and contaminated Gaussian distributions in navigation systems, which both have a tremendous deleterious effect on the performance of the traditional Kalman filtering. We propose a non-linear, multiple-step, randomly delayed, robust filter, referred to as the multiple-step, randomly delayed, dynamic-covariance-scaling cubature Kalman filter (MRD-DCSCKF). First, Bernoulli random variables are adopted to describe the measurement system in the presence of multiple-step random delays. Then, the MRD-DCSCKF uses the framework of the multiple-step randomly delayed filter, based on a state-augmentation approach, to address the problem of delayed measurements. Meanwhile, it depends on a dynamic-covariance-scaling (DCS) robust kernel to reject the outliers in the measurements. Consequently, the proposed filter can simultaneously address the problem of delayed measurements and inherit the virtue of robustness of the DCS kernel function. The MRD-DCSCKF has been applied to vision-based spacecraft-relative navigation simulations, where quaternions are adopted to represent spacecraft’s attitude kinematics, and the attitude update is completed with quaternions and generalized Rodrigues parameters. Monte Carlo simulations have illustrated that MRD-DCSCKF is superior to other well-known algorithms by providing high-accuracy position and attitude estimations in an environment with different delay probabilities and/or different outlier-contamination probabilities. Therefore, the proposed filter is robust to delayed measurements and can suppress outliers.

1. Introduction

Formation flights, on-orbit servicing, asteroid exploration, rendezvous and docking, active debris removal, and other space-based missions commonly require high-precision position and attitude estimations [1]. Molli et al. has proposed a satellite navigation system that combines inter-satellite links and a batch-sequential filter, and it reconstructs high-quality orbits for the constellation nodes and provides high-level autonomous navigation for Mars exploration [2]. Anna et al. has designed the space exploration rover with a positioning and mapping system which is equipped with light detection and ranging instruments, and it could achieve safe navigation in rough terrain and avoid obstacles [3]. Andolfo et al. has processed stereo images captured during the rover’s travel with 3D visual odometry, where 2D image keypoints are identified to estimate the relative position and attitude between every step. This method has the advantage of mitigating trajectory inconsistencies from dead-reckoning techniques [4]. Junkins et al. has developed a vision-based spacecraft-relative navigation system using a position sensing diode (PSD) [5]. The vision-based navigation (VISNAV) system has the virtues of light weight, small size, and low power consumption [6]. It is usually adopted to determine the relative positions and attitudes of spacecraft that are within several hundred meters from each other. Optical sensors and beacons are essential components of the VISNAV system, and they enable optional vision or adaptive vision. An optical sensor consists of a wide-angle lens and a position-sensing diode. A PSD possesses the merits of measuring the intensity and position of a light-point simultaneously. In vision-based spacecraft-relative navigation systems, light-emitting diodes (LEDs) are mounted as optical beacons on the chief spacecraft, and the deputy spacecraft is equipped with PSD-based optical sensors. The energy emitted from the optical beacons is then focused on the PSD through a wide-angle lens, so the optical signal is transformed into an electrical signal through electronic processing to acquire the image information of the target. In the more sophisticated VISNAV systems, the PSD can be programmed to only recognize a specified light source [7]. In VISNAV systems, the relative motion equations are built in the local–vertical–local–horizontal (LVLH) coordinate frame to obtain the motion state of targets. Generally, the beacon position is defined and known in the chief spacecraft body frame, and the position vector from the deputy spacecraft with respect to chief spacecraft is defined within the LVLH coordinate frame. Typically, the research in this field has assumed that the chief body is consistent within the LVLH coordinate system as well. However, this assumption is not rigorous and is just a special case. To eliminate the above assumptions, an efficient method is to estimate the attitudes of the two spacecrafts with respect to the LVLH coordinate frame, and the relative attitudes could then be obtained as well.

The spacecraft’s orientation relative to the reference coordinate frame can be determined through the estimation of its attitude [8]. Euler angles, rotation vectors, direction cosine matrices, and quaternions are usually used to describe spacecraft attitude. Specifically, quaternions are useful for describing spacecraft attitude kinematics due to their non-singularity and the bi-linear kinematics equation [9]. To ensure that quaternions meet the normalization constraint, we adopted quaternions for propagation and updating and introduced the generalized Rodrigues parameter (GRP) [10] to denote the local attitude error. Jitter, vibration, and multiple disruptions can degrade the measurements’ quality [11]. Vision-based spacecraft-relative navigation systems are subjected to randomly delayed measurements, and measurement noise is always disturbed by outliers. These factors motivated us to develop a filter that could simultaneously handle randomly delayed measurements and outliers.

The Kalman filter (KF) is the most commonly used estimation technique for the assumption of linear systems and Gaussian noise [12]. However, many problems do not satisfy the linearity assumption, and non-linear systems are more common in practical engineering. Vision-based spacecraft-relative navigation uses the optical camera to obtain measurements, which are modeled with non-linear equations. Among various non-linear filters, the extended Kalman filter (EKF), as a broadly available method, linearizes non-linear systems using the first-order Taylor series expansions. Unfortunately, this can result in significant truncation errors, and the procedure of deriving the Jacobi matrix is tedious [13]. A family of sigma-point filters and particle filters can avoid the loss of high-order terms, and therefore, they exhibit better performances than the EKF. The sigma-point filter approximates the probability density distributions (PDFs) of states through a group of defined sigma points and propagates the mean and covariance of random variables through non-linear processes. Julier has proposed the unscented Kalman filter (UKF) based on an intuitive statistical information transformation [14]. Arasaratnam adopted the spherical-radial cubature rule to calculate the means and covariances of state variables after non-linear propagation and proposed the cubature Kalman filter (CKF) [15]. Jia proposed the high-degree cubature Kalman filter (HCKF) based on the Genz’s code [16] and the generalized Gauss–Laguerre quadrature (GGLQ) rule [17]. Arulampalam proposed the particle filter (PF) using the sequential Monte Carlo method, which is a powerful state estimation method for handling non-Gaussian noise with arbitrary non-linear models and arbitrary noise distributions [18]. However, the weights of sigma points could be negative in the unscented transformation (UT), so UKF does not behave stably in high-dimensional systems. As compared to UKF and CKF, HCKF has a very high computational burden. Particle filters suffer from particle degradation and a heavy computational burden. Considering both filtering stability and computational demands, CKF is the more suitable choice for solving non-linear filtering problems in this work.

In practice, various unknown factors, such as environmental factors and equipment failures, can disturb navigation systems, and when data are transmitted along unreliable communication channels, the phenomenon of delayed measurements cannot be ignored [19]. The estimation accuracy is dramatically reduced when filters use delayed measurements. Handling the challenges associated with randomly delayed measurements and ensuring the accuracy of state estimation are crucial for spacecraft-relative navigation. Wang derived a Gaussian approximation (GA) filter using Gaussian approximation of the posterior predictive PDFs of states and delayed measurements [20]. Zhao adopted Bernoulli random variables (BRV) to re-represent the likelihood PDF in exponential multiplicative form and approximated the state vector with variational Bayesian methods in order to propose an improved one-step, randomly delayed KF [21]. Fei proposed a modified adaptive EKF (MAEKF) algorithm, which adopts adaptive estimation to alleviate the effects of modeling uncertainty and error [22]. Hermoso used BRV to build measurement information and proposed a randomly delayed EKF. However, these filters are all restricted to one-step or two-step delay [23,24]. Esmzad proposed a multiple-step randomly delayed CKF (MRD-CKF) that computes the likelihood function through marginalizing delay variables to mitigate the random delay and loss [25]. In the aforementioned research, however, the impact of outliers on the navigation accuracy is not considered, and this also degrades the filter significantly. Therefore, this paper extends the above-mentioned researches.

Outliers (contaminated Gaussian distribution) degrade the effectiveness of Gaussian filters. Relevant robust filters have been developed to address this problem. The robust Student’s t filter (STF) replaces the Gaussian probability distribution in the Gaussian filtering framework with the t-distribution to obtain robustness [26]. Huang indicated that the STF required information regarding the degrees-of-freedom and scale matrix of the t-distribution, corresponding to measurement noise [27]. However, it is difficult to determine the relevant parameters of the t-distribution in advance. Another approach is to use generalized great likelihood estimations to revise the updating process of KF to obtain robustness [28]. The performances of robust filters based on generalized great likelihood estimations mainly depend on robust kernel functions, which restrict the anomalous measurements [29]. The most widely used robust filter adopts a Huber kernel function that combines minimum l1 and l2 norm estimation techniques [30]. Gaussian kernel function is another robust kernel function, namely, the maximum correntropy criterion (MCC). Since the weight function is smaller for the same residual, the MCC-based KF has more robustness than the Huber-based KF [31].

In this paper, we develop a non-linear, multiple-step, randomly delayed, robust filter and refer to as the multiple-step, randomly delayed, dynamic-covariance-scaling cubature Kalman filter (MRD-DCSCKF). The time update of MRD-DCSCKF is derived from the third-degree, the spherical-radial cubature rule, and the multiple-step, randomly delayed system model. The proposed filter adopts a multiple-step, randomly delayed filtering framework to weaken the impact of delayed measurements on the estimation accuracy of the filter, and it suppresses outliers with a dynamic-covariance-scaling kernel, which incorporates the advantages of the Huber kernel and Gaussian kernel.

The remaining part of the paper is organized as follows. In Section 2, some preliminaries are briefly reviewed. Section 3 presents the DCS kernel. Section 4 presents the derivation of MRD-DCSCKF. Section 5 introduces the vision-based spacecraft-relative navigation model. The simulation is reported in Section 6. Finally, conclusions are drawn in Section 7.

2. Preliminaries

The general discrete-time non-linear system is as follows:

$${\mathit{x}}_k=f\left({\mathit{x}}_{k-1}\right)+{\mathsf{\varsigma }}_{k-1}$$

(1)

$${\mathit{z}}_k=h\left({\mathit{x}}_k\right)+{\mathit{w}}_k$$

(2)

where ${\mathit{x}}_k$ is the n-dimensional state vector at time ${\mathit{t}}_k$. The variable ${\mathit{z}}_k$ generated from the sensor is the ideal measurement vector without delay; ${\mathsf{\varsigma }}_{k-1}$ is the process noise, which satisfies zero-mean Gaussian white noises ($E\left[{\mathsf{\varsigma }}_k{\mathsf{\varsigma }}_l^T\right]={\mathit{Q}}_k{\delta }_{kl}$); and $\delta$ indicates the Kronecker delta function. The variable ${\mathit{w}}_k$ is the measurement noise, and $f(·)$ and $h(·)$denote the state function and measurement function, respectively.

2.1. Measurements with Multiple-Step Random Delays

The measurements obtained from Equation (2) need to proceed to the data processor (filter). However, the phenomenon of measurements transmission along unreliable communication channels, due to the occurrence of equipment failures, bandwidth limitations, environmental disturbances [32], etc., could not be ignored, as these delays result in the ideal measurements ${\mathit{z}}_k$ generated by the sensors and the actual measurements ${\mathit{y}}_k$ received by the filter becoming asynchronous. Figure 1 represents a schematic diagram of the system with multiple-step randomly delayed measurements, where the filter performs state estimation (SE) at point A and the associated measurements should reach the buffer before A. Due to the unreliability of the data transmission channel, there is one-step or multiple-step delay in the measurements received from sensor 1, where the solid line indicates that the measurements are synchronized and the dashed line indicates that the measurements are delayed. In the case of the d-step delay, the filter receives the actual measurements ${\mathit{y}}_k$, which could be ${\mathit{z}}_{k-i}\left(0\le i\le d\right)$. Therefore, the actual measurements ${\mathit{y}}_k$ can be described as follows:

$$\begin{array}{c}{\mathit{y}}_k=\left(1-{\tau }_1\right){\mathit{z}}_k+{\tau }_1\left(1-{\tau }_2\right){\mathit{z}}_{k-1}+{\tau }_1{\tau }_2\left(1-{\tau }_3\right){\mathit{z}}_{k-2}+\hfill \\ \cdots +\left({\displaystyle \prod _{s=1}^{d-1}}{\tau }_s\right)\left(1-{\tau }_d\right){\mathit{z}}_{k-d+1}+\left[1-\left(1-{\tau }_1\right)\right.-\hfill \\ {\tau }_1\left(1-{\tau }_2\right)-\cdots -\left({\displaystyle \prod _{s=1}^{d-1}}{\tau }_s\right)\left.\left(1-{\tau }_d\right)\right]{\mathit{z}}_{k-d}\hfill \\ ={\displaystyle \sum _{s=0}^d}{\tau }^{\left(s,j\right)}{\mathit{z}}_{k-s}\hfill \end{array}$$

(3)

where ${\tau }_0=1$ and ${\tau }_i\in \left[0,1\right]$ are independent Bernoulli random variables, which satisfy the following:

$$\begin{array}{c}p\left({\tau }_i=1\right)=E\left({\tau }_i\right)=p_i\hfill \\ p\left({\tau }_i=0\right)=1-p_i\hfill \\ E\left[{\left({\tau }_i-p_i\right)}^2\right]=\left(1-p_i\right)p_i\hfill \end{array}$$

(4)

where $p_i$ is the expectation of ${\tau }_i=1$. ${\tau }^{\left(s,j\right)}$ is then the following:

$${\tau }^{\left(s,j\right)}=\left\{\begin{array}{c}\left({\displaystyle \prod _{j=0}^s}{\tau }_j\right)\left(1-{\tau }_{s+1}\right),0\le s\le d-1\hfill \\ {\displaystyle \prod _{j=0}^d}{\tau }_j,s=d\hfill \end{array}\right.$$

(5)

Figure 1. Schematic diagram of the system with multiple-step, randomly delayed measurements.

The probability of delayed measurements for $s\left(0\le s\le d\right)$step [33] is the following:

$$\begin{array}{c}{p}^{\left(s,j\right)}=\left({\displaystyle \prod _{j=0}^s}{p}_j\right)\left(1-p_{s+1}\right),s=0,1,2,\cdots ,d-1\hfill \\ p^{\left(d,j\right)}={\displaystyle \prod _{j=0}^d}{p}_j\hfill \end{array}$$

(6)

2.2. Non-Gaussian Noise in Measurements

A Gaussian distribution is often adopted to describe the distribution of sensor noise. However, the sensor noise does not satisfy the Gaussian assumption in practice. Furthermore, the sensors used in vision-based spacecraft-relative navigation are often perturbed by outliers, so they generally regarded as giving contaminated Gaussian distributions [30], and the probability density function (PDF) is expressed as follows:

$$p\left(v\right)=(1-\epsilon )\frac{1}{\sqrt{2\pi }{\sigma }_1}exp\left(-\frac{{\left(v/\phantom{v{\sigma }_1}{\sigma }_1\right)}^2}{2}\right)+\epsilon \frac{1}{\sqrt{2\pi }{\sigma }_2}exp\left(-\frac{{\left(v/\phantom{v{\sigma }_2}{\sigma }_2\right)}^2}{2}\right)$$

(7)

where $\epsilon \in \left[0,1\right]$ is the perturbing parameter that denotes contamination probability. ${\sigma }_1$ and ${\sigma }_2$ are standard deviations of the individual Gaussian distributions, which are satisfied with $∥{\sigma }_2∥>∥{\sigma }_1∥$. Figure 2 presents an example that compares the Gaussian distribution and the contaminated Gaussian distribution with $\epsilon =0.05$, ${\sigma }_1=1$and ${\sigma }_2=7.5{\sigma }_1$. Obviously, the contaminated Gaussian distribution exhibits more clutter.

Figure 2. The measurement noise of the two distributions.

3. Brief Review of DCS Kernel

The DCS approach originates from the area of visual simultaneous localization and mapping (SLAM), and its core is a robust function, namely, the dynamic-covariance-scaling kernel. The DCS kernel enables L2 estimation to be optimal under Gaussian environment, and can completely eliminate the effect of relatively large residuals on the estimation results under non-Gaussian environment. Therefore, the DCS kernel is more robust than the Huber kernel and can reduce the loss of measurement information under Gaussian noise, as compared to the Gaussian kernel [34].

Two highly efficient approaches, switchable constraint (SC) and dynamic covariance scaling (DCS), have been been rapidly developed in SLAM to handle outliers in images. Agarwal [35] has derived the specific form of the cost function of SC, as follows:

$$\rho \left(\xi \right)=s^2{\xi }^2+\eta {(1-s)}^2$$

(8)

where $s$ is the switchable variable, $\eta >0$ is the kernel width, and $\xi$ is the residual error.

When the function $\rho \left(·\right)$ is continuous and bounded, the derivative of Equation (8) concerning $s$should satisfy the following conditions:

$$\frac{\partial \rho \left(\xi \right)}{\partial s}=2s{\xi }^2-2\eta (1-s)=0$$

(9)

$$s=\frac{\eta }{\eta +{\xi }^2}$$

(10)

By bringing Equation (10) into Equation (8), the specific form of the cost function of SC could be derived, as follows:

$$\rho \left(\xi \right)={\left(\frac{\eta }{\eta +{\xi }^2}\right)}^2{\xi }^2+\eta {\left(1-\frac{\eta }{\eta +{\xi }^2}\right)}^2=\frac{\eta {\xi }^2}{\eta +{\xi }^2}$$

(11)

The limit value of $\rho \left(·\right)$at $\pm \infty$ is determined by the following:

$$\underset{\xi \to \pm \infty }{lim}\rho \left(\xi \right)=\underset{\xi \to \pm \infty }{lim}\frac{\eta }{\eta /\phantom{\eta {\xi }^2}{\xi }^2+1}=\eta$$

(12)

According to Equation (12), $\eta$is the upper bound of $\rho \left(·\right)$. We could determine the range of s.

$$s\in \left[0,\frac{2\eta }{\eta +{\xi }^2}\right]$$

(13)

Sünderhauf [36] suggested that the range of values for $s$is [0,1], and when this was combined with Equation (13), it resulted in the following:

$$s=min\left(1,\frac{2\eta }{\eta +{\xi }^2}\right)$$

(14)

Agarwal [35] suggested that $s^2$could be used as a form of weight function for M-estimation, and then the DCS weight function could be calculated, as follows:

$${\psi }_D\left(\xi \right)=s^2=min\left(1,\frac{4{\eta }^2}{{(\eta +{\xi }^2)}^2}\right)=\left\{\begin{array}{c}1,\mathrm{for}{\xi }^2<\eta \hfill \\ \frac{4{\eta }^2}{{(\eta +{\xi }^2)}^2},\mathrm{for}{\xi }^2\ge \eta \hfill \end{array}\right.$$

(15)

By integrating Equation (15), the cost function of DCS is as follows:

$${\rho }_D\left(\xi \right)=\int \xi {\psi }_D\left(\xi \right)d\xi =\left\{\begin{array}{c}\frac{{\xi }^2}{2},\mathrm{for}{\xi }^2<\eta \hfill \\ \frac{\eta (3{\xi }^2-\eta )}{2({\xi }^2+\eta )},\mathrm{for}{\xi }^2\ge \eta \hfill \end{array}\right.$$

(16)

The cost function and the weight function of a DCS kernel are shown in Figure 3.

Figure 3. Cost and weight functions of a DCS kernel.

4. Multiple-Step, Randomly Delayed, Robust Cubature Kalman Filter

4.1. State-Augmentation

According to Equations (2) and (3), the actual measurement ${\mathit{y}}_k$received by the filter is a mixture of ${\left\{{\mathit{z}}_{k-s}\right\}}_{s=0}^d$. The derivation of ${\mathit{y}}_k$needs the state vectors ${\mathit{x}}_k,{\mathit{x}}_{k-1},\dots ,{\mathit{x}}_{k-d}$, which are then joined together. The augmented state vector is as follows:

$${\mathit{X}}_k={\left[\begin{array}{c}{\mathit{x}}_k\hfill \\ {\mathit{x}}_{k-1}\hfill \\ ⋮\hfill \\ {\mathit{x}}_{k-d}\hfill \end{array}\right]}_{n_a\times 1}$$

(17)

where ${\mathit{X}}_k$denotes the $n_a=\left(d+1\right)n$-dimensional augmented state.

The augmentation system is expressed by the following:

$${\mathit{X}}_k=F\left({\mathit{X}}_{k-1}\right)+\mathit{C}{\mathsf{\varsigma }}_{k-1}$$

(18)

$${\mathit{y}}_k={\mathit{z}}_{k-s}=h\left({\mathit{D}}_s{\mathit{X}}_k\right)+{\mathit{w}}_{k-1},s=0,1,\dots ,d$$

(19)

where $F\left({\mathit{X}}_{k-1}\right)$, $\mathit{C}$, and ${\mathit{D}}_s{\mathit{X}}_k$are then computed by the following:

$$F\left({\mathit{X}}_{k-1}\right)={\left[f^T\left({\mathit{x}}_{k-1}\right),{\mathit{x}}_{k-1}^T,\dots ,{\mathit{x}}_{k-d}^T\right]}^T$$

(20)

$$\mathit{C}={\left[{\mathit{I}}_{n\times n},0,\dots ,0\right]}^T$$

(21)

$${\mathit{D}}_s{\mathit{X}}_k={\mathit{x}}_{k-s},s=0,1,2,\dots ,d$$

(22)

4.2. Prediction

Based on Equation (17), the state estimation and error covariance are expressed by the following:

$${\widehat{\mathit{X}}}_{k-1/\phantom{k-1k-1}k-1}=\left[\begin{array}{c}{\widehat{\mathit{x}}}_{k-1/\phantom{k-1k-1}k-1}\hfill \\ ⋮\hfill \\ {\widehat{\mathit{x}}}_{k-d/\phantom{k-dk-1}k-1}\hfill \\ {\widehat{\mathit{x}}}_{k-d-1/\phantom{k-d-1k-1}k-1}\hfill \end{array}\right]$$

(23)

$${\mathit{P}}_{k-1/\phantom{k-1k-1}k-1}=\left[\begin{array}{c}{\mathit{P}}_{k-1,k-1/\phantom{k-1k-1}k-1}^{xx}\cdots {\mathit{P}}_{k-1,k-d/\phantom{k-dk-1}k-1}^{xx}{\mathit{P}}_{k-1,k-d-1/\phantom{k-d-1k-1}k-1}^{xx}\hfill \\ ⋮\ddots ⋮⋮\hfill \\ {\mathit{P}}_{k-d,k-1/\phantom{k-1k-1}k-1}^{xx}\cdots {\mathit{P}}_{k-d,k-d/\phantom{k-dk-1}k-1}^{xx}{\mathit{P}}_{k-d,k-d-1/\phantom{k-d-1k-1}k-1}^{xx}\hfill \\ {\mathit{P}}_{k-d-1,k-1/\phantom{k-1k-1}k-1}^{xx}\cdots {\mathit{P}}_{k-d-1,k-d/\phantom{k-dk-1}k-1}^{xx}{\mathit{P}}_{k-d-1,k-d-1/\phantom{k-d-1k-1}k-1}^{xx}\hfill \end{array}\right]$$

(24)

The predicted state ${\widehat{\mathit{X}}}_{k/\phantom{kk-1}k-1}$ and corresponding covariance ${\mathit{P}}_{k/\phantom{kk-1}k-1}$ are determined by the following:

$${\widehat{\mathit{X}}}_{k/\phantom{kk-1}k-1}=\left[\begin{array}{c}{\widehat{\mathit{x}}}_{k/\phantom{kk-1}k-1}\hfill \\ {\widehat{\mathit{x}}}_{k-1/\phantom{k-1k-1}k-1}\hfill \\ ⋮\hfill \\ {\widehat{\mathit{x}}}_{k-d/\phantom{k-1k-1}k-1}\hfill \end{array}\right]$$

(25)

$${\mathit{P}}_{k/\phantom{kk-1}k-1}=\left[\begin{array}{c}{\mathit{P}}_{k,k/\phantom{kk-1}k-1}^{xx}{\mathit{P}}_{k,k-1/\phantom{k-1k-1}k-1}^{xx}\cdots {\mathit{P}}_{k,k-d/\phantom{k-dk-1}k-1}^{xx}\hfill \\ {\mathit{P}}_{k-1,k/\phantom{kk-1}k-1}^{xx}{\mathit{P}}_{k-1,k-1/\phantom{k-1k-1}k-1}^{xx}\cdots {\mathit{P}}_{k-1,k-d/\phantom{k-dk-1}k-1}^{xx}\hfill \\ ⋮⋮\ddots ⋮\hfill \\ {\mathit{P}}_{k-d,k/\phantom{kk-1}k-1}^{xx}{\mathit{P}}_{k-d,k-1/\phantom{k-1k-1}k-1}^{xx}\cdots {\mathit{P}}_{k-d,k-d/\phantom{k-dk-1}k-1}^{xx}\hfill \end{array}\right]$$

(26)

According to Appendix A.1, the cubature points are generated according to ${\widehat{\mathit{X}}}_{k-1/\phantom{k-1k-1}k-1}$ and ${\mathit{P}}_{k-1/\phantom{k-1k-1}k-1}$ as follows:

$${\mathsf{\chi }}_{i,k-1|k-1}=\mathrm{Trans}\left[{\widehat{\mathit{X}}}_{k-1/\phantom{k-1k-1}k-1},{\mathit{P}}_{k-1/\phantom{k-1k-1}k-1}\right]$$

(27)

The predicted state and corresponding covariance matrix are expressed as Equations (28)–(31).

$${\widehat{\mathit{x}}}_{k|k-1}=\sum _{i=1}^{2n_a}{w}_{i}f\left({\mathsf{\chi }}_{i,k-1|k-1}\right)$$

(28)

$${\mathit{P}}_{k,k/\phantom{kk-1}k-1}^{xx}=\sum _{i=1}^{2n_a}{w}_i\left(f\left({\mathsf{\chi }}_{i,k-1|k-1}\right)-{\widehat{\mathit{x}}}_{k|k-1}\right){\left(f\left({\mathsf{\chi }}_{i,k-1|k-1}\right)-{\widehat{\mathit{x}}}_{k|k-1}\right)}^T+{\mathit{Q}}_{k-1}$$

(29)

$${\mathit{P}}_{k,k-s/\phantom{k-sk-1}k-1}^{xx}=\sum _{i=1}^{2n_a}{w}_i\left(f\left({\mathsf{\chi }}_{i,k-1|k-1}\right)-{\widehat{\mathit{x}}}_{k|k-1}\right){\left({\chi }_{i,k-s|k-1}-{\widehat{\mathit{x}}}_{k-s|k-1}\right)}^T,s=1,2,\cdots ,d$$

(30)

$${\mathit{P}}_{k-s,k/\phantom{kk-1}k-1}^{xx}={\left({\mathit{P}}_{k,k-s/\phantom{k-sk-1}k-1}^{xx}\right)}^T,s=1,2,\cdots ,d$$

(31)

4.3. Update

The cubature points generated with ${\widehat{\mathit{X}}}_{k/\phantom{kk-1}k-1}$ and ${\mathit{P}}_{k/\phantom{kk-1}k-1}$ is calculated as follows:

$${\mathsf{\chi }}_{i,k|k-1}=\mathrm{Trans}\left[{\widehat{\mathit{X}}}_{k/\phantom{kk-1}k-1},{\mathit{P}}_{k/\phantom{kk-1}k-1}\right]$$

(32)

We calculate the measurement prediction ${\widehat{\mathit{y}}}_{k/\phantom{kk-1}k-1}^s$, covariance ${\mathit{P}}_{k/\phantom{kk-1}k-1}^{yy,s}$, and cross-covariance ${\mathit{P}}_{k/\phantom{kk-1}k-1}^{Xy,s}$ at the s-th $\left(0\le s\le d\right)$ step delay as shown in Equations (33)–(35).

$${\widehat{\mathit{y}}}_{k/\phantom{kk-1}k-1}^s=\sum _{i=1}^{2n_a}{w}_{i}h\left({\mathit{D}}_s{\mathsf{\chi }}_{i,k|k-1}\right)$$

(33)

$${\mathit{P}}_{k/\phantom{kk-1}k-1}^{yy,s}=\sum _{i=1}^{2n_a}{w}_i\left(h\left({\mathit{D}}_s{\mathsf{\chi }}_{i,k|k-1}\right)-{\widehat{\mathit{y}}}_{k/\phantom{kk-1}k-1}^s\right){\left(h\left({\mathit{D}}_s{\mathsf{\chi }}_{i,k|k-1}\right)-{\widehat{\mathit{y}}}_{k/\phantom{kk-1}k-1}^s\right)}^T$$

(34)

$${\mathit{P}}_{k/\phantom{kk-1}k-1}^{Xy,s}=\sum _{i=1}^{2n_a}{w}_i\left({\mathsf{\chi }}_{i,k|k-1}-{\widehat{\mathit{X}}}_{k|k-1}\right){\left(h\left({\mathit{D}}_s{\mathsf{\chi }}_{i,k|k-1}\right)-{\widehat{\mathit{y}}}_{k/\phantom{kk-1}k-1}^s\right)}^T$$

(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 $\left[\mathit{M},\stackrel{⌢}{\mathit{z}}]\right.=\mathrm{Robust}\_\mathrm{update}\left[{\mathit{y}}_k,h\left(·\right),{\mathit{P}}_{k/\phantom{kk-1}k-1},{\mathit{P}}^{xy},{\widehat{\mathit{x}}}_{k|k-1},\mathit{R}\right]$, according to Appendix A.3. The corresponding quantities generated at the s-th $\left(0\le s\le d\right)$ step delay is expressed as follows:

$$\left[{{\mathit{M}}_k^s,\stackrel{⌢}{\mathit{z}}}_k^s]\right.=\mathrm{Robust}\_\mathrm{update}\left[{\mathit{y}}_k,h\left(·\right),{\mathit{P}}_{k/\phantom{kk-1}k-1},{\mathit{P}}_{k/\phantom{kk-1}k-1}^{Xy,s},{\widehat{\mathit{X}}}_{k|k-1},{\mathit{R}}_{k-s}\right]$$

(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 $\left(0\le s\le d\right)$ step delay are as follows:

$${\mathsf{\xi }}_k^s={\mathit{M}}_k^s{\widehat{\mathit{X}}}_k^s-{\stackrel{⌢}{\mathit{z}}}_k^s$$

(37)

$${\mathbf{\Psi }}_D^s=diag\left[{\psi }_D\left({\mathsf{\xi }}_k^{l,s}\right)\right]$$

(38)

where ${\mathsf{\xi }}_k^{l,s}$ is the l-th element of residuals ${\mathsf{\xi }}_k^s$. In addition, ${\psi }_D\left(·\right)$ 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 ${\widehat{\mathit{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.

$${\widehat{\mathit{X}}}_{k/\phantom{kk}k}^{(j+1),s}={\left({\left({\mathit{M}}_k^s\right)}^T{\mathbf{\Psi }}_D^{j,s}{\mathit{M}}_k^s\right)}^{-1}{\left({\mathit{M}}_k^s\right)}^T{\mathbf{\Psi }}_D^{j,s}{\stackrel{⌢}{\mathit{z}}}_k^s$$

(39)

$${\mathit{P}}_{k|k}^{(j+1),s}={\left({\left({\mathit{M}}_k^s\right)}^T{\mathbf{\Psi }}_D^{j,s}{\mathit{M}}_k^s\right)}^{-1}$$

(40)

The state estimate and corresponding covariance with sub-update results can be computed as:

$${\widehat{\mathit{X}}}_{k/\phantom{kk}k}=\sum _{s=0}^d{\mu }_k^s{\widehat{\mathit{X}}}_{k/\phantom{kk}k}^s$$

(41)

$${\mathit{P}}_{k/\phantom{kk}k}=\sum _{s=0}^d{\mu }_k^s{\mathit{P}}_{k/\phantom{kk}k}^s$$

(42)

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 ${\widehat{\mathit{y}}}_{k/\phantom{kk-1}k-1}^s$, and covariance ${\mathit{P}}_{k/\phantom{kk-1}k-1}^{yy,s}$[25].

$${\mu }_k^s=\frac{p^{s}N\left({\mathit{y}}_k;{\widehat{\mathit{y}}}_{k/\phantom{kk-1}k-1}^s,{\mathit{P}}_{k/\phantom{kk-1}k-1}^{yy,s}+{\mathit{R}}_{k-s}\right)}{{\displaystyle \sum _{i=0}^d}\left[p^{i}N\left({\mathit{y}}_k;{\widehat{\mathit{y}}}_{k/\phantom{kk-1}k-1}^i,{\mathit{P}}_{k/\phantom{kk-1}k-1}^{yy,i}+{\mathit{R}}_{k-i}\right)\right]}$$

(43)

In delayed system, the data received by the filter may be the measurements without delay (s = 0) or delayed measurements $\left(1\le s\le d\right)$. The state-augmentation approach can effectively use the previous states of the system, namely, the state information, such as (s = 0,1…d), is sufficiently exploited in MRD-DCSCKF. Through calculating the weights of sub-updates to obtain the posterior PDF of the Gaussian mixture, the MRD-DCSCKF algorithm can obtain accurate information in the augmented states and improve the state estimation accuracy. Finally, to better illustrate the computational procedures of MRD-DCSCKF, the primary calculations of the proposed filter are outlined in Figure 4, where the non-occurrence delay $\left(s=0\right)$ and the occurrence delay $\left(1\le s\le d\right)$ are drawn separately to distinguish them from each other.

Figure 4. Flowchart of the MRD-DCSCKF.

5. Spacecraft-Relative Navigation Model

5.1. Reference Frames

To describe the spacecraft’s relative motion and sensor measurement model, three related coordinate frames are defined in this section, as shown in Figure 5.

Figure 5. Definition of related coordinate frames.

1.

Earth-centered inertial (ECI) frame ($O_I-X_I{Y}_I{Z}_I$): The origin $O_I$ is located at the center of Earth, the $X_I$-axis points to the vernal equinox, the $Z_I$-axis points to the North Pole, and the $Y_I$-axis forms a right-handed system with the $X_I$-axis and the $Z_I$-axis.

2.

Local–vertical–local–horizontal (LVLH) frame ($O_L-X_L{Y}_L{Z}_L$): The origin $O_L$ is located at the mass center of the chief spacecraft, the $X_L$-axis points from the center of Earth to the center of the chief spacecraft, the $Z_L$-axis points in the same direction as the orbital angular velocity, and the $Y_L$-axis forms a right-handed system with the $X_L$-axis and the $Z_L$-axis.

3.

Spacecraft body coordinate frame ($O_b-X_b{Y}_b{Z}_b$): The chief body and deputy body are denoted as $O_c-X_c{Y}_c{Z}_c$ and $O_d-X_d{Y}_d{Z}_d$, respectively. The $O_b-X_b{Y}_b{Z}_b$ is fixed to the spacecraft and its origin $O_b$ is located at the mass center of the spacecraft. The three axes of $O_b-X_b{Y}_b{Z}_b$, a right-handed system, coincide with the inertial axes of spacecraft. When $O_c-X_c{Y}_c{Z}_c$ is coincident with $O_L-X_L{Y}_L{Z}_L$, the $X_c$-axis points outward radially along the orbit (yaw axis), the $Y_c$-axis points toward the direction of flight (roll axis), and the $Z_c$-axis forms a right-handed system with the $X_c$-axis and the $Y_c$-axis (pitch axis).

5.2. Relative Dynamics

The chief and deputy spacecraft move along in Earth-centered orbits, as shown in Figure 5. The relative position vector of the deputy spacecraft with respect to the chief spacecraft is $\mathsf{\rho }={\mathit{r}}_d-{\mathit{r}}_c={\left[x,y,z\right]}^T$ in the LVLH frame based on the chief spacecraft. The non-linear dynamics of the relative motion between them are described with Tschauner–Hempel (T-H) equations [37]. Then, the relative dynamics are calculated using the following equations:

$$\ddot{x}=2\dot{\nu }\dot{y}+\ddot{\nu }y+{\dot{\nu }}^{2}x-\frac{\mu (r_c+x)}{{\left[{(r_c+x)}^2+y^2+z^2\right]}^{3/\phantom{32}2}}+\frac{\mu }{r_c^2}$$

(44)

$$\ddot{y}=-2\dot{\nu }\dot{x}-\ddot{\nu }x+{\dot{\nu }}^{2}y-\frac{\mu y}{{\left[{(r_c+x)}^2+y^2+z^2\right]}^{3/\phantom{32}2}}$$

(45)

$$\ddot{z}=-\frac{\mu z}{{\left[{(r_c+x)}^2+y^2+z^2\right]}^{3/\phantom{32}2}}$$

(46)

where $\mu$ is the gravitational parameter of Earth, $v$ denotes the true anomaly, and $r_c$is the orbit’s radius. They are described by $\ddot{\nu }=\frac{-2{\dot{r}}_c\dot{\nu }}{r_c}$ and ${\ddot{r}}_c=r_c{\dot{\nu }}^2-\frac{\mu }{r_c^2}$, respectively.

5.3. Relative Attitude Kinematics

Since the attitude kinematics described by quaternions have a linear form, it is more convenient to calculate the spacecraft attitude with quaternions [38]. The quaternion is defined by the following:

$$\mathit{q}\equiv \left[\begin{array}{c}\mathsf{\varrho }\hfill \\ q_4\hfill \end{array}\right]$$

(47)

with

$$\mathsf{\varrho }\equiv {\left[q_1,q_2,q_3\right]}^T=\mathit{e}sin\left(\vartheta /\phantom{\vartheta 2}2\right)$$

(48)

$$q_4=cos\left(\vartheta /\phantom{\vartheta 2}2\right)$$

(49)

where $\mathit{e}$ and $\vartheta$denote the unit Euler axis and the rotation angle, respectively. Quaternions satisfy ${\mathit{q}}^T\mathit{q}={∥\mathit{q}∥}^2=1$. The relationship between quaternions and the attitude matrix is as follows:

$$A\left(\mathit{q}\right)={\mathbf{\Xi }}^T\left(\mathit{q}\right)\psi \left(\mathit{q}\right)$$

(50)

with

$$\mathbf{\Xi }\left(\mathit{q}\right)\equiv \left[\begin{array}{c}{q}_4{\mathit{I}}_{3\times 3}+\left[\mathsf{\varrho }\times \right]\hfill \\ -{\mathsf{\varrho }}^T\hfill \end{array}\right]$$

(51)

$$\psi \left(\mathit{q}\right)\equiv \left[\begin{array}{c}{q}_4{\mathit{I}}_{3\times 3}-\left[\mathsf{\varrho }\times \right]\hfill \\ -{\mathsf{\varrho }}^T\hfill \end{array}\right]$$

(52)

where $\left[\mathsf{\varrho }\times \right]$ is the cross-product matrix, provided by the following:

$$\left[\mathsf{\varrho }\times \right]\equiv \left[\begin{array}{c}0-q_3{q}_2\hfill \\ q_{3}0-q_1\hfill \\ -q_2{q}_{1}0\hfill \end{array}\right]$$

(53)

Successive rotations could be converted into quaternion multiplication, as follows:

$$A\left(\mathit{q}\right)A\left({\mathit{q}}^{\prime }\right)=A\left(\mathit{q}\otimes {\mathit{q}}^{\prime }\right)$$

(54)

with

$$\mathit{q}\otimes {\mathit{q}}^{\prime }=\left[\psi \left(\mathit{q}\right)\mathit{q}\right]{\mathit{q}}^{\prime }=\left[\mathbf{\Xi }\left({\mathit{q}}^{\prime }\right){\mathit{q}}^{\prime }\right]\mathit{q}$$

(55)

The quaternion kinematics equation could be written as the following:

$$\dot{\mathit{q}}=\frac{1}{2}\mathbf{\Xi }\left(\mathit{q}\right)\mathsf{\omega }=\frac{1}{2}\Omega \left(\mathsf{\omega }\right)\mathit{q}$$

(56)

where $\mathsf{\omega }={\left[{\omega }_1,{\omega }_2,{\omega }_3\right]}^T$ indicates the angular rate vector and

$$\Omega \left(\mathsf{\omega }\right)\equiv \left[\begin{array}{c}-\left[\mathsf{\omega }\times \right]\mathsf{\omega }\hfill \\ -{\mathsf{\omega }}^{T}0\hfill \end{array}\right]$$

(57)

The error of quaternion is defined as $\delta \mathit{q}={\left[\delta {\mathsf{\varrho }}^T,\delta q_4\right]}^T$. To ensure that the quaternion-based results follow the unit constraint, generalized Rodrigues parameters (GRPs) [10] are introduced to represent the pose error, which is defined as follows:

$$\delta \mathit{p}\equiv f\frac{\delta \mathsf{\varrho }}{\left(a+\delta q_4\right)}$$

(58)

where $∥\delta \mathit{p}∥$ is equal to the error-angle for small errors, $f$ is a scale factor that is commonly taken as $f=2\left(a+1\right)$, and a is a parameter from 0 to 1. The inverse transformation from $\delta \mathit{p}$ to $\delta \mathit{q}$ is determined as follows:

$$\delta q_4=\frac{-a{∥\delta \mathit{p}∥}^2+f\sqrt{f^2+\left(1-a^2\right){∥\delta \mathit{p}∥}^2}}{f^2+{∥\delta \mathit{p}∥}^2}$$

(59)

$$\delta \mathsf{\varrho }=f^{-1}\left(a+\delta q_4\right)\delta \mathit{p}$$

(60)

5.4. Measurement Model

Several optical beacons of known number and position are mounted on the chief spacecraft. The deputy spacecraft is equipped with PSD, and this paper does not assume that chief body ($O_c-X_c{Y}_c{Z}_c$) and its LVLH frame ($O_L-X_L{Y}_L{Z}_L$) are consistent. When the chief spacecraft and deputy spacecraft are close to each other, e.g., distances ranging from several meters to several hundred meters [39], the details of each part of chief spacecraft could be displayed on the pixel plane [40]. Based on the measurements from the cameras, it is possible to estimate the relative position and velocity between them and also to estimate their relative attitude. The VISNAV system is illustrated in Figure 6, where PSD is denoted with a cube, and this paper assumes that the coordinate system of PSD overlaps with deputy body $O_d-X_d{Y}_d{Z}_d$. The i-th measurement line of sight (LOS) vector is defined by the following:

$${\tilde{\mathit{b}}}_i=\mathit{A}\left({\mathit{q}}_{d/\phantom{dL}L}\right){\mathit{r}}_i+{\mathit{w}}_i$$

(61)

with

$${\mathit{r}}_i=\frac{{{\mathit{X}}^{\prime }}_i-\mathsf{\rho }}{∥{{\mathit{X}}^{\prime }}_i-\mathsf{\rho }∥}=\frac{{\left[{X^{\prime }}_i-x,{Y^{\prime }}_i-y,{Z^{\prime }}_i-z\right]}^T}{\sqrt{{\left({X^{\prime }}_i-x\right)}^2+{\left({Y^{\prime }}_i-y\right)}^2+{\left({Z^{\prime }}_i-z\right)}^2}}$$

(62)

where ${\tilde{\mathit{b}}}_i$ is the measurement of the i-th beacon in $O_d-X_d{Y}_d{Z}_d$. $(x,y,z)$ indicates the position of deputy spacecraft in $O_L-X_L{Y}_L{Z}_L$. $\mathit{A}\left({\mathit{q}}_{d/\phantom{dL}L}\right)$denotes the rotation matrix from $O_L-X_L{Y}_L{Z}_L$ to $O_d-X_d{Y}_d{Z}_d$. In addition, ${\mathit{X}}_i^{\prime }={\left[\mathit{A}\left({\mathit{q}}_{c/\phantom{cL}L}\right)\right]}^T{\mathit{X}}_i$is the coordinate of beacon in $O_L-X_L{Y}_L{Z}_L$, and ${\mathit{X}}_i\equiv \left(X_i,Y_i,Z_i\right)$ is the i-th beacon’s known position in $O_c-X_c{Y}_c{Z}_c$.

Figure 6. Vision-based navigation system.

Therefore, with N optical beacons providing N lines of sight, the optical camera measurement equation is written as follows:

$${\mathit{z}}_k={\left[\begin{array}{c}{\tilde{\mathit{b}}}_1\hfill \\ {\tilde{\mathit{b}}}_2\hfill \\ ⋮\hfill \\ {\tilde{\mathit{b}}}_N\hfill \end{array}\right]}_k={\left[\begin{array}{c}\mathit{A}\left({\mathit{q}}_{d/\phantom{dL}L}\right){\mathit{r}}_1\hfill \\ \mathit{A}\left({\mathit{q}}_{d/\phantom{dL}L}\right){\mathit{r}}_2\hfill \\ ⋮\hfill \\ \mathit{A}\left({\mathit{q}}_{d/\phantom{dL}L}\right){\mathit{r}}_N\hfill \end{array}\right]}_k+{\left[\begin{array}{c}{\mathit{w}}_1\hfill \\ {\mathit{w}}_2\hfill \\ ⋮\hfill \\ {\mathit{w}}_N\hfill \end{array}\right]}_k$$

(63)

5.5. Gyro Measurement Model

The classical model of the angular rate of the gyro output [41] is expressed by the following:

$$\left\{\begin{array}{c}\tilde{\mathsf{\omega }}=\mathsf{\omega }+\mathsf{\beta }+{\mathsf{\eta }}_v\hfill \\ \dot{\mathsf{\beta }}={\mathsf{\eta }}_u\hfill \end{array}\right.$$

(64)

where $\tilde{\mathsf{\omega }}$is the actual angular rate, and $\mathsf{\omega }$is the true inertial angular rate. The variable $\mathsf{\beta }$is the gyro drift, and ${\mathsf{\eta }}_v$and ${\mathsf{\eta }}_u$are Gaussian white-noise, which satisfy

$$\left\{\begin{array}{c}E\left\{{\mathsf{\eta }}_v\left(t\right){\mathsf{\eta }}_v^T\left(\tau \right)\right\}={\sigma }_v^2\delta \left(t-\tau \right){\mathit{I}}_{3\times 3}\hfill \\ E\left\{{\mathsf{\eta }}_u\left(t\right){\mathsf{\eta }}_u^T\left(\tau \right)\right\}={\sigma }_u^2\delta \left(t-\tau \right){\mathit{I}}_{3\times 3}\hfill \end{array}\right.$$

(65)

where ${\sigma }_v$ and ${\sigma }_u$indicate the angle random walk and angular rate random walk, respectively.

The discrete recursive form of Equation (64) is defined as follows:

$$\left\{\begin{array}{c}{\tilde{\mathsf{\omega }}}_{k+1}={\mathsf{\omega }}_{k+1}+\frac{1}{2}\left({\mathsf{\beta }}_{k+1}+{\mathsf{\beta }}_k\right)+{\left(\frac{{\sigma }_v^2}{\Delta t}+\frac{1}{12}{\sigma }_u^2\Delta t\right)}^{1/\phantom{12}2}{\mathit{N}}_v\hfill \\ {\mathsf{\beta }}_{k+1}={\mathsf{\beta }}_k+{\sigma }_u\Delta t^{1/\phantom{12}2}{\mathit{N}}_u\hfill \end{array}\right.$$

(66)

where $\Delta t$is the discrete sampling period. The variables ${\mathit{N}}_v$and ${\mathit{N}}_u$are the zero-mean Gaussian white-noise.

6. Numerical Simulations

To demonstrate the superiority of MRD-DCSCKF for handling multiple-step, randomly delayed measurements and suppressing outliers, we designed simulations to compare the properties of the proposed filter with CKF [15], the one-step randomly delayed CKF (ORD-CKF) [20], and the multiple-step randomly delayed CKF (MRD-CKF) [25].

6.1. Experimental Scenario Settings

The installation positions of the optical beacons under the chief spacecraft body coordinate frame are shown in Table 1. Table 2 lists the initial orbital elements of the chief spacecraft. Table 3 summarizes the relevant parameters of the simulation. This paper assumes that the chief spacecraft gyro drift is relatively small, or it has been corrected by autonomous navigation. Therefore, chief spacecraft gyro drift ${\mathsf{\beta }}_{c0}=0$ is adopted. The nominal trajectory of the chief spacecraft is depicted in Figure 7.

Table 1. Installation positions of beacons.

Beacon No.	x (m)	y (m)	z (m)
1	0.5	0.5	0.0
2	−0.5	0.5	0.0
3	0.5	−0.5	0.0
4	−0.5	−0.5	0.0
5	0.2	0.5	0.1
6	0.1	0.2	−0.1

Table 2. Initial orbital elements of the chief spacecraft.

Orbital Elements	Corresponding Value
Semi-major axis $a$	26,555.137 km
Eccentricity e	0.7395
Orbit inclination $i$	$63.{465}^{\circ }$
Argument of perigee $\omega$	$274.{163}^{\circ }$
Right ascension of the ascending node $\Omega$	$115.{024}^{\circ }$
True anomaly $\nu$	$23.{612}^{\circ }$

Table 3. Simulation parameters.

Parameter	Corresponding Value
Number of Monte Carlo simulations	100
Discrete sampling period	$0.1$ s
The update interval of camera	$0.2$ s
Simulation time	600 s
Perturbing parameter	$0.05$
Tuning parameters of kernel	5
Number of delay steps	3
Delay probability	$0.1$
Delay probability for each step	$p_0=0.9,p_1=0.09,p_2=0.009,p_3=0.001$
Initial relative position	${\mathsf{\rho }}_0={\left[0,-27.444,0\right]}^T$ (m)
Initial relative velocity	${\mathit{v}}_0={\left[0,-6.340\times {10}^{-3},0\right]}^T$(m/s)
Initial attitude quaternion of chief spacecraft	${\mathit{q}}_{c/\phantom{cL,0}L}={\left[0.0086,0.0086,0.0086,0.9999\right]}^T$
Initial relative attitude quaternion	${\mathit{q}}_{d/\phantom{dL,0}c}={\left[0.0433,0.0142,0.0256,0.9986\right]}^T$
Initial generalized Rodrigues parameters	$\delta {\mathit{p}}_0={\left[0,0,0\right]}^T$
Chief spacecraft angular velocity	${\mathsf{\omega }}_{c,0}={\left[0.0013,0.0013,0.0013\right]}^T$ (rad/s)
Deputy spacecraft angular velocity	${\mathsf{\omega }}_{d,0}={\left[0.0020,0.0020,0.0020\right]}^T$ (rad/s)
Gyro drift	${\mathsf{\beta }}_{d0}={\left[1,1,1\right]}^T\left({}^{\circ }/\phantom{{}^{\circ }h}\mathrm{h}\right)$
Angle random walk	${\sigma }_u=\sqrt{10}\times {10}^{-8}\left(\mathrm{rad}/\phantom{\mathrm{rad}{\mathrm{s}}^{1/\phantom{12}2}}{\mathrm{s}}^{1/\phantom{12}2}\right)$
Angular rate random walk	${\sigma }_v=\sqrt{10}\times {10}^{-10}\left(\mathrm{rad}/\phantom{\mathrm{rad}{\mathrm{s}}^{3/\phantom{32}2}}{\mathrm{s}}^{3/\phantom{32}2}\right)$
Power spectral density of perturbation acceleration	$q_{\varpi }=\sqrt{10}\times {10}^{-6}\left({\left(\mathrm{m}/\phantom{\mathrm{m}{\mathrm{s}}^2}{\mathrm{s}}^2\right)}^2/\phantom{{\left(m/\phantom{m{s}^2}{s}^2\right)}^2\mathrm{Hz}}\mathrm{Hz}\right)$
Process noise covariance matrix	${\mathit{Q}}_k=diag\left[{\sigma }_u^2{\mathit{I}}_{3\times 3},{\sigma }_v^2{\mathit{I}}_{3\times 3},0_{3\times 3},q_{\varpi }^2{\mathit{I}}_{3\times 3}\right]$
Initial state covariance matrix	${\mathit{P}}_0=diag\left[{\left(1^{\circ }\right)}^2{\mathit{I}}_{3\times 3},{\left(1^{\circ }/\phantom{1^{\circ }\mathrm{h}}\mathrm{h}\right)}^2{\mathit{I}}_{3\times 3},{\left(1\mathrm{m}\right)}^2{\mathit{I}}_{3\times 3},{\left(1\mathrm{m}/\phantom{\mathrm{m}\mathrm{s}}\mathrm{s}\right)}^2{\mathit{I}}_{3\times 3}\right]$
Covariance matrix of measurement noise	${\mathit{R}}_k={\left(1.8^{\prime \prime }\right)}^2{\mathit{I}}_{12\times 12}$
Covariance matrix of contaminated measurement noise	${\mathit{R}}_{con}=7.5{\mathit{R}}_k$
Initial state vector true value	${\mathit{x}}_0={\left[\delta {\mathit{p}}_0^T,{\mathsf{\beta }}_{d0}^T,{\mathsf{\rho }}_0^T,{\mathit{v}}_0^T\right]}^T$
Initial state vector estimate	${\widehat{\mathit{x}}}_0={\mathit{x}}_0+\sqrt{{\mathit{P}}_0}·{\mathit{N}}_{12\times 1}$

Figure 7. The nominal trajectory of the chief spacecraft.

6.2. Simulation Results and Analyses

Monte Carlo simulations were conducted to evaluate the effectiveness of several filters for estimation. Then, the root-mean-square error (RMSE) and the average RMSE (ARMSE) of the state estimations were calculated, as follows:

$$RMS{E}_k\left(i\right)=\sqrt{\frac{1}{M}\sum _{m=1}^M{\left({\widehat{\mathit{x}}}_k^m\left(i\right)-{\mathit{x}}_k^m\left(i\right)\right)}^2}$$

(67)

$$ARMSE\left(i\right)=\frac{1}{k_n-k_0}\sum _{k=k_0}^{k_n}RMS{E}_k\left(i\right)$$

(68)

where M denotes the number of Monte Carlo simulations. k is the k-th instant, and ${\widehat{\mathit{x}}}_k^m$ and ${\mathit{x}}_k^m$are the estimation and the true value, respectively.

Figure 8, Figure 9, Figure 10 and Figure 11 compare the performances of the CKF, ORD-CKF, MRD-CKF, and MRD-DCSCKF. The simulation results show that CKF had the worst estimation. The performance of ORD-CKF was between CKF and MRD-CKF because ORD-CKF could only handle one-step randomly delayed measurements. The performance of MRD-CKF was significantly better than CKF and ORD-CKF because it could handle multiple-step randomly delayed measurements. There is no doubt that MRD-DCSCKF had the best performance among all filters. MRD-DCSCKF adopts the multiple-step randomly delayed filtering framework in combination with the DCS kernel function, which enables its ability to address both delayed measurements and outliers.

Figure 8. RMSEs of the position with CKF, ORD-CKF, MRD-CKF, and MRD-DCSCKF.

Figure 9. RMSEs of the velocity with CKF, ORD-CKF, MRD-CKF, and MRD-DCSCKF.

Figure 10. RMSEs of the attitude with CKF, ORD-CKF, MRD-CKF, and MRD-DCSCKF.

Figure 11. RMSEs of the gyro drift with CKF, ORD-CKF, MRD-CKF, and MRD-DCSCKF.

Figure 12, Figure 13, Figure 14 and Figure 15 present the tracking errors of MRD-DCSCKF. As can be seen, the error curves converge quickly to the $3\sigma$ bounds, which are indicated by the dashed line. This demonstrates that the convergence property of MRD-DCSCKF is sufficient.

Figure 12. Position error.

Figure 13. Velocity error.

Figure 14. Attitude error.

Figure 15. Gyro drift error.

Simulations to investigate the effects of delay probability on the MRD-DCSCKF were conducted, and Figure 16, Figure 17, Figure 18 and Figure 19 show the ARMSEs from 450 to 600 s with delay probabilities of $0\le p\le 0.2$. It is clear that MRD-DCSCKF achieved the highest estimation accuracy among the four filters.

Figure 16. ARMSEs of position under different delay probabilities.

Figure 17. ARMSEs of velocity under different delay probabilities.

Figure 18. ARMSEs of attitude under different delay probabilities.

Figure 19. ARMSEs of gyro drift under different delay probabilities.

Figure 20, Figure 21, Figure 22 and Figure 23 show the ARMSEs from 450 to 600 s with perturbing parameters of $0\le \epsilon \le 0.2$. As shown, the estimation accuracy of all the filters gradually decreases with increasing perturbing parameters. However, the MRD-DCSCKF exhibits superior robustness and performance of all the filters, even under these conditions.

Figure 20. ARMSEs of position under different perturbing parameters.

Figure 21. ARMSEs of velocity under different perturbing parameters.

Figure 22. ARMSEs of attitude under different perturbing parameters.

Figure 23. ARMSEs of gyro drift under different perturbing parameters.

7. Conclusions

This paper proposes a novel multiple-step randomly delayed, robust filter, referred to as the multiple-step, randomly delayed, dynamic-covariance-scaling cubature Kalman filter (MRD-DCSCKF), to effectively handle randomly delayed measurements and outliers. The MRD-DCSCKF uses a state-augmentation approach to break the limitations of the delayed steps and reformulates the state update equations of Kalman filter based on the delayed measurements modeled according to a set of Bernoulli random variables. Meanwhile, the proposed filter relies on dynamic-covariance-scaling, robust kernel to suppress measurement-outliers. The application of MRD-DCSCKF to vision-based spacecraft-relative navigation is investigated, where the relative dynamics are described with the T-H equations, and quaternions and generalized Rodrigues parameters are introduced to estimate spacecraft relative attitudes. The simulation results illustrate that the MRD-DCSCKF is able to precisely estimate the status of the spacecraft with high precision, even with randomly delayed measurements and outliers, as compared to other algorithms.

Appendix A.1

According to the third-order the spherical-radial cubature rule [42], the evaluation of the CKF sampling points with n-dimensional state vector $\mathit{x}$and covariance $\mathit{P}$ is computed, as follows.

$${\mathsf{\chi }}_i=\mathrm{Trans}\left[\mathit{x},\mathit{P}\right]=\mathit{x}+\mathit{S}{\varsigma }_i,i=1,2,\cdots ,2n$$

(A1)

where $\mathit{S}$ is the square root of $\mathit{P}$$\left(\mathit{P}=\mathit{S}·{\mathit{S}}^T\right)$. ${\varsigma }_i$is an element in the $2n$ cubature points, and its weight is ${\omega }_i=\frac{1}{2n}$. In addition, $\left\{\left.{\varsigma }_i\right\}\right.$ is as follows

$$\sqrt{n}\left[\left(\begin{array}{c}1\hfill \\ 0\hfill \\ ⋮\hfill \\ 0\hfill \end{array}\right),\dots ,\left(\begin{array}{c}0\hfill \\ 0\hfill \\ ⋮\hfill \\ 1\hfill \end{array}\right),\left(\begin{array}{c}-1\hfill \\ 0\hfill \\ ⋮\hfill \\ 0\hfill \end{array}\right),\dots ,\left(\begin{array}{c}0\hfill \\ 0\hfill \\ ⋮\hfill \\ -1\hfill \end{array}\right)\right]$$

(A2)

Appendix A.2

The sub-update of the multiple-step randomly delayed cubature Kalman filter [25] at the s-th step is calculated as below

$${\mathit{K}}_k^s={\mathit{P}}_{k/\phantom{kk-1}k-1}^{Xy,s}{\left({\mathit{P}}_{k/\phantom{kk-1}k-1}^{yy,s}+{\mathit{R}}_{k-s}\right)}^{-1}$$

(A3)

$${\widehat{\mathit{X}}}_{k/\phantom{kk}k}^s={\widehat{\mathit{X}}}_{k/\phantom{kk-1}k-1}+{\mathit{K}}_k^s\left({\mathit{y}}_k-{\widehat{\mathit{y}}}_{k/\phantom{kk-1}k-1}^s\right),s=0,1,2,\dots ,d$$

(A4)

$${\mathit{P}}_{k/\phantom{kk}k}^s={\mathit{P}}_{k/\phantom{kk-1}k-1}-{\mathit{K}}_k^s{\left({\mathit{P}}_{k/\phantom{kk-1}k-1}^{Xy,s}\right)}^T,s=0,1,2,\dots ,d$$

(A5)

Appendix A.3

The combination of CKF and the M-estimator results in a robust CKF, which adopts different robust kernel functions to suppress outliers [30,43]. The state updates of robust CKF are the same as CKF, and its measurement updates through constructing a linear regression model with the prior estimation of the filter and the measurement model. Finally, it completes the robust update process by iteration. This section briefly reviews the measurement updates of the linear regression robust CKF.

The measurement equation is approximated as follows:

$${\mathit{y}}_k\approx h\left({\widehat{\mathit{x}}}_{k|k-1}\right)+{\mathit{H}}_k\left({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k-1}\right)+{\mathit{w}}_k$$

(A6)

The state prediction error ${\mathsf{\delta }}_k$ is denoted as following:

$${\mathsf{\delta }}_k={\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k-1}$$

(A7)

Then, the regression problem takes the form

$$\left[\begin{array}{c}{\mathit{y}}_k-h\left({\widehat{\mathit{x}}}_{k|k-1}\right)+{\mathit{H}}_k{\widehat{\mathit{x}}}_{k|k-1}\hfill \\ {\widehat{\mathit{x}}}_{k|k-1}\hfill \end{array}\right]=\left[\begin{array}{c}{\mathit{H}}_k\hfill \\ \mathit{I}\hfill \end{array}\right]{\mathit{x}}_k+\left[\begin{array}{c}\mathit{w}{}_k\hfill \\ -{\mathsf{\delta }}_k\hfill \end{array}\right]$$

(A8)

where ${\mathit{H}}_k={\left[{\left({\mathit{P}}_{k|k-1}\right)}^{-1}\left({\mathit{P}}^{xy}\right)\right]}^T$ is the measurement matrix.

Some quantities are given as

$${\mathit{S}}_k=\left[\begin{array}{c}{\mathit{R}}_{k}0\hfill \\ 0{\mathit{P}}_{k|k-1}\hfill \end{array}\right]$$

(A9)

$${\stackrel{⌢}{\mathit{z}}}_k={\mathit{S}}_k^{-1/2}\left[\begin{array}{c}{\mathit{y}}_k-h\left({\widehat{\mathit{x}}}_{k|k-1}\right)+{\mathit{H}}_k{\widehat{\mathit{x}}}_{k|k-1}\hfill \\ {\widehat{\mathit{x}}}_{k|k-1}\hfill \end{array}\right]$$

(A10)

$${\mathit{M}}_k={\mathit{S}}_k^{-1/2}\left[\begin{array}{c}{\mathit{H}}_k\hfill \\ \mathit{I}\hfill \end{array}\right]$$

(A11)

$${\mathsf{\xi }}_k={\mathit{S}}_k^{-1/2}\left[\begin{array}{c}{\mathit{w}}_k\hfill \\ -{\mathsf{\delta }}_k\hfill \end{array}\right]$$

(A12)

Then Equation (A8) is transformed to

$${\stackrel{⌢}{\mathit{z}}}_k={\mathit{M}}_k{\mathit{x}}_k+{\mathsf{\xi }}_k$$

(A13)

where ${\mathsf{\xi }}_k$ is residual error with ${\mathsf{\xi }}_k={\mathit{M}}_k{\mathit{x}}_k-{\stackrel{⌢}{\mathit{z}}}_k$.

Minimize the following cost function to solve above-mentioned regression problem:

$$J\left({\mathit{x}}_k\right)=\sum _{i=1}^{m+n}\rho \left({\mathsf{\xi }}_k\right)$$

(A14)

where $\rho \left(·\right)$ is the cost function. $m$ denotes the dimension of measurement ${\mathit{y}}_k$.

The solution of Equation (A14) satisfies

$$\sum _{k=1}^{m+n}\phi \left({\mathsf{\xi }}_k\right)\frac{\partial {\mathsf{\xi }}_k}{\partial {\mathit{x}}_k}=0$$

(A15)

where $\phi \left({\mathsf{\xi }}_k\right)={\rho }^{\prime }\left({\mathsf{\xi }}_k\right)$. $\psi \left(·\right)$ is the weight function defined as $\psi \left({\mathsf{\xi }}_k\right)=\frac{\phi \left({\mathsf{\xi }}_k\right)}{{\mathsf{\xi }}_k}$, and the corresponding weight matrix is $\mathbf{\Psi }=diag\left[\psi \left({\mathsf{\xi }}_i\right)\right]$. ${\mathsf{\xi }}_{i,k}$ is the i-th component of ${\mathsf{\xi }}_k$. Equation (A15) could be written in matrix form, as follows:

$${\mathit{M}}_k^T\mathbf{\Psi }({\mathit{M}}_k{\mathit{x}}_k-{\stackrel{⌢}{\mathit{z}}}_k)=0$$

(A16)

The solution of Equation (A16) can be obtained after $j$ times iteration, as follows:

$${\mathit{x}}_k^{(j+1)}={\left({\mathit{M}}_k^T{\mathbf{\Psi }}^{\left(j\right)}{\mathit{M}}_k\right)}^{-1}{\mathit{M}}_k^T{\mathbf{\Psi }}^{\left(j\right)}{\stackrel{⌢}{\mathit{z}}}_k$$

(A17)

The state estimation covariance could be computed by the following:

$${\mathit{P}}_{k|k}={\left({\mathit{M}}_k^T{\mathbf{\Psi }}^{\left(j\right)}{\mathit{M}}_k\right)}^{-1}$$

(A18)

In this paper, the calculation process of Equation (A8)-Equation (A16) is summarized as follows:

$$\left[\mathit{M},\stackrel{⌢}{\mathit{z}}]\right.=\mathrm{Robust}\_\mathrm{update}\left[{\mathit{y}}_k,h\left(·\right),{\mathit{P}}_{k/\phantom{kk-1}k-1},{\mathit{P}}^{xy},{\widehat{\mathit{x}}}_{k|k-1},\mathit{R}\right]$$

(A19)