# Stereo-Vision-Based Relative Pose Estimation for the Rendezvous and Docking of Noncooperative Satellites

Authors: Feng Yu; Zhen He; Bing Qiao; Xiaoting Yu
Journal: Mathematical Problems in Engineering
 (2014)

---

## Abstract

Autonomous on-orbit servicing is expected to play an important role in future space activities. Acquiring the relative pose information and inertial parameters of target is one of the key technologies for autonomous capturing. In this paper, an estimation method of relative pose based on stereo vision is presented for the final phase of the rendezvous and docking of noncooperative satellites. The proposed estimation method utilizes the sparse stereo vision algorithm instead of the dense stereo algorithm. The method consists of three parts: (1) body frame reestablishment, which establishes the body-fixed frame for the target satellite using the natural features on the surface and measures the relative attitude based on TRIAD and QUEST; (2) translational parameter estimation, which designs a standard Kalman filter to estimate the translational states and the location of mass center; (3) rotational parameter estimation, which designs an extended Kalman filter and an unscented Kalman filter, respectively, to estimate the rotational states and all the moment-of-inertia ratios. Compared to the dense stereo algorithm, the proposed method can avoid degeneracy when the target has a high degree of axial symmetry and reduce the number of sensors. The validity of the proposed method is verified by numerical simulations.


---

## Body

## 1. Introduction

The autonomous on-orbit servicing is expected to be one of the most challenging and exciting space activities in the future [1, 2]. In the past, many expensive satellites were out of service in orbit due to various failures, such as solar panel undeployment and gyro malfunction. However, in these cases, most of the other parts of the satellites were still functional [3, 4]. The on-orbit servicing, which is the execution of repair, refueling, orbit maintenance, and reorbiting, can extend the life of malfunctioned satellites and save a large amount of expense.Therefore, more and more attentions have been paid to the technologies of autonomous on-orbit servicing. Several on-orbit servicing projects including on-orbit servicing demonstration experiments and conceptual on-orbit servicing systems have been carried out [5, 6].As malfunctioned satellites are generally noncooperative targets tumbling in space and have no equipment which can be used for relative pose measurement [7], it is necessary to develop the method of relative pose measurement without the cooperation of target satellites. For example, the purpose of the Spacecraft for the Universal Modification of Orbits (SUMO) program is to demonstrate the integration of machine vision, robotics, mechanisms, and autonomous control algorithms to accomplish autonomous rendezvous and grapple of a variety of interfaces traceable to future spacecraft servicing operations [8]. In the SUMO program, the main concept is to be able to capture an unaided target satellite; that is, target satellite is equipped without special facilities such as grapple fixtures or reflectors which is compatible with the SUMO. SUMO was later renamed as the Front-End Robotics Enabling Near-term Demonstration (FREND) to develop a series of key components including a seven degree-of-freedom (DOF) flight robotic arm which was demonstrated in a test bed with a stereo photogrammetric imaging system [9]. Similarly, DEOS (Deutsche Orbitale Servicing Mission) project focuses on finding and evaluating the procedures and techniques for rendezvous, capture, and deorbiting of a noncooperative target satellite [10]. In the automatic mode, a number of images are taken and dumped to the ground segment, and then target motion is estimated by the control software in the ground station.The relative pose information of two involved satellites is of vital importance for the end-effector to successfully grasp the tumbling satellite. If the dynamic model of the target is known, a filter can be designed to suppress the measurement noise and estimate the states which cannot be measured directly. Furthermore, the dynamic model can also be used to predict the motion of the target satellite, and then the optimal path planning can be conducted to guide a robotic manipulator to capture the tumbling target at a rendezvous point with the same velocity [11]. Vision system is more and more commonly utilized for the relative pose measurements in the rendezvous missions due to the low cost, low mass, and low energy requirement of CCD/CMOS cameras [12, 13]. Nagamatsu et al. proposed a 12th order extended Kalman filter to estimate the position and attitude of a torque-free target satellite by using a simplified dynamical model [14]. NASA investigated the feasibility of robotic autonomous servicing of the Hubble Space Telescope (HST). A nonlinear estimator was developed to estimate the HST body rates by using vision-based sensors which can output relative quaternion [15, 16]. NASA/GSFC continued to develop a Goddard Natural Feature Image Recognition (GNFIR) algorithm. A 6-DOF pose estimation was demonstrated in Robotic Refueling Mission in order to augment the traditional overlays [17]. Tweddle described a new approach to solve a SLAM problem for unknown and uncooperative objects that are spinning about an arbitrary axis. An experimental test-bed known as “Goggles” with computer vision-based navigation capability by the stereo cameras is used to test the algorithm [18]. Lichter and Dubowsky proposed an important and effective architecture of estimating dynamic state, geometric shape, and model parameters of objects in orbit [19]. A team of cooperating 3D vision sensors captures sequences of range images to determine the rough target poses before filtering. The relative positions and orientations between sensors mounted on different satellites are required to be known with high accuracy, which is perhaps a cost and operation burden for on-orbit servicing. A model-based pose refinement algorithm was proposed to perform the relative pose estimation of a floating object from the visual information of a stereo-vision system [20]. Du et al. proposed a method based on two collaborative cameras sharing the recognition task to determine the pose of a large noncooperative target [21], and details of image processing were also discussed.In this paper, the problem of relative pose estimation for the final phase of the rendezvous and docking of noncooperative satellites is investigated. Instead of the dense stereo algorithm presented in the literature [17], here the relative pose estimation method is based on the sparse stereo algorithm. The body-fixed coordinate system of the target satellite is reestablished by utilizing the natural features on the target surface. Two relative attitude measurement methods based on TRIAD and QUEST are presented. Then, a standard Kalman filter is designed to estimate the translational states and the location of the mass center. An extended Kalman filter (EKF) and an unscented Kalman filter (UKF) are designed, respectively, to estimate the rotational states and the moment-of-inertia ratios including the ratios of product of inertia, and their performances are compared with each other.This paper is structured as follows. Section2 defines the coordinate systems and transformation matrixes used in the relative pose estimation. Section 3 presents two algorithms for relative attitude measurements including the scheme for updating the target feature coordinate system when target is tumbling. Then, an approach based on Kalman filter to estimate the location of mass center and translation parameters is proposed in Section 4. In Section 5, the EKF and UKF are designed, respectively, to estimate the moment-of-inertia ratios and orientation parameters. Numerical simulation is conducted to verify the proposed algorithm and results are discussed as well in Section 6. The last section contains conclusions.


## 2. Coordinate Systems and Transformation Matrixes

As various physical quantities are usually defined in different coordinate systems, the coordinate systems and coordinate transformation matrixes are defined to illustrate the relative geometrical relationship between the target and chaser. The relative geometrical relationship and coordinate systems are illustrated in Figure1.Figure 1
Geometrical relationship and coordinate systems.


### 2.1. Inertial Coordinate System (Σ
i)

The origin of the inertial coordinate systemo
i
-
x
i
y
i
z
i is centered on Earth, the z
i axis is aligned with the rotation axis, and the x
i axis is defined to point toward the Vernal equinox. The y
i axis completes the right-handed orthogonal coordinate system. The attitude of target satellite and service satellite can be described as the rotation from the inertial frame to the body frame.


### 2.2. Local Orbital Coordinate System (Σ
o & Σ
o
′)

The origin of the local orbital coordinate systemo
o
-
x
o
y
o
z
o (Σ
o) lies in the mass center of service satellite. The x
o axis is in the opposite direction of the Earth center, the y
o axis is in the flight direction, and the z
o axis which completes the right-handed orthogonal coordinate system is in the direction of the angular momentum of the orbit.Similarly, the local orbital coordinate systemΣ
o
′ is attached to target satellite. The rotation matrix between the two satellite orbital frames is denoted as C
o
′
o. As two satellites are almost in the same orbit and the service satellite is very close to the target, C
o
′
o is nearly a 3
×
3 identity matrix. For instance, if the target is a leading satellite in the circular orbit of 700 km altitude and the chaser is 100 m behind the target in the same orbit, the rotation angle between two orbital fames is only about 3
′
′.


### 2.3. Chaser Body-Fixed Coordinate System (Σ
s)

The origin of chaser body-fixed coordinate systemo
s
-
x
s
y
s
z
s lies in the mass center of service satellite, and the three body axes of symmetry are defined as three coordinate axes x
b, y
b, and z
b. The orientation of the Σ
s relative to Σ
i or Σ
o is determined by attitude and orbit determination system which is equipped with gyro, star sensor, GNSS receiver, and so on. The coordinate transformation matrix from inertial coordinate system Σ
i to Σ
s is denoted as C
i
s, and the coordinate transformation matrix from Σ
o to Σ
s is denoted as C
o
s.


### 2.4. Stereovision-Fixed Coordinate System (Σ
c)

The origin of stereovision-fixed coordinate systemo
c
-
x
c
y
c
z
c lies in the stereo vision system. The axis z
c is parallel with the optical axis of the cameras, x
c is vertical to the optical axis, and the direction of axis y
c obeys the right-hand role. The measurement information (coordinates of feature points) of stereo vision system is described in this coordinate system.The rotation matrixC
s
c and the translation vector ρ
s
c from Σ
s to Σ
c can be acquired by calibration on the ground. Therefore, relationship between the coordinates of feature points in Σ
s and Σ
c can be described as
(1)
p
i
s
=
C
c
s
·
p
i
c
+
ρ
s
c
,
where p
i
s
=
x
i
s
,
y
i
s
,
z
i
s are the coordinates of ith feature point in the frame Σ
s and p
i
c
=
x
i
c
,
y
i
c
,
z
i
c are in Σ
c.


### 2.5. Target Feature Coordinate System (Σ
f)

As there are no artificial marks which can provide effective cooperative information to service satellite, natural features on the target surface are utilized to establish target feature coordinate systemo
f
-
x
f
y
f
z
f. The natural features are usually on frameworks of the antenna backboard, solar panels, and so on. The frame Σ
f is applied to determine the relative pose between target and chaser satellite. The methods to establish the frame Σ
f are described in Section 3.


### 2.6. Target Body-Fixed Coordinate System (Σ
t)

The origin of target body-fixed coordinate systemo
t
-
x
t
y
t
z
t lies in the mass center of target satellite, and the three coordinate axes x
t, y
t, and z
t are set to be parallel to x
f, y
f, and z
f, respectively. Obviously, the axes x
t, y
t, and z
t are probably not aligned with three body axes of symmetry of target.


### 2.7. Target Nominal Body-Fixed Coordinate System (Σ
r)

The origin of target nominal body-fixed coordinate systemo
r
-
x
r
y
r
z
r lies in approximate mass center of target satellite, and the three coordinate axes x
r, y
r, and z
r are parallel to x
t, y
t, and z
t, respectively. Approximate location of mass center is estimated by ground staff using teleoperation loop or estimated automatically by service spacecraft. For example, the approximate mass center of target satellite can be supposed to lie in the target centroid. Then, the vector o
r
o
f
→ is known and the offset o
t
o
r
→ which is denoted as b will be estimated.


## 2.1. Inertial Coordinate System (Σ
i)

The origin of the inertial coordinate systemo
i
-
x
i
y
i
z
i is centered on Earth, the z
i axis is aligned with the rotation axis, and the x
i axis is defined to point toward the Vernal equinox. The y
i axis completes the right-handed orthogonal coordinate system. The attitude of target satellite and service satellite can be described as the rotation from the inertial frame to the body frame.


## 2.2. Local Orbital Coordinate System (Σ
o & Σ
o
′)

The origin of the local orbital coordinate systemo
o
-
x
o
y
o
z
o (Σ
o) lies in the mass center of service satellite. The x
o axis is in the opposite direction of the Earth center, the y
o axis is in the flight direction, and the z
o axis which completes the right-handed orthogonal coordinate system is in the direction of the angular momentum of the orbit.Similarly, the local orbital coordinate systemΣ
o
′ is attached to target satellite. The rotation matrix between the two satellite orbital frames is denoted as C
o
′
o. As two satellites are almost in the same orbit and the service satellite is very close to the target, C
o
′
o is nearly a 3
×
3 identity matrix. For instance, if the target is a leading satellite in the circular orbit of 700 km altitude and the chaser is 100 m behind the target in the same orbit, the rotation angle between two orbital fames is only about 3
′
′.


## 2.3. Chaser Body-Fixed Coordinate System (Σ
s)

The origin of chaser body-fixed coordinate systemo
s
-
x
s
y
s
z
s lies in the mass center of service satellite, and the three body axes of symmetry are defined as three coordinate axes x
b, y
b, and z
b. The orientation of the Σ
s relative to Σ
i or Σ
o is determined by attitude and orbit determination system which is equipped with gyro, star sensor, GNSS receiver, and so on. The coordinate transformation matrix from inertial coordinate system Σ
i to Σ
s is denoted as C
i
s, and the coordinate transformation matrix from Σ
o to Σ
s is denoted as C
o
s.


## 2.4. Stereovision-Fixed Coordinate System (Σ
c)

The origin of stereovision-fixed coordinate systemo
c
-
x
c
y
c
z
c lies in the stereo vision system. The axis z
c is parallel with the optical axis of the cameras, x
c is vertical to the optical axis, and the direction of axis y
c obeys the right-hand role. The measurement information (coordinates of feature points) of stereo vision system is described in this coordinate system.The rotation matrixC
s
c and the translation vector ρ
s
c from Σ
s to Σ
c can be acquired by calibration on the ground. Therefore, relationship between the coordinates of feature points in Σ
s and Σ
c can be described as
(1)
p
i
s
=
C
c
s
·
p
i
c
+
ρ
s
c
,
where p
i
s
=
x
i
s
,
y
i
s
,
z
i
s are the coordinates of ith feature point in the frame Σ
s and p
i
c
=
x
i
c
,
y
i
c
,
z
i
c are in Σ
c.


## 2.5. Target Feature Coordinate System (Σ
f)

As there are no artificial marks which can provide effective cooperative information to service satellite, natural features on the target surface are utilized to establish target feature coordinate systemo
f
-
x
f
y
f
z
f. The natural features are usually on frameworks of the antenna backboard, solar panels, and so on. The frame Σ
f is applied to determine the relative pose between target and chaser satellite. The methods to establish the frame Σ
f are described in Section 3.


## 2.6. Target Body-Fixed Coordinate System (Σ
t)

The origin of target body-fixed coordinate systemo
t
-
x
t
y
t
z
t lies in the mass center of target satellite, and the three coordinate axes x
t, y
t, and z
t are set to be parallel to x
f, y
f, and z
f, respectively. Obviously, the axes x
t, y
t, and z
t are probably not aligned with three body axes of symmetry of target.


## 2.7. Target Nominal Body-Fixed Coordinate System (Σ
r)

The origin of target nominal body-fixed coordinate systemo
r
-
x
r
y
r
z
r lies in approximate mass center of target satellite, and the three coordinate axes x
r, y
r, and z
r are parallel to x
t, y
t, and z
t, respectively. Approximate location of mass center is estimated by ground staff using teleoperation loop or estimated automatically by service spacecraft. For example, the approximate mass center of target satellite can be supposed to lie in the target centroid. Then, the vector o
r
o
f
→ is known and the offset o
t
o
r
→ which is denoted as b will be estimated.


## 3. Algorithms of Relative Attitude Measurement

Natural feature points are utilized to establish the target feature coordinate systemΣ
f. As image processing including image filtering, edge detection, feature point recognition, and matching is discussed [19], it is assumed in this paper that the coordinates of natural feature points in frame Σ
c are outputted by the stereo vision system.


### 3.1. Attitude Determination Based on TRIAD

There are several feature points tracked by the stereo vision system, which is illustrated in Figure1. Three points p
1, p
2, and p
3 are selected according to geometry and imaging quality of natural feature points to establish the frame Σ
f by right-handed rule. The p
1 is the origin of Σ
f, the axis x
f is in the direction of vector p
1
p
2
→, and the axis z
f is vertical to the plane which contains the three points. The direction of axis y
f obeys the right-hand role. The rotation matrix C
c
f from the frame Σ
c to Σ
f can be determined by direction cosine matrix as follows:
(2)
C
c
f
=
c
1
,
c
2
,
c
3
,
c
1
=
p
1
p
2
→
p
1
p
2
→
,
c
3
=
p
1
p
2
→
×
p
1
p
3
→
p
1
p
2
→
×
p
1
p
3
→
,
c
2
=
c
3
×
c
1
,
where |
| is the magnitude calculation and a
×
b is the representation of the operation of vector cross product. The error of TRIAD can be described as rotation angle ϕ about the Euler axis
(3)
ϕ
2
=
1
2
α
1
2
+
α
2
2
1
+
cs
c
2
θ
,
where θ is the angle between p
1
p
2
→ and p
1
p
3
→, and it produces minimum error when it is 90°. α
1 and α
2 are pointing errors of p
1
p
2
→ and p
1
p
3
→, respectively, and the pointing errors will be reduced with the extension of the line segments. Then, it is concluded that the three points which contribute the largest area are to be selected to obtain better accuracy in practice.


### 3.2. Attitude Determination Based on QUEST

As there are often more than 3 feature points available in the view of stereo vision system, it is better that all available points (the number of points is denoted asm
+
1) in view are utilized to improve the accuracy of relative attitude determination. Then coordinates of all feature points except p
1 in the frame Σ
f can be calculated as follows:
(4)
x
i
f
=
c
1
·
p
1
p
i
,
→
y
i
f
=
c
2
·
p
1
p
i
,
→
z
i
f
=
c
3
·
p
1
p
i
→
,
i
=
2,3
,
…
,
m
+
1
,
where a
·
b is the vector inner product. As measuring is repeated, an average processing is utilized and then more accurate coordinates X
i
f are determined in the frame Σ
f. The components of X
^
i
f are described as
(5)
x
^
i
=
∑
k
=
1
n
x
i
,
k
n
,
y
^
i
=
∑
k
=
1
n
y
i
,
k
n
,
z
^
i
=
∑
k
=
1
n
z
i
,
k
n
,
where n is the number of measurements.A set of observation unit vectorsW
1
,
…
,
W
m are defined as
(6)
W
j
=
X
i
f
X
i
f
,
j
=
i
-
1
,
j
=
1
,
…
,
m
.Similarly, a set of reference unit vectorsV
1
,
…
,
V
m are defined as
(7)
V
j
=
p
1
p
i
→
p
1
p
i
→
,
j
=
i
-
1
,
j
=
1
,
…
,
m
.According to the Quest algorithm, an attitude matrixC
c
f can be found to minimize the loss function
(8)
L
(
C
c
f
)
=
1
2
∑
j
=
1
m
a
j
W
j
-
C
c
f
·
V
j
2
,
where a
j is the weight of the jth vector pair which is defined as
(9)
a
j
=
X
i
f
∑
i
=
2
m
+
1
X
i
f
.The algorithm of optimal processing for (8) is presented in the literature [22].


### 3.3. Strategy for Updating the Target Feature Coordinate System

As the target satellite is tumbling in space, some feature points will be lost and new feature points will appear in the view of the stereo vision system. The new target feature coordinate systemΣ
f
,
1 is established dynamically according to the measurement requirements. It is necessary to determine the offset and orientation between the frames Σ
f
,
1 and Σ
f
,
0(Σ
f
,
0 is the original target feature coordinate system and the rotation angle between the frame Σ
f
,
0 and Σ
t is 0) to keep continuous pose estimation.According to (4) and (5), accurate coordinates of the feature points in Σ
f
,
0 are obtained. Three new feature points p
l, p
m, and p
n are selected among the feature point set to establish Σ
f
,
1 with p
l as the new origin. The offset of the origin of the frame Σ
f
,
1 in Σ
f
,
0 is (
x
^
l
,
y
^
l
,
z
^
l
), which is denoted as ρ
f
,
0
f
,
1. Similar to (2), the orientation of the frame Σ
f
,
1 relative to Σ
f
,
0 can be described as
(10)
C
f
,
0
f
,
1
=
c
1
0,1
,
c
2
0,1
,
c
3
0,1
,
c
1
0,1
=
p
l
p
m
→
p
l
p
m
→
,
c
3
0,1
=
p
l
p
m
→
×
p
l
p
n
→
p
l
p
m
→
×
p
l
p
n
→
,
c
2
0,1
=
c
3
0,1
×
c
1
0,1
.In general, the relationship between the coordinates of the feature points inΣ
f
,
k and Σ
f
,
k
-
1can be described as
(11)
p
i
f
,
k
-
1
=
C
f
,
k
f
,
k
-
1
·
p
i
f
,
k
+
ρ
f
,
k
-
1
f
,
k
,
where k is the number of establishing target feature coordinate systems, p
i
f
,
k
=
x
i
f
,
k
,
y
i
f
,
k
,
z
i
f
,
k are the coordinates of ith feature point in the frame Σ
f
,
k, p
i
f
,
k
-
1
=
x
i
f
,
k
-
1
,
y
i
f
,
k
-
1
,
z
i
f
,
k
-
1 are the coordinates in Σ
f
,
k
-
1, and ρ
f
,
k
-
1
f
,
k is the translation vector between Σ
f
,
k
-
1 and Σ
f
,
k.As the rotation matrixC
f
,
k
f
,
0 and translation vector ρ
f
,
0
f
,
k between the frames Σ
f
,
k and Σ
f
,
0 can be derived according to (11), the coordinates of any natural feature point in the frame Σ
f
,
0 can be obtained even if feature points which are used to establish the frame Σ
f
,
0 are lost in view.


## 3.1. Attitude Determination Based on TRIAD

There are several feature points tracked by the stereo vision system, which is illustrated in Figure1. Three points p
1, p
2, and p
3 are selected according to geometry and imaging quality of natural feature points to establish the frame Σ
f by right-handed rule. The p
1 is the origin of Σ
f, the axis x
f is in the direction of vector p
1
p
2
→, and the axis z
f is vertical to the plane which contains the three points. The direction of axis y
f obeys the right-hand role. The rotation matrix C
c
f from the frame Σ
c to Σ
f can be determined by direction cosine matrix as follows:
(2)
C
c
f
=
c
1
,
c
2
,
c
3
,
c
1
=
p
1
p
2
→
p
1
p
2
→
,
c
3
=
p
1
p
2
→
×
p
1
p
3
→
p
1
p
2
→
×
p
1
p
3
→
,
c
2
=
c
3
×
c
1
,
where |
| is the magnitude calculation and a
×
b is the representation of the operation of vector cross product. The error of TRIAD can be described as rotation angle ϕ about the Euler axis
(3)
ϕ
2
=
1
2
α
1
2
+
α
2
2
1
+
cs
c
2
θ
,
where θ is the angle between p
1
p
2
→ and p
1
p
3
→, and it produces minimum error when it is 90°. α
1 and α
2 are pointing errors of p
1
p
2
→ and p
1
p
3
→, respectively, and the pointing errors will be reduced with the extension of the line segments. Then, it is concluded that the three points which contribute the largest area are to be selected to obtain better accuracy in practice.


## 3.2. Attitude Determination Based on QUEST

As there are often more than 3 feature points available in the view of stereo vision system, it is better that all available points (the number of points is denoted asm
+
1) in view are utilized to improve the accuracy of relative attitude determination. Then coordinates of all feature points except p
1 in the frame Σ
f can be calculated as follows:
(4)
x
i
f
=
c
1
·
p
1
p
i
,
→
y
i
f
=
c
2
·
p
1
p
i
,
→
z
i
f
=
c
3
·
p
1
p
i
→
,
i
=
2,3
,
…
,
m
+
1
,
where a
·
b is the vector inner product. As measuring is repeated, an average processing is utilized and then more accurate coordinates X
i
f are determined in the frame Σ
f. The components of X
^
i
f are described as
(5)
x
^
i
=
∑
k
=
1
n
x
i
,
k
n
,
y
^
i
=
∑
k
=
1
n
y
i
,
k
n
,
z
^
i
=
∑
k
=
1
n
z
i
,
k
n
,
where n is the number of measurements.A set of observation unit vectorsW
1
,
…
,
W
m are defined as
(6)
W
j
=
X
i
f
X
i
f
,
j
=
i
-
1
,
j
=
1
,
…
,
m
.Similarly, a set of reference unit vectorsV
1
,
…
,
V
m are defined as
(7)
V
j
=
p
1
p
i
→
p
1
p
i
→
,
j
=
i
-
1
,
j
=
1
,
…
,
m
.According to the Quest algorithm, an attitude matrixC
c
f can be found to minimize the loss function
(8)
L
(
C
c
f
)
=
1
2
∑
j
=
1
m
a
j
W
j
-
C
c
f
·
V
j
2
,
where a
j is the weight of the jth vector pair which is defined as
(9)
a
j
=
X
i
f
∑
i
=
2
m
+
1
X
i
f
.The algorithm of optimal processing for (8) is presented in the literature [22].


## 3.3. Strategy for Updating the Target Feature Coordinate System

As the target satellite is tumbling in space, some feature points will be lost and new feature points will appear in the view of the stereo vision system. The new target feature coordinate systemΣ
f
,
1 is established dynamically according to the measurement requirements. It is necessary to determine the offset and orientation between the frames Σ
f
,
1 and Σ
f
,
0(Σ
f
,
0 is the original target feature coordinate system and the rotation angle between the frame Σ
f
,
0 and Σ
t is 0) to keep continuous pose estimation.According to (4) and (5), accurate coordinates of the feature points in Σ
f
,
0 are obtained. Three new feature points p
l, p
m, and p
n are selected among the feature point set to establish Σ
f
,
1 with p
l as the new origin. The offset of the origin of the frame Σ
f
,
1 in Σ
f
,
0 is (
x
^
l
,
y
^
l
,
z
^
l
), which is denoted as ρ
f
,
0
f
,
1. Similar to (2), the orientation of the frame Σ
f
,
1 relative to Σ
f
,
0 can be described as
(10)
C
f
,
0
f
,
1
=
c
1
0,1
,
c
2
0,1
,
c
3
0,1
,
c
1
0,1
=
p
l
p
m
→
p
l
p
m
→
,
c
3
0,1
=
p
l
p
m
→
×
p
l
p
n
→
p
l
p
m
→
×
p
l
p
n
→
,
c
2
0,1
=
c
3
0,1
×
c
1
0,1
.In general, the relationship between the coordinates of the feature points inΣ
f
,
k and Σ
f
,
k
-
1can be described as
(11)
p
i
f
,
k
-
1
=
C
f
,
k
f
,
k
-
1
·
p
i
f
,
k
+
ρ
f
,
k
-
1
f
,
k
,
where k is the number of establishing target feature coordinate systems, p
i
f
,
k
=
x
i
f
,
k
,
y
i
f
,
k
,
z
i
f
,
k are the coordinates of ith feature point in the frame Σ
f
,
k, p
i
f
,
k
-
1
=
x
i
f
,
k
-
1
,
y
i
f
,
k
-
1
,
z
i
f
,
k
-
1 are the coordinates in Σ
f
,
k
-
1, and ρ
f
,
k
-
1
f
,
k is the translation vector between Σ
f
,
k
-
1 and Σ
f
,
k.As the rotation matrixC
f
,
k
f
,
0 and translation vector ρ
f
,
0
f
,
k between the frames Σ
f
,
k and Σ
f
,
0 can be derived according to (11), the coordinates of any natural feature point in the frame Σ
f
,
0 can be obtained even if feature points which are used to establish the frame Σ
f
,
0 are lost in view.


## 4. Estimation of the Location of Mass Center and Translation Parameters

In order to estimate the location of the mass center for target satellite and the relative position and velocity between chaser and target, a standard Kalman filter is designed to process the coordinates of feature points with the CW equations as the dynamic model.


### 4.1. CW Equations

When two satellites are nearly in the same circular orbit with close distance, the CW equations can perfectly describe the translational motion dynamics,(12)
ρ
˙
ρ
¨
=
0
3
×
3
I
3
×
3
K
n
-
2
n
×
ρ
ρ
˙
+
0
3
×
3
I
3
×
3
η
,
where n
=
0
0
n, n
×
=
0
-
n
0
n
0
0
0
0
0, and K
n
=
-
3
n
2
0
0
0
0
0
0
0
n
2; it is due to the effect of orbital mechanics, 0
j
×
j and I
j
×
j are identity and zero matrices, respectively, j is matrix dimension, and η is the force disturbance on a unit mass. ρ is the displacement vector from the target mass center to the chaser mass center and it is expressed in the target orbital frame Σ
o
′, and ρ
˙ is the relative velocity between target and chaser.


### 4.2. Measurement Model

The estimation of relative position and velocity is also the process of locating the target mass center. When a series of feature points are continuously tracked, the coordinates of feature points are outputted in the frameΣ
c. The relative attitude between the target satellite and the stereo vision system is determined according to methods in Section 3. According to the geometrical relationship, the displacement vector ρ is described as
(13)
ρ
=
C
t
o
′
r
i
+
b
-
ρ
i
,
where r
i is the coordinate value of the ith natural feature point in the frame Σ
r, b is the location error of mass center which is illustrated as o
t
o
r
→ in Figure 1, ρ
i is the vector from the origin of the frame Σ
s to the ith feature point and is expressed in the frame Σ
o
′, and C
o
′
t is the rotation matrix from the frame Σ
o
′ to Σ
t. As the target is a malfunctioned satellite which is noncooperative, no information including the attitude parameters can be provided to the service spacecraft. An available way to measure the target attitude is combining the relative attitude and service spacecraft attitude. It is written as
(14)
C
t
o
′
=
C
o
o
′
·
C
s
o
·
C
c
s
·
C
f
,
k
c
·
C
t
f
,
k
≈
C
s
o
·
C
c
s
·
C
f
,
k
c
·
C
t
f
,
k
=
C
t
o
,
where C
o
s is the rotation matrix from the frame Σ
o to the frame Σ
s which will be provided by attitude and orbit control system (AOCS), C
o
o
′ is ignored as it is nearly a 3 × 3 identity matrix, C
c
s is defined in Section 2.4, C
f
,
k
c is determined by methods described in Sections 3.1 and 3.2, and C
t
f
,
k is rewritten as
(15)
C
t
f
,
k
=
C
f
,
k
-
1
f
,
k
·
C
f
,
k
-
2
f
,
k
-
1
⋯
C
f
,
0
t
f
,
1
.ρ
i can be described as
(16)
ρ
i
=
C
o
o
′
·
C
s
o
·
C
c
s
·
ρ
c
,
i
+
ρ
s
c
≈
C
s
o
·
C
c
s
·
ρ
c
,
i
+
ρ
s
c
,
where ρ
c
,
i is the coordinates of the ith natural feature point in the frame Σ
c, which is measured by stereo vision system, and C
c
s and ρ
s
c are defined in Section 2.4.There are several natural feature points on the surface of the target spacecraft. If stereo vision system tracks these feature points simultaneously during the relative pose estimation, better measurements will be achieved by an averaging operation. Substituting (14) into (13), the observation equations can be rewritten as
(17)
∑
i
=
1
n
C
t
o
·
r
i
-
ρ
i
n
=
ρ
-
C
t
o
·
b
+
ε
,
where ε is the measurement noise. If stereo vision system is carefully calibrated, the coordinate error of a feature point can be approximately regarded as white noise. Therefore, ε is approximately modeled as zero mean white Gaussian process with covariance matrix of R
1. Obviously,
(18)
b
˙
=
0
.


### 4.3. Filter Design

In order to suppress measurement noise and estimate states which cannot be measured directly, a standard Kalman filter is designed.Define state variables as follows:(19)
X
1
T
=
ρ
T
ρ
˙
T
b
T
.Then, the relative position and velocity estimation dynamic equations can be concluded as follows:(20)
ρ
˙
ρ
¨
b
˙
=
0
3
×
3
I
3
0
K
n
-
2
n
×
0
0
0
0
ρ
ρ
˙
b
+
0
3
×
3
I
3
0
3
×
3
η
.The math model of estimating mass center location and translational states is a linear system according to (17) and (20). Setting the linear system (20) into the standard state-space form X
˙
1
=
A
1
X
1
+
B
1
η, the solution to the corresponding state transfer matrix Φ
1
,
k
/
k
-
1 and covariance matrix of the discrete-time process noise Q
1
,
k is as follows:
(21)
Φ
1
,
k
/
k
-
1
=
I
+
A
1
,
k
-
1
·
T
,
Q
1
,
k
=
T
I
+
A
1
,
k
-
1
·
T
2
·
η
η
T
I
+
A
1
,
k
-
1
·
T
2
T
T
.Equation (17) is rewritten as measurement equations:
(22)
z
1
,
k
=
H
1
,
k
·
X
1
,
k
+
ε
k
.Initialization is(23)
X
^
1,0
=
E
X
1,0
,
P
1,0
=
E
X
1,0
-
E
X
1,0
X
1,0
-
E
X
1,0
T
.Time update is(24)
X
^
1
,
k
/
k
-
1
=
Φ
1
,
k
/
k
-
1
·
X
^
1
,
k
-
1
,
P
1
,
k
/
k
-
1
=
Φ
1
,
k
/
k
-
1
·
P
1
,
k
-
1
·
Φ
1
,
k
/
k
-
1
T
+
Q
1
,
k
-
1
.Measurement update is(25)
K
1
,
k
=
P
1
,
k
/
k
-
1
·
H
1
,
k
T
·
H
1
,
k
·
P
1
,
k
/
k
-
1
·
H
1
,
k
T
+
R
1
,
k
-
1
,
X
^
1
,
k
=
X
^
1
,
k
/
k
-
1
+
K
1
,
k
z
1
,
k
-
H
1
,
k
·
X
^
1
,
k
/
k
-
1
,
P
1
,
k
=
I
-
K
1
,
k
·
H
1
,
k
·
P
1
,
k
/
k
-
1
.


## 4.1. CW Equations

When two satellites are nearly in the same circular orbit with close distance, the CW equations can perfectly describe the translational motion dynamics,(12)
ρ
˙
ρ
¨
=
0
3
×
3
I
3
×
3
K
n
-
2
n
×
ρ
ρ
˙
+
0
3
×
3
I
3
×
3
η
,
where n
=
0
0
n, n
×
=
0
-
n
0
n
0
0
0
0
0, and K
n
=
-
3
n
2
0
0
0
0
0
0
0
n
2; it is due to the effect of orbital mechanics, 0
j
×
j and I
j
×
j are identity and zero matrices, respectively, j is matrix dimension, and η is the force disturbance on a unit mass. ρ is the displacement vector from the target mass center to the chaser mass center and it is expressed in the target orbital frame Σ
o
′, and ρ
˙ is the relative velocity between target and chaser.


## 4.2. Measurement Model

The estimation of relative position and velocity is also the process of locating the target mass center. When a series of feature points are continuously tracked, the coordinates of feature points are outputted in the frameΣ
c. The relative attitude between the target satellite and the stereo vision system is determined according to methods in Section 3. According to the geometrical relationship, the displacement vector ρ is described as
(13)
ρ
=
C
t
o
′
r
i
+
b
-
ρ
i
,
where r
i is the coordinate value of the ith natural feature point in the frame Σ
r, b is the location error of mass center which is illustrated as o
t
o
r
→ in Figure 1, ρ
i is the vector from the origin of the frame Σ
s to the ith feature point and is expressed in the frame Σ
o
′, and C
o
′
t is the rotation matrix from the frame Σ
o
′ to Σ
t. As the target is a malfunctioned satellite which is noncooperative, no information including the attitude parameters can be provided to the service spacecraft. An available way to measure the target attitude is combining the relative attitude and service spacecraft attitude. It is written as
(14)
C
t
o
′
=
C
o
o
′
·
C
s
o
·
C
c
s
·
C
f
,
k
c
·
C
t
f
,
k
≈
C
s
o
·
C
c
s
·
C
f
,
k
c
·
C
t
f
,
k
=
C
t
o
,
where C
o
s is the rotation matrix from the frame Σ
o to the frame Σ
s which will be provided by attitude and orbit control system (AOCS), C
o
o
′ is ignored as it is nearly a 3 × 3 identity matrix, C
c
s is defined in Section 2.4, C
f
,
k
c is determined by methods described in Sections 3.1 and 3.2, and C
t
f
,
k is rewritten as
(15)
C
t
f
,
k
=
C
f
,
k
-
1
f
,
k
·
C
f
,
k
-
2
f
,
k
-
1
⋯
C
f
,
0
t
f
,
1
.ρ
i can be described as
(16)
ρ
i
=
C
o
o
′
·
C
s
o
·
C
c
s
·
ρ
c
,
i
+
ρ
s
c
≈
C
s
o
·
C
c
s
·
ρ
c
,
i
+
ρ
s
c
,
where ρ
c
,
i is the coordinates of the ith natural feature point in the frame Σ
c, which is measured by stereo vision system, and C
c
s and ρ
s
c are defined in Section 2.4.There are several natural feature points on the surface of the target spacecraft. If stereo vision system tracks these feature points simultaneously during the relative pose estimation, better measurements will be achieved by an averaging operation. Substituting (14) into (13), the observation equations can be rewritten as
(17)
∑
i
=
1
n
C
t
o
·
r
i
-
ρ
i
n
=
ρ
-
C
t
o
·
b
+
ε
,
where ε is the measurement noise. If stereo vision system is carefully calibrated, the coordinate error of a feature point can be approximately regarded as white noise. Therefore, ε is approximately modeled as zero mean white Gaussian process with covariance matrix of R
1. Obviously,
(18)
b
˙
=
0
.


## 4.3. Filter Design

In order to suppress measurement noise and estimate states which cannot be measured directly, a standard Kalman filter is designed.Define state variables as follows:(19)
X
1
T
=
ρ
T
ρ
˙
T
b
T
.Then, the relative position and velocity estimation dynamic equations can be concluded as follows:(20)
ρ
˙
ρ
¨
b
˙
=
0
3
×
3
I
3
0
K
n
-
2
n
×
0
0
0
0
ρ
ρ
˙
b
+
0
3
×
3
I
3
0
3
×
3
η
.The math model of estimating mass center location and translational states is a linear system according to (17) and (20). Setting the linear system (20) into the standard state-space form X
˙
1
=
A
1
X
1
+
B
1
η, the solution to the corresponding state transfer matrix Φ
1
,
k
/
k
-
1 and covariance matrix of the discrete-time process noise Q
1
,
k is as follows:
(21)
Φ
1
,
k
/
k
-
1
=
I
+
A
1
,
k
-
1
·
T
,
Q
1
,
k
=
T
I
+
A
1
,
k
-
1
·
T
2
·
η
η
T
I
+
A
1
,
k
-
1
·
T
2
T
T
.Equation (17) is rewritten as measurement equations:
(22)
z
1
,
k
=
H
1
,
k
·
X
1
,
k
+
ε
k
.Initialization is(23)
X
^
1,0
=
E
X
1,0
,
P
1,0
=
E
X
1,0
-
E
X
1,0
X
1,0
-
E
X
1,0
T
.Time update is(24)
X
^
1
,
k
/
k
-
1
=
Φ
1
,
k
/
k
-
1
·
X
^
1
,
k
-
1
,
P
1
,
k
/
k
-
1
=
Φ
1
,
k
/
k
-
1
·
P
1
,
k
-
1
·
Φ
1
,
k
/
k
-
1
T
+
Q
1
,
k
-
1
.Measurement update is(25)
K
1
,
k
=
P
1
,
k
/
k
-
1
·
H
1
,
k
T
·
H
1
,
k
·
P
1
,
k
/
k
-
1
·
H
1
,
k
T
+
R
1
,
k
-
1
,
X
^
1
,
k
=
X
^
1
,
k
/
k
-
1
+
K
1
,
k
z
1
,
k
-
H
1
,
k
·
X
^
1
,
k
/
k
-
1
,
P
1
,
k
=
I
-
K
1
,
k
·
H
1
,
k
·
P
1
,
k
/
k
-
1
.


## 5. Estimation of the Moment-of-Inertia Ratios and Orientation Parameters

As noncooperative target satellite is often a malfunctioned satellite that is tumbling freely with unknown rates in space, the moment-of-inertia ratios can be estimated instead of the moment-of-inertia. Because the axes of the frameΣ
t are not aligned with three body axes of symmetry, the products of inertia cannot be neglected.


### 5.1. Measurement Model

The AOCS of the service spacecraft can also output the attitude relative to inertial system. A measurement of the target attitude will be provided by combining the relative attitude and service spacecraft attitude. Then,(26)
C
i
t
=
C
c
t
·
C
s
c
·
C
i
s
,
where C
i
s is the attitude of service satellite relative to the frame Σ
i, C
s
c is described in Section 2, and C
t
c is C
f
,
k
c
·
C
t
f
,
k which is described in (14). The rotation matrix C
i
t can be rewritten in quaternion form q
i
t and q
i
t can be expressed as
(27)
q
i
t
=
q
⊗
q
υ
,
where q
i
t is target quaternion measurement, q is the quaternion which defines the rotation from inertial to the target body frame, and q
υ is the measurement noise. The symbol ⊗ designates the multiplication of quaternion.


### 5.2. System Dynamic Equations

The time-derivative of the quaternion can be expressed as follows:(28)
q
˙
=
0.5
·
q
⊗
ω
-
,
where ω
-
=
0
ω
T
T, and ω is angular velocity of target satellite with respect to inertial space, resolved in target body-fixed coordinate system Σ
t.According to Euler’s equation, dynamics of the rotational motion for a rigid target satellite can be expressed as(29)
I
′
·
ω
˙
=
-
ω
×
I
′
·
ω
+
w
′
,
where w
′ is disturbing torque vector and I
′ is the inertia matrix,
(30)
I
′
=
I
x
x
′
I
x
y
′
I
x
z
′
I
x
y
′
I
y
y
′
I
y
z
′
I
x
z
′
I
y
z
′
I
z
z
′
.We define the elementI
x
x
′ as 1, and then we obtain the moment-of-inertia ratios matrix as
(31)
I
=
I
′
I
x
x
′
=
1
I
x
y
I
x
z
I
x
y
I
y
y
I
y
z
I
x
z
I
y
z
I
z
z
.Then, (29) can be rewritten as
(32)
I
·
ω
˙
=
-
ω
×
I
·
ω
+
w
,
where w
=
w
′
/
I
x
x
′ and the elements I
y
y, I
z
z, I
x
y, I
x
z, and I
y
z are constants. For the dynamic equations are nonlinear system, EKF and UKF will be employed to estimate the moment-of-inertia ratios and the relative attitude.


### 5.3. EKF Filter Design

Define state variables as follows:(33)
X
2
T
=
q
T
ω
T
Ι
v
T
,
where Ι
v
T
=
[
I
y
y
,
I
z
z
,
I
x
y
,
I
x
z
,
I
y
z
]. When dynamic equations are linearized, δ
q
0 is ignored as δ
q
0 is not an independent variable and it has variations of only the second order. Then, the error-state vector δ
X
2 is described as
(34)
δ
X
2
T
=
δ
q
13
T
δ
ω
T
δ
Ι
v
T
,
where the components of δ
X
2 are defined as follows:
(35)
δ
q
=
q
^
-
1
⊗
q
,
δ
ω
=
ω
-
ω
^
,
δ
I
=
I
-
I
^
,
where δ
q
=
1
δ
q
13
T
T.The dynamic equations can be given as(36)
X
˙
2
=
f
X
2
=
0.5
·
q
⊗
ω
-
-
I
-
1
ω
×
I
·
ω
+
I
-
1
w
0
.For simplicity, process equations (36) and measurement equations (27) are rewritten as
(37)
X
˙
2
,
k
-
1
=
f
X
2
,
k
-
1
,
w
k
-
1
,
q
i
,
k
t
=
h
X
2
,
k
,
q
υ
,
k
,
where the state noise w
k and the measurement noise q
υ
,
k are assumed to be independent of each other, white noise.Define the state transfer matrixΦ
2
,
k
/
k
-
1
(38)
Φ
2
,
k
/
k
-
1
=
I
+
A
2
,
k
-
1
·
T
,
where A
2
,
k
-
1 is the Jacobian matrix of partial derivatives of f
(
) with respect to state variable X
2; that is,
(39)
A
2
,
k
-
1
=
∂
f
(
X
2
)
∂
X
2
X
2
=
X
^
2
,
k
-
1
=
-
ω
^
×
0.5
·
I
3
×
3
0
3
×
5
0
3
×
3
N
I
^
,
ω
^
M
I
^
,
ω
^
0
5
×
3
0
5
×
3
0
5
×
5
11
×
11
,
where
(40)
M
I
^
,
ω
^
=
I
^
-
1
·
0
0
m
2
m
3
0
m
2
0
m
1
0
m
3
0
m
3
0
m
1
m
2
-
ω
^
×
·
0
0
ω
^
2
ω
^
3
0
ω
^
2
0
ω
^
1
0
ω
^
3
0
ω
^
3
0
ω
^
1
ω
^
2
,
m
1
m
2
m
3
T
=
I
^
-
1
ω
^
×
Ι
^
ω
^
,
N
I
^
,
ω
^
=
I
^
-
1
·
Ι
^
ω
^
×
-
ω
^
×
·
Ι
^
.If (27) is quaternion multiplied by q
^
-
1 in left, the linearized measurement equation is obtained by ignoring the high order terms,
(41)
δ
q
i
,
13
,
k
t
≈
δ
q
13
,
k
+
q
υ
,
13
,
k
.For simplicity, (41) is rewritten as
(42)
z
2
,
k
=
H
2
,
k
·
X
2
,
k
+
υ
k
.Initialization is(43)
X
^
2,0
=
E
X
2,0
,
P
2,0
=
E
X
2,0
-
E
X
2,0
X
2,0
-
E
X
2,0
T
.Time update is(44)
X
2
,
k
/
k
-
1
=
F
X
2
,
k
-
1
,
w
k
-
1
,
P
2
,
k
/
k
-
1
=
Φ
2
,
k
/
k
-
1
·
P
2
,
k
-
1
·
Φ
2
,
k
/
k
-
1
T
+
Q
2
,
k
-
1
.Measurement update is(45)
K
2
,
k
=
P
2
,
k
/
k
-
1
·
H
2
,
k
T
·
H
2
,
k
·
P
2
,
k
/
k
-
1
·
H
2
,
k
T
+
R
k
-
1
,
δ
X
^
2
,
k
=
K
2
,
k
·
z
2
,
k
,
P
2
,
k
=
I
-
K
2
,
k
·
H
2
,
k
·
P
2
,
k
/
k
-
1
.Therefore, the estimated statesq
^
k, ω
^
k, and I
^
k can be innovated right after measurement update according to (35).


### 5.4. UKF Filter Design

Although the EKF is the estimation algorithm which is widely used for nonlinear systems, UKF is more accurate when the model is highly nonlinear [23, 24]. Therefore, a UKF filter is designed to achieve a better estimation.The initialization of UKF is the same to EKF. Fork
=
1,2
,
…, calculate sigma points:
(46)
χ
2
,
k
-
1
=
X
^
2
,
k
-
1
X
^
2
,
k
-
1
+
n
+
λ
P
2
,
k
-
1
X
^
2
,
k
-
1
-
n
+
λ
P
2
,
k
-
1
,
where n is the number of states, and λ is the scaling factor for the sigma points which is calculated as
(47)
λ
=
α
2
n
+
k
-
n
,
where α is the scaling parameter which determines the spread of the sigma points around and is usually set to be a small positive value (0.0001
<
α
<
1), and k is a secondly scaling parameter which is usually set to 0.Time update is(48)
χ
2
,
k
/
k
-
1
i
=
F
χ
2
,
k
-
1
i
,
X
^
2
,
k
/
k
-
1
=
∑
i
=
0
2
n
W
i
(
m
)
χ
2
,
k
/
k
-
1
i
,
P
2
,
k
/
k
-
1
=
∑
i
=
0
2
n
W
i
(
c
)
χ
2
,
k
/
k
-
1
i
-
X
^
2
,
k
/
k
-
1
χ
2
,
k
/
k
-
1
i
-
X
^
2
,
k
/
k
-
1
T
,
γ
2
,
i
k
/
k
-
1
=
h
(
χ
2
,
k
/
k
-
1
i
)
,
z
^
2
,
k
/
k
-
1
=
∑
i
=
0
2
n
W
i
(
m
)
γ
2
,
k
/
k
-
1
i
,
where W
i
(
m
) and W
i
(
c
) are the weighted coefficients and defined as follows:
(49)
W
0
(
m
)
=
λ
n
+
λ
,
W
0
(
c
)
=
λ
n
+
λ
+
1
-
α
2
+
β
,
W
i
m
=
W
i
c
=
0.5
n
+
λ
,
i
=
1
,
…
,
2
n
,
where β is the scaling parameter and is used to incorporate prior knowledge of the distribution of X
2 (for Gaussian distribution, β
=
2 is optimal).Measurement update is(50)
P
2
,
y
k
y
k
=
∑
i
=
0
2
n
W
i
(
c
)
[
γ
2
,
k
/
k
-
1
i
-
z
^
2
,
k
/
k
-
1
]
γ
2
,
k
/
k
-
1
i
-
z
^
2
,
k
/
k
-
1
T
+
R
k
,
P
2
,
x
k
y
k
=
∑
i
=
0
2
n
W
i
(
c
)
[
χ
2
,
k
/
k
-
1
i
-
X
^
2
,
k
/
k
-
1
]
γ
2
,
k
/
k
-
1
i
-
z
^
2
,
k
/
k
-
1
T
,
K
2
,
k
=
P
2
,
x
k
y
k
P
2
,
y
k
y
k
-
1
,
δ
X
^
2
,
k
=
K
2
,
k
·
z
2
,
k
,
P
2
,
k
=
P
2
,
k
/
k
-
1
-
K
2
,
k
P
2
,
y
k
y
k
K
2
,
k
T
,
where z
2
,
k, q
^
k, ω
^
k, and I
^
k are calculated by using the same method described in Section 5.3.


## 5.1. Measurement Model

The AOCS of the service spacecraft can also output the attitude relative to inertial system. A measurement of the target attitude will be provided by combining the relative attitude and service spacecraft attitude. Then,(26)
C
i
t
=
C
c
t
·
C
s
c
·
C
i
s
,
where C
i
s is the attitude of service satellite relative to the frame Σ
i, C
s
c is described in Section 2, and C
t
c is C
f
,
k
c
·
C
t
f
,
k which is described in (14). The rotation matrix C
i
t can be rewritten in quaternion form q
i
t and q
i
t can be expressed as
(27)
q
i
t
=
q
⊗
q
υ
,
where q
i
t is target quaternion measurement, q is the quaternion which defines the rotation from inertial to the target body frame, and q
υ is the measurement noise. The symbol ⊗ designates the multiplication of quaternion.


## 5.2. System Dynamic Equations

The time-derivative of the quaternion can be expressed as follows:(28)
q
˙
=
0.5
·
q
⊗
ω
-
,
where ω
-
=
0
ω
T
T, and ω is angular velocity of target satellite with respect to inertial space, resolved in target body-fixed coordinate system Σ
t.According to Euler’s equation, dynamics of the rotational motion for a rigid target satellite can be expressed as(29)
I
′
·
ω
˙
=
-
ω
×
I
′
·
ω
+
w
′
,
where w
′ is disturbing torque vector and I
′ is the inertia matrix,
(30)
I
′
=
I
x
x
′
I
x
y
′
I
x
z
′
I
x
y
′
I
y
y
′
I
y
z
′
I
x
z
′
I
y
z
′
I
z
z
′
.We define the elementI
x
x
′ as 1, and then we obtain the moment-of-inertia ratios matrix as
(31)
I
=
I
′
I
x
x
′
=
1
I
x
y
I
x
z
I
x
y
I
y
y
I
y
z
I
x
z
I
y
z
I
z
z
.Then, (29) can be rewritten as
(32)
I
·
ω
˙
=
-
ω
×
I
·
ω
+
w
,
where w
=
w
′
/
I
x
x
′ and the elements I
y
y, I
z
z, I
x
y, I
x
z, and I
y
z are constants. For the dynamic equations are nonlinear system, EKF and UKF will be employed to estimate the moment-of-inertia ratios and the relative attitude.


## 5.3. EKF Filter Design

Define state variables as follows:(33)
X
2
T
=
q
T
ω
T
Ι
v
T
,
where Ι
v
T
=
[
I
y
y
,
I
z
z
,
I
x
y
,
I
x
z
,
I
y
z
]. When dynamic equations are linearized, δ
q
0 is ignored as δ
q
0 is not an independent variable and it has variations of only the second order. Then, the error-state vector δ
X
2 is described as
(34)
δ
X
2
T
=
δ
q
13
T
δ
ω
T
δ
Ι
v
T
,
where the components of δ
X
2 are defined as follows:
(35)
δ
q
=
q
^
-
1
⊗
q
,
δ
ω
=
ω
-
ω
^
,
δ
I
=
I
-
I
^
,
where δ
q
=
1
δ
q
13
T
T.The dynamic equations can be given as(36)
X
˙
2
=
f
X
2
=
0.5
·
q
⊗
ω
-
-
I
-
1
ω
×
I
·
ω
+
I
-
1
w
0
.For simplicity, process equations (36) and measurement equations (27) are rewritten as
(37)
X
˙
2
,
k
-
1
=
f
X
2
,
k
-
1
,
w
k
-
1
,
q
i
,
k
t
=
h
X
2
,
k
,
q
υ
,
k
,
where the state noise w
k and the measurement noise q
υ
,
k are assumed to be independent of each other, white noise.Define the state transfer matrixΦ
2
,
k
/
k
-
1
(38)
Φ
2
,
k
/
k
-
1
=
I
+
A
2
,
k
-
1
·
T
,
where A
2
,
k
-
1 is the Jacobian matrix of partial derivatives of f
(
) with respect to state variable X
2; that is,
(39)
A
2
,
k
-
1
=
∂
f
(
X
2
)
∂
X
2
X
2
=
X
^
2
,
k
-
1
=
-
ω
^
×
0.5
·
I
3
×
3
0
3
×
5
0
3
×
3
N
I
^
,
ω
^
M
I
^
,
ω
^
0
5
×
3
0
5
×
3
0
5
×
5
11
×
11
,
where
(40)
M
I
^
,
ω
^
=
I
^
-
1
·
0
0
m
2
m
3
0
m
2
0
m
1
0
m
3
0
m
3
0
m
1
m
2
-
ω
^
×
·
0
0
ω
^
2
ω
^
3
0
ω
^
2
0
ω
^
1
0
ω
^
3
0
ω
^
3
0
ω
^
1
ω
^
2
,
m
1
m
2
m
3
T
=
I
^
-
1
ω
^
×
Ι
^
ω
^
,
N
I
^
,
ω
^
=
I
^
-
1
·
Ι
^
ω
^
×
-
ω
^
×
·
Ι
^
.If (27) is quaternion multiplied by q
^
-
1 in left, the linearized measurement equation is obtained by ignoring the high order terms,
(41)
δ
q
i
,
13
,
k
t
≈
δ
q
13
,
k
+
q
υ
,
13
,
k
.For simplicity, (41) is rewritten as
(42)
z
2
,
k
=
H
2
,
k
·
X
2
,
k
+
υ
k
.Initialization is(43)
X
^
2,0
=
E
X
2,0
,
P
2,0
=
E
X
2,0
-
E
X
2,0
X
2,0
-
E
X
2,0
T
.Time update is(44)
X
2
,
k
/
k
-
1
=
F
X
2
,
k
-
1
,
w
k
-
1
,
P
2
,
k
/
k
-
1
=
Φ
2
,
k
/
k
-
1
·
P
2
,
k
-
1
·
Φ
2
,
k
/
k
-
1
T
+
Q
2
,
k
-
1
.Measurement update is(45)
K
2
,
k
=
P
2
,
k
/
k
-
1
·
H
2
,
k
T
·
H
2
,
k
·
P
2
,
k
/
k
-
1
·
H
2
,
k
T
+
R
k
-
1
,
δ
X
^
2
,
k
=
K
2
,
k
·
z
2
,
k
,
P
2
,
k
=
I
-
K
2
,
k
·
H
2
,
k
·
P
2
,
k
/
k
-
1
.Therefore, the estimated statesq
^
k, ω
^
k, and I
^
k can be innovated right after measurement update according to (35).


## 5.4. UKF Filter Design

Although the EKF is the estimation algorithm which is widely used for nonlinear systems, UKF is more accurate when the model is highly nonlinear [23, 24]. Therefore, a UKF filter is designed to achieve a better estimation.The initialization of UKF is the same to EKF. Fork
=
1,2
,
…, calculate sigma points:
(46)
χ
2
,
k
-
1
=
X
^
2
,
k
-
1
X
^
2
,
k
-
1
+
n
+
λ
P
2
,
k
-
1
X
^
2
,
k
-
1
-
n
+
λ
P
2
,
k
-
1
,
where n is the number of states, and λ is the scaling factor for the sigma points which is calculated as
(47)
λ
=
α
2
n
+
k
-
n
,
where α is the scaling parameter which determines the spread of the sigma points around and is usually set to be a small positive value (0.0001
<
α
<
1), and k is a secondly scaling parameter which is usually set to 0.Time update is(48)
χ
2
,
k
/
k
-
1
i
=
F
χ
2
,
k
-
1
i
,
X
^
2
,
k
/
k
-
1
=
∑
i
=
0
2
n
W
i
(
m
)
χ
2
,
k
/
k
-
1
i
,
P
2
,
k
/
k
-
1
=
∑
i
=
0
2
n
W
i
(
c
)
χ
2
,
k
/
k
-
1
i
-
X
^
2
,
k
/
k
-
1
χ
2
,
k
/
k
-
1
i
-
X
^
2
,
k
/
k
-
1
T
,
γ
2
,
i
k
/
k
-
1
=
h
(
χ
2
,
k
/
k
-
1
i
)
,
z
^
2
,
k
/
k
-
1
=
∑
i
=
0
2
n
W
i
(
m
)
γ
2
,
k
/
k
-
1
i
,
where W
i
(
m
) and W
i
(
c
) are the weighted coefficients and defined as follows:
(49)
W
0
(
m
)
=
λ
n
+
λ
,
W
0
(
c
)
=
λ
n
+
λ
+
1
-
α
2
+
β
,
W
i
m
=
W
i
c
=
0.5
n
+
λ
,
i
=
1
,
…
,
2
n
,
where β is the scaling parameter and is used to incorporate prior knowledge of the distribution of X
2 (for Gaussian distribution, β
=
2 is optimal).Measurement update is(50)
P
2
,
y
k
y
k
=
∑
i
=
0
2
n
W
i
(
c
)
[
γ
2
,
k
/
k
-
1
i
-
z
^
2
,
k
/
k
-
1
]
γ
2
,
k
/
k
-
1
i
-
z
^
2
,
k
/
k
-
1
T
+
R
k
,
P
2
,
x
k
y
k
=
∑
i
=
0
2
n
W
i
(
c
)
[
χ
2
,
k
/
k
-
1
i
-
X
^
2
,
k
/
k
-
1
]
γ
2
,
k
/
k
-
1
i
-
z
^
2
,
k
/
k
-
1
T
,
K
2
,
k
=
P
2
,
x
k
y
k
P
2
,
y
k
y
k
-
1
,
δ
X
^
2
,
k
=
K
2
,
k
·
z
2
,
k
,
P
2
,
k
=
P
2
,
k
/
k
-
1
-
K
2
,
k
P
2
,
y
k
y
k
K
2
,
k
T
,
where z
2
,
k, q
^
k, ω
^
k, and I
^
k are calculated by using the same method described in Section 5.3.


## 6. Simulation and Evaluation

The computer simulation is developed to verify the validity of the proposed algorithms in this paper. The simulation software integrates the orbital and attitude dynamics, camera photograph model, and relative pose estimation algorithms. The simulation is realized in MATLAB and Figure2 shows the simulation blocks in the MATLAB environment.Figure 2
Realization of the simulation software.It is assumed that the service satellite and the malfunctioned satellite are in the same orbit with different orbital phases. The initial orbit elements of the target and chaser are given in Table1.Table 1
Initial orbit elements.
a  (km)
e
i   (°)
Ω  (°)
ω  (°)
M  (°)
Target
6700
0
0
0
0
0
Chaser
6700
0
0
0
0
−8.5 × 10−5So the orbit altitude is about 300 km and the service satellite stands at the position which is about 10 m behind target satellite.The target satellite is under torque-free tumbling motion. The target satellite is initially aligned with orbital frame and initial angular velocity is assumed asω
t
=
2.5
5.0
3.0
°
/
s. The moment-of-inertia matrix in the target body-fixed coordinate system (Σ
t) is set as
(51)
I
′
=
10
3.0
2.5
3.0
13
1.5
2.5
1.5
12
kg
·
m
2
.Therefore, the corresponding moment-of-inertia ratios matrix is as follows:(52)
I
=
1.00
0.30
0.25
0.30
1.30
0.15
0.25
0.15
1.20
.As the image processing including image filtering, edge detection, and feature matching is discussed in many papers, the image processing is assumed as a solved problem in this paper, and then coordinates of natural feature points in image plane are regarded to be available for the estimation processing. The specifications of feature points in frameΣ
tare shown in Table 2.Table 2
Feature points specifications in the simulation.
Number
x (m)
y (m)
z (m)
1
0.5
-
1.0
0.5
2
0.5
-
1.0
-
0.5
3
-
0.5
-
1.0
0.5
4
-
0.5
-
1.0
-
0.5
5
-
0.5
-
1.0
0.0
6
0.0
-
1.0
-
0.5Two cameras are parallel to each other and the baseline is 0.5 m. The focus length of camera is set as 2.5 cm. The two cameras of the stereo vision system are mounted at[
0,0.55,0.25
] m and [
0,0.55
,
-
0.25
] m in frame Σ
s. The CCD pixel elements are assumed as 2048 × 2048 and the pixel size is 3.2 μm × 3.2 μm. The coordinates of feature points in image plane are corrupted by white Gaussian noises 0.5 pixels (1
σ) to simulate the image processing error.The location errors of mass centero
t
o
r
→ are set as [
-
100,200
,
-
300
] mm and the initial values of the corresponding states are supposed as [
0,0
,
0
] mm. The initial relative position errors are set as [
300,300,300
] mm and initial relative velocity errors are [
50,50,50
] mm/s. The initial relative attitude errors and angular velocity errors are given as 1,2
,
-
1
° and 0.5,0.3
,
-
0.5
°
/
s, respectively.The first 3 points listed in Table2 are selected to establish the frame Σ
f
,
0 by TRIAD and number 1 point is used as the origin. Assume that number 1 point is lost at 10 s and number 2–4 points are selected to establish the new frame Σ
f
,
1 with number 4 point as the origin. According to (10) and (15), target attitude with respect to chaser (the orientation of Σ
f
,
0 relative to Σ
s) can be continuously measured. The attitude determination errors are plotted in Figure 3. In Figure 3, the errors in period of 10 s–300 s vary similarly to the errors in period of 0 s–10 s, which proves that method described in Section 3.3 is feasible.Figure 3
Attitude determination errors using TRIAD.Attitude determination errors using QUEST are shown in Figure4. Obviously, QUEST algorithm achieves a higher precision than TRIAD, because all feature points tracked are utilized. The standard deviations of TRIAD are 0.27°, 0.11°, and 0.26°, and the standard deviations of QUEST are 0.19°, 0.04°, and 0.18°. Then, the TRIAD algorithm is applied to the following simulations.Figure 4
Attitude determination errors using QUEST.The standard Kalman filter for estimating the location of the mass center and the translation parameters is tuned. Filter parameters are defined asP
1,0
=
1
×
1
0
-
2
I
9
×
9, Q
1
=
1
×
1
0
-
8
I
9
×
9, and R
1
,
k
=
4
×
1
0
-
4
I
3
×
3. A series of simulation figures are shown from Figure 5 to Figure 11.Figure 5
Estimation errors of relative position.The estimation errors of relative position and relative velocity are plotted in Figures5 and 6, respectively, and the estimation of mass center location is displayed in Figure 7. The convergence time for recognition of the mass center location and estimating the relative position and velocity is less than 20 s. The accuracy of relative navigation is fairly high with the triaxial relative position errors less than 2.5 mm, 5.5 mm, and 3.0 mm, respectively, and the triaxial relative velocity errors less than 0.2 mm/s, 0.2 mm/s, and 0.2 mm/s, respectively. The location errors of the mass center are less than 2.0 mm, 6.0 mm, and 1.1 mm in three axes. It also should be noticed that Y axial errors are larger than the other two axial errors. The corresponding reason is that the positioning errors of stereo vision system are bigger along the optical axis direction.Figure 6
Estimation errors of relative velocity.Figure 7
Estimation of the mass center location.The EKF and UKF are utilized to estimate the moment-of-inertia ratios and the orientation parameters. In order to compare performances of the two filters, the filter parameters are defined the same asP
2,0
=
8
×
1
0
-
5
I
6
×
6
1
×
1
0
-
1
I
5
×
5
11
×
11, Q
2
=
1
×
1
0
-
12
I
11
×
11, and R
2
,
k
=
2
×
1
0
-
5
I
3
×
3. Other parameters of UKF are tuned as k
=
0, α
=
0.005, and β
=
3.The differences between the referenced attitude of the target and the estimated attitude by using EKF and UKF, which are attitude estimation errors, are plotted together in Figure8. The simulation results show that both estimation algorithms can assure the convergence. Comparing UKF output errors with those from EKF, the accuracy of UKF is better than the accuracy of EKF. In the period of 50 s–300 s, the attitude determination errors of UKF are less than 0.08°, 0.12°, and 0.08° and errors of EKF are less than 0.38°, 0.52°, and 0.34°.Figure 8
Estimation errors of relative attitude.Figure9 shows the estimation errors of angular velocity. The estimation performances of angular velocity of the two algorithms are similar to the performances of attitude. In the same time period, the estimation errors of UKF are less than 0.01°/s, 0.025°/s, and 0.01°/s and errors of EKF are less than 0.038°/s, 0.11°/s, and 0.038°/s.Figure 9
Estimation errors of relative angular velocity.Estimation errors of moment-of-inertia ratios are shown in Figures10 and 11. Although Ι
˙
v
=
0 is the linear model, the convergence time of UKF is shorter than that of EKF as the UKF estimation accuracy for angular velocity is better. However, UKF and EKF achieve similar accuracy for estimating the moment-of-inertia ratios in the period of 100 s–300 s. At the same time, it is noticed that the convergence time of estimating moment-of-inertia ratios is longer than attitude and angular velocity. The reason why the estimation of moment-of-inertia ratios needs a longer convergence time lies in that the moment-of-inertia ratios cannot be observed directly and more time will be required to measure attitude errors which are produced by the errors of moment-of-inertia ratios. The estimation errors are displayed in Table 3 according to errors of UKF in the period of 200 s–250 s.Table 3
Estimation of moment-of-inertia ratios.
I
y
y
/
I
x
x
I
z
z
/
I
x
x
I
x
y
/
I
x
x
I
x
z
/
I
x
x
I
y
z
/
I
x
x
True value
1.3
1.2
0.30
0.25
0.15
Estimation
1.29
1.19
0.30
0.25
0.15Figure 10
Estimation of moment-of-inertia ratios.Figure 11
Estimation of moment-of-inertia ratios.


## 7. Conclusions

In order to capture the malfunctioned satellite, this paper proposes an algorithm for estimating the relative pose and inertial parameters of the target satellite by using a single stereo vision system. Compared to the dense stereo algorithm presented in literature 17, the proposed method can avoid degeneracy when the target has a high degree of axial symmetry and reduce the numbers of sensors and the complexity of the operation. As UKF achieves a better performance than EKF, the UKF is recommended for the rotational estimation. The simulation results demonstrate that the developed algorithm can estimate the relative pose, the location of mass center, and the moment-of-inertia ratios with high precision. However, the proposed method also has degeneracy; if there are no obvious features on the target surface, it is a better choice that the two methods are jointly used in practice. The experimental verification of the proposed algorithm should be conducted to ensure the practical application, which will be a focus in the further study.



---

*Source: 461283-2014-11-25.xml*