## 1. INTRODUCTION

Precise relative attitude and position estimation between two spacecraft are crucial to many space missions today, such as spacecraft formation flying and rendezvous and docking. Presently, Vision-based Navigation (VISNAV) systems, consisting of an optical sensor combined with specific light sources (beacons), are usually used to determine the relative attitude and position within the range of a few hundred metres (Alonso et al., Reference Alonso, Crassidis and Junkins2000; Kim et al., Reference Kim, Crassidis, Cheng, Fosbury and Junkins2007; Xing et al., Reference Xing, Cao, Zhang, Guo and Wang2010; Tang et al., Reference Tang, Yan and Zhong2010). In order to determine the coupled attitude and position from the Line-Of-Sight (LOS) measurements, a Gaussian Least Squares Differential Correction (GLSDC) algorithm is usually used to provide a deterministic solution. To overcome the shortcoming of the iterative computation, Crassidis et al. (Reference Crassidis, Alonso and Junkins2000) presented an optimal attitude and position determination approach derived from a generalised predictive filter for nonlinear systems. However, these unitary vision-based estimation methods, which only utilise the static geometrical relations to determine the relative attitude and position are liable to be affected by the error factors, such as measurement errors, quantization errors, and extraction errors of the beacon locations, etc. Therefore, it is necessary to adopt the state estimation method to design a navigation filter. At present, two types of navigation filters include the absolute navigation filter (Wodffinden and Geller, Reference Wodffinden and Geller2007; Schmidt et al., Reference Schmidt, Geller and Chavez2010; Hablani, Reference Hablani2009) and the relative navigation filter (Alonso et al., Reference Alonso, Crassidis and Junkins2000; Kim et al., Reference Kim, Crassidis, Cheng, Fosbury and Junkins2007; Xing et al., Reference Xing, Cao, Zhang, Guo and Wang2010; Junkins et al., Reference Junkins, Hughes, Wazni and Pariyapong1999; Hablani et al., Reference Hablani, Tapper and Dana-Bashian2002). The absolute navigation system formulates the dynamics models of the two spacecraft in the inertial frame and acquires the relative motion parameters by computing the differences between two spacecraft. The relative navigation system estimates the relative position and velocity based on the relative equations of motion established in a rotating Local Vertical Local Horizontal (LVLH) frame. For close-range relative motion, the relative navigation system is usually adopted due to its sufficient accuracy and better computational efficiency.

The VISNAV system consists of an optical sensor combined with specific light sources (beacons) to achieve a selective vision. In general, the known beacon locations are defined in the master's body frame, whereas the relative position vector between the master and slave spacecraft is expressed in its LVLH frame. In Alonso et al. (Reference Alonso, Crassidis and Junkins2000), Kim et al. (Reference Kim, Crassidis, Cheng, Fosbury and Junkins2007) and Xing et al. (Reference Xing, Cao, Zhang, Guo and Wang2010), a simplified assumption that the master body frame coincides with its LVLH frame is made to construct the LOS observations for convenience. Unfortunately, this assumption is not valid under all situations, or in some rigorous senses, it is only a special case. One approach to solve this problem is to formulate the relative equations of motion in the master body frame (Tang et al., Reference Tang, Yan and Zhong2010), and thus the beacon location vectors and relative position vector are described in the same coordinates. The main disadvantages of this approach are that the angular velocity of the master body frame generally varies rapidly and its measured value is contaminated by the gyro measurement error. They may cause large computation errors in the relative equations of motion. Another approach is to additionally estimate the attitudes of both spacecraft relative to the LVLH frame (Zhang et al., Reference Zhang, Yang, Zhang, Cai and Qian2014), and the assumption that both the master's body and LVLH frames are the same can be removed.

