Advertisement

Vision-Based Relative Navigation Using Dual Quaternion for Spacecraft Proximity Operations

  • Yunju Na
  • Hyochoong BangEmail author
  • Sung-Hoon Mok
Open Access
Original Paper
  • 293 Downloads

Abstract

Dual quaternion is the parameter that can describe position and attitude in a unified form. It is extension of dual number and quaternion; so, mathematical and parameter characteristics of those are holding. The attitude and orbital kinematics model can be concurrently integrated in a simplified and intuitive formulation with dual quaternion. From these merits, dual quaternion-based relative navigation for spacecraft proximity operation is investigated in this paper. Especially, the case is considered which interested points are not coincident with center of mass of spacecraft. At this time, it is obvious that position is affected by rotational motion of rigid body. With conventional decoupled relative kinematics, this attitude–position coupling effect cannot be described at once. However, the new dual quaternion-based relative kinematics can represent coupled orbital motion in a simple form. Motivated by these backgrounds, in this paper, to estimate relative position and attitude states using vision sensor, dual quaternion-based relative kinematics is formulated. To reduce the number of state variables, error dual quaternion is applied, which makes the number of state variable be six instead of eight, by using parameter constraints of dual quaternion. Extended Kalman filter and unscented Kalman filter are adopted to realize relative navigation. Since the necessity of velocity information in dual quaternion kinematics, two types system models are constructed—velocity propagation and velocity measurement model. In addition, the six line-of-sight measurements are used to update the dual quaternion parameter. Simulation results show that the kinematics model is accurately derived, states are well estimated, and statistics of estimation errors are consistent with estimated error covariance.

Keywords

Dual quaternion Vision-based relative navigation Spacecraft proximity operation Attitude–position coupling Extended Kalman filter Unscented Kalman filter 

1 Introduction

Relative navigation for multiple spacecraft is a key technology for space missions such as formation flying, rendezvous and docking. Thanks to hardware technologies, various and complicated mission designs with multiple spacecraft have become possible; so, precise relative navigation is considered as even more important than before. Especially, in the case of deep space missions being subject to limitation in communication with ground stations, real-time computation of relative navigation solution is required using available measurements only.

Precise solution of navigation is usually based on accurate kinematics. In general, independent relative kinematics for position and attitude has been widely employed. It is valid with point masses assumption if two spacecrafts are located at a distance. However, in the case of close range, relative navigation error tends to increase in size. Segal and Gurfil [1] showed that relative position is affected by attitude change during proximity operation, and it obviously takes place between arbitrary points of the spacecraft not the center of mass. It means that the position–attitude coupling problem requires additional considerations to find the position of an arbitrary point affected by rotational motion. In on-orbit servicing missions such as docking and rendezvous, the interesting points as sensors or docking spots may not be coincident with the center of mass, so that target re-positioning and corresponding control are required for precise proximity operation.

With the background addressed above, three possible approaches are available to address the translation–rotation coupling problem; an indirect way determining position of an arbitrary point from the center of mass, adoption of position–attitude coupled kinematics, and applying the so-called dual quaternion kinematics. In the first approach, conventional relative kinematics is used to formulate the issue between centers of mass of two satellites, and then position of target points is re-calculated. This approach could be regarded as being intuitive and reliable based on geometrical vector calculation, but additional computation is required. The second approach takes position–attitude coupled kinematics, that is relative kinematics substituting position, velocity, and acceleration equations including angular velocity about an arbitrary point [1, 2]. The kinematics does not require additional calculation because the relative angular velocity is involved in the equations of motion; however, it is expressed in a very complicated manner. Motivated by such reasons, this study suggests dual quaternion kinematics, the third approach that is describing translational–rotational motion simultaneously in a simple way.

Dual quaternion-based kinematics is known to be described in a compact form in general, and position and attitude parameters are clearly defined. In this paper, the position–attitude coupling problem is solved using dual quaternion formulation to estimate relative navigation states between two spacecraft, chief and deputy satellites. Translational and rotational motions are constructed with dual quaternion and dual velocity parameters. Because dual quaternion-based kinematics includes translational motion affected by rotational motion, it is possible to determine the position and attitude of an arbitrary point at one time in a compact and simplified form. Moreover, the navigation solution in form of dual quaternion parameter is applicable to control or guidance system which has translation and rotation maneuver complexly.

The dual quaternion introduced by Clifford [3] is the parameter representing position and attitude in a unified form. It is an expansion of the dual number and a unit quaternion; so, parametric and mathematic properties are retained [4, 5]. With its inherent advantages for minimal efforts in computational rigid transformation, it is widely employed in three-dimensional computer graphics and robotics fields. Due to the advantages in performance, dual quaternion-related studies have been gradually increasing in recent years in aerospace engineering applications also for guidance, navigation and control. The feedback landing guidance control laws are introduced to handle the translational and rotational motion of rigid body as a single screw motion [6, 7, 8]. And some adaptive control laws are suggested for spacecraft in relative operation missions [9, 10, 11]. In relative navigation, estimation techniques using extended Kalman filter (EKF) have been intensively studied. Many of those approaches employ dual inertia operator to estimate angular and linear velocity under the assumption of known mass moment of inertia information of spacecraft [12, 13]. Dual quaternion-based relative navigation-applied extended Kalman filter (EKF) is introduced in asteroid circumnavigation considering gravity field and integrated navigation sensor system [14]. In addition, extended and unscented Kalman filter (UKF) using dual quaternion-based twistor parameters are proposed for relative navigation between two spacecraft [15, 16]. Those are based upon dual quaternion measurements with angular, linear and acceleration measurements. However, it is difficult to obtain dual quaternion measurements directly from conventional sensors. As a more practical sensor in spacecraft relative navigation system, the vision sensors can give information for relative position and attitude [17]. Zivan and Choukroun applied dual quaternion-based EKF and UKF to VISNAV system that is configured with beacons and PSD, but PSD is aligned to the center of mass of the spacecraft and measured minimum 15 line-of-sights [18]. Sensor alignment generally does not coincide with the center of mass, thereby causing the position–attitude coupling problem. Thus, this study aims to configure dual quaternion-based relative navigation for an arbitrary point on the spacecraft using six beacons as available vision sensors. This approach to the point of inconsistency with the center of mass shows that estimation of the unknown target point can be extended in the future.

EKF is a principal tool to handle relative attitude and position estimation problem. It successively updates state vectors by applying prior states into a first-order linearized model of nonlinear systems. Estimation problem of nonlinear systems can be established using EKF, but nonlinearity of system and measurement model cannot be fully taken into account with first-order linearization. As another way for nonlinear systems, UKF approach has been popular alternative [19, 20, 21]. Since Jacobian matrix is not required for system model in UKF, it can be helpful to avoid error arising from linearization. The results of EKF and UKF using dual quaternion are compared in vision-based relative navigation system in this paper. First, error dual quaternion kinematics equations between desired and current states are derived in EKF framework. Using error dual quaternion, parameter number can be reduced from eight to six, and it brings an advantage in computational burden for covariance calculation. For UKF, nonlinear dual quaternion-based system and measurement model kinematics are used in the process of calculation of sigma points and unscented transformation. Simulation results show that position and attitude are effectively estimated from dual quaternion-based filter without additional calculation for position correction affected by rotation.

This paper is comprised as follows. Section 2 briefly describes dual quaternion and its mathematical preliminaries. The conventional relative position and attitude kinematics is summarized in the same section. The dual quaternion-based kinematics and relative navigation system targeted in this research are presented in Sect. 3. In Sects. 4 and 5, the EKF using error dual quaternion kinematics and the UKF are derived. Relative navigation simulation results using vision sensor measurements are discussed in Sect. 6, while Sect. 7 provides conclusions.

2 Overview on Dual Quaternion and Relative Kinematics

This section is an overview on dual quaternion and conventional relative kinematics between two spacecraft. The mathematical characteristics of dual quaternion are summarized in subsection. In general, relative position kinematics is described regarding two spacecraft, a chief and a deputy as point masses. And relative attitude kinematics is expressed using quaternion parameters. Following subsections briefly present kinematics as base of the research.

2.1 Quaternion and Dual Quaternion

A unit quaternion as \( q = \left[ {\begin{array}{*{20}c} {q_{0} } & {{\mathbf{q}}_{v}^{T} } \\ \end{array} } \right]^{T} \), defined with vector, \( {\mathbf{q}}_{v} \), and scalar, \( q_{0} \), could be the most widely used attitude parameter in spacecraft. And the parameter \( \theta \) represents rotational angle of object about a principal axis, \( {\mathbf{n}} \).

