1 Introduction

In recent years,it has witnessed a tremendous research interest in the spacecraft missions that consist of multiple space vehicles,such as spacecraft formation flying,autonomous rendezvous and proximity operations^[1-3]. A successful implementation of these multiple-spacecraft missions is highly dependent on the ability to determine the relative motion between the spacecraft. Typically,to estimate the relative spacecraft motion,it requires both high-fidelity dynamical models and specialized measurement methods.

The relative motion between two spacecraft is a six-degree-of-freedom (6-DOF) motion,which is composed of relative rotational motion,relative translational motion and the coupling thereof^[4]. A growing interest has been paid for the modeling of 6-DOF spacecraft relative motion in recent years. For example,Pan and Kapila^[5] addressed the coupled translational and rotational dynamics for the leader-follower spacecraft formation; Kristiansen^[6] derived the Lagrange-like 6-DOF model by combining the relative translational dynamics and the rotational one. It is worth pointing out that the translational parts in the aforementioned models only account for the relative position between the centers of the mass (c.m.) of two spacecraft,and the rotation-translation coupling in this case is just induced by external torques. However,in general missions,the relative translational dynamics of arbitrary points on the spacecraft may be concerned. Whenever one of these points does not coincide with the spacecraft’s c.m,a kinematic coupling,which is essentially a projection of the rotational motion about the c.m. onto the relative translational configuration space,is obtained^[4]. It is this kinematic coupling that the current paper is concerned with. As shown in the previous work,dual quaternion has been an effective tool for motion description and dynamics modeling^[7-13].Among these works,Wang^[13] was the first to use dual quaternion for spacecraft motion modeling.To go further step,this paper employs dual quaternion to model the relative motion between arbitrary points of spacecraft,and derives the kinematically-coupled relative dynamics.

Estimating the relative motion also involves the problem of sensing the relative information between spacecraft. In the past years,the single vision-based method has been used widely for the pose estimation problem ^[14-15]. Primary researches have focused on using two-dimensional/three-dimensional (2D/3D) point correspondences^[16-19]. Among these literatures,Kim ^[18] and Xing ^[19] employed the line-of-sight (LOS) measurements and Extended Kalman Filter (EKF) to estimate the relative position and attitude for spacecraft. However,the point-based methods are often limited to the difficulty of point matching and high sensitivity to noise. In view of these issues,several researchers have investigated the pose estimation problem based on higher-level geometric features such as lines and curves. For instance,using a set of 2D/3D line correspondences,the direct solutions^[20-21],and the iterative method ^[22-23] have been proposed to estimate the external camera parameters. From a pair of conics in both 2D and 3D,Song^[24] introduced an iterative technique for the pose estimation problem. However,to the best knowledge of the authors,these methods have been seldom used for spacecraft relative motion estimation problem. In addition,nearly all the existing solutions concentrate on one specific type of correspondences,that is,they either use points or lines or conics but not a combination of features. In order to improve the robustness and accuracy for estimating the relative motion,this paper describes the feature points,lines and circles in dual number algebra,and establishes an integrated observation model based on multiple geometric features.

2 Kinematic-Coupled Dynamics Model Using Dual Number

Throughout the development of the dynamics model,some standard reference frames are to be used (see Fig. 1): Ψ_I,the standard Earth-centered,inertial,Cartesian right-hand reference frame; Ψ_l,a local-vertical,local-horizontal reference frame fixed to the c.m. of leader spacecraft,with its x_l axis directing from the spacecraft radially outward,z_l axis normal to the leader orbital plane,and y_l axis set up by the right-hand rule; Ψ_lb (resp. Ψ_fb),the body-fixed reference frame of the leader spacecraft (resp. the follower spacecraft),with its origin in the center of mass of the spacecraft,and axes pointing along the principal axes of inertia. In the following discussion,it is assumed that the leader orbital frame Ψ_l is controlled aligned with the body-fixed frameΨ_lb.

As shown in Fig. 1(b),there exists an arbitrary feature point P_l located on the leader spacecraft. Let p_l^l be the position vector directed from the point P_l to the c.m. of the leader spacecraft. By translating the coordinate system Ψ_lb along the opposite direction of p_l^l ,one can obtain a new coordinate system Ψ_lP with its origin being the point P_l,and axes parallel to those of Ψ_lb. In the same way,a new coordinate system Ψ_fP for the follower spacectaft is also derived.

In the remainder of this paper,the following notations are employed: ${{\dot{x}}^{f}}$(resp.${{\dot{x}}^{l}}$,${{\dot{x}}^{fp}}$,${{\dot{x}}^{lp}}$) denotes the time derivative of x in Ψ_fb (resp. Ψ_lb,Ψ_fP,Ψ_lP); x^f (resp. x^l,x^fP,x^lP) denotes the components of x expressed in Ψ_fb (resp. Ψ_lb,Ψ_fP,Ψ_lP) ; ω_f^f (resp. ω_l^l, ω_fP^fP,ω_lP^lP) denotes the angular velocity of Ψ_fb (resp. Ψ_lb,Ψ_fP,Ψ_lP) relative to Ψ_I; ω_fl,P^fP denotes the angular velocity of Ψ_fP relative to Ψ_lP,expressed in Ψ_fP. For the mathematical preliminaries including definitions of dual numbers and dual quaternions,one can refer to Refs. ^[8] and ^[13].

By definingthe operations ${{\hat{P}}_{l}}$=1+ε[p_l^l×] and ${{\hat{P}}_{f}}$f=1+εp_f^f×, the velocity motor of Ψ_lP (resp.Ψ_fP) can be calculated from that of Ψ_lb(resp. Ψ_fb) using the following equations:

$\left\{ \begin{align} & \hat{\omega }_{lP}^{lP}=\hat{\omega }_{lP}^{l}={{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \\ & \hat{\omega }_{fP}^{fP}=\hat{\omega }_{fP}^{f}={{{\hat{P}}}_{f}}\hat{\omega }_{f}^{f} \\ \end{align} \right.$

(1)

where $\left[ {{\Lambda }^{\times }} \right]=\left[ \begin{matrix} 0 & -{{\Lambda }_{z}} & {{\Lambda }_{y}} \\ {{\Lambda }_{z}} & 0 & -{{\Lambda }_{x}} \\ -{{\Lambda }_{y}} & {{\Lambda }_{x}} & 0 \\ \end{matrix} \right]$ represents a skew-symmetric matrix of the vector $\Lambda ={{\left[ \begin{matrix} {{\Lambda }_{x}} & {{\Lambda }_{y}} & {{\Lambda }_{z}} \\ \end{matrix} \right]}^{\text{T}}};\hat{\omega }_{l}^{l}$ is the velocity motor of Ψ_lb relative to Ψ_I,which is given by $\hat{\omega }_{l}^{l}=\omega _{l}^{l}+\varepsilon \left( \omega _{l}^{l}\times r_{l}^{l} \right)$ with ω_l^l and r_l^l being the angular velocity and position vector of Ψ_lb,respectively; $\hat{\omega }_{f}^{f}=\omega _{f}^{f}+\varepsilon \left( \omega _{f}^{f}\times r_{f}^{f} \right)$ is defined in a similar way to denote the velocity motor of Ψ_fb relative to Ψ_I,with ω_f^f and r_f^f being the angular velocity and position vector of Ψ_fb,respectively; $\hat{\omega }_{lP}^{lP}$ is the velocity motor of Ψ_lP relative to Ψ_I,the definition of which is given by $\hat{\omega }_{lP}^{lP}=\omega _{lP}^{lP}+\varepsilon \left[ \omega _{lP}^{lP}\times \left( r_{l}^{l}-p_{l}^{l} \right) \right];\hat{\omega }_{fP}^{fP}$ is the velocity motor of Ψ_fP with $\hat{\omega }_{fP}^{fP}={{\omega }^{fP}}_{fP}+\varepsilon \left[ \omega _{fP}^{fP}\times \left( r_{f}^{f}-p_{f}^{f} \right) \right]$. It is noted that the properties ω_lP^lP=ω_l^l and ω_fP^fP=ω_f^f are used in the calculation of Eq. (1). Employing ${{\hat{q}}_{fl,P}}$ to describe the motion of Ψ_fP relative to Ψ_lP,the relative kinematics can be written as

$2{{\dot{\hat{q}}}_{fl,P}}={{\hat{q}}_{fl,P}}\cdot {{\hat{\omega }}_{fPfl,P}}$

(2)

where ${{\hat{q}}_{fl,P}}={{q}_{fl,P}}+\varepsilon \frac{1}{2}{{q}_{fl,P}}\cdot r_{fl,P}^{fP}$ with q_fl,P being the relative attitude quaternion and r_fl,P^fP being the position vector from P_l to P_f;$\hat{\omega }_{fl,P}^{fP}$ denotes the velocity motor of Ψ_fP relative to Ψ_lP,which is defined by $\hat{\omega }_{fl,P}^{fP}=\omega _{fl,P}^{fP}+\varepsilon \left( \dot{r}_{fl,P}^{fP}+\omega _{fl,P}^{fP}\times {{r}_{fPfl,P}} \right)$ with ω_fl,P^fP being the relative angular velocity. Throughout this paper,the following appointment is made: a vector is treated as a quaternion with null scalar part when calculated with quaternions,and a dual vector is regarded as a dual quaternion with null dual scalar part when calculated with dual quaternions.

In terms of Eq. (1),$\hat{\omega }_{fl,P}^{fP}$ can be also expressed as

$\hat{\omega }_{fl,P}^{fP}={{\hat{P}}_{f}}\hat{\omega }_{f}^{f}-\hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)\cdot {{\hat{q}}_{fl,P}}$

(3)

where $\hat{q}_{fl,P}^{*}=q_{fl,P}^{*}+\varepsilon {{{q}'}^{*}}$ is the conjugate of ${{\hat{q}}_{fl,P}}$. By differentiating Eq. (3) and according to the dynamics of a single spacecraft in Ref. ^[13],one can obtain

$\begin{align} & \dot{\hat{\omega }}_{fl,P}^{fP}={{{\hat{P}}}_{f}}\dot{\hat{\omega }}_{f}^{f}-\hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\dot{\hat{\omega }}_{l}^{l} \right)\cdot {{{\hat{q}}}_{fl,P}}- \\ & \hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)\cdot \hat{q}_{fl,P}^{\cdot }- \\ & \dot{\hat{q}}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)\cdot {{{\hat{q}}}_{fl,P}} \\ & ={{{\hat{P}}}_{f}}\dot{\hat{\omega }}_{f}^{f}-\hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\dot{\hat{\omega }}_{l}^{l} \right)\cdot {{{\hat{q}}}_{fl,P}}- \\ & \hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\hat{\omega }_{\text{l}}^{\text{l}} \right)\cdot \frac{1}{2}{{{\hat{q}}}_{fl,P}}\cdot \hat{\omega }_{fl,P}^{f}+ \\ & \frac{1}{2}\hat{\omega }_{fl,P}^{\text{f}}\cdot \hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)\cdot {{{\hat{q}}}_{fl,P}} \\ & ={{{\hat{P}}}_{f}}\left( -\hat{M}_{f}^{-1}\left( \hat{\omega }_{f}^{f}\times {{{\hat{M}}}_{f}}\hat{\omega }_{f}^{f} \right)+\hat{M}_{f}^{-1}\hat{F}_{f}^{f} \right)- \\ & \hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\dot{\hat{\omega }}_{l}^{l} \right)\cdot {{{\hat{q}}}_{fl,P}}+\hat{\omega }_{fl,P}^{f}\times \\ & \left( \hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)\cdot {{{\hat{q}}}_{fl,P}} \right) \\ \end{align}$

where the dual inertia matrix of the follower spacecraft is given by ${{\hat{M}}_{f}}={{m}_{f}}\frac{\text{d}}{\text{d}\varepsilon }I+\varepsilon {{J}_{f}}$ with m_f being the mass and J_f being the inertia matrix,the inverse of ${{\hat{M}}_{f}}$ is defined as $\hat{M}_{f}^{-1}=J_{f}^{-1}\frac{\text{d}}{\text{d}\varepsilon }+\varepsilon \frac{1}{{{m}_{\text{f}}}}I;\hat{F}_{f}^{f}=f_{f}^{f}+\tau _{f}^{f}$ with f_f^f and τ_f^f being the force vector and the torque of the follower,respectively; $\hat{\omega }_{f}^{f}$ can be obtained by $\hat{\omega }_{f}^{f}=\hat{P}_{f}^{*}\left( \hat{\omega }_{fl,P}^{fP}+q_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)\cdot {{{\hat{q}}}_{fl,P}} \right)$ with $\hat{P}_{f}^{*}=1-\varepsilon \left[ p_{f}^{f\times } \right]$.

3 Observation Models Using Different Geometric Feature Correspondences

Before developing the observation models in this section,two reference frames are defined. The camera reference frame Ψ_c,a 3D Cartesian right-hand coordinate system with its origin is located at the lens center of the camera,x_c axis along the horizontal direction of the image,y_c axis along the vertical direction of the image,and z_c axis parallel to the optical axis of the camera. The image reference frame Ψ_i,a 2D coordinate system,contained in the image plane,and is parallel to the x_c and y_c axes of Ψ_c. Without loss of generality,it is assumed that Ψ_c is aligned with Ψ_fP which is defined in the previous section.

3.1 Observation Model Based on Feature Points

In this subsection,the pinhole lens model is used as the perspective projection model,and the process of a 3-D point to its 2-D projection is briefly summarized as follows.

Let D be a 3D point fixed on the leader spacecraft. The first step is to transform the point D to the camera reference frame. In the dual quaternion algebra,this transformation can be expressed as

${{\hat{D}}^{fP}}=\hat{q}_{fl,P\varepsilon }^{*}{{\hat{D}}^{lP}}{{\hat{q}}_{fl,P}}$