The Extended Kalman Filter (EKF) is the most widely used for nonlinear filtering problems so far. The EKF operates by approximating the state distribution and relevant noise densities as Gaussian random variables (GRVs) and then propagating them through a first-order linearization of the nonlinear system (Wan and van der Merwe, Reference Wan and van der Merwe2001). Unfortunately, this can result in large errors in the true *posterior* means and covariances of the transformed GRVs, which may lead to suboptimal performance and sometimes divergence of the filter. To overcome this problem, the Unscented Kalman Filter (UKF) (Julier and Uhlmann, Reference Julier and Uhlmann1997; Julier et al., Reference Julier, Uhlmann and Durrant-Whyte2000) uses a deterministic sampling approach to approximate the state distribution as a GRV, which can capture the *posterior* mean and covariance accurately to at least second order for any nonlinearity. The UKF has proven to be far superior to the EKF, especially when accurate initial condition states are not well known. In addition, the UKF avoids the derivation of Jacobian matrices and has the same computational complexity as the EKF. The motivation of this paper is to derive a novel relative spacecraft attitude and position estimation approach based on an Unscented Kalman Filter. A quaternion-based method is used to describe the relative attitude kinematics. In order to maintain the quaternion normalization constraint in the filter, an unconstrained three-component vector of generalized Rodrigues parameters (GRPs) is used to represent quaternion error vectors and a multiplicative quaternion-error method is employed. This method was first proposed by Crassidis and used in the Unscented Quaternion Estimator (USQUE) (Crassidis and Markley, Reference Crassidis and Markley2003) and Inertial Navigation System/Global Positioning System (INS/GPS) integrated navigation systems (Crassidis, Reference Crassidis2005). To obtain the propagated sigma-point GRPs, the propagated sigma-point error quaternions should be first computed by multiplying the propagated sigma-point quaternions with a reference quaternion. Recently, Chang et al. (Reference Chang, Hu and Chang2014) pointed out that the better choice for the reference quaternion was not the sigma-point quaternion at the centre but the mean of the propagated sigma-point quaternions. It is reported that this can further improve the filtering performance when the nonlinearities of the dynamics model are severe or a good *a priori* estimate of the state is unavailable. An averaging-quaternion algorithm (Cheng et al., Reference Cheng, Markley, Crassidis and Oshman2007) is used to average a set of weighted sigma-point quaternions, which can maintain the unit-norm property of the quaternion. Thus, we adopt this scheme and give a modified version of the UKF. The performances of the EKF and two versions of the UKF with respect to initial condition errors are compared.

The organisation of this paper proceeds as follows. The Unscented Kalman Filter is briefly reviewed in Section 2. In Section 3, various reference frames used in this paper are summarised and a review of the relative equations of motion for eccentric orbits is provided. The relative quaternions that map the master's LVLH frame to the slave and master body frames are defined, and the corresponding relative quaternion kinematics equations are given. In Section 4, the gyro measurement model is reviewed and a stringent VISNAV measurement model for the LOS observations is shown. In Section 5, a brief review of the implementation equations for the EKF is shown, and a novel relative spacecraft attitude and position estimation approach based on UKF is derived. A modified UKF is also proposed based on the averaging-quaternion algorithm. In Section 6, simulation results are presented to compare the performances of the EKF and the UKF with respect to initial condition errors. Concluding remarks are given in Section 7.

## 2. UNSCENTED KALMAN FILTER

In this section, the UKF is reviewed. Consider the following discrete-time nonlinear dynamical system with additive process and measurement noises:

where *x*_{k}∈ℝ^{n} is the state vector at time *k*; ${\bi \tilde y}_k \in {\opf R}^m $ is the measurement vector at time *k*; ** f** (

*·*) and

**(**

*h**·*) are known nonlinear functions;

*w*_{k−1}and

*v*_{k}are independent Gaussian white process noise and measurement noise with covariances

*Q*_{k−1}and

*R*_{k}, respectively.

In the UKF, the Unscented Transformation (UT) is employed to calculate the statistics of a random variable which undergoes a nonlinear transformation. Compared with the EKF, the UKF has better convergence characteristics and greater accuracy for nonlinear systems. Furthermore, the UKF avoids the derivation of Jacobian matrices and has the same computational complexity as the EKF.

The entire algorithm is shown as follows (Wan and van der Merwe, Reference Wan and van der Merwe2001):

(a) Initialisation:

(3)$${\bi \hat x}_0 = E\left[ {{\bi x}_0} \right],{\kern 1pt} {\kern 1pt} {\bi P}_0 = E\left[ {\left( {{\bi x}_0 - {\bi \hat x}_0} \right)\left( {{\bi x}_0 - {\bi \hat x}_0} \right)^T} \right]$$(b) Time update equations:

1) Evaluate the sigma points:

(4)$${\bi X}_{k - 1} = \left[ {{\bi \hat x}_{k - 1} \;{\bi \hat x}_{k - 1} + \gamma \sqrt {{\bi P}_{k - 1}} \;{\bi \hat x}_{k - 1} - \gamma \sqrt {{\bi P}_{k - 1}} \;} \right]^T, \;{\kern 1pt} k = 1, \ldots, \infty $$where $\gamma = \sqrt {n + \lambda} $,*n*is the dimension of the state, and*λ*=*α*^{2}(*n*+*κ*)*−n*is a composite scaling parameter. The constant*α*determines the spread of the sigma points and is usually chosen as a small positive value (e.g., 1×10^{−4}⩽*α*⩽1). The constant*κ*is a secondary scaling parameter, which is usually chosen as*κ*=3*−n*.2) Evaluate the propagated sigma points (

*i*=0, 1, … 2*n*)(5)$${\bi X}_{k \vert k - 1}^* (i) = {\bi f}\left( {{\bi X}_{k - 1} (i)} \right)$$3) Estimate the predicted state and covariance matrix

