Vision-Based Relative Attitude and Position Estimation for Small Satellites with Robust Filtering Technique ^†

Abstract

Relative satellite navigation is critical for formation flying, rendezvous, and docking. This study augments a vision-based relative navigation framework with a robust multiplicative extended Kalman filter (RMEKF) that adaptively scales the measurement covariance using innovation-based covariance matching and a chi-square fault-detection test. A two-spacecraft scenario is simulated in which a deputy monocular camera observes six active beacons on a chief spacecraft. To evaluate fault tolerance, constant line-of-sight (LOS) errors are injected on two beacon measurements during a fixed interval. Over the fault-centered evaluation window, the RMEKF reduces attitude root mean square error (RMSE) by approximately 71–73% compared to the conventional multiplicative extended Kalman filter (MEKF), while also improving relative/orbital state accuracy by 19–93%. These results indicate improved robustness to LOS measurement faults without degrading overall estimation stability.

1. Introduction

Relative navigation between a chief and a deputy spacecraft is crucial for missions such as formation flying, on-orbit servicing, rendezvous, and docking. In such applications, the deputy must estimate its relative position and attitude with sufficient accuracy to meet guidance and control requirements. For small satellites, this task might be particularly challenging because of strict limitations on mass, power, and onboard sensors [1,2]. A common approach is to describe the relative motion in the Hill (or RTN) frame and to use linearized relative motion equations, such as the elliptic Clohessy–Wiltshire formulation implemented in this work, i.e., a time-varying extension of CW for an elliptic chief orbit [1].

Vision-based navigation (VISNAV) has been widely studied as a relative navigation solution for small satellites. In a typical VISNAV setup, a monocular camera on the deputy observes active light sources mounted on the chief, and the image-plane measurements are converted to line-of-sight (LOS) vectors using a pinhole camera model [3]. These LOS vectors carry information on both relative attitude and translation, and can be combined with quaternion attitude kinematics and relative orbit dynamics. Kim et al. [2] presented a VISNAV scheme in which a multiplicative extended Kalman filter (MEKF) estimates relative position, velocity, attitude, and gyro biases by combining LOS measurements with a relative dynamics model. The MEKF framework is well-suited to spacecraft applications because it preserves the unit-norm constraint of the quaternion while representing attitude errors with a three-parameter vector [4,5].

Despite its advantages, the performance of the MEKF can degrade significantly when sensor measurements are corrupted by faults or when their statistical properties deviate from the assumed noise model. This motivates the use of robust and adaptive Kalman filtering techniques that modify the measurement covariance based on innovation statistics and employ explicit fault-detection logic [6]. Soken et al. [7] proposed a robust EKF in which multiple scale factors are computed from a covariance-matching procedure over a moving window and applied selectively to the measurement covariance. This concept allows the filter to penalize only the affected measurements instead of uniformly downweighting all sensors.

Beyond the VISNAV-MEKF baseline in [2], vision/optical relative navigation has also been studied in a variety of proximity-operations contexts, including relative navigation for inspection spacecraft [8], autonomous cooperative docking scenarios [9], and multi-sensor optical relative navigation systems for satellite servicing [10]. In addition, robustness and applicability considerations for monocular pose-estimation pipelines have been surveyed extensively in the literature [11]. These works motivate robust estimation mechanisms when practical LOS measurements deviate from the nominal noise model assumed by conventional filters, including cooperative beacon-based VISNAV.

In this paper, a VISNAV-based relative navigation framework for a two-spacecraft formation is adopted, in which the deputy carries a monocular camera, and the chief is equipped with active beacons, following the modeling approach of [2]. On top of the baseline MEKF, a robust estimation algorithm inspired by [6,7] that uses innovation-based covariance matching and a chi-square fault-detection test to adapt the measurement covariance via multiple scale factors is implemented. Through numerical simulations, fault-injected LOS measurements are used to compare the conventional MEKF and the proposed robust MEKF in terms of attitude, relative state, and bias estimation performance.

The main contributions of this paper are:

• integrating an innovation-driven covariance-matching mechanism into the VISNAV-MEKF so that individual LOS measurements can be selectively downweighted via multiple scale factors rather than uniformly penalizing all beacons;
• coupling this adaptation with a chi-square innovation test to activate robustness only during suspected fault conditions;
• quantifying the resulting fault-tolerance improvement under injected LOS direction errors via a direct RMSE comparison against the conventional MEKF.

2. Materials and Methods

2.1. Relative Orbital Dynamics