$$ {\mathbf{q}} = \left[ {\begin{array}{*{20}c} {\cos \frac{\theta }{2}} & {{\mathbf{n}}^{T} \sin \frac{\theta }{2}} \\ \end{array} } \right]^{T} , $$
(1)
whereas quaternion only describes attitude, dual quaternion includes both rigid rotational and translational motion information simultaneously. Dual quaternion is an extension of a unit quaternion and is defined in a similar concept to dual number [3, 4, 5] as
$$ {\bar{\mathbf{q}}} = {\mathbf{q}}_{\text{r}} + \varepsilon {\mathbf{q}}_{\text{d}} . $$
(2)

Dual quaternion consists of two parts: real part, \( {\mathbf{q}}_{\text{r}} \), and dual part, \( {\mathbf{q}}_{\text{d}} \). Each part is formatted in quaternion form and \( \varepsilon \) denotes dual part parameter. Real part is a unit quaternion for rotation and dual part contains translational information combined with rotational part.

$$ {\mathbf{q}}_{\text{r}} = \left[ {\begin{array}{*{20}c} {q_{0} } & {{\mathbf{q}}_{v}^{T} } \\ \end{array} } \right]^{T} , $$
(3)
$$ {\mathbf{q}}_{\text{d}} = \frac{1}{2}{\mathbf{t}} \otimes {\mathbf{q}}_{\text{r}} , $$
(4)
where \( {\mathbf{t}} = \left[ {\begin{array}{*{20}c} 0 & {{\mathbf{r}}^{T} } \\ \end{array} } \right]^{T} \) is also in quaternion form with position vector, \( {\mathbf{r}} \), and zero scalar parameter. And \( \otimes \) denotes quaternion multiplication of two quaternion vectors, \( {\mathbf{q}} \) and \( {\mathbf{p}} \), defined as
$$ {\mathbf{q}} \otimes {\mathbf{p}} = \left[ {\begin{array}{*{20}c} {q_{0} p_{0} - {\mathbf{q}}_{v}^{T} {\mathbf{p}}_{v} } \\ {q_{0} {\mathbf{p}}_{v} + p_{0} {\mathbf{q}}_{v} + {\mathbf{q}}_{v} \times {\mathbf{p}}_{v} } \\ \end{array} } \right]. $$
(5)
Basic mathematical preliminaries of dual quaternion are the same as dual number. For two dual quaternion, \( {\bar{\mathbf{q}}} = {\mathbf{q}}_{\text{r}} + \varepsilon {\mathbf{q}}_{\text{d}} \) and \( {\bar{\mathbf{p}}} = {\mathbf{p}}_{\text{r}} + \varepsilon {\mathbf{p}}_{\text{d}} \), scalar multiplication, addition, dual quaternion multiplication, and conjugate are as follows
$$ \begin{aligned} & s{\bar{\mathbf{q}}} = s{\mathbf{q}}_{\text{r}} + \varepsilon s{\mathbf{q}}_{\text{d}} , \\ & {\bar{\mathbf{q}}} + {\bar{\mathbf{p}}} = \left( {{\mathbf{q}}_{\text{r}} + {\mathbf{p}}_{\text{r}} } \right) + \varepsilon \left( {{\mathbf{q}}_{\text{d}} + {\mathbf{p}}_{\text{d}} } \right) , \\ & {\bar{\mathbf{q}}} \otimes_{\text{d}} {\bar{\mathbf{p}}} = \left( {{\mathbf{q}}_{\text{r}} \otimes {\mathbf{p}}_{\text{r}} } \right) + \varepsilon \left( {{\mathbf{q}}_{\text{r}} \otimes {\mathbf{p}}_{\text{d}} + {\mathbf{q}}_{\text{d}} \otimes {\mathbf{p}}_{\text{r}} } \right) , \\ & {\bar{\mathbf{q}}}^{*} = {\bar{\mathbf{q}}}^{ - 1} = {\mathbf{q}}_{\text{r}}^{*} + \varepsilon {\mathbf{q}}_{\text{d}}^{*} = {\mathbf{q}}_{\text{r}}^{ - 1} + \varepsilon {\mathbf{q}}_{\text{d}}^{ - 1} , \\ \end{aligned} $$
(6)
where \( \otimes_{\text{d}} \) denotes dual quaternion multiplication and * indicates a conjugate operator.

2.2 Relative Orbital Motion

For two spacecrafts, a chief and a deputy satellites, which are in relative orbit, the relative position vector, \( {\varvec{\uprho}} = \left[ {\begin{array}{*{20}c} x & y & z \\ \end{array} } \right]^{T} \), is expressed in the Hill’s coordinate frame, \( \left\{ {{\hat{\mathbf{o}}}_{\text{r}} ,\;{\hat{\mathbf{o}}}_{\theta } ,\;{\hat{\mathbf{o}}}_{\text{h}} } \right\} \), which is pointed to the deputy from the chief as shown in Fig. 1.
Fig. 1

Relative orbital motion between two spacecraft

The details of orbital relative equations of motion are derived in Ref. [17]. With the assumption of small relative distance compared with the chief orbit radius, \( r_{\text{c}} \), relative position kinematics is expressed in a vector form as
$$ {{\ddot{\varvec \uprho }}} + 2{\varvec{\upomega}}_{0} \times {\dot{\varvec{\uprho }}} + {\dot{\varvec{\omega }}}_{0} \times {\varvec{\uprho}} + {\varvec{\upomega}}_{0} \times {\varvec{\upomega}}_{0} \times {\varvec{\uprho}} = - \frac{\mu }{{r_{c}^{3} }}{}^{o}\left[ {\begin{array}{*{20}c} { - 2x} \\ y \\ z \\ \end{array} } \right] + {\mathbf{w}}_{\rho } , $$
(7)
where \( {\varvec{\upomega}}_{0} = \left[ {\begin{array}{*{20}c} 0 & 0 & {\dot{\theta }} \\ \end{array} } \right]^{T} \) is the orbital rate of the chief in rotating Hill’s frame with true anomaly, \( \theta \), of the chief, \( \mu \) is the gravitational parameter, and \( {\mathbf{w}}_{\rho } = \left[ {\begin{array}{*{20}c} {w_{x} } & {w_{y} } & {w_{z} } \\ \end{array} } \right]^{T} \) represents acceleration disturbance modelled as zero-mean Gaussian white-noise processes with variance of \( \sigma_{x}^{2} \), \( \sigma_{y}^{2} \), and \( \sigma_{z}^{2} \).

2.3 Relative Attitude Dynamics

Relative attitude dynamics between the two spacecraft is defined as quaternion multiplication. Let the attitude of the chief and the deputy satellites with respect to inertial frame is \( {\mathbf{q}}_{\text{c}} \) and \( {\mathbf{q}}_{\text{d}} \), relative attitude is \( {\mathbf{q}} \), and relative angular velocity is \( {\varvec{\upomega}} \). Then, the attitude and angular velocity of the deputy satellite with respect to chief are
$$ {\mathbf{q}} = {\mathbf{q}}_{\text{c}}^{*} \otimes {\mathbf{q}}_{\text{d}} , $$
(8)
$${\varvec{\upomega}}= {\mathbf{R}}_{{\mathbf{q}}}^{T} {\varvec{\upomega}}_{\text{d}} - {\varvec{\upomega}}_{\text{c}}, $$
(9)
where \( {\varvec{\upomega}}_{\text{c}} \) and \( {\varvec{\upomega}}_{\text{d}} \) are body angular velocities of the chief and the deputy satellites, and \( {{\bf R}}_{{\mathbf{q}}} \) is the rotational transformation matrix of relative attitude.
$$ {\mathbf{R}}_{{\mathbf{q}}} = \left( {q_{0}^{2} - {\mathbf{q}}_{v}^{T} {\mathbf{q}}_{v} } \right){\mathbf{I}}_{{3 \times 3}} + 2{\mathbf{q}}_{v} {\mathbf{q}}_{v}^{T} - 2q_{0} \left[ {{\mathbf{q}}_{v} \times } \right], $$
(10)
where \( \left[ {{\varvec{\upalpha}} \times } \right] \) is a skew-symmetric matrix form for cross product. Then the relative attitude kinematics to describe rotational motion is defined by
$$ {\dot{\mathbf{q}}} = \frac{1}{2}{\mathbf{q}} \otimes {\varvec{\upomega}}. $$
(11)

3 Dual Quaternion-Based Relative Navigation System