(6)$${\bi \hat x}_k^ - = \sum\limits_{i = 0}^{2n} {W_i^{{\rm mean}} {\bi X}_{k \vert k - 1}^* (i)} $$(7)$${\bi P}_k^ - = \sum\limits_{i = 0}^{2n} {W_i^{{\mathop{\rm cov}}} \left( {{\bi X}_{k \vert k - 1}^* (i) - {\bi \hat x}_k^ -} \right)\left( {{\bi X}_{k \vert k - 1}^* (i) - {\bi \hat x}_k^ -} \right)^T} + {\bi Q}_{k - 1} $$The weights,*W*_{i}^{mean}and*W*_{i}^{cov}, are computed using(8)$$\eqalign{W_0^{{\rm mean}} = & {\lambda / {(n + \lambda )}} \cr W_0^{{\mathop{\rm cov}}} = & W_0^{{\rm mean}} + (1 - \alpha ^2 + \beta ) \cr W_i^{{\mathop{\rm cov}}} = & W_i^{{\rm mean}} = {1 / {\left[ {2(n + \lambda )} \right]}}{\kern 1pt}, {\kern 1pt} \;i = 1, \ldots, {\kern 1pt} 2n} $$where*β*is a nonnegative weighting coefficient for incorporating prior knowledge of the distribution (a good starting guess is*β*=2).4) Redraw a complete new set of sigma points

(9)$${\bi X}_{k \vert k - 1} = \left[ {{\bi \hat x}_k^ - \quad {\bi \hat x}_k^ - + \gamma \sqrt {{\bi P}_k^ -} \quad {\bi \hat x}_k^ - - \gamma \sqrt {{\bi P}_k^ -}} \right]^T $$This alternative approach introduces fewer sigma points being used than the augmenting sigma point method (Wan and van der Merwe, Reference Wan and van der Merwe2001).5) Estimate the predicted measurement (

*i*=0, 1, … 2*n*)(10)$${\bf {\cal Y}}_{\left. k \right \vert k - 1} (i) = {\bi h}\left( {{\bi X}_{k \vert k - 1} (i)} \right)$$(11)$${\bi \hat y}_k^ - = \sum\limits_{i = 0}^{2n} {W_i^{{\rm mean}} {\bf {\cal Y}}_{\left. k \right \vert k - 1} (i)} $$

(c) Measurement update equations:

1) Estimate innovation covariance matrix and cross-covariance matrix

(12)$${\bi P}_{{\bf y}_k {\bf y}_k} = \sum\limits_{i = 0}^{2n} {W_i^{{\mathop{\rm cov}}} \left( {{\bf {\cal Y}}_{\left. k \right \vert k - 1} (i) - {\bi \hat y}_k^ -} \right)\left( {{\bf {\cal Y}}_{\left. k \right \vert k - 1} (i) - {\bi \hat y}_k^ -} \right)^T} + {\bi R}_k$$(13)$${\bi P}_{{\rm x}_k {\rm y}_k} = \sum\limits_{i = 0}^{2n} {W_i^{{\mathop{\rm cov}}} \left( {{\bi X}_{\left. k \right \vert k - 1} (i) - {\bi \hat x}_k^ -} \right)\left( {{\bf {\cal Y}}_{\left. k \right \vert k - 1} (i) - {\bi \hat y}_k^ -} \right)^T} $$2) Estimate the Kalman gain, updated state and covariance

(14)$${\bi K}_k = {\bi P}_{ {\rm x}_k {\rm y}_k} {\bi P}_{ {\rm y}_k {\rm y}_k} ^{ - 1}$$(15)$${\bi \hat x}_k^ + = {\bi \hat x}_k^ - + {\bi K}_k \left( {{\bi \tilde y}_k - {\bi \hat y}_k^ -} \right)$$(16)$${\bi P}_k^ + = {\bi P}_k^ - - {\bi K}_k {\bi P}_{{\rm y}_k {\rm y}_k} {\bi K}_k^T$$

For higher-dimensional systems, caution should be exercised when *κ* is negative since a possibility exists that the predicted covariance can become non-positive and semi-definite. A modified form of the prediction algorithm can be employed to overcome this problem (Julier and Uhlmann, Reference Julier and Uhlmann1997).

## 3. RELATIVE ORBITAL MOTION AND QUATERNION KINEMATICS

In this section, an overview of the general relative equations of motion for eccentric orbits and the quaternion kinematics is shown. Two relative quaternions that map the master's LVLH frame to the master body frame and to the slave body frame are defined. The corresponding relative quaternion kinematics equations are derived.

### 3.1. Reference Frames

(1) Earth-Centred-Inertial (ECI) frame (

*I*frame): The frame has its origin at the centre of the Earth and is non-rotating with respect to the stars (except for precession of equinoxes). The*z*axis points in the direction of the North pole, the*x*axis points in the direction of the Earth's prime meridian, and the*y*axis completes the right-handed system.(2) Local-Vertical-Local-Horizontal (LVLH) frame (

*H*frame): The LVLH frame is centred at the master spacecraft body, the*x*axis is directed from the spacecraft radially outward and often labelled as the R-bar,*z*axis is normal to the master's orbital plane, and*y*axis is defined as the cross-product of the other two axes.(3) Body Frame: This frame is fixed onto the spacecraft body and rotates with it. Body frames fixed to the two spacecraft are designated as master (

*m*frame) and slave (*s*frame), respectively.

### 3.2. Relative Orbital Motion Equations

In this section, the relative equations of motion using Cartesian coordinates in the rotating LVLH frame are summarised. The relative orbit position vector * ρ* is expressed in the master's LVLH frame components as

*=[*

**ρ***x, y, z*]

^{T}. If the relative orbit coordinates are small compared to the master orbit radius, the general relative equations of motion for eccentric orbits are given by (Kim et al., Reference Kim, Crassidis, Cheng, Fosbury and Junkins2007)

where *p, r* _{m} and $\dot \theta $ are the semilatus rectum, orbit radius and true anomaly rate of the master spacecraft, respectively. The acceleration disturbance vector *ϖ*≡[*ϖ* _{x}*ϖ* _{y}*ϖ _{z}*]

^{T}is modelled as a zero-mean Gaussian white-noise process with

The true anomaly acceleration and orbit-radius acceleration of the master spacecraft are given by

Thus, a ten-dimensional nonlinear state-space model can be derived by using Equations (17) and (19). The state vector *x*_{p} consists of relative position and velocity of the slave, radius and radial rate as well as the true anomaly and rate of the master, given by

Using Equations (17) and (19), the ten-dimensional nonlinear differential equations are given by

### 3.3. Relative Quaternion Kinematics

In this section a brief review of the relative quaternion kinematics between two rotated coordinate systems is shown. The quaternion is defined by * q*≡ [

*ϱ*^{T}q

^{4}]

^{T}, with

**≡[**

*ϱ**q*

_{1}

*q*

_{2}

*q*

_{3}]

^{T}=

**sin (**

*e**ϕ*/2) and

*q*

_{4}=cos(

*ϕ*/2), where

*is the unit Euler axis and*

**e***ϕ*is the rotation angle (Shuster, Reference Shuster1993). The quaternion

**obeys the unit normalisation constraint given by ||**

*q***||=1. The attitude matrix is related to the quaternion by**

*q*With

where *I*_{3×3} is a 3×3 identity matrix and [* ϱ*×] is a cross product matrix defined by

Successive rotations can be accomplished by using quaternion multiplication. In this paper, quaternion multiplication is defined using the convention of Lefferts et al. (Reference Lefferts, Markley and Shuster1982) in which the quaternion multiplication expression appears in the same order as the corresponding attitude matrix multiplication: ** A**(

**)**

*q′***(**

*A***)=**

*q***(**

*A***⊗**

*q′***). The composition of the quaternions is bilinear, with**

*q*The quaternion kinematics equation is given by

where ** ω** is the three-component angular rate vector and

The relative quaternions *q*_{s/H} and *q*_{m/H}, which map the master's LVLH frame (*H* frame) to the slave body frame (*s* frame) and to the master body frame (*m* frame), are defined as

where *q*_{s}, *q*_{m} and ** q _{H}** are the inertial attitudes of the slave body frame, master body frame and master's LVLH frame, respectively. The relative quaternion kinematics equations are given by (Mayo, Reference Mayo1979)

where *ω*_{s/H}^{s} is the angular velocity of the *s* frame relative to the *H* frame expressed in *s* coordinates, and *ω*_{m/H}^{m} is the angular velocity of the *m* frame relative to the *H* frame expressed in *m* coordinates. They are defined as