In a two-spacecraft formation flight, the relative motion is expressed in the Hill frame ($\left\{{\widehat{\mathit{o}}}_r,{\widehat{\mathit{o}}}_{\theta },{\widehat{\mathit{o}}}_h\right\}$) of the chief spacecraft, about which the other spacecraft orbits, as demonstrated in Figure 1. The orbiting spacecraft is referred to as the deputy, and $\rho ={(x,y,z)}^T$ is the relative position vector. When the relative distance is much smaller than the chief’s orbital radius, ${\mathit{r}}_c$, relative equations of motion are expressed as [1]

$$\begin{array}{cc}\hfill \ddot{x}-x{\dot{\nu }}^2\left(1+2{\displaystyle \frac{r_c}{p}}\right)-2\dot{\nu }\left(\dot{y}-y{\displaystyle \frac{{\dot{r}}_c}{r_c}}\right)& =0\hfill \\ \hfill \ddot{y}-y{\dot{\nu }}^2\left(1-{\displaystyle \frac{r_c}{p}}\right)+2\dot{\nu }\left(\dot{x}-x{\displaystyle \frac{{\dot{r}}_c}{r_c}}\right)& =0\hfill \\ \hfill \ddot{z}+z{\dot{\nu }}^2{\displaystyle \frac{r_c}{p}}& =0\hfill \end{array}$$

(1)

where p is the semi-latus rectum and $\nu$ is true anomaly of the chief with its rate $\dot{\nu }$ given by the relation

$$\ddot{\nu }=-2{\displaystyle \frac{{\dot{r}}_c}{r_c}}\dot{\nu },{\ddot{r}}_c=r_c{\dot{\nu }}^2\left(1-{\displaystyle \frac{r_c}{p}}\right).$$

(2)

2.2. Relative Attitude Kinematics

For rigid-body attitude representation, quaternions provide a compact, singularity-free parameterization of three-dimensional rotations. The quaternion kinematics are given by [4,5]

$$\dot{\mathit{q}}={\displaystyle \frac{1}{2}}\Xi \left(\mathit{q}\right)\mathit{\omega },$$

(3)

where $\mathit{\omega }$ is the angular velocity vector.

The relative attitude quaternion of the deputy with respect to the chief is expressed as $\delta \mathit{q}$, and obtained through quaternion multiplication.

$$\delta \mathit{q}={\mathit{q}}_d\otimes {\mathit{q}}_c^{-1}$$

(4)

where ${\mathit{q}}_c$ is the attitude of the chief, ${\mathit{q}}_d$ is the attitude of the deputy, ${(·)}^{-1}$ denotes the quaternion inverse. The relative quaternion kinematics equation is given below.

$$\delta \dot{\mathit{q}}={\displaystyle \frac{1}{2}}\Xi \left(\delta \mathit{q}\right){\mathit{\omega }}_{dc}$$

(5)

where ${\mathit{\omega }}_{dc}$ is the deputy angular rate relative to the chief, expressed in the deputy frame.

2.3. Visual Navigation System

The visual navigation system consists of a monocular camera mounted on the deputy spacecraft, and light sources (beacons) on the chief spacecraft, as illustrated in Figure 2. The deputy body frame is assumed to coincide with the camera coordinate system. The z-axis of the camera coordinate system is aligned with the boresight, and the image plane is the two-dimensional plane separated from the camera frame by the focal length, f. The body frame of the chief spacecraft is taken as the object–space frame.

Image-plane measurements are converted to unit line-of-sight (LOS) vectors using a standard pinhole camera model [3]. Let ${\mathit{r}}_i$ denote the unit vector from the deputy camera center to the ith beacon, expressed in the chief frame, and let A denote the relative attitude matrix that maps chief-frame vectors to the camera frame. The corresponding LOS measurement in the camera frame is modeled as

$${\tilde{\mathit{b}}}_i=A{\mathit{r}}_i+{\mathit{v}}_i,{\mathit{v}}_i^{T}A{\mathit{r}}_i=0,$$

(6)

where ${\mathit{v}}_i$ is an approximately zero-mean Gaussian sensor error.

The measured angular rate is modeled as [12]

$$\tilde{\mathit{\omega }}=\mathit{\omega }+\mathit{\beta }+{\mathit{\eta }}_v,\dot{\mathit{\beta }}={\mathit{\eta }}_u,$$

(7)

where $\mathit{\beta }$ is a gyro bias (random walk) and ${\mathit{\eta }}_v,{\mathit{\eta }}_u$ are zero-mean white noises.