where ${{\hat{q}}_{fl,P}}$ represents the relative motion from Ψ_lP to Ψ_fP, $\hat{q}_{fl,P\varepsilon }^{*}-q_{fl,P}^{*}-\varepsilon {{{q}'}^{*}}_{fl,P}$ is another definition of the conjugate of ${{\hat{q}}_{fl,P}};{{\hat{D}}^{fP}}=1+\varepsilon {{d}^{fP}}$ and ${{\hat{D}}^{lP}}=1+\varepsilon {{d}^{lP}}$ are the representations of the point D in dual quaternion algebra,with d^fP and d^lP being the coordinates relative to Ψ_fP and Ψ_lP,respectively.

Next step is to project the transformed points onto the image plane. The result is the x and y coordinates of the projected points. Given the camera coordinates ${{d}^{fP}}={{\left[ \begin{matrix} {{d}_{fpx}} & {{d}_{fpy}} & {{d}_{fpz}} \\ \end{matrix} \right]}^{\text{T}}}$,the image coordinates are calculated by

${{x}_{id}}=\lambda \frac{{{d}_{fpx}}}{{{d}_{fpz}}},{{y}_{id}}=\lambda \frac{{{d}_{fpy}}}{{{d}_{fpz}}}$

(4)

where λ is the focal length of the camera.

3.2 Observation Model Based on Feature Lines

In this section,the given model features from the leader spacecraft are lines. The projection of a 3D line onto the 2D image is shown in Fig. 2. In the dual quaternion algebra,the expression of the feature line L relative to Ψ_lP is represented by

${{\hat{L}}^{lP}}={{l}^{lP}}+\varepsilon {{m}^{lP}}$

where l^lP denotes the unit direction vector and m^lP is the moment with respect to the origin of Ψ_lP. Using dual quaternion,one can calculate the expression of $\hat{L}$ in Ψ_fP as follows:

${{\hat{L}}^{fP}}={{l}^{fP}}+\varepsilon {{m}^{fP}}=\hat{q}_{fl,p}^{*}\cdot {{\hat{L}}^{lP}}\cdot {{\hat{q}}_{fl,p}}$

(5)

Perspective projection is then applied to the transformed lines. In terms of projection geometry,an equation of the projected line is obtained:

${{m}_{x}}{{x}_{i}}+{{m}_{y}}{{y}_{i}}={{m}_{z}}\lambda $

(6)

where x_i and y_i are the image plane coordinates. From Eq. (9),one can calculate the direction vector of the image line by

${{l}_{i}}={{\left[ \begin{matrix} {{l}_{ix}} & {{l}_{iy}} & {{l}_{iz}} \\ \end{matrix} \right]}^{\text{T}}}={{\left[ \begin{matrix} \frac{-{{m}_{y}}}{\sqrt{m_{x}^{2}+m_{y}^{2}}} & \frac{{{m}_{x}}}{\sqrt{m_{x}^{2}+m_{y}^{2}}} & 0 \\ \end{matrix} \right]}^{\text{T}}}$

(7)

Since the momentum of the image line m_i defines the same plane as m,one may have

${{m}_{i}}={{\left[ \begin{matrix} {{m}_{ix}} & {{m}_{iy}} & {{m}_{iz}} \\ \end{matrix} \right]}^{\text{T}}}={{d}_{pc}}\frac{m}{\left\| m \right\|}=\frac{\lambda }{\sqrt{m_{x}^{2}+m_{y}^{2}}}m$

where d_pc is the distance from the image line to the perspective center.

A format is needed to compare these transformed lines with lines measured from the acquired images. In Fig. 2(b),the line point^[23],which is defined as the intersection of the line feature with a line passing through the image origin that is perpendicular to the line feature,is employed. From the image line,the coordinates of the line point are calculated as

${{x}_{il}}={{l}_{iy}}{{m}_{iz}},{{y}_{il}}=-{{l}_{ix}}{{m}_{iz}}$

(8)

In terms of Eqs. (7) and (8),the line point can be also derived from the 3D transformed line by the equation:

${{x}_{il}}=\lambda \frac{{{m}_{x}}{{m}_{z}}}{m_{x}^{2}+m_{y}^{2}},{{y}_{il}}=\lambda \frac{{{m}_{y}}{{m}_{z}}}{m_{x}^{2}+m_{y}^{2}}$

(9)

Therefore,the relations between the line point from the acquired image and the 3D line on the leader spacecraft are established by Eqs. (5) and (9),and these equations can be used as the observation model for the estimation problem.

3.3 Observation Model Based on Feature Circles

Besides the point and line features,circle features are also common tobe found on a spacecraft. However,in the dual quaternion algebra,circles cannot be represented and transformed directly like the way of the points and lines. For the aim of dealing with all the three types of features in a unified algebra,this section proposes an operation definition for circles. With this definition,the equations which related the 3D circles and 2D conics are formalized accordingly.

As shown in Fig. 3,a circle can be regarded as a twist L_t modeling a general rotation and a starting point S₀ on the circle. According to the definition of screw motion,this twist transformation can be understood as a screw motion with the screw pitch being zero.Given the generating twist parameters,the twist transformation corresponds to the dual quaternion:

${{\hat{q}}_{\phi }}=\left[ \cos \frac{\phi }{2},{{{\hat{L}}}_{t}}\sin \frac{\phi }{2} \right]$

where $\hat{\phi }=\phi +\varepsilon 0$ with $\phi $ being the rotation angle; ${{\hat{L}}_{t}}={{l}_{t}}+\varepsilon {{m}_{t}}$ denotes the generating twist with l_t being the direction vector and m_t being the moment with respect to S₀. By this dual quaternion,an arbitrary point on the circle can be simply obtained from the starting point by

$\hat{S}_{\phi }^{lP}={{\hat{q}}_{\phi }}\hat{S}_{0}^{lP}\hat{q}_{\phi ,\varepsilon }^{*}$

(10)

where $\hat{S}_{0}^{lP}=1+\varepsilon s_{0}^{lP}$ and $\hat{S}_{\phi }^{lP}=1+\varepsilon s_{\phi }^{lP}$ denote the expressions of the starting point S₀ and the result point S_φ in Ψ_lP,respectively. $\hat{S}_{\phi }^{lP}$ is then transformed to the camera coordinates using the following equation:

$\hat{S}_{\phi }^{fP}=1+\varepsilon S_{\phi }^{fP}=q_{fl,P\varepsilon }^{*}\hat{S}_{\phi }^{lP}{{\hat{q}}_{fl,P}}$

In terms of Eq. (4),the image coordinates of the point S_φ are calculated by

${{x}_{is}}=\lambda \frac{{{s}_{\phi fPx}}}{{{S}_{\phi fPz}}},{{y}_{is}}=\lambda \frac{{{s}_{\phi fPy}}}{{{S}_{\phi fPz}}}$

(11)

where s_φfPx,s_φfPy and s_φfPz are the components of the s_φ^fP.

Taking the image coordinatesx_is,y_is as the measurement variables,the observation model based on the circle features is now set up with the above equations.It should be noted that ${{\hat{q}}_{fl,p}}$ are the only unknowns to be estimated in the measurement modelsof thefeature points and lines. While for the case of feature circles,the computing loads become higher due to the parameterization of the circle itself. For the measurement equations,the angle φ is the additional unknown parameter.Therefore,the relative estimation problem using circle features means to estimate both the dual quaternion ${{\hat{q}}_{fl,p}}$ and the angle φ.

4 Relative Motion Estimation Based on Multiple Features 4.1 Process Model

The state vector of the system is selected as

$X={{\left[ \begin{matrix} {{{\hat{q}}}_{fl,P}} & \hat{\omega }_{fl,P}^{fP} & \Phi \\ \end{matrix} \right]}^{\text{T}}}$

where Φ=[φ₁…φ_3n] are the unknown parameters of the observed n feature circles. Based on the kinematics and dynamics proposed in Section 2,one may have the following continuous-time process model:

$\dot{X}\left( t \right)=f\left( X\left( t \right),t \right)+w\left( t \right)$

where f(X(t),t) is a non-linear function given by

$\begin{align} & f\left( X\left( t \right),t \right)={{\left[ \begin{matrix} {{{\dot{\hat{q}}}}_{fl,P}} & \dot{\hat{\omega }}_{fl,P}^{fP} & {\dot{\Phi }} \\ \end{matrix} \right]}^{\text{T}}}= \\ & \left[ \begin{matrix} \frac{1}{2}{{{\hat{q}}}_{fl,P}}\cdot \hat{\omega }_{fl,P}^{fP} \\ \begin{matrix} {{{\hat{P}}}_{f}}\left( -\hat{M}_{f}^{-1}\left( \hat{\omega }_{f}^{f}\times {{{\hat{M}}}_{f}}\hat{\omega }_{f}^{f} \right)+\hat{M}_{f}^{-1}\hat{F}_{f}^{f} \right)- \\ \hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\dot{\hat{\omega }}_{l}^{l} \right)\cdot {{{\hat{q}}}_{fl,P}}+\hat{\omega }_{fl,P}^{fP}\times \left( \hat{q}_{fl,P}^{*}\cdot \left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)\cdot {{{\hat{q}}}_{fl,P}} \right) \\ \end{matrix} \\ {{0}_{3n\times 1}} \\ \end{matrix} \right] \\ \end{align}$