where *ω*_{s/I}^{s} and *ω*_{m/I}^{m} are the inertial angular velocities of the slave and master spacecraft, respectively; *A*_{H}^{s} and *A*_{H}^{m} are the attitude matrices that convert vectors from the *H* frame to the *s* frame and to the *m* frame, respectively; and *ω*_{H/I}^{H} is the angular velocity of master's *H* frame expressed in *H* coordinates defined as

A discrete-time propagation of Equation (29) is given by (Mayo, Reference Mayo1979)

With

where

and Δ*t* is the sampling interval.

Likewise, the discrete-time propagation of the relative quaternion *q*_{m/H} is given by

where ${\rm \bar \Omega} ( {{\bi \omega} _{m/I}^m} )$ can be obtained by substituting *ω*_{m/I}^{m} for *ω*_{s/I}^{s} in Equation (34).

The relative attitude quaternion that maps the master body frame to the slave body frame is obtained by

## 4. SENSOR MODELS

In this section the VISNAV measurement model and gyro measurement model are reviewed. The corresponding attitude matrices for the relative quaternions mentioned above are used to construct the LOS observations, and the assumption that both the master body and LVLH frames are the same can be removed.

### 4.1. VISNAV Measurement Model

The VISNAV system consists of an optical sensor combined with specific light sources (beacons), which can be used for close range photogrammetry-type applications (Kim et al., Reference Kim, Crassidis, Cheng, Fosbury and Junkins2007). In general, the beacon locations are known within the master body frame and the sensor focal plane location is known within the slave body frame. Without loss of generality, the assumption that the master body frame coincides with its LVLH frame is not made here. An illustration of the VISNAV system is provided in Figure 1. The measurement model in unit vector form for the *i* ^{th} LOS is given by

with

where ${\bi \tilde b}_i $ denotes the measured unit vector for the *i* ^{th} beacon in the *s* frame, and (*x, y, z*) are the relative position coordinates modelled by Equation (17). ${\bf {\cal X}}'_i \equiv \left( {X'_i \;,Y'_i \;,\;Z'_i} \right)$ are the coordinates of the *i* ^{th} beacon with respect to the master body frame expressed in the *H* frame, given by

where ${\bf {\cal X}}_i \equiv \left( {X_i, \;Y_i, \;Z_i} \right)$ are the known coordinates of the *i* ^{th} beacon with respect to the master body frame expressed in the *m* frame.

The sensor measurement noise *υ*_{i} in Equation (39) is approximately Gaussian which satisfies

and *E*{·} denotes expectation. Equation (43) is known as the QUEST measurement model, which is quite accurate for small field-of-view sensors. According to Shuster (Reference Shuster1990), the measurement covariance matrix in Equation (43) can be replaced by *σ* _{i}^{2}*I*_{3×3}. Multiple vector measurements can be expressed as

where *N* is the total number of observations. The corresponding measurement noise covariance matrix is given by

where “blkdiag” denotes a block diagonal matrix of appropriate dimension.

### 4.2. Gyro Measurement Model

The common gyro measurement model is given by (Farrenkopf, Reference Farrenkopf1978)

where ** ω** is the true inertial angular rate, ${\bi \tilde \omega} $ is the measured inertial angular rate,

**is the drift, and**

*β**η*

_{υ}and

*η*

_{u}are independent zero-mean Gaussian white-noise processes with

where *δ* (*t−τ*) is the Dirac delta function. The discrete-time gyro measurements can be generated using the following recursive equations (Crassidis, Reference Crassidis2005):

where the subscript *k* denotes the *k* ^{th} time-step, Δ*t* is the gyro sampling interval, and *N*_{υk} and *N*_{uk} are zero-mean Gaussian white-noise processes with covariance each given by the identity matrix.

## 5. RELATIVE ATTITUDE AND POSITION ESTIMATION

In this section, the EKF and UKF implementation equations for relative spacecraft attitude and position estimation are shown.

### 5.1. Relative Attitude and Position Estimation Using Extended Kalman Filter

As mentioned in the previous section, the estimation equations are given by

The state and error-state vectors are defined as

where *δ α* is the vector of small attitude (roll, pitch and yaw) errors, and the other error-state terms Δ

*•*are defined as ${\rm \Delta} \bullet \equiv \bullet - \hat \bullet $.

According to the derivations in Zhang et al. (Reference Zhang, Yang, Zhang, Cai and Qian2014), the error-state dynamics equations used in the EKF propagation are given by

where the matrices ** F, G**, process noise vector

**and spectral density matrix**

*w***are given by**

*Q*The complete EKF implementation equations are given in Zhang et al. (Reference Zhang, Yang, Zhang, Cai and Qian2014) and are not shown here for brevity.

### 5.2. Relative Attitude and Position Estimation Using Unscented Kalman Filter

#### 5.2.1. UKF1

In the UKF design, since the predicted quaternion mean is computed by using an averaged sum of quaternions, the resulting quaternion cannot be guaranteed to have the unit-norm constraint. In order to overcome this problem, an unconstrained three-component vector of Generalised Rodrigues Parameters (GRPs) is used to propagate and update a nominal quaternion, which is defined by (Crassidis and Markley, Reference Crassidis and Markley2003)

where *a* is a parameter from 0 to 1, and *f* is a scale factor. Notice that when *a*=0 and *f*=1 then Equation (67) gives the Gibbs vector, and when *a*=1 and *f*=1 then Equation (67) gives the standard vector of Modified Rodrigues Parameters (MRPs). We will choose *f*=2(*a*+1) so that ||*δ p*|| is equal to the rotational error-angle for small errors.

We start by defining the following state vector:

where ${\bi X}_k^{\delta {\bi p}_{s/H}} $ and ${\bi X}_k^{\delta {\bi p}_{m/H}} $ are from the attitude-error parts of relative quaternions *q*_{s/H} and *q*_{m/H}, respectively; ${\bi X}_k^{{\bi \beta} _s} $ and ${\bi X}_k^{{\bi \beta} _m} $ are from the gyro drift parts for the slave and master spacecraft, respectively; ${\bi X}_k^{{\bi x}_p} $ is from the part of ten-dimensional state vector *x*_{p}. In order to describe ${\bi X}_k^{\delta {\bi p}_{s/H}} $ and ${\bi X}_k^{\delta {\bi p}_{m/H}} $, we need to define a new quaternion generated by multiplying an error quaternion by the current estimate. Take ${\bi X}_k^{\delta {\bi p}_{s/H}} $ for instance, the following quaternions are firstly computed:

where $\delta {\bi \hat q}_{s/H_k} ^ + (i) \equiv \left[ {\delta {\bi \varrho} _{s/H_k} ^{ + T} (i)\;\delta q_{4,s/H_k} ^ + (i)} \right]^T $ is represented by

Next, the sigma-point quaternions in Equation (70) are propagated forward using Equation (33), with

in which the estimated inertial angular velocities of the slave spacecraft are given by

and the estimated angular velocities of the master's *H* frame are given by

The propagated error quaternions are then computed using

Notice that $\delta {\bi \hat q}_{s/H_{k + 1}} ^ - (0)$ is the identity quaternion. Finally, the propagated sigma-point GRPs are computed using Equation (67) as

with $\left[ {\delta {\bi \varrho} _{s/H_{k + 1}} ^{ - T} (i)\;\delta q_{4,s/H_{k + 1}} ^ - (i)} \right]^T = \delta {\bi \hat q}_{s/H_{k + 1}} ^ - (i)$. Likewise, the sigma-point GRPs ${\bi X}_k^{\delta {\bi p}_{m/H}} $ can be propagated in the same manner.

In addition, notice that as the gyro drifts are expected to stay at their previous values (due to zero-mean process noises), we have

All other sigma-point quantities formed from *x*_{p}, such as relative position and velocity of the slave, radius and radial rate in addition to the true anomaly and rate of the master, are propagated using Equation (21). The predicted state and covariance are then computed using Equations (6) and (7). The propagated quaternions and relative positions are used to calculate the sigma-point measurements

with

The predicted measurement, innovation covariance matrix and cross-covariance matrix are computed using Equations (11) to (13). Next, the state vector and covariance matrices are updated using Equations (15) and (16), with ${\bi \hat x}_{k + 1}^ + \equiv \left[ {\matrix{ {\delta {\bi \hat p}_{s/H_{k + 1}} ^{ + T}} & {\delta {\bi \hat p}_{m/H_{k + 1}} ^{ + T}} & {{\bi \hat \beta} _{s_{k + 1}} ^{ + T}} & {{\bi \hat \beta} _{m_{k + 1}} ^{ + T}} & {{\bi \hat x}_{\,p_{k + 1}} ^{ + T}} \cr}} \right]^T $. Then, $\delta {\bi \hat p}_{s/H_{k + 1}} ^ + $ and $\delta {\bi \hat p}_{m/H_{k + 1}} ^ + $ are converted to $\delta {\bi \hat q}_{s/H_{k + 1}} ^ + $ and $\delta {\bi \hat q}_{m/H_{k + 1}} ^ + $ using Equations (71) and (72), respectively. The updated quaternions are computed using

Finally, $\delta {\bi \hat p}_{s/H_{k + 1}} ^ + $ and $\delta {\bi \hat p}_{m/H_{k + 1}} ^ + $ are reset to zero for the next propagation.

#### 5.2.2. UKF2

In the previous section, a generalized Rodrigues error-vector is used to represent the quaternion error vector and the updates are performed using quaternion multiplication, which guarantees that quaternion normalization is maintained in the filter. This method has been successfully applied in the unscented quaternion estimator and INS/GPS integrated navigation systems. Recently, Chang et al. (Reference Chang, Hu and Chang2014) pointed out that the better choice for the reference quaternion in Equation (76) was not the sigma-point quaternion at the centre but the mean of the propagated sigma-point quaternions. Actually, the best choice for the reference quaternion is the true quaternion if available. Obviously, it is unavailable, thus, a quaternion estimate closer to the true quaternion is preferred. The sigma-point quaternion at the centre ${\bi \hat q}_{s/H_{k + 1}} ^ - (0)$ is just viewed as the linear propagation of the mean of the state, whereas the mean of the propagated sigma-point quaternions derived by the UT is more accurate than any one of these sigma-point quaternions. Therefore, the mean of the propagated sigma-point quaternions should be used in Equations (76) and (83) instead of ${\bi \hat q}_{s/H_{k + 1}} ^ - (0)$. It is reported that this can further improve the filtering performance when the nonlinearities of the dynamics model are severe or a good *a priori* estimate of the state is unavailable (Chang et al, Reference Chang, Hu and Chang2014). The focus is how to average a set of weighted sigma-point quaternions and maintain the unit-norm property of the quaternion. Cheng et al. (Reference Cheng, Markley, Crassidis and Oshman2007) extended the results in Oshman and Carmi (Reference Oshman and Carmi2006) and derived an algorithm to determine an optimal average norm-preserving quaternion by minimizing the weighted sum of the squared Frobenius norms of the attitude matrix differences. Herein, we adopt this method to modify the previous proposed UKF.

From the intuitive perspective, the mean of the propagated sigma-point quaternions can be computed as

where *W* _{i}^{mean} is the scalar weight for the *i*th sigma-point quaternion which has been described in Equation (8). However, this simple procedure has two drawbacks. One obvious drawback is that the unit-norm property of the resulting quaternion is destroyed by the averaging operation. Another is that the change of the sign of any ${\bi \hat q}_{s/H_{k + 1}} ^ - (i)$ will change the average, whereas it is clear that the quaternions ** q** and

*−*denote the same rotation. To overcome this problem, Cheng et al. (Reference Cheng, Markley, Crassidis and Oshman2007) derived an optimal averaging-quaternion algorithm based on the viewpoint that we really want to average attitudes rather than quaternions. Following this viewpoint, the average norm-preserving quaternion ${\bi \hat q}_{s/H_{k + 1}} ^ - $ can be determined by minimizing the following weighted sum of the squared Frobenius norms of the attitude matrix differences:

**q**
where ${\opf S}^3$ denotes the unit 3-sphere, ||•||_{F} denotes the Frobenius norm, and ** A** (

**) is the attitude matrix corresponding to quaternion**

*q***. According to Cheng et al. (Reference Cheng, Markley, Crassidis and Oshman2007), the determination for the average quaternion ${\bi \hat q}_{s/H_{k + 1}} ^ - $ can be transformed to solve the following maximisation problem:**

*q*where

A more detailed description of the averaging-quaternion algorithm can be found in Cheng et al. (Reference Cheng, Markley, Crassidis and Oshman2007) and is not repeated here for conciseness. It is well known that the solution of Equation (86) is the eigenvector of ** M** corresponding to the maximum eigenvalue. The eigenvector is chosen to hold unit norm to avoid the first drawback. The second drawback can also be avoided because changing the sign of any ${\bi \hat q}_{s/H_{k + 1}} ^ - (i)$ does not change the value of

**. Thus, the average norm-preserving quaternion ${\bi \hat q}_{s/H_{k + 1}} ^ - $ solved by Equation (86) is used to substitute for ${\bi \hat q}_{s/H_{k + 1}} ^ - (0)$ in Equations (76) and (83). Similarly, the average norm-preserving quaternion ${\bi \hat q}_{m/H_{k + 1}} ^ - $ is treated in the same manner. This is the modification version of the previous proposed UKF.**

*M*## 6. SIMULATION RESULTS

In this section, simulations are performed to estimate relative quaternions, relative position, and velocity between two spacecraft, as well as the gyro biases. A summary of the true values of simulation parameters is given in Table 1, and six beacon locations are shown in Table 2. The performances of the EKF and two versions of the UKF with respect to initial condition errors are compared. The transformation parameters for the GRPs are given by *a*=1 and *f*=4. The parameters used in the UKF are given by *α*=0·005, *β*=2 and *κ*=3*−n*, where *n*=22. The entire simulation time is 300 minutes. For a fair comparison, the simulation results are based on 50 Monte Carlo runs.

In Kim et al. (Reference Kim, Crassidis, Cheng, Fosbury and Junkins2007) and Zhang et al. (Reference Zhang, Yang, Zhang, Cai and Qian2014), all initial variables are set to their true values, except for the gyro biases, which are set to zero. In this scenario, the attitude errors of [10°*−*10° 5°]^{T} and [*−*10° 10° 5°]^{T} are added into initial estimates of *q*_{s/H} and *q*_{m/H} for each axis, respectively. The initial gyro bias estimates for the slave and master spacecraft are set to zero. The 1*σ* errors are added into the initial relative position and velocity, radius and radial rate as well as the true anomaly and rate. The initial covariance matrix is diagonal. The three attitude error parts of the initial covariance for *q*_{s/H} and *q*_{m/H} are each set to one standard deviation error of 10°, i.e., [10×(*π*/180)]^{2} rad^{2}. The three gyro-bias parts of the initial covariance for the slave and master spacecraft are each set to 2° per hour, i.e., $\left[ {2 \times \left( {{\pi / {(180 \times 3600)}}} \right)} \right]^2 \left( {{\rm rad/s}} \right)^2 $. The relative position and velocity parts of the initial covariance are each set to 5 m^{2} and 0·02 (m/s)^{2}, respectively. The master's radius and radial rate parts of the initial covariance are each set to 1000 m^{2} and 0·01(m/s)^{2}, respectively. The master's true anomaly and rate parts of the initial covariance are set to 1×10^{−4}rad^{2} and 1×10^{−4}(rad/s)^{2}, respectively.

The performances of the EKF and two versions of the UKF are compared in Figures 2–9. Figures 2 and 3 show the roll, pitch and yaw errors for *q*_{s/H} and *q*_{m/H}, respectively. As can be seen, two versions of the UKF attitude estimation errors do converge to within their respective 3*σ* bounds within 300 minutes, whereas the EKF attitude estimation errors do not converge to within their respective 3*σ* bounds. Figures 4 and 5 show the norms of relative position and velocity errors, respectively. It is seen that the EKF does not produce acceptable performance, which indicates the first-order approximation cannot capture the initial condition errors. However, the designed UKF is working properly. The norms of relative position and velocity estimated errors are within 0·03 m and 3×10^{−5} m/s. From the partial enlargement drawings, we can see that the UKF2 is a little superior than the UKF1, which agrees well with the previous analysis. Figures 6 and 7 show the norms of slave and master gyro bias errors, respectively. It can also be seen that the UKF2 performs a bit better than the UKF1. A plot of the norm of relative attitude errors is shown in Figure 8. The relative attitude between the slave and master spacecraft is estimated by using Equation (38). As shown, both UKF1 and UKF2 can reach the norm of relative attitude errors of less than 0·05° within 300 minutes, whereas the EKF does not converge. Figure 9 shows the master orbital element errors of the UKF1 and UKF2, and the results of the EKF are not shown because it fails to converge.

## 7. CONCLUSIONS

In this paper, a novel navigation approach based on an Unscented Kalman Filter has been derived to estimate the relative attitude and position between slave and master spacecraft. Our approach is to additionally estimate the attitudes of both spacecraft relative to the master's LVLH frame, so that the simplified assumption that the master's body frame coincides with its LVLH frame is removed. The general relative equations of motion for eccentric orbits are used to describe the positional dynamics. In the UKF design, an unconstrained three-component vector of Generalized Rodrigues Parameters is employed to maintain the quaternion normalization constraint. Besides the traditional UKF, a modified version of the UKF is also presented. An averaging-quaternion algorithm is employed to average a set of weighted propagated sigma-point quaternions, and then the mean of these sigma-point quaternions is used for the reference quaternion instead of the sigma-point quaternion at the centre. The performances of the EKF and two versions of the UKF with respect to initial condition errors are compared. Simulation results indicate that the proposed filters can provide more accurate estimates for relative attitude and position than the Extended Kalman Filter. The modified UKF performs a little better than the traditional UKF.

## ACKNOWLEDGMENT

This work was supported by Innovation Fund of Graduate Program of National University of Defense Technology (B130105) and Scientific Research Fund of Graduate Program of Hunan Province (CX2013B004).