2.4. Multiplicative Extended Kalman Filter

The relative attitude and position of the two-spacecraft formation are estimated using a multiplicative extended Kalman filter. The global states to be estimated are:

$$\mathit{X}={\left[\begin{array}{cccccccccc}x\hfill & y\hfill & z\hfill & \dot{x}\hfill & \dot{y}\hfill & \dot{z}\hfill & r_c\hfill & {\dot{r}}_c\hfill & \nu \hfill & \dot{\nu }\hfill \end{array}\right]}^T$$

(8)

The nonlinear state-space model $\dot{\mathit{X}}=\mathit{f}\left(\mathit{X}\right)$ is obtained from Equations (1) and (2) by defining the state vector in Equation (8). The linearized error dynamics are expressed as $\Delta \dot{\mathit{x}}=F\Delta \mathit{x}+G\mathbf{w}$, where the state-transition matrix F and the noise-distribution matrix G follow the formulation in [2]. The error states vector, including the attitude, chief, and deputy gyro biases for the filter, is given in Equation (9).

$$\Delta \mathit{x}\equiv {\left[\begin{array}{ccccccccc}\mathit{\delta }{\mathit{\alpha }}^T\hfill & \Delta {\mathit{\beta }}_c^T\hfill & \Delta {\mathit{\beta }}_d^T\hfill & \Delta {\mathit{\rho }}^T\hfill & \Delta {\dot{\mathit{\rho }}}^T\hfill & \Delta r_c\hfill & \Delta {\dot{r}}_c\hfill & \Delta \nu \hfill & \Delta \dot{\nu }\hfill \end{array}\right]}^T$$

(9)

The MEKF time update is obtained by propagating the nonlinear dynamics in Equations (1) and (2) and the quaternion kinematics in Equation (3), while the covariance is propagated using the corresponding linearized error dynamics. The measurement update uses the LOS measurement model in Equation (6) with a Jacobian consistent with [2]. For brevity, standard MEKF prediction and correction equations are not repeated here; the implementation follows [5].

2.5. Robust Estimation Algorithm

To improve robustness against intermittent LOS faults, the measurement-noise covariance is adapted using innovation statistics, with separate scale factors applied to individual LOS measurements so that only faulted beacons are downweighted.

In the presence of a measurement fault, the actual innovation covariance exceeds its theoretical value. To counter this, the robustness can then be enhanced by adapting the measurement-noise covariance matrix R. The real and theoretical innovation covariances are matched as [7]:

$${\displaystyle \frac{1}{\xi }}\sum _{j=k-\xi +1}^k{\mathit{e}}_j{\mathit{e}}_j^T=H_k\left({\widehat{\mathit{x}}}_k^{-}\right)P_k^{-}{H}_k^T\left({\widehat{\mathit{x}}}_k^{-}\right)+S_k{R}_k,$$

(10)

where $\xi$ is the width of the moving window, and ${\mathit{e}}_k=\left[{\tilde{\mathit{y}}}_k-{\mathit{h}}_k\left({\widehat{\mathit{x}}}_k^{-}\right)\right]$ is the innovation vector with ${\tilde{\mathit{y}}}_k\in {\mathbb{R}}^m$ denoting the stacked LOS measurement vector.

$$S_k=\left\{{\displaystyle \frac{1}{\xi }}\sum _{j=k-\xi +1}^k{\mathit{e}}_j{\mathit{e}}_j^T-H_k\left({\widehat{\mathit{x}}}_k^{-}\right)P_k^{-}{H}_k^T\left({\widehat{\mathit{x}}}_k^{-}\right)\right\}{R}_k^{-1}.$$

(11)

The scale matrix should be diagonalized since only the diagonal terms affect the adaptation process. Moreover, its elements must be lower-bounded by 1 so that only the covariance entries corresponding to faulted measurements are increased in $R_k$, and nominal measurements are not artificially downweighted.

$$S_k^{\ast }=\mathrm{diag}\left(max\{1,S_k(1,1)\},\dots ,max\{1,S_k(m,m)\}\right).$$

(12)

Finally, the Kalman gain is updated using the scaled covariance.

$$K_k=P_k^{-}{H}_k^T\left({\widehat{\mathit{x}}}_k^{-}\right){\left[H_k\left({\widehat{\mathit{x}}}_k^{-}\right)P_k^{-}{H}_k^T\left({\widehat{\mathit{x}}}_k^{-}\right)+S_k^{\ast }{R}_k\right]}^{-1}$$