where w(t) is a white Gaussian process noise with power spectral density matrix Q.

Linearizing and discretizing of the process model yields

${{X}_{k}}={{F}_{k,k-1}}{{X}_{k-1}}+{{W}_{k-1}}$

where F_k,k－1=I+C_k－1T with T being the sample time,and

${{\left. {{C}_{k-1}}=\frac{\partial \text{f}}{\partial {{X}^{\text{T}}}} \right|}_{X={{X}_{k-1}}}}=\left[ \begin{matrix} \frac{\partial {{{\dot{\hat{q}}}}_{fl,P}}}{\partial {{{\hat{q}}}^{\text{T}}}_{fl,P}} & \frac{\partial {{{\dot{\hat{q}}}}_{fl,P}}}{\partial \hat{\omega }_{fl,P}^{fPT}} & {{0}_{8\times 3n}} \\ \frac{\partial {{{\dot{\hat{\omega }}}}_{fl,P}}}{\partial {{{\hat{q}}}^{\text{T}}}_{fl,P}} & \frac{\partial {{{\dot{\hat{\omega }}}}_{fl,P}}}{\partial {{{\hat{\omega }}}^{fPT}}_{fl,P}} & {{0}_{6\times 3n}} \\ {{0}_{3n\times 8}} & {{0}_{3n\times 6}} & {{0}_{3n\times 3n}} \\ \end{matrix} \right]$

with the expressions given as follows:

$\begin{align} & \frac{\partial {{{\dot{\hat{q}}}}_{fl,P}}}{\partial {{{\hat{q}}}^{\text{T}}}_{fl,P}}=\frac{1}{2}\left[ \hat{\omega }_{fl,P}^{fP-} \right],\frac{\partial {{{\dot{\hat{q}}}}_{fl,P}}}{\partial \hat{\omega }_{fl,P}^{fPT}}=\frac{1}{2}\left[ \hat{q}_{fl,P}^{+} \right] \\ & \frac{\partial {{{\dot{\hat{\omega }}}}_{fl,P}}}{\partial {{{\hat{q}}}^{\text{T}}}_{fl,P}}=-\left( {{\left[ \hat{q}_{fl,P}^{+} \right]}^{\text{T}}}\left[ {{\left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)}^{+}} \right]+ \right. \\ & \left. \left[ \hat{q}_{fl,P}^{-} \right]\left[ {{\left( {{{\hat{P}}}_{l}}\dot{\hat{\omega }}_{l}^{l} \right)}^{-}} \right]{{E}_{8}} \right)+ \\ & \left[ \hat{\omega }_{fl,P}^{fP\times } \right]\left( {{\left[ \hat{q}_{fl,P}^{+} \right]}^{\text{T}}}\left[ {{\left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)}^{+}} \right] \right.+ \\ & \left. \left[ \hat{q}_{fl,P}^{-} \right]\left[ {{\left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)}^{-}} \right]{{E}_{8}} \right)+ \\ & {{{\hat{P}}}_{f}}\left( \hat{M}_{f}^{-1}\left( \left( \left[ {{\left( {{{\hat{M}}}_{f}}\hat{\omega }_{f}^{f} \right)}^{\times }} \right] \right. \right. \right.- \\ & \left. \left. \left. \left[ \hat{\omega }_{f}^{f\times } \right]{{{\hat{M}}}_{f}} \right)\frac{\partial \hat{\omega }_{f}^{f}}{\partial \hat{q}_{fl,P}^{\text{T}}}+\frac{\partial \hat{F}_{f}^{f}}{\partial \hat{q}_{fl,P}^{\text{T}}} \right) \right) \\ & \frac{\partial {{{\dot{\hat{\omega }}}}_{fl,P}}}{\partial {{{\hat{\omega }}}^{fPT}}_{fl,P}}={{{\hat{P}}}_{f}}\left( \hat{M}_{f}^{-1}\left( \left[ {{\left( {{{\hat{M}}}_{f}}\hat{\omega }_{f}^{f} \right)}^{\times }} \right]-\left[ \hat{\omega }_{f}^{f\times } \right]{{{\hat{M}}}_{f}} \right) \right)- \\ & \left[ {{\left( \hat{q}_{fl,P}^{*}\cdot {{{\hat{P}}}_{f}}\hat{\omega }_{l}^{l}\cdot {{{\hat{q}}}_{fl,P}} \right)}^{\times }} \right]+{{{\hat{P}}}_{f}}\left( \hat{M}_{f}^{-1}\frac{\partial \hat{F}_{f}^{f}}{\partial \hat{\omega }_{fl,P}^{fPT}} \right) \\ \end{align}$

where

$\frac{\partial \hat{\omega }_{f}^{f}}{\partial {{{\hat{q}}}^{\text{T}}}_{fl,P}}=\hat{P}_{f}^{*}\left( {{\left[ \hat{q}_{fl,P}^{+} \right]}^{\text{T}}}\left[ {{\left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)}^{+}} \right]+\left[ \hat{q}_{fl,P}^{-} \right]\left[ {{\left( {{{\hat{P}}}_{l}}\hat{\omega }_{l}^{l} \right)}^{-}} \right]{{E}_{8}} \right)$