Conventional relative kinematics equations describe translational orbit and rotational motion independently. Otherwise, dual quaternion kinematics can represent both relationships simultaneously; so, it is possible to effectively express translational motion affected by rotation. In this section, relative kinematics equations are constructed using dual quaternion. Additionally, vision-based relative navigation approach is introduced, which is the main subject of this research.

3.1 Dual Quaternion-Based Relative Kinematics

Dual quaternion is defined with real and dual part quaternion or attitude and position vector.

$$ {\bar{\mathbf{q}}} = {\mathbf{q}}_{\text{r}} + \varepsilon {\mathbf{q}}_{\text{d}} = {\mathbf{q}} + \varepsilon \frac{1}{2}\left[ {\begin{array}{*{20}c} 0 \\ {\varvec{\uprho}} \\ \end{array} } \right] \otimes {\mathbf{q}}, $$
(12)
where the real part quaternion means attitude quaternion and dual part quaternion is represented by multiplication of position and attitude. Inversely, from the dual quaternion, attitude and position vector can be obtained as
$$ {\mathbf{q}} = {\mathbf{q}}_{\text{r}} , $$
(13)
$$ {\varvec{\uprho}} = \left( {2{\mathbf{q}}_{\text{d}} \otimes {\mathbf{q}}_{\text{r}}^{*} } \right)_{2:4} . $$
(14)
Dual quaternion kinematics is similarly defined as quaternion part using the dual velocity vector, \( {\bar{\varvec{\omega }}} \).
$$ {\dot{\bar{\mathbf{{{q}}}}}} = \frac{1}{2}{\bar{\mathbf{q}}} \otimes_{\text{d}} {\bar{{\varvec\omega }}}. $$
(15)
The dual velocity also has a real part, \( {\varvec{\upomega}}_{\text{r}} = \left[ {\begin{array}{*{20}c} 0 & {{\varvec{\upomega}}^{T} } \\ \end{array} } \right]^{T} \), for angular velocity and dual part, \( {\mathbf{v}}_{\text{d}} = \left[ {\begin{array}{*{20}c} 0 & {{\mathbf{v}}^{T} } \\ \end{array} } \right]^{T} \) for linear velocity, \( {\mathbf{v}} \).
$$ {\bar{\varvec{\omega }}} = {\varvec{\upomega}}_{\text{r}} + \varepsilon {\mathbf{v}}_{\text{d}} . $$
(16)
Equation (15) can be rewritten using dual quaternion multiplication of Eq. (6), which yields
$$ {{\dot{\bar{\bf q}}}} = \frac{1}{2}{\mathbf{q}}_{\text{r}} \otimes {\varvec{\upomega}}_{\text{r}} + \varepsilon \frac{1}{2}\left( {{\mathbf{q}}_{\text{r}} \otimes {\mathbf{v}}_{\text{d}} + {\mathbf{q}}_{\text{d}} \otimes {\varvec{\upomega}}_{\text{r}} } \right). $$
(17)

3.2 Vision-Based Relative Navigation

This subsection describes the problem set, which is comprised of two satellites as a chief and a deputy under relative orbit motion. It is assumed that two satellites have constant body angular velocities. The chief satellite is equipped with six beacon sensors and the deputy satellite employs position sensing diode (PSD) sensors [17]. The six line-of-sight measurements from beacons can be used to estimate position and attitude. Positions of beacons attached on the chief are known and PSD attached on the deputy is not aligned on the center of mass. Let the relative position vector between centers of mass of the two satellites is \( {\varvec{\uprho}}_{0} = \left[ {\begin{array}{*{20}c} {x_{0} } & {y_{0} } & {z_{0} } \\ \end{array} } \right]^{T} \) and position vector between the chief center of mass and PSD location, i.e., an arbitrary point, of the deputy is \( {\varvec{\uprho}} = \left[ {\begin{array}{*{20}c} x & y & z \\ \end{array} } \right]^{T} \) as shown in Fig. 2. Then, the goal of navigation is to estimate relative attitude and position vector about an arbitrary point, that is, \( {\mathbf{q}} \) and \( {\varvec{\uprho}} \).
Fig. 2

Vision-based navigation system configuration

The vision sensor measurement from \( i \)th beacon is defined in a unit vector form as
$$ {{\tilde{{\mathbf{b}}}}}_{i} = {\mathbf{Ar}}_{i} + {\varvec{\upupsilon}}_{i} ,\quad i = 1,2, \ldots ,N $$
(18)
where \( N \) is the total number of observations, \( {\mathbf{A}} \) is attitude matrix, and \( {\mathbf{r}}_{i} \) is distance vector from \( i \)th beacon to PSD.
$$ {\mathbf{r}}_{i} = \frac{1}{{\sqrt {\left( {X_{i} - x} \right)^{2} + \left( {Y_{i} - y} \right)^{2} + \left( {Z_{i} - z} \right)^{2} } }}\left[ {\begin{array}{*{20}c} {X_{i} - x} \\ {Y_{i} - y} \\ {Z_{i} - z} \\ \end{array} } \right] . $$
(19)
And \( {\varvec{\upupsilon}}_{i} \) is a zero-mean Gaussian white-noise process from sensor with variance of \( \sigma_{i}^{2} \) which satisfies \( E\left\{ {{\varvec{\upupsilon}}_{i} } \right\} = {\mathbf{0}} \) and \( E\left\{ {{\varvec{\upupsilon}}_{i} {\varvec{\upupsilon}}_{i}^{T} } \right\} = \sigma_{i}^{2} \left( {{\mathbf{I}}_{3 \times 3} - {\mathbf{b}}_{i} {\mathbf{b}}_{i}^{T} } \right) \). To obtain relative angular velocity between two spacecraft, the angular velocities for each spacecraft are measured by rate gyro and Eq. (9) is applied. Each gyro measurement, \( {\tilde{\varvec{\omega }}} \), is generally modelled as
$$ {\tilde{\varvec{\omega }}} = {\varvec{\upomega}} + {\varvec{\upbeta}} + {\varvec{\upeta}}_{v} , $$
(20)
$$ {\dot{\varvec{\upbeta }}} = {\varvec{\upeta}}_{u} , $$
(21)
where \( {\varvec{\upomega}} \) is the true angular velocity vector, \( {\varvec{\upbeta}} \) is the bias, and \( {\varvec{\upeta}}_{v} \) and \( {\varvec{\upeta}}_{u} \) are the sensor noise approximated as Gaussian with variances of \( \sigma_{v}^{2} \) and \( \sigma_{u}^{2} \). In this problem, it is assumed that chief and deputy gyro measurements are known a priori and the subscripts of c and d are used to distinguish values belonging to each spacecraft.
In the similar manner to the angular velocity measurement model, the translational velocity measurement model can be defined with the true velocity vector, \( {\mathbf{v}} \), such that
$$ {\tilde{\mathbf{v}}} = {\mathbf{v}} + {\varvec{\upbeta}}_{\text{r}} + {\varvec{\upeta}}_{{{\text{r}}v}} , $$
(22)
$$ {\dot{\varvec{\upbeta }}}_{\text{r}} = {\varvec{\upeta}}_{{{\text{r}}u}} , $$
(23)
where \( {\tilde{\mathbf{v}}} \) is the linear velocity measurement, \( {\varvec{\upbeta}}_{\text{r}} \) is the velocimeter bias, and \( {\varvec{\upeta}}_{{{\text{r}}v}} \), \( {\varvec{\upeta}}_{{{\text{r}}u}} \) are zero-mean Gaussian white-noise processes with variances, \( \sigma_{{{\text{r}}v}}^{2} \) and \( \sigma_{{{\text{r}}u}}^{2} \), respectively.

4 Dual Quaternion-Based Extended Kalman Filter (DQ-EKF)

In this section, equations for dual quaternion-based relative navigation are derived using EKF algorithm. It has been most widely adopted for estimation of nonlinear systems by utilizing linearized systems and measurement model. For EKF, error dual quaternion is derived first, and it provides advantages with less computational burden by reduction of error states from eight to six. The line-of-sight measurements from six beacons are used for update procedure. In addition, rate gyros are used for the angular velocity measurements. One consideration should be given to the velocity state. By having the linear velocity in dual quaternion kinematics, velocity propagation or measurement is required. This research discusses these two cases together.

4.1 Error Dual Quaternion Kinematics

In a manner similar to the error quaternion, with the estimated value indicated as ˄, the definition of error dual quaternion is
$$ \begin{aligned} \delta {\bar{\mathbf{q}}} & = {{\hat{\bar{\bf q}}}}^{ - 1} \otimes_{\text{d}} {\bar{\mathbf{q}}} \\ & = {\hat{\mathbf{q}}}_{\text{r}}^{ - 1} \otimes {\mathbf{q}}_{\text{r}} + \varepsilon \left( {{\hat{\mathbf{q}}}_{\text{d}}^{ - 1} \otimes {\mathbf{q}}_{\text{r}} + {\hat{\mathbf{q}}}_{\text{r}}^{ - 1} \otimes {\mathbf{q}}_{\text{d}} } \right) \\ & = \delta {\mathbf{q}}_{\text{r}} + \varepsilon \delta {\mathbf{q}}_{\text{d}} . \\ \end{aligned} $$
(24)
Then, the error dual quaternion kinematics is described as
$$ \delta {{\dot{\bar{\bf q}}}} = \delta {\dot{\mathbf{q}}}_{\text{r}} + \varepsilon \delta {\dot{\mathbf{q}}}_{\text{d}} $$
(25)
and the real and dual parts error kinematics is given such that
$$ \delta {\dot{\mathbf{q}}}_{\text{r}} = - \frac{1}{2}{\hat{\varvec{\omega }}}_{\text{r}} \otimes \delta {\mathbf{q}}_{\text{r}} + \frac{1}{2}\delta {\mathbf{q}}_{\text{r}} \otimes {\varvec{\upomega}}_{\text{r}} , $$
(26)
$$ \delta {\dot{\mathbf{q}}}_{\text{d}} = \frac{1}{2}\delta {\mathbf{q}}_{\text{r}} \otimes {\mathbf{v}}_{\text{d}} - \frac{1}{2}{\hat{\mathbf{v}}}_{\text{d}} \otimes \delta {\mathbf{q}}_{\text{r}} + \frac{1}{2}\delta {\mathbf{q}}_{\text{d}} \otimes {\varvec{\upomega}}_{\text{r}} - \frac{1}{2}{\hat{\varvec{\omega }}}_{\text{r}} \otimes \delta {\mathbf{q}}_{\text{d}} . $$
(27)
With the constraints for dual quaternion as
$$ q_{{{\text{r}}0}} = \sqrt {1 - {\mathbf{q}}_{{{\text{r}}v}}^{T} {\mathbf{q}}_{{{\text{r}}v}} } ,\quad q_{{{\text{d}}0}} = \frac{{ - {\mathbf{q}}_{{{\text{r}}v}}^{T} {\mathbf{q}}_{{{\text{d}}v}} }}{{q_{{{\text{r}}0}} }} $$
(28)
and the assumption of small angle and short distance, the error quaternion can be derived by linearization as
$$ \delta {\mathbf{q}}_{\text{r}} = \left[ {\begin{array}{*{20}c} {\delta q_{{{\text{r}}0}} } \\ {\delta {\mathbf{q}}_{{{\text{r}}v}} } \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} 1 \\ {\tfrac{1}{2}\delta {\varvec{\upalpha}}} \\ \end{array} } \right],\quad \delta {\mathbf{q}}_{\text{d}} = \left[ {\begin{array}{*{20}c} {\delta q_{{{\text{d}}0}} } \\ {\delta {\mathbf{q}}_{{{\text{d}}v}} } \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} 0 \\ {\tfrac{1}{2}\delta {\varvec{\upkappa}}} \\ \end{array} } \right]. $$
(29)
It turns out that eight parameters of dual quaternion are reduced to six. The details of real part error quaternion kinematics are described in [17] as
$$ \delta {{\dot{\varvec{{\upalpha}} }}} = - \left[ {{{\hat{\varvec{{\upomega}} }}}_{\text{d}} \times } \right]\delta {\varvec{\upalpha }} - \Delta {\varvec{\upbeta }}_{\text{d}} + {\mathbf{R}}_{{{{\hat{\mathbf{{q}}}}}_{{\mathbf{r}}} }} \Delta {{\beta }}_{\text{c}} - {\varvec{\upeta }}_{{\text{d}v}} + {\mathbf{R}}_{{{\hat{\mathbf{q}}}_{{\mathbf{r}}} }} {\varvec{\upeta }}_{{\text{c}v}} . $$
(30)
Equation (27) can be simplified with the first-order linearization as
$$ \delta {\dot{\mathbf{q}}}_{\text{d}} \cong \frac{1}{2}\left( {\left( {{\mathbf{R}}_{{\hat{{\mathbf{q}}}_{\text{r}} }} \Delta {\dot{\varvec{\uprho}}}} \right) - 2\left[ {\delta {\mathbf{q}}_{\text{rv}} \times } \right]\left( {{\mathbf{R}}_{\hat{{\mathbf{{q}}}_{{\mathbf{r}}} }} {\dot{\hat{\varvec{\uprho }}}}} \right)} \right) + \frac{1}{2}\left( {\left[ {\delta {\mathbf{q}}_{{\text{d}v}} \times } \right]{\hat{\varvec{\upomega }}}_{\text{d}} - \left[ {\delta {\mathbf{q}}_{{\text{d}v}} \times } \right]{\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }} {\hat{\varvec{\upomega }}}_{\text{c}} } \right) $$
(31)
And dual part error quaternion can be derived such that
$$ \delta {\dot{\varvec{\upkappa }}} = {\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }} \Delta {\dot{\varvec{\uprho }}} + \left[ {{\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }} {\dot{\hat{\varvec{\uprho }}}} \times } \right]\delta {\varvec{\alpha }} + \left( {\left[ {{\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }} {\hat{\varvec{\upomega }}}_{\text{c}} \times } \right] - \left[ {{\hat{\varvec{\upomega }}}_{d} \times } \right]} \right)\delta {\varvec{\upkappa }} $$
(32)
If linear velocity measurement is used, the dual part error quaternion is defined with velocimeter bias and noise model.
$$ \delta {\dot{\varvec{\upkappa }}} = \left[ {{\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }} {\dot{\hat{\varvec{\uprho }}}} \times } \right]\delta {\varvec{\alpha }} + \left( {\left[ {{\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }} {\hat{\varvec{\upomega }}}_{\text{c}} \times } \right] - \left[ {{\hat{\varvec{\upomega }}}_{\text{d}} \times } \right]} \right)\delta {\varvec{\upkappa }} - {\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }} \Delta {\varvec{\upbeta }}_{\text{r}} - {\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }} {\varvec{\upeta }}_{{\text{r}v}} . $$
(33)

4.2 Case 01: Linear Velocity Propagation Model

For the full state vector for update, \( {\mathbf{x}} = \left[ {\begin{array}{*{20}c} {{\mathbf{q}}_{\text{r}}^{T} } & {{\mathbf{q}}_{\text{d}}^{T} } & {{\dot{\varvec{\uprho }}}^{T} } & {{\varvec{\upbeta}}_{\text{c}}^{T} } & {{\varvec{\upbeta}}_{\text{d}}^{T} } \\ \end{array} } \right]^{T} \), the error state vector and process noise vector are introduced as
$$ \Delta {\mathbf{x}} = \left[ {\begin{array}{*{20}c} {\delta {\varvec{\upalpha}}^{T} } & {\delta {\varvec{\upkappa}}^{T} } & {\Delta {\dot{\varvec{\uprho }}}^{T} } & {\Delta {\varvec{\upbeta}}_{\text{c}}^{T} } & {\Delta {\varvec{\upbeta}}_{\text{d}}^{T} } \\ \end{array} } \right]^{T} , $$
(34)
$$ {\mathbf{w}} = \left[ {\begin{array}{*{20}c} {{\varvec{\upeta}}_{{{\text{c}}v}}^{T} } & {{\varvec{\upeta}}_{{{\text{d}}v}}^{T} } & {{\varvec{\upeta}}_{{{\text{c}}u}}^{T} } & {{\varvec{\upeta}}_{{{\text{d}}u}}^{T} } & {{\mathbf{w}}_{\uprho } } \\ \end{array} } \right]^{T} . $$
(35)
With error state equations of Eqs. (30) and (32), and bias errors such that
$$ \Delta {\dot{\varvec{\upbeta }}}_{\text{c}} = {\dot{\varvec{\upbeta }}}_{\text{c}} - {{\dot{\hat{\varvec{\upbeta }}}}}_{\text{c}} = {\varvec{\upeta}}_{{{\text{c}}u}} , $$
(36)
$$ \Delta {\dot{\varvec{\upbeta }}}_{\text{d}} = {\dot{\varvec{\upbeta }}}_{\text{d}} - {{\dot{\hat{\varvec\upbeta }}}}_{\text{d}} = {\varvec{\upeta}}_{{{\text{d}}u}} , $$
(37)
the error state dynamics for EKF is defined as
$$ \Delta {\varvec{\dot{x}}} = F\Delta {\mathbf{x}} + G{\mathbf{w}} $$
(38)
where
$$ F = \left[ {\begin{array}{c@{\quad}c@{\quad}l@{\quad}l@{\quad}l} { - \left[ {{\varvec{\hat{\upomega }}}_{\rm{d}} \times } \right]} & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{R}}_{{\hat{{\mathbf{q}}}_{\rm{r}}}}} & { - {\mathbf{I}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } \\ {\left[ {{\varvec{\dot{\hat{\uprho }}}} \times } \right]} & {\left( {\left[ {\mathbf{R}_{{\hat{\mathbf{q}}}_{\rm{r}} } {\varvec{\hat{\upomega }}}_{\rm{c}} \times } \right] - \left[ {{\hat{\varvec{\upomega }}}_{\rm{d}} \times } \right]} \right)} & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & { - {\mathbf{I}}_{{3 \times 3}} } \\ {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } \\ {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } \\ {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } \\ \end{array} } \right] . $$
(39)
$$ G = \left[ {\begin{array}{*{20}c} {\!\!\!\mathbf{R}_{{\hat{\mathbf{q}}}_{\rm{r}} } } & { - {\mathbf{I}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } \\ {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & { - {\mathbf{I}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } \\ {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{I}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } \\ {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{I}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } \\ {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{I}}_{{3 \times 3}} } \\ \end{array} } \right] , $$
(40)
and the process noise matrix can be constructed using variances of sensor and process noise.
$$ Q = \left[ {\begin{array}{*{20}c} {\sigma _{{cv}}^{2} {\mathbf{I}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } \\ {{\mathbf{0}}_{{3 \times 3}} } & {\sigma _{{dv}}^{2} {\mathbf{I}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } \\ {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {\sigma _{{cu}}^{2} {\mathbf{I}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } \\ {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {\sigma _{{du}}^{2} {\mathbf{I}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } \\ {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {{\mathbf{0}}_{{3 \times 3}} } & {\sigma _{{xyz}}^{2} {\mathbf{I}}_{{3 \times 3}} } \\ \end{array} } \right] . $$
(41)
For the true and estimated measurement from a vision sensor,
$$ {\mathbf{b}} = {\mathbf{R}}_{{{\mathbf{q}}_{{\mathbf{r}}} }} {\mathbf{r}}_{i} , $$
(42)
$$ {\hat{\mathbf{b}}} = {\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }} {\hat{\mathbf{r}}}_{i} , $$
(43)
the measurement error is represented as
$$ \begin{aligned} \Delta {\mathbf{b}} &= {\mathbf{b}} - {\hat{\mathbf{b}}} \hfill \\ \;\;\;\;\, &= \left[ {{\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }} {\hat{\mathbf{r}}}_{i} \times } \right]\delta {\varvec{\upalpha }} + {\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }} \Delta {\mathbf{r}} \hfill \\ \;\;\;\;\, &\approx \left[ {{\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }} {\hat{\mathbf{r}}}_{i} \times } \right]\delta {\varvec{\upalpha }} + {\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }} \frac{{\partial {\hat{\mathbf{r}}}_{i} }}{{\partial {\hat{\varvec{\rho }}}}}{\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }}^{T} \delta {\varvec{\upkappa }}. \hfill \\ \end{aligned} $$
(44)
Then, the Jacobian sensitivity matrix is
$$ H = \left[ {\begin{array}{*{20}c} {\frac{{\partial {\mathbf{b}}_{1} }}{{\partial {\mathbf{q}}_{\text{r}} }}} & {\frac{{\partial {\mathbf{b}}_{1} }}{{\partial {\mathbf{q}}_{\text{d}} }}} \\ \vdots & \vdots \\ {\frac{{\partial {\mathbf{b}}_{{\mathbf{N}}} }}{{\partial {\mathbf{q}}_{\text{r}} }}} & {\frac{{\partial {\mathbf{b}}_{{\mathbf{N}}} }}{{\partial {\mathbf{q}}_{\text{d}} }}} \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {\left[ {{\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }} {\hat{\mathbf{r}}}_{1} \times } \right]} & {{\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }} \frac{{\partial {\hat{\mathbf{r}}}_{1} }}{{\partial {\hat{\varvec{\rho }}}}}{\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }}^{T} } \\ \vdots & \vdots \\ {\left[ {{\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }} {\hat{\mathbf{r}}}_{N} \times } \right]} & {{\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }} \frac{{\partial {\hat{\mathbf{r}}}_{N} }}{{\partial {\varvec{\hat{\rho }}}}}{\mathbf{R}}_{{\hat{{\mathbf{q}}}_{{\mathbf{r}}} }}^{T} } \\ \end{array} } \right] . $$
(45)

4.3 Case 02: Linear Velocity Measurement Model

In the linear velocity measurement case, velocimeter bias and noise are considered instead of velocity states. Since relative velocity for an arbitrary point is obtained by velocimeter directly, the arbitrary point information is not required.

With linear velocity measurement, the full state vector consists of velocimeter bias instead of velocity such that \( {\mathbf{x}} = \left[ {\begin{array}{*{20}c} {{\mathbf{q}}_{\text{r}}^{T} } & {{\mathbf{q}}_{\text{d}}^{T} } & {{\varvec{\upbeta}}_{\text{c}}^{T} } & {{\varvec{\upbeta}}_{\text{d}}^{T} } & {{\varvec{\upbeta}}_{\text{r}}^{T} } \\ \end{array} } \right]^{T} \). Then, the error state and the process noise vector are defined as
$$ \Delta {\mathbf{x}} = \left[ {\begin{array}{*{20}c} {\delta {\varvec{\upalpha}}^{T} } & {\delta {\varvec{\upkappa}}^{T} } & {\Delta {\varvec{\upbeta}}_{\text{c}}^{T} } & {\Delta {\varvec{\upbeta}}_{\text{d}}^{T} } & {\Delta {\varvec{\upbeta}}_{\text{r}}^{T} } \\ \end{array} } \right]^{T} , $$
(46)
$$ {\mathbf{w}} = \left[ {\begin{array}{*{20}c} {{\varvec{\upeta}}_{{{\text{c}}v}}^{T} } & {{\varvec{\upeta}}_{{{\text{d}}v}}^{T} } & {{\varvec{\upeta}}_{{{\text{r}}v}}^{T} } & {{\varvec{\upeta}}_{{{\text{c}}u}}^{T} } & {{\varvec{\upeta}}_{{{\text{d}}u}}^{T} } & {{\varvec{\upeta}}_{{{\text{r}}u}}^{T} } \\ \end{array} } \right]^{T} . $$
(47)
To propagate states, error state kinematics of Eqs. (30) and (33) is used. And velocimeter bias error equations are augmented additionally with gyro bias error in Eqs. (36) and (37).
$$ \Delta {\dot{\varvec{\upbeta }}}_{\text{r}} = {\dot{\varvec{\upbeta }}}_{\text{r}} - {{\dot{\hat{\varvec{\upbeta }}}}}_{\text{r}} = {\varvec{\upeta}}_{{{\text{r}}u}} . $$
(48)

The sensitivity matrix for the line-of-sight measurements is not different from linear velocity kinematics model case.

5 Dual Quaternion-Based Unscented Kalman Filter (DQ-UKF)

Since EKF has linearization process with small error assumption in the first-order terms only, it may be difficult to guarantee performance in highly nonlinear systems or measurement models. As an alternative approach, UKF is an estimator without linearization process; so in this study, dual quaternion-based UKF is attempted. The approach of UKF is based on a sigma-point implementation called the unscented transformation to propagate means and covariance matrices [15, 19, 20, 21]. Since UKF uses nonlinear systems and measurement models directly, linearization procedure for system model is not required.

For a random variable \( x \) with
$$ x\sim N\left( {x_{m} \;,\;\;P_{x} } \right) $$
(49)
sigma points and weight are defined as
$$\begin{aligned} \chi_{1} &= x,\quad \chi_{i + 1} = x + u_{i} ,\\ \chi_{i + n + 1} &= x - u_{i} \quad {\text{where}}\quad i = 1 \ldots n,\end{aligned} $$
(50)
and
$$\begin{aligned} &W_{1} = \frac{\kappa }{n + k},\quad W_{i + 1} = \frac{1}{{2\left( {n + k} \right)}},\\ &W_{i + n + 1} = \frac{1}{{2\left( {n + k} \right)}}\quad {\text{where}}\quad i = 1 \ldots n,\end{aligned} $$
(51)
where \( u_{i} \) is a row vector of matrix \( U \) satisfying
$$ U^{T} U = \left( {n + \kappa } \right)P_{x} , $$
(52)
and \( \kappa \) is an arbitrary constant. Then, the mean and covariance of a function given by
$$ y = f\left( x \right) $$
(53)
are constructed as
$$ y_{m} = \sum\limits_{i = 1}^{2n + 1} {W_{i} f\left( {\chi_{i} } \right)} , $$
(54)
$$ P_{y} = \sum\limits_{i = 1}^{2n + 1} {W_{i} \left\{ {f\left( {\chi_{i} } \right) - x_{m} } \right\}} \left\{ {f\left( {\chi_{i} } \right) - x_{m} } \right\}^{T} . $$
(55)

The procedures above are called as unscented transformation.

Next, the error dual quaternion-based UKF can be derived also in the following step.

Step 1 Temporal update
$$ \begin{aligned} &\left( {\chi _{i} \left( {\delta {\hat{\bar{\mathbf{q}}}}_{{k - 1}} } \right),\;W_{i} } \right) \leftarrow \left( {\delta {\hat{\bar{\mathbf{q}}}}_{{k - 1}} ,\;P_{{k - 1}} } \right) \\ &\quad \chi _{i} \left( {{\hat{\bar{\mathbf{q}}}}_{{k - 1}} } \right) \leftarrow \chi _{i} \left( {\delta {\hat{\bar{\mathbf{q}}}}_{{k - 1}} } \right) \\ &\left( {{\hat{\bar{\mathbf{q}}}}_{k}^{ - } ,\;P_{k}^{ - } } \right) = {\text{Unscented Transformation}}\left( {f\left( {\chi _{i} \left( {{\hat{\bar{\mathbf{q}}}}_{{k - 1}} } \right)} \right),\;W_{i} ,\;Q} \right) \\ &\delta {\hat{\bar{\mathbf{q}}}}_{k}^{ - } = {\hat{\bar{\mathbf{q}}}}_{{k - 1}}^{{ - 1}} \otimes f\left( {\chi _{i} \left( {{\hat{\bar{\mathbf{q}}}}_{{k - 1}} } \right)} \right) \\ \end{aligned} $$
(56)
Step 2 Measurement update
$$ \begin{aligned} &\left( {\chi _{{i + 1}} \left( {\delta {\hat{\bar{\mathbf{q}}}}_{k}^{ - } } \right),\;W_{{i + 1}} } \right) \leftarrow \left( {\delta {\hat{\bar{\mathbf{q}}}}_{k}^{ - } ,\;P_{k}^{ - } } \right) \\ & \chi _{{i + 1}} \left( {{\hat{\bar{\mathbf{q}}}}_{k}^{ - } } \right) \leftarrow \chi _{i} \left( {\delta {\hat{\bar{\mathbf{q}}}}_{k}^{ - } } \right) \\ & \left( {{\hat{\mathbf{z}}}_{k} ,\;P_{z} } \right) = {\text{Unscented Transformation}}\left( {h\left( {\chi _{{i + 1}} \left( {{\hat{\bar{\mathbf{q}}}}_{k}^{ - } } \right)} \right),\;W_{{i + 1}} ,\;R} \right), \\ \end{aligned} $$
(57)
$$ \begin{aligned} P_{{xz}} &= \sum\limits_{{i = 1}}^{{2n + 1}} {W_{i} } \left\{ {f\left( {\chi _{i} } \right) - {\hat{\mathbf{x}}}_{k}^{ - } } \right\}\left\{ {h\left( {\chi _{{i + 1}} } \right) - {\hat{\mathbf{z}}}_{k} } \right\}^{T} \\ K_{k} &= P_{{xz}} /P_{z} \\ \end{aligned} $$
(58)
$$ \begin{aligned} \delta {\hat{\bar{\mathbf{q}}}}_{k} &= K_{k} \left( {{\mathbf{z}}_{k} - {\hat{\mathbf{z}}}_{k} } \right) \\ {\hat{\bar{\mathbf{q}}}}_{k} &= {\hat{\bar{\mathbf{q}}}}_{k}^{ - } \otimes \delta {\hat{\bar{\mathbf{q}}}}_{k} \\ P_{k} &= P_{k}^{ - } - K_{k} P_{z} K_{k}^{T} . \\ \end{aligned} $$
(59)

DQ-UKF is implemented without Jacobian matrix as the result of linearization. So, pervious measurement formulation and state vectors are cast into UKF approach with simulation results presented in the next section.

6 Simulation Results

In this section, simulation results are presented. Before discussing the results, relative orbital motion using dual quaternion kinematics is summarized to verify that dual quaternion can represent position–attitude coupling scenarios. Subsequently, simulation results of vision-based relative navigation systems using DQ-EKF and DQ-UKF are provided.

6.1 Relative Orbit Motion

In proximity operation, relative position between two satellites is affected by attitude changes. Such a coupling effect is represented about an arbitrary point of a satellite, which is not the center of mass. Figure 3 shows the difference of relative position between an arbitrary point and the center of mass of a deputy satellite. The semi-major axis and eccentricity of a chief orbit are \( a = 6,998,455\;{\text{m}} \), and \( e = 0.00172 \), respectively. Orbital parameters are propagated using the following equations.
$$ \ddot{r}_{\text{c}} \left( t \right) = r_{\text{c}} \dot{\theta }^{2} \left( {1 - \frac{1}{1 + e\cos \theta }} \right), $$
(60)
$$ \ddot{\theta }\left( t \right) = - 2\dot{r}_{\text{c}} \frac{{\dot{\theta }}}{{r_{\text{c}} }}. $$
(61)
Fig. 3

Relative orbit motion with conventional kinematics and position difference of an arbitrary point

Initial orbital radius and rate of the chief satellite are defined as \( r_{\text{c}} \left( {t_{0} } \right) = a\left( {1 - e} \right)\;\;{\text{m}} \) and \( \dot{r}_{\text{c}} \left( {t_{0} } \right) = 0\;\;{\rm m}/{\rm s} \), while true anomaly rate is determined as \( \dot{\theta }\left( {t_{0} } \right) = \sqrt {\mu /r_{\text{c}} \left( {1 + e\cos \theta } \right)} \left( {1 + e} \right)/r_{\text{c}} \;\;{\rm rad}/{\rm s} \) with \( \theta \left( {t_{0} } \right) = 0 \) at perigee. Here, the earth gravitational constant is \( \mu = 3.985744e14\;{\text{m}}^{ 2} / {\text{s}}^{ 2} \). The initial conditions for orbital motion are as follows.
$$ \begin{aligned} {\mathbf{q}}\left( {t_{0} } \right) & = \left[ {\begin{array}{*{20}c} {\sqrt 2 /2} & 0 & 0 & {\sqrt 2 /2} \\ \end{array} } \right]^{T}, \\ {\varvec{\rho }}_{0} \left( {t_{0} } \right) & = \left[ {\begin{array}{*{20}c} {200} & {200} & {100} \\ \end{array} } \right]^{T} \;{\text{m}}, \\ {\varvec{\dot{\rho }}}_{0} \left( {t_{0} } \right) & = \left[ {\begin{array}{*{20}c} {0.01} & { - 0.4325} & {0.01} \\ \end{array} } \right]^{T} \;{\rm m}/{\rm s} , \\ {\varvec{\omega }}_{{\text{c}}} & = \left[ {\begin{array}{*{20}c} 0 & {0.0011} & { - 0.0011} \\ \end{array} } \right]^{T} \;{\rm rad}/{\rm s}, \\ {\varvec{\omega }}_{d} & = \left[ {\begin{array}{*{20}c} { - 0.002} & 0 & {0.0011} \\ \end{array} } \right]^{T} \;{\rm rad}/{\rm s} . \\ \end{aligned} $$

The PSD sensor position, \( P = \left[ {\begin{array}{*{20}c} 1 & 1 & 1 \\ \end{array} } \right]^{T} \;\;{\text{m}} \), is set to an arbitrary point. The position vector from the chief to an arbitrary point on the deputy is constructed using vector geometry after finding \( {\varvec{\uprho}}_{0} \). Relative orbit motion is propagated using Eqs. (7) and (11) in conventional kinematics case, and Eq. (15) is used for dual quaternion-based kinematics case with time step of 10 s.

With the same condition, Fig. 4 represents position difference of an arbitrary point in relative orbital motion generated by dual quaternion-based relative kinematics. This kinematics produces almost the same results for relative position about an arbitrary point. The negligible differences in Fig. 4 may come from parameter conversion between dual quaternion and position vector as computational error. These simulation results indicate that the dual quaternion-based kinematics can represent position–attitude coupling effect without additional processing about an arbitrary point.
Fig. 4

Position difference of an arbitrary point and error between dual quaternion and conventional kinematics

6.2 Vision-Based Relative Navigation with DQ-EKF

In this subsection, relative navigation with a vision sensor is simulated. The dual quaternion-based extended Kalman filter (DQ-EKF) is implemented. The initial conditions of relative orbit are used same as above subsection with initial position and attitude errors such that
$$ \begin{aligned} \Delta {\varvec{\alpha }} & = \left[ {\begin{array}{*{20}c} 1 & 1 & 1 \\ \end{array} } \right]^\circ ,\quad \Delta {\varvec{\rho }}_{0} = \left[ {\begin{array}{*{20}c} { - 5} & 3 & { - 3} \\ \end{array} } \right]^{T} \;{\text{m}}, \\ \Delta {\varvec{\dot{\rho }}}_{0} & = \left[ {\begin{array}{*{20}c} 0 & {0.01} & {0.02} \\ \end{array} } \right]^{T} \;{\text{m}}/{\text{s}}{\text{.}} \\ \end{aligned} $$
Initial position and velocity errors for \( {\varvec{\uprho}}_{0} \) and \( {\dot{\varvec{\uprho }}}_{0} \) are used to compute errors for \( {\mathbf{q}}_{\text{d}} \) and \( {\dot{\varvec{\uprho }}} \). The gyro biases are \( {\varvec{\upbeta}}_{\text{c}} = {\varvec{\upbeta}}_{\text{d}} = \left[ {\begin{array}{*{20}c} 1 & 1 & 1 \\ \end{array} } \right]^{T} \;^\circ / {\text{h}} \) for both chief and deputy satellites. In addition, gyro and process noise parameters are set to \( \sigma_{{{\text{c}}u}} = \sigma_{{{\text{d}}u}} = \sqrt 2 \times 10^{ - 10} \;{\text{rad/s}}\sqrt {\text{s}} ,\quad \sigma_{{{\text{c}}v}} = \sigma_{{{\text{d}}v}} = \sqrt 2 \times 10^{ - 5} \;{\text{rad/s}}\sqrt {\text{s}} \) and \( w_{xyz} = \sqrt {10} \times 10^{ - 10} \;{\text{m/s}}\sqrt {\text{s}} \), respectively. In addition, the line-of-sight measurements are generated with Eq. (18) with noise of \( \sigma_{i} = 0.0005^\circ \). Six beacons are located on the chief satellite at
$$ \left[ {\begin{array}{*{20}c} {X_{i} } & {Y_{i} } & {Z_{i} } \\ \end{array} } \right] = \left\{ {\begin{array}{*{20}c} {\left[ {\begin{array}{*{20}c} {0.5} & {0.5} & {0.0} \\ \end{array} } \right]} \\ {\left[ {\begin{array}{*{20}c} { - 0.5} & { - 0.5} & {0.0} \\ \end{array} } \right]} \\ {\left[ {\begin{array}{*{20}c} { - 0.5} & {0.5} & {0.0} \\ \end{array} } \right]} \\ {\left[ {\begin{array}{*{20}c} {0.5} & { - 0.5} & {0.0} \\ \end{array} } \right]} \\ {\left[ {\begin{array}{*{20}c} {0.2} & {0.5} & {0.1} \\ \end{array} } \right]} \\ {\left[ {\begin{array}{*{20}c} {0.0} & {0.2} & { - 0.1} \\ \end{array} } \right]} \\ \end{array} } \right\}\;\;{\text{m}} $$
and the initial error covariance matrix for state is defined as below for linear velocity kinematics model case.
$$ \begin{aligned} P_{0} = diag\left( {\left[ {\begin{array}{*{20}c} {\sigma _{{qr}}^{2} {\mathbf{I}}_{{3 \times 3}} } & {\sigma _{{qd}}^{2} {\mathbf{I}}_{{3 \times 3}} } & {\sigma _{{\dot{\rho }}}^{2} {\mathbf{I}}_{{3 \times 3}} } & {\sigma _{{\beta _{c} }}^{2} {\mathbf{I}}_{{3 \times 3}} } & {\sigma _{{\beta _{d} }}^{2} {\mathbf{I}}_{{3 \times 3}} } \\ \end{array} } \right]} \right) \hfill \\ \;\;\,\; = diag\left( {\left[ {\begin{array}{*{20}c} {{\mathbf{I}}_{{3 \times 3}} \left( ^\circ \right)^{2} } & {2.5{\mathbf{I}}_{{3 \times 3}} } & {0.02{\mathbf{I}}_{{3 \times 3}} \left( {{\text{m/s}}} \right)^{2} } & {4{\mathbf{I}}_{{3 \times 3}} \left( {^\circ {\text{/h}}} \right)^{2} } & {4{\mathbf{I}}_{{3 \times 3}} \left( {^\circ {\text{/h}}} \right)^{2} } \\ \end{array} } \right]} \right) \hfill \\ \end{aligned} $$
In the case of velocity measurement model, the initial error covariance matrix is set to
$$ \begin{aligned} P_{0} = diag\left( {\left[ {\begin{array}{*{20}c} {\sigma _{{qr}}^{2} {\mathbf{I}}_{{3 \times 3}} } & {\sigma _{{qd}}^{2} {\mathbf{I}}_{{3 \times 3}} } & {\sigma _{{\beta _{c} }}^{2} {\mathbf{I}}_{{3 \times 3}} } & {\sigma _{{\beta _{d} }}^{2} {\mathbf{I}}_{{3 \times 3}} } & {\sigma _{{\beta _{r} }}^{2} {\mathbf{I}}_{{3 \times 3}} } \\ \end{array} } \right]} \right) \hfill \\ \;\;\,\; = diag\left( {\left[ {\begin{array}{*{20}c} {{\mathbf{I}}_{{3 \times 3}} \left( ^\circ \right)^{2} } & {2.5{\mathbf{I}}_{{3 \times 3}} } & {4{\mathbf{I}}_{{3 \times 3}} \left( {^\circ {\text{/h}}} \right)^{2} } & {4{\mathbf{I}}_{{3 \times 3}} \left( {^\circ {\text{/h}}} \right)^{2} } & {0.02{\mathbf{I}}_{{3 \times 3}} \left( {{\text{m/s}}} \right)^{2} } \\ \end{array} } \right]} \right) \hfill \\ \end{aligned} $$

The true velocimeter bias is \( {\varvec{\upbeta}}_{\text{r}} = \left[ {\begin{array}{*{20}c} {10} & {10} & {10} \\ \end{array} } \right]^{T} \;{\text{m/h}} \) and \( \sigma_{{{\text{r}}u}} = \sqrt 2 \times 10^{ - 5} \;{\text{m/s}}\sqrt {\text{s}} ,\quad \sigma_{{{\text{r}}v}} = \sqrt 2 \times 10^{ - 2} \;{\text{m/s}}\sqrt {\text{s}} \) for noise levels.

Figures 5, 6, 7 show estimation results of dual quaternion EKF for linear velocity kinematics model (DQEKF-01). In this case, dual quaternion and linear velocity kinematics is implemented as described in Sect. 4.2 with line-of-sight and gyro measurements. Attitude and position are shown to converge quickly in seconds, and estimation errors result in within \( 0.1^\circ \) and \( 0.3\;{\text{m}} \).
Fig. 5

Attitude estimation error with DQEKF in linear velocity propagation model case

Fig. 6

Position estimation error with DQEKF in linear velocity propagation model case

Fig. 7

Gyro bias estimation with DQEKF in linear velocity propagation model case

Fig. 8

Attitude estimation error with DQEKF in linear velocity measurement case

The results of linear velocity measurement case using dual quaternion-based EKF (DQEKF-02) are in Figs. 8, 9, 10. In DQEKF-2, velocimeter bias state is considered instead of velocity kinematics. Velocity state is not propagated in kinematics, whereas position is propagated in dual quaternion kinematics and updated using line-of-sight measurements. In the simulation results, attitude, position and biases are well estimated. Attitude estimation error within \( 0.1^\circ \) is not different from the case DQEKF-01. Position knowledge affecting relative distance and attitude is changing periodically between \( 0.3\;{\text{m}} \) and \( 1\;{\text{m}} \). Since DQEKF-02 does not propagate velocity states through the system model unlike DQEKF-01, it can be seen that the measured value according to the relative position and attitude in the orbit is more reflected to the position states markedly.
Fig. 9

Position estimation error with DQEKF in linear velocity measurement case

Fig. 10

Velocimeter bias estimation with DQEKF in linear velocity measurement case

6.3 Vision-Based Relative Navigation with DQ-UKF

With the same initial condition of DQEKF, UKF using dual quaternion are simulated for two cases of velocity kinematics and measurement model. In UKF, nonlinear system and measurement model are adopted without linearization; so, simulation results show that estimation knowledge tends to be affected by model nonlinearity and noise more than the EKF cases. For the first case of velocity kinematics model (DQUKF-01), the results are presented in Figs. 11 and 12. Attitude and position estimation errors are within \( 0.1^\circ \) and \( 0.3\;{\text{m}} \) and gyro biases are well identified by filter algorithms.
Fig. 11

Attitude estimation error with DQUKF in linear velocity propagation model case

Fig. 12

Position estimation error with DQUKF in linear velocity propagation model case

Velocity measurement case with UKF (DQUKF-02) exhibits similar tendency as DQEKF-02. States are well estimated as in Fig. 13 and 14. Since estimation is conducted without linearization, the velocimeter sensor noise is reflected in results.
Fig. 13

Attitude estimation error with DQUKF in linear velocity measurement case

Fig. 14

Position estimation error with DQUKF in linear velocity measurement case

7 Conclusion

This study formulates dual quaternion kinematics for relative navigation, which is applicable to estimate relative pose between arbitrary points attached to two on-orbit satellites. A set of six beacons and PSD is applied to generate vision sensor measurements to facilitate relative navigation. Two well-known filtering methods, EKF and UKF, are adopted to estimate relative pose and slowly varying sensor parameters. Since the measurement observability is affected by relative orbit in vision-based navigation, the error covariance periodically increases and shrinks. It stands out in the model using velocity measurements only without propagation. However, simulation results showed that the dual quaternion-based nonlinear estimation filters converged well and yielded reasonable performance for the unknown arbitrary points, even the velocity is not propagated in the system model. Although the dual quaternion formulation has advantages regarding translational-rotational coupling motion over the traditional decoupled formulation, it has not yet been widely used as the state variables in spacecraft proximity operations. This study demonstrated the possibility of the dual quaternion-based relative navigation approach, especially for cases when interesting points such as docking port or special marker are not attached to the center of mass of spacecraft, which covers the general proximate missions.

Notes

Acknowledgements

This research was supported by the research program through the National Research Foundation of Korea funded by the Ministry of Education (NRF-2018M1A3A3A02065409).

References

  1. 1.
    Segal S, Gurfil P (2009) Effect of kinematic rotation-translation coupling on relative spacecraft translational dynamics. J Guid Control Dyn 32:1045–1050.  https://doi.org/10.2514/1.39320 CrossRefGoogle Scholar
  2. 2.
    Lee D, Bang H, Butcher EA, Sanyal AK (2015) Kinematically coupled relative spacecraft motion control using the state-dependent Riccati equation method. J Aerosp Eng 28(4):1–13.  https://doi.org/10.1061/(ASCE)AS.1943-5525.0000436 CrossRefGoogle Scholar
  3. 3.
    Clifford WK (1873) Preliminary sketch of bi-quaternions. Proc Lond Math Soc 4:381–395zbMATHGoogle Scholar
  4. 4.
    Akyar B (2008) Dual quaternions in spatial kinematics in an algebraic sense. Turk J Math 32(4):373–391MathSciNetzbMATHGoogle Scholar
  5. 5.
    Kenwright B (2012) A beginners guide to dual-quaternions. In: Paper presented at the WSCG 2012 The 20th international conference on computer graphics, visualization and computer vision, Plzen, Czech RepublicGoogle Scholar
  6. 6.
    Lee U, Mesbahi M (2015) Optimal power descent guidance with 6-DoF line of sight constraints via unit dual quaternions. In: AIAA guidance, navigation, and control conference, AIAA SciTech Forum.  https://doi.org/10.2514/6.2015-0319
  7. 7.
    Lee U, Mesbahi M (2012) Dual quaternions, rigid body mechanics, and powered-descent guidance. In: Decision and control (CDC), 2012 IEEE 51st annual conference on, IEEE, pp 3386–3391Google Scholar
  8. 8.
    Kwon J-W, Lee D, Bang H (2016) Virtual trajectory augmented landing control based on dual quaternion for Lunar Lander. J Guid Control Dyn 39(9):2044–2057.  https://doi.org/10.2514/1.G001459 CrossRefGoogle Scholar
  9. 9.
    Dong H, Hu Q, Akella MR (2018) Dual-quaternion-based spacecraft autonomous rendezvous and docking under six-degree-of-freedom motion constraints. J Guid Control Dyn 41(5):1150–1162.  https://doi.org/10.2514/1.G003094 CrossRefGoogle Scholar
  10. 10.
    Wu J, Liu K, Han D (2013) Adaptive sliding mode control for six-DOF relative motion of spacecraft with input constraint. Acta Astronaut 87:64–76.  https://doi.org/10.1016/j.actaastro.2013.01.015 CrossRefGoogle Scholar
  11. 11.
    Filipe N, Tsiotras P (2015) Adaptive position and attitude-tracking controller for satellite proximity operations using dual quaternions. J Guid Control Dyn 38(4):566–577.  https://doi.org/10.2514/1.G000054 CrossRefGoogle Scholar
  12. 12.
    Sun J, Zhang S, Wu X, Guo F, Xie Y (2014) Relative status determination for spacecraft relative motion based on dual quaternion. Math Probl Eng 2014:1–7.  https://doi.org/10.1155/2014/602724 CrossRefGoogle Scholar
  13. 13.
    Hou X, Ma C, Wang Z, Yuan J (2017) Adaptive pose and inertial parameters estimation of free-floating tumbling space objects using dual vector quaternions. Adv Mech Eng 10:1–17.  https://doi.org/10.1177/1687814017714210 CrossRefGoogle Scholar
  14. 14.
    Razgus B, Mooij E, Choukroun D (2017) Relative navigation in asteroid missions using dual quaternion filtering. J Guid Control Dyn 40(9):2151–2166.  https://doi.org/10.2514/1.G002805 CrossRefGoogle Scholar
  15. 15.
    Deng Y, Wang Z, Liu L (2016) Unscented Kalman filter for spacecraft pose estimation using twistors. J Guid Control Dyn 39(8):1844–1856.  https://doi.org/10.2514/1.G001957 CrossRefGoogle Scholar
  16. 16.
    Filipe N, Kontitsis M, Tsiotras P (2015) Extended Kalman filter for spacecraft pose estimation using dual quaternion. J Guid Control Dyn 38(9):1625–1641.  https://doi.org/10.2514/1.G000977 CrossRefGoogle Scholar
  17. 17.
    Kim S-G, Crassidis JL, Cheng Y, Fosbury AM, Junkins JL (2007) Kalman filtering for relative spacecraft attitude and position estimation. J Guid Control Dyn 30:133–143.  https://doi.org/10.2514/1.22377 CrossRefGoogle Scholar
  18. 18.
    Zivan Y, Choukroun D (2018) Dual quaternion Kalman filters for spacecraft relative navigation. In: Paper presented at the 2018 AIAA guidance, navigation, and control conferenceGoogle Scholar
  19. 19.
    Myung H-S, Yong K-L, Bang H-C (2011) Unscented Kalman filtering for hybrid estimation of spacecraft attitude dynamics and rate sensor alignment. In: Advances in spacecraft technologies. InTech, London, pp 197–212Google Scholar
  20. 20.
    Crassidis JL, Markley FL (2003) Unscented filtering for spacecraft attitude estimation. J Guid Control Dyn 26(4):536–542.  https://doi.org/10.2514/2.5102 CrossRefGoogle Scholar
  21. 21.
    Xuedong W, Xinhua J, Rongjin Z, Jiashan H (2006) An application of unscented Kalman filter for pose and motion estimation on monocular vision. In: Industrial electronics, 2006 IEEE international symposium on, Quebec, Canada, IEEE, pp 2614–2619Google Scholar

Copyright information

© The Author(s) 2019

Open AccessThis article is distributed under the terms of the Creative Commons Attribution 4.0 International License (http://creativecommons.org/licenses/by/4.0/), which permits unrestricted use, distribution, and reproduction in any medium, provided you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons license, and indicate if changes were made.

Authors and Affiliations

  1. 1.Korea Advanced Institute of Science and TechnologyDaejeonRepublic of Korea

Personalised recommendations