(13)

2.6. Fault Detection

The robust Kalman filter is used only in the presence of a sensor fault; otherwise, the algorithm uses the conventional MEKF. Fault detection is performed using statistical information from the innovation sequence. For this purpose, the following two hypotheses may be proposed [6]:

• ${\gamma }_0$: the system is operating normally,
• ${\gamma }_1$: there is a malfunction in the estimation system.

The following statistical function is introduced with degrees of freedom m and confidence level $\alpha$:

$${\beta }_k={\mathit{e}}_k^T{\left[H_k\left({\widehat{\mathit{x}}}_k^{-}\right)P_k^{-}{H}_k^T\left({\widehat{\mathit{x}}}_k^{-}\right)+R_k\right]}^{-1}{\mathit{e}}_k$$

(14)

$$\mathsf{P}\{{\chi }_m^2>{\chi }_{\alpha ,m}^2\}=\alpha ,0<\alpha <1.$$

(15)

A fault is declared at time step k when the normalized innovation statistic exceeds the threshold, i.e., ${\beta }_k>{\chi }_{\alpha ,m}^2$; otherwise, the filter remains in nominal (MEKF) mode.

3. Results

3.1. Simulation Results for Robust Filter

The relative and chief-orbit dynamics Equations (1) and (2) are integrated using RK4 over 600 min with a 10 s step. Initial conditions are $x_0=y_0=200$ m, $z_0=100$ m, ${\dot{x}}_0=0.1$ m/s, ${\dot{y}}_0=-0.4325$ m/s, ${\dot{z}}_0=0.01$ m/s, ${\nu }_0=0$, and ${\dot{r}}_{c,0}=0$; the chief-orbit parameters follow the Hubble Space Telescope (i.e., the semi-major axis and eccentricity are $a$ = 6,858,863 m and $e=0.0002579$, respectively).

The initial quaternion is $\mathit{q}\left(t_0\right)={[\sqrt{2}/200\sqrt{2}/2]}^T$ and angular velocities given as ${\mathit{\omega }}_c={\left[000.00111\right]}^T$ rad/s and ${\mathit{\omega }}_d={\left[-0.00200.0011\right]}^T$ rad/s during the entire simulation. The initial biases of both the chief and deputy gyros are the same for all axes and given by 1 deg/hr. Six beacons are assumed to be mounted on the chief, and their placement is as follows: $B_1:(0.5,0.5,0.0)$, $B_2:(-0.5,-0.5,0.0)$, $B_3:(-0.5,0.5,0.0)$, $B_4:(0.5,-0.5,0.0)$, $B_5:(0.2,0.5,0.1)$, $B_6:(0.0,0.2,-0.1)$.

Due to the relative distance and relative orientation, the camera might measure the LOS vectors with some error. To test the robust filter algorithm, a constant error is added to two LOS measurements (the third and sixth beacons) between the 250th and 260th minutes, when the separation between the spacecraft is largest. The faulty measurements are modeled as

$${\tilde{\mathit{b}}}_{i,err}=A_{err,i}{\mathit{b}}_i+{\mathit{v}}_i$$

(16)

where $A_{err,i}$ is a direction cosine matrix (DCM) that represents the small error angles ${\alpha }_{err,3}(0.{01}^{°},0.{002}^{°},0^{°})$ and ${\alpha }_{err,6}(0.{002}^{°},0.{003}^{°},0.{001}^{°})$. The size of the moving window is chosen as $\xi =30$. A chi-square distribution with $m=18$ degrees of freedom (size of the innovation vector) and a confidence level of 97.5% is used. The simulation results for estimation errors with their $\pm 3\sigma$ bounds are given in Figure 3 and Figure 4.

Figures show that all estimation errors remain within their $\pm 3\sigma$ bounds derived from the covariance matrix, and the actual errors are reasonably small. Please note that the $r_c$ subplot is shown with wide axis limits for visualization; the actual $r_c$ error magnitude during the fault-centered interval is quantified in