considering the gravitation force $f_{f}^{f}=\frac{-{{\mu }_{\oplus }}{{m}_{f}}}{{{\left\| r_{f}^{f} \right\|}^{3}}}r_{f}^{f}$,one can obtain that $\frac{\partial \hat{F}_{f}^{f}}{\partial \hat{q}_{fl,P}^{\text{T}}}=\frac{\partial \hat{F}_{f}^{f}}{\partial r_{f}^{f\text{T}}}\frac{\partial r_{f}^{f}}{\partial \hat{q}_{fl,P}^{\text{T}}}=\left[ \frac{3{{\mu }_{\oplus }}{{m}_{f}}}{{{\left\| r_{f}^{f} \right\|}^{5}}}r_{f}^{f}r_{f}^{f\text{T}}-\frac{{{\mu }_{\oplus }}{{m}_{f}}}{{{\left\| r_{f}^{f} \right\|}^{3}}}{{I}_{3}};{{0}_{3\times 3}} \right]\frac{\partial r_{f}^{f}}{\partial \hat{q}_{fl,P}^{\text{T}}}$ and $\frac{\partial \hat{F}_{f}^{f}}{\partial \hat{\omega }_{fl,P}^{fPT}}={{0}_{6\times 6}}$,with $r_{f}^{f}=r_{fl,P}^{fP}+q_{fl,P}^{*}\cdot \left( r_{l}^{l}-p_{l}^{l} \right)\cdot {{q}_{fl,P}}+p_{f}^{f}$ and ${{\partial r_f^f} \over {\partial q_{fl,P}^{\rm{T}}}} = 2\left[ {q\rm{'}_{fl,P}^ - } \right]{E_4} + {\left[ {q_{fl.P}^ + } \right]^{\rm{T}}}\left[ {{{\left( {r_l^l - p_l^l} \right)}^ + }} \right] + \left[ {q_{fl,P}^ - } \right]\left[ {{{\left( {r_l^l - p_l^l} \right)}^ - }} \right]{E_4},$${{\partial r_f^f} \over {\partial q\rm{'}_{fl,P}^{\rm{T}}}} = 2{\left[ {q_{fl,P}^ + } \right]^{\rm{T}}};\left[ {{{\hat \Lambda }^ \times }} \right] = \left[ {\matrix{ {\left[ {{\Lambda ^ \times }} \right]} & {{0_{3 \times 3}}} \cr {\left[ {\Lambda ' \times } \right]} & {\left[ {{\Lambda ^ \times }} \right]} \cr } } \right]$ is defined for $\hat{\Lambda }=\Lambda +\varepsilon {\Lambda }';{{E}_{4}}=\left[ \begin{matrix} 1 & {{0}_{1\times 3}} \\ {{0}_{3\times 1}} & -{{I}_{3}} \\ \end{matrix} \right],{{E}_{8}}=\left[ \begin{matrix} {{E}_{4}} & {{0}_{4\times 4}} \\ {{0}_{4\times 4}} & {{E}_{4}} \\ \end{matrix} \right]$.

4.2 Integrated Observation Model

It is supposed that there are g feature points,m feature lines and n feature circles can be observed,the measurement variable can be denoted as

Z=[x_id1 y_id1…x_idg y_idg x_il1 y_il1…x_ilm y_ilm x_is1 y_is1…x_isn y_isn]^T

and the nonlinear measurement function is given by

$Z\left( t \right)=h\left( X\left( t \right),t \right)+v\left( t \right)$

(12)

where h(X(t),t) can be obtained from Eqs. (4),(9) and (11),v(t) is the noise of the measurement which is assumed to be white Gaussian noise with power spectral density matrix R. After linearizing and discretizing Eq. (12),one can obtain

${{Z}_{k}}={{H}_{k}}{{X}_{k}}+{{V}_{k}}$

where H_k is the observation matrix,which can be derived from the partial derivative of h(X(t),t):

${{\left. {{H}_{k}}=\frac{\partial h}{\partial {{X}^{\text{T}}}} \right|}_{X={{X}_{k}}}}={{\left[ \frac{\partial {{x}_{id1}}}{\partial {{X}^{\text{T}}}}\frac{\partial {{y}_{id1}}}{\partial {{X}^{\text{T}}}}\cdots \frac{\partial {{x}_{idg}}}{\partial {{X}^{\text{T}}}}\frac{\partial {{y}_{idg}}}{\partial {{X}^{\text{T}}}}\frac{\partial {{x}_{il1}}}{\partial {{X}^{\text{T}}}}\frac{\partial {{y}_{il1}}}{\partial {{X}^{\text{T}}}}\cdots \frac{\partial {{x}_{ilm}}}{\partial {{X}^{\text{T}}}}\frac{\partial {{y}_{ilm}}}{\partial {{X}^{\text{T}}}}\frac{\partial {{x}_{is1}}}{\partial {{X}^{\text{T}}}}\frac{\partial {{y}_{is1}}}{\partial {{X}^{\text{T}}}}\cdots \frac{\partial {{x}_{isn}}}{\partial {{X}^{\text{T}}}}\frac{\partial {{y}_{isn}}}{\partial {{X}^{\text{T}}}} \right]}^{\text{T}}}$

with the expressions given as follows.

For the measurement variables of feature points,

$\begin{align} & \frac{\partial {{x}_{id}}}{\partial {{d}^{fPT}}}=\left[ \begin{matrix} \frac{\lambda }{{{d}_{fPz}}} & 0 & -\frac{\lambda {{d}_{fPx}}}{d_{fPz}^{2}} \\ \end{matrix} \right] \\ & \frac{\partial {{y}_{id}}}{\partial {{d}^{fPT}}}=\left[ \begin{matrix} 0 & \frac{\lambda }{{{d}_{fPz}}} & -\frac{\lambda {{d}_{fPx}}}{d_{fPz}^{2}} \\ \end{matrix} \right] \\ & \frac{\partial {{d}^{fP}}}{\partial {{X}^{\text{T}}}}={{\left[ \begin{matrix} \frac{\partial {{d}^{fP}}}{\partial q_{fl,P}^{\text{T}}} & \frac{\partial {{d}^{fP}}}{\partial q\rm{'}_{fl,P}^{\text{T}}} & \frac{\partial {{d}^{fP}}}{\partial \hat{\omega }_{fl,P}^{fP\text{T}}} & \frac{\partial {{d}^{fP}}}{\partial {{\Phi }^{\text{T}}}} \\ \end{matrix} \right]}^{\text{T}}} \\ \end{align}$

where

$\begin{align} & \frac{\partial {{x}_{id}}}{\partial {{d}^{fPT}}}=\left[ \begin{matrix} \frac{\lambda }{{{d}_{fPz}}} & 0 & -\frac{\lambda {{d}_{fPx}}}{d_{fPz}^{2}} \\ \end{matrix} \right] \\ & \frac{\partial {{y}_{id}}}{\partial {{d}^{fPT}}}=\left[ \begin{matrix} 0 & \frac{\lambda }{{{d}_{fPz}}} & -\frac{\lambda {{d}_{fPx}}}{d_{fPz}^{2}} \\ \end{matrix} \right] \\ & \frac{\partial {{d}^{fP}}}{\partial {{X}^{\text{T}}}}={{\left[ \begin{matrix} \frac{\partial {{d}^{fP}}}{\partial q_{fl,P}^{\text{T}}} & \frac{\partial {{d}^{fP}}}{\partial q\rm{'}_{fl,P}^{\text{T}}} & \frac{\partial {{d}^{fP}}}{\partial \hat{\omega }_{fl,P}^{fP\text{T}}} & \frac{\partial {{d}^{fP}}}{\partial {{\Phi }^{\text{T}}}} \\ \end{matrix} \right]}^{\text{T}}} \\ \end{align}$

with

$\begin{align} & \frac{\partial {{d}^{fP}}}{\partial q_{fl,P}^{\text{T}}}=\left[ q_{fl,P}^{-} \right]\left[ {{d}^{lP-}} \right]{{E}_{4}}+{{\left[ q_{fl,P}^{+} \right]}^{\text{T}}}\left[ {{d}^{lP+}} \right]+ \\ & \left[ q\rm{'}_{fl,P}^{-} \right]{{E}_{4}}-{{\left[ q\rm{'}_{fl,P}^{+} \right]}^{\text{T}}} \\ & \frac{\partial {{d}^{fP}}}{\partial q\rm{'}_{fl,P}^{\text{T}}}={{\left[ q_{fl,P}^{+} \right]}^{\text{T}}}-\left[ q_{fl,P}^{-} \right]{{E}_{4}} \\ & \frac{\partial {{d}^{fP}}}{\partial \hat{\omega }_{fl,P}^{fPT}}={{0}_{3\times 6}},\frac{\partial {{d}^{fP}}}{\partial {{\Phi }^{\text{T}}}}={{0}_{3\times 3n}} \\ \end{align}$

For the measurement variables of feature lines,

$\frac{\partial {{x}_{il}}}{\partial {{X}^{\text{T}}}}=\frac{\partial {{x}_{il}}}{\partial {{m}^{fPT}}}\frac{\partial {{m}^{fP}}}{\partial {{X}^{\text{T}}}},\frac{\partial {{y}_{il}}}{\partial {{X}^{\text{T}}}}=\frac{\partial {{y}_{il}}}{\partial {{m}^{fPT}}}\frac{\partial {{m}^{fP}}}{\partial {{X}^{\text{T}}}}$

where

$\begin{align} & \frac{\partial {{x}_{il}}}{\partial {{m}^{fPT}}}={{\left[ \begin{matrix} \frac{\lambda {{m}_{z}}}{m_{x}^{2}+m_{y}^{2}}-2\frac{\lambda m_{x}^{2}{{m}_{z}}}{{{\left( m_{x}^{2}+m_{y}^{2} \right)}^{2}}} \\ -2\frac{\lambda {{m}_{x}}{{m}_{y}}{{m}_{z}}}{{{\left( m_{x}^{2}+m_{y}^{2} \right)}^{2}}} \\ \frac{\lambda {{m}_{x}}}{m_{x}^{2}+m_{y}^{2}} \\ \end{matrix} \right]}^{\text{T}}} \\ & \frac{\partial {{y}_{il}}}{\partial {{m}^{fPT}}}=\left[ \begin{matrix} -2\frac{\lambda {{m}_{x}}{{m}_{y}}{{m}_{z}}}{{{\left( m_{x}^{2}+m_{y}^{2} \right)}^{2}}} \\ \frac{\lambda {{m}_{z}}}{m_{x}^{2}+m_{y}^{2}}-2\frac{\lambda m_{y}^{2}{{m}_{z}}}{{{\left( m_{x}^{2}+m_{y}^{2} \right)}^{2}}} \\ \frac{\lambda {{m}_{y}}}{m_{x}^{2}+m_{y}^{2}} \\ \end{matrix} \right] \\ & \frac{\partial {{m}^{fP}}}{\partial {{X}^{\text{T}}}}={{\left[ \begin{matrix} \frac{\partial {{m}^{fP}}}{\partial \hat{q}_{fl,P}^{\text{T}}} & \frac{\partial {{m}^{fP}}}{\partial \hat{\omega }_{fl,P}^{fPT}} & \frac{\partial {{m}^{fP}}}{\partial {{\Phi }^{\text{T}}}} \\ \end{matrix} \right]}^{\text{T}}} \\ \end{align}$

where

$\begin{align} & \frac{\partial {{d}^{fP}}}{\partial q_{fl,P}^{\text{T}}}=\left[ q_{fl,P}^{-} \right]\left[ {{d}^{lP-}} \right]{{E}_{4}}+{{\left[ q_{fl,P}^{+} \right]}^{\text{T}}}\left[ {{d}^{lP+}} \right]+ \\ & \left[ q\rm{'}_{fl,P}^{-} \right]{{E}_{4}}-{{\left[ q\rm{'}_{fl,P}^{+} \right]}^{\text{T}}} \\ & \frac{\partial {{d}^{fP}}}{\partial q\rm{'}_{fl,P}^{\text{T}}}={{\left[ q_{fl,P}^{+} \right]}^{\text{T}}}-\left[ q_{fl,P}^{-} \right]{{E}_{4}} \\ & \frac{\partial {{d}^{fP}}}{\partial \hat{\omega }_{fl,P}^{fPT}}={{0}_{3\times 6}},\frac{\partial {{d}^{fP}}}{\partial {{\Phi }^{\text{T}}}}={{0}_{3\times 3n}} \\ \end{align}$

For the measurement variables of feature circles,

$\frac{\partial {{x}_{is}}}{\partial {{X}^{\text{T}}}}=\frac{\partial {{x}_{is}}}{\partial s_{\phi }^{fPT}}\frac{\partial s_{\phi }^{fP}}{\partial {{X}^{\text{T}}}},\frac{\partial {{y}_{is}}}{\partial {{X}^{\text{T}}}}=\frac{\partial {{y}_{is}}}{\partial s_{\phi }^{fPT}}\frac{\partial s_{\phi }^{fP}}{\partial {{X}^{\text{T}}}}$

where

$\begin{align} & \frac{\partial {{x}_{is}}}{\partial s_{\phi }^{fPT}}=\left[ \begin{matrix} \frac{\lambda }{{{s}_{\phi fPz}}} & 0 & -\frac{\lambda {{s}_{\phi fPx}}}{s_{\phi fPz}^{2}} \\ \end{matrix} \right] \\ & \frac{\partial {{y}_{is}}}{\partial s_{\phi }^{fPT}}=\left[ \begin{matrix} 0 & \frac{\lambda }{{{s}_{\phi fPz}}} & -\frac{\lambda {{s}_{\phi fPy}}}{s_{\phi fPz}^{2}} \\ \end{matrix} \right] \\ & \frac{\partial s_{\phi }^{fP}}{\partial {{X}^{\text{T}}}}={{\left[ \begin{matrix} \frac{\partial s_{\phi }^{fP}}{\partial q_{fl,P}^{\text{T}}} & \frac{\partial s_{\phi }^{fP}}{\partial q\rm{'}_{fl,P}^{\text{T}}} & \frac{\partial s_{\phi }^{fP}}{\partial \hat{\omega }_{fl,P}^{fPT}} & \frac{\partial s_{\phi }^{fP}}{\partial {{\Phi }^{\text{T}}}} \\ \end{matrix} \right]}^{\text{T}}} \\ \end{align}$

where

$\begin{align} & \frac{\partial s_{\phi }^{fP}}{\partial q_{fl,P}^{\text{T}}}=\left[ q_{fl,P}^{-} \right]\left[ s_{\phi }^{lP-} \right]{{E}_{4}}+{{\left[ q_{fl,P}^{+} \right]}^{\text{T}}}\left[ s_{\phi }^{lP+} \right]+ \\ & \left[ q\rm{'}_{fl,P}^{-} \right]{{E}_{4}}-{{\left[ q\rm{'}_{fl,P}^{+} \right]}^{\text{T}}} \\ & \frac{\partial s_{\phi }^{fP}}{\partial q_{fl,P}^{\text{T}}}={{\left[ q_{fl,P}^{+} \right]}^{\text{T}}}-\left[ q_{fl,P}^{-} \right]{{E}_{4}},\frac{\partial s_{\phi }^{fP}}{\partial \hat{\omega }_{fl,P}^{fPT}}={{0}_{3\times 6}} \\ & \frac{\partial s_{\phi }^{fP}}{\partial {{\Phi }^{\text{T}}}}=\frac{\partial s_{\phi }^{fP}}{\partial \hat{q}_{\phi }^{\text{T}}}\frac{\partial \hat{q}}{\partial {{\Phi }^{\text{T}}}} \\ \end{align}$

where $\frac{\partial {{{\hat{q}}}_{\phi }}}{\partial {{\Phi }^{\text{T}}}}=\left[ \begin{matrix} 0 & \frac{\partial {{q}_{\phi }}}{\partial {{\phi }_{j}}} & 0 \\ \end{matrix} \right]$ with φ_j being the rotation angle of the resulting point,

$\begin{align} & \frac{\partial s_{\phi }^{fP}}{\partial q_{\phi }^{\text{T}}}={{\left[ q_{fl,P}^{+} \right]}^{\text{T}}}\left[ q_{fl,P}^{-} \right]{{\left( \left[ q_{\phi }^{-} \right] \right.}^{\text{T}}}\left[ s_{0}^{lP-} \right]+ \\ & \left. \left[ q_{\phi }^{+} \right]\left[ s_{0}^{lP+} \right]{{E}_{4}}+\left[ q\rm{'}_{\phi }^{+} \right]{{E}_{4}}+{{\left[ q\rm{'}_{\phi }^{-} \right]}^{\text{T}}} \right) \\ & \frac{\partial s_{\phi }^{fP}}{\partial q_{\phi }^{\text{T}}}={{\left[ q_{fl,P}^{+} \right]}^{\text{T}}}\left[ q_{fl,P}^{-} \right]\left( {{\left[ q_{\phi }^{-} \right]}^{\text{T}}}-\left[ q_{\phi }^{+} \right]{{E}_{4}} \right) \\ & \frac{\partial {{q}_{\phi }}}{\partial {{\phi }_{j}}}=\frac{1}{2}{{\left[ \begin{matrix} -\sin \left( \frac{{{\phi }_{j}}}{2} \right) & l_{l}^{\text{T}}\cos \left( \frac{{{\phi }_{j}}}{2} \right) \\ \end{matrix} \right]}^{\text{T}}} \\ & \frac{\partial {{{{q}'}}_{\phi }}}{\partial {{\phi }_{j}}}={{\left[ \begin{matrix} 0 & \frac{1}{2}m_{t}^{\text{T}}\cos \left( \frac{{{\phi }_{j}}}{2} \right) \\ \end{matrix} \right]}^{\text{T}}} \\ \end{align}$

To illustrate the reason why a combination of features can improve the performance of the navigation system,the following analysis is presented.

Let ${{\dot{x}}_{k}}$ be any unbiased estimator of the real state ${{\dot{x}}_{k}}$ based on all kinds of measurements collected up to and including time ${{\dot{x}}_{k}},\text{i}\text{.e}\text{.}$,

${{Z}_{lk}}=\left\{ {{z}_{11}},{{z}_{12}},...,{{z}_{1k}},{{z}_{21}},{{z}_{22}},...,{{z}_{2k}},...{{z}_{l1}},{{z}_{l2}},...,{{z}_{lk}} \right\}$

where l denotes the number of independent measurements,k denotes the current moment. The covariance of ${{\dot{x}}_{k}}$ has a lower bound expressed by

${{C}_{k}}=E\left[ \left( {{x}_{k}}-{{{\hat{x}}}_{k}} \right){{\left( {{x}_{k}}-{{{\hat{x}}}_{k}} \right)}^{\text{T}}} \right]\ge J_{k}^{-1}$

where J_k is the Fisher matrix with

${{J}_{k}}=E\left\{ \left[ \frac{\partial \ln p\left( {{z}_{11:lk}}\left| x \right. \right)}{\partial x} \right]{{\left[ \frac{\partial \ln p\left( {{z}_{11:lk}}\left| x \right. \right)}{\partial x} \right]}^{\text{T}}} \right\}$

According to the condition of independent observation,$p\left( {{z}_{11:lk}}\left| x \right. \right)=\prod\limits_{i=11}^{lk}{p\left( {{z}_{i}}\left| x \right. \right)}$,thus we have

${{J}_{k}}=\sum\limits_{i=11}^{lk}{E}\left\{ \left[ \frac{\partial \ln p\left( {{z}_{i}}\left| x \right. \right)}{\partial x} \right]{{\left[ \frac{\partial \ln p\left( {{z}_{i}}\left| x \right. \right)}{\partial x} \right]}^{\text{T}}} \right\}$

(13)

From Eq. (13),it is seen that as the number of independent measurements l getting more J_k becomes larger,thus the lower bound of the covariance of ${{\hat{x}}_{k}}$ becomes smaller.So one can draw the conclusion that more observed information may improve the performance of the whole navigation system.

4.3 Equations of Extended Kalman Filter

In this paper,based on the process model and the integrated observation model provided in the previous subsections,the EKF method is utilized to estimate the relative state between two spacecraft. A summary of the EKF equations are shown as:

$\left\{ \begin{align} & {{X}_{k,k-1}}={{X}_{k-1}}+f\left( {{X}_{k-1}} \right)T \\ & {{P}_{k,k-1}}={{F}_{k,k-1}}{{P}_{k-1}}F_{k,k-1}^{\text{T}}+{{Q}_{k-1}} \\ & {{K}_{k}}={{P}_{k,k-1}}H_{k}^{\text{T}}{{\left( {{H}_{k}}{{P}_{k,k-1}}H_{k}^{\text{T}}+{{R}_{k}} \right)}^{-1}} \\ & {{P}_{k}}=\left( I-{{K}_{k}}{{H}_{k}} \right){{P}_{k,k-1}} \\ & {{X}_{k}}={{X}_{k,k-1}}+{{K}_{k}}\left( {{Z}_{k}}-h\left( {{x}_{k,k-1}} \right) \right) \\ \end{align} \right.$

Seen from the equations,the inputs of the filter consist of the measurements Z_k,the measurement noise covariance R_k,the process noise covariance Q_k－1 and an initial error covariance matrix P₀; the outputs are the next state estimate ${{\tilde{X}}_{k}}$ and the error covariance matrix P_k.

5 Simulation Results

In this section,the simulations are presented to characterize the performance of the proposed integrated estimation technique. In the simulation,the 3D data,which consist of 3D points,3D lines and 3D circles,are given in advance. The 2D data are generated by projecting the 3D data onto the image plane,followed by perturbing the projected image data with Guassian white noise. From the generated 2D-3D data,the relative attitude and position are estimated using the proposed integrated estimation algorithm. The simulation parameters are presented as follows.

The leader spacecraft is specified to follow an elliptic orbit with the semi-major axis of 7 200 000 m and the eccentricity of 0.02,and it is assumed that the leader body-fixed frame is perfectly aligned with its orbit frame. The focal length of the camera on the follower spacecraft is 0.5 m. The mass and the inertia matrix of the follower spacecraft are given by m_f=500 kg and J_f=diag522,420,600 kg·m²,respectively.

The observed feature points fixed on the leader spacecraft are

${{D}_{1}}={{\left[ 1,1,0 \right]}^{\text{T}}};\text{ }{{D}_{2}}={{\left[ -1,1,0 \right]}^{\text{T}}}$

${{D}_{3}}={{\left[ -1,-1,0 \right]}^{\text{T}}};\text{ }{{D}_{4}}={{\left[ 1,-1,0 \right]}^{\text{T}}}$

The observed feature lines are represented by the following dual vectors:

$\begin{matrix} {{{\hat{L}}}_{1}}={{\left[ -1,0,0 \right]}^{\text{T}}}+\varepsilon {{\left[ 0,0,1 \right]}^{\text{T}}} \\ {{{\hat{L}}}_{2}}={{\left[ 0,-1,0 \right]}^{\text{T}}}+\varepsilon {{\left[ 0,0,1 \right]}^{\text{T}}} \\ {{{\hat{L}}}_{3}}={{\left[ 1,0,0 \right]}^{\text{T}}}+\varepsilon {{\left[ 0,0,1 \right]}^{\text{T}}} \\ {{{\hat{L}}}_{4}}={{\left[ 0,1,0 \right]}^{\text{T}}}+\varepsilon {{\left[ 0,0,1 \right]}^{\text{T}}} \\ \end{matrix}$

The observed feature circle 1,which is denoted as ⊙₁,has its twist ${{\hat{L}}_{t1}}$ and starting point S₁:

$\begin{matrix} {{{\hat{L}}}_{t1}}={{\left[ 0,\sqrt{2}/2,\sqrt{2}/2 \right]}^{\text{T}}}+\varepsilon {{\left[ -3,1,-1 \right]}^{\text{T}}} \\ {{S}_{1}}={{\left[ 1,3,-2 \right]}^{\text{T}}} \\ \end{matrix}$

By rotating the start point S₁ around the line $ with φ₁=0,φ₂=π/2 and φ₃=π,three resulting points on Circle 1 can be obtained using Eq. (15). Similarly,for the case of Circle 2,denoted as ⊙₂,the parameters are

${{\hat{L}}_{t2}}={{\left[ 1,0,0 \right]}^{\text{T}}}+\varepsilon {{\left[ 0,1,0 \right]}^{\text{T}}},{{S}_{2}}={{\left[ 2,1,-1 \right]}^{\text{T}}}$

and the rotation angles corresponding to the resulting points are ${{{\phi }'}_{1}}=\pi /6,{{{\phi }'}_{2}}=2\pi /3,{{{\phi }'}_{3}}=2\pi $.

The realinitial relative states are given as follows

$\begin{align} & {{q}_{fl,P}}\left( 0 \right)={{\left[ 0.9982,0.0336,0.0366,0.0336 \right]}^{\text{T}}} \\ & r_{fl,P}^{fP}\left( 0 \right)={{\left[ -32.1,40,51.2 \right]}^{\text{T}}}\text{m,}\hat{\omega }_{fl,P}^{fP}\left( 0 \right)=\hat{0} \\ \end{align}$

The initial guess of the estimation is

$\begin{align} & {{{\tilde{q}}}_{fl,P}}\left( 0 \right)={{\left[ 1,0,0,0 \right]}^{\text{T}}} \\ & \tilde{r}_{fl,P}^{fP}\left( 0 \right)={{\left[ -30,45,50 \right]}^{\text{T}}}\text{m} \\ & \tilde{\hat{\omega }}_{fl,P}^{fP}\left( 0 \right)=\hat{0} \\ \end{align}$

The sample time of the filter is T=1 s,and the other parameters are given by

$P\left( 0 \right)=\text{diag}\left( \sigma _{{{W}_{q}}}^{2}{{I}_{4}},\sigma _{{{W}_{{{q}'}}}}^{2}{{I}_{4}},\sigma _{{{W}_{\omega }}}^{2}{{I}_{3}},\sigma _{{{W}_{\phi }}}^{2}{{I}_{6}} \right)$

where $\sigma _{{{W}_{q}}}^{2}=0.000\text{ }{{01}^{2}},\sigma _{{{W}_{{{q}'}}}}^{2}=1,\sigma _{{{W}_{\omega }}}^{2}=0.000\text{ }{{01}^{2}},\sigma _{{{W}_{v}}}^{2}=0.001,\sigma _{{{W}_{\phi }}}^{2}={{0.001}^{2}}$,

$Q=\text{diag}\left( \sigma _{{{Q}_{q}}}^{2}{{I}_{4}},\sigma _{{{Q}_{{{q}'}}}}^{2}{{I}_{4}},\sigma _{{{Q}_{\omega }}}^{2}{{I}_{3}},\sigma _{{{Q}_{v}}}^{2}{{I}_{3}}\sigma _{{{Q}_{\phi }}}^{2}{{I}_{6}} \right)$

where $\sigma _{{{Q}_{q}}}^{2}=0.000\text{ }{{\text{5}}^{2}},\sigma _{{{Q}_{{{q}'}}}}^{2}=0.01,\sigma _{{{Q}_{\omega }}}^{2}=0.000\text{ }{{01}^{2}},\sigma _{{{Q}_{v}}}^{2}=0.01,\sigma _{{{Q}_{\phi }}}^{2}={{0.001}^{2}}$,and R=0.0005²I₂₄

Based on a combination of the measurements from feature points (D₁,D₃),lines (${{\hat{L}}_{1}},{{\hat{L}}_{2}},{{\hat{L}}_{3}},{{\hat{L}}_{4}}$) and circles (⊙₁,⊙₂),the attitude and position of the follower spacecraft relative to the leader spacecraft are estimated using the proposed strategy. Simulation results are presented in Figs. 4 and 5. Seen from Fig. 4,it is clear that both the translational and rotational estimation errors converge to a small value after about 400 s. The first and second subfigures show the time histories of relative attitude and angular velocity estimation errors,respectively. Seen from these subfigures,the Euler angle estimation errors are within 0.02^o,and the relative angular velocity estimation errors converge to below 0.001^o/s. The third subfigure shows the estimation error of the relative position between the points P_l and P_f. It is seen that the position knowledge is within 0.02 m. The relative velocity estimation error is shown in the last subfigure,which demonstrates that each norm of its component converges to below 0.01 m/s. Fig. 5 shows the rotation angle estimation errors for ⊙₁ and ⊙₂,and it is seen that the errors also converge to a small value fast. Therefore,it can be concluded that despite the noise of the measurement,both the relative translation and rotation estimation errors converge rapidly with the desired high estimation accuracy.

To compare the performance of the proposed integrated estimation algorithm with other combinations of the features,the following cases are considered: 1) Use point features only (D₁,D₂,D₃,D₄); 2) Use line features only (${{\hat{L}}_{1}},{{\hat{L}}_{2}},{{\hat{L}}_{3}},{{\hat{L}}_{4}}$); 3) Use circle features only (⊙₁,⊙₂); 4) Use point and line features (${{\hat{L}}_{1}},{{\hat{L}}_{2}},{{\hat{L}}_{3}},{{\hat{L}}_{4}}$ and D₁,D₂,D₃,D₄); 5) Use point and circle features (D₁,D₂,D₃,D₄ and ⊙₁); 6) Use line and circle features (${{\hat{L}}_{1}},{{\hat{L}}_{2}},{{\hat{L}}_{3}},{{\hat{L}}_{4}}$ and ⊙₁). Case 1-3 use only one type of feature while Case 4-6 apply different combinations of geometric features. The numerical results for Case 1-6 are shown in Figs. 6-11,respectively. A summarization of the results for all cases is given in Table 1,where the accuracy is represented by the maximum norm of the errors after they converge. Seen from the simulation results,it is obvious that all the approaches can estimate the relative position and attitude effectively,but with different performance. Compared to Case 1-6,the integrated technique yields a superior estimation accuracy and fastest convergence rate. It can be also found that using more features will help to improve accuracy and robustness.

In order to validate the proposed method in the case of lager initial errors,the initial guess of the estimation is given as

$\begin{align} & {{{\tilde{q}}}_{fl,P}}\left( 0 \right)={{\left[ 0.919\text{ }7,0.026\text{ }5,0.065\text{ }4,0.386\text{ }1 \right]}^{\text{T}}} \\ & \tilde{r}_{fl,P}^{fP}\left( 0 \right)={{\left[ -60,80,100 \right]}^{\text{T}}}\text{m,}\tilde{\hat{\omega }}_{fl,P}^{fP}\left( 0 \right)=\hat{0} \\ \end{align}$

The simulation results are shown in Fig. 12,which validate the effectiveness of the proposed estimation method in the case of lager initial errors. However,the larger initial estimation errors cost longer time to converge and the estimation convergence errors become larger. To improve the performance of the estimation,the author considers employing other advanced filtering algorithms such as the Unscented Kalman Filter (UKF) and Partical Fliter (PF),and it will be the direction for the future research.

6 Conclusions

By employing dual numbers,an integrated observation model is developed based on a combination of multiple geometric features including points,lines and circles. In addition,a six-degree-of-freedom relative motion model is derived,which can describe the rotation-translation coupling effect due to the points deviating from the center of the mass. Based on the proposed relative dynamics and the integrated observation model,Extended Kalman Filter is presented to estimate the relative state between spacecraft. Finally,numerical simulations are provided to illustrate the effectiveness of the proposed algorithms. The simulation results demonstrate that high accuracy can be achieved by using the presented estimation technique,and by comparing with other algorithms using different combinations of features,the integrated technique yields a superior estimation accuracy and fastest convergence rate.