Table 1. Root mean square error (RMSE) for attitude and gyro-bias estimates for the multiplicative extended Kalman filter (MEKF) and robust multiplicative extended Kalman filter (RMEKF), sampled between minutes 245 and 265.

	$\mathit{\phi }{(}^{°})$	$\mathit{\theta }{(}^{°})$	$\mathit{\psi }{(}^{°})$	${\mathit{\beta }}_{{\mathit{c}}_{\mathit{x}}}{(}^{°}/\mathbf{h})$	${\mathit{\beta }}_{{\mathit{c}}_{\mathit{y}}}{(}^{°}/\mathbf{h})$	${\mathit{\beta }}_{{\mathit{c}}_{\mathit{z}}}{(}^{°}/\mathbf{h})$	${\mathit{\beta }}_{{\mathit{d}}_{\mathit{x}}}{(}^{°}/\mathbf{h})$	${\mathit{\beta }}_{{\mathit{d}}_{\mathit{y}}}{(}^{°}/\mathbf{h})$	${\mathit{\beta }}_{{\mathit{d}}_{\mathit{z}}}{(}^{°}/\mathbf{h})$
MEKF	0.2036	0.1181	0.2332	0.1163	0.1200	0.0402	0.0483	0.0987	0.0582
RMEKF	0.0548	0.0338	0.0632	0.0489	0.0375	0.0209	0.0118	0.0631	0.0302

and

Table 2. RMSE for translational and orbital-state estimates for the MEKF and RMEKF, sampled between minutes 245 and 265.

	$\mathit{x}\left(\mathbf{m}\right)$	$\mathit{y}\left(\mathbf{m}\right)$	$\mathit{z}\left(\mathbf{m}\right)$	$\dot{\mathit{x}}(\mathbf{m}/\mathbf{s})$	$\dot{\mathit{y}}(\mathbf{m}/\mathbf{s})$	$\dot{\mathit{z}}(\mathbf{m}/\mathbf{s})$	${\mathit{r}}_{\mathit{c}}\left(\mathbf{m}\right)$	${\dot{\mathit{r}}}_{\mathit{c}}(\mathbf{m}/\mathbf{s})$	$\mathit{\nu }\left(\mathbf{rad}\right)$	$\dot{\mathit{\nu }}(\mathbf{rad}/\mathbf{s})$
MEKF	0.0159	0.1211	0.0108	$3.64\times {10}^{-5}$	$3.47\times {10}^{-5}$	$2.53\times {10}^{-5}$	66.67	0.0134	$1.34\times {10}^{-4}$	$1.48\times {10}^{-8}$
RMEKF	0.0045	0.0101	0.0087	$4.92\times {10}^{-6}$	$1.00\times {10}^{-5}$	$2.12\times {10}^{-5}$	4.562	0.0036	$4.62\times {10}^{-5}$	$4.23\times {10}^{-9}$

, with the chief-orbital-radius RMSE reported in Table 2 (MEKF: 66.67 m, RMEKF: 4.562 m). In Figure 3, it is seen that the injected measurement fault causes an increase in the attitude errors; however, thanks to the robust algorithm, the RMEKF quickly converges again after the fault disappears, whereas the regular MEKF algorithm diverges or exhibits significantly larger errors.

3.2. Error Comparison

The results for MEKF and RMEKF are compared by calculating the root mean square error (RMSE) of the estimations around the times of measurement error, such that

$${\mathrm{RMSE}}_{\gamma }=\sqrt{{\displaystyle \frac{1}{N_s}}\sum _{k=k_i}^{k_f}{\left(x_{\gamma }\left(k\right)-{\widehat{x}}_{\gamma }\left(k\right)\right)}^2},\gamma =1,\dots ,n,$$

(17)

where $k_i$ is the step at 245th minute, $k_f$ is the step at 265th minute, and $N_s$ is the number of samples on this interval. The results tabulated in Table 1 and Table 2 show that the RMEKF outperforms the MEKF in the presence of LOS vector measurement errors.

This behavior is reflected quantitatively in Table 1 and Table 2: during 245–265 min, the RMEKF reduces attitude RMSE by approximately 72% and decreases the chief orbital radius RMSE from 66.67 m to 4.562 m, while also reducing the y-position RMSE from 0.1211 m to 0.0101 m.

4. Conclusions

A multiplicative extended Kalman filter framework for relative navigation and attitude determination using LOS beacon measurements was augmented with an innovation-based robust mechanism and chi-square fault detection to maintain estimation quality under measurement faults. During the fault-centered interval (245–265 min), the proposed RMEKF reduces attitude RMSE by approximately 71–73% (e.g., roll 0.2036° → 0.0548°, pitch 0.1181° → 0.0338°, yaw 0.2332° → 0.0632°) and improves key translational/orbital estimates (e.g., y-position RMSE reduced by 91.7% and $r_c$ RMSE reduced by 93.2%) compared to the conventional MEKF. These results indicate that adaptive, per-measurement covariance scaling improves fault tolerance without sacrificing overall stability.