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
figure 1

Relative orbital motion between two spacecraft

Full size image

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
figure 2

Vision-based navigation system configuration

Full size image

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
figure 3

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

Full size image

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
figure 4

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

Full size image

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
figure 5

Attitude estimation error with DQEKF in linear velocity propagation model case

Full size image
Fig. 6
figure 6

Position estimation error with DQEKF in linear velocity propagation model case

Full size image
Fig. 7
figure 7

Gyro bias estimation with DQEKF in linear velocity propagation model case

Full size image
Fig. 8
figure 8

Attitude estimation error with DQEKF in linear velocity measurement case

Full size image

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
figure 9

Position estimation error with DQEKF in linear velocity measurement case

Full size image
Fig. 10
figure 10

Velocimeter bias estimation with DQEKF in linear velocity measurement case

Full size image

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
figure 11

Attitude estimation error with DQUKF in linear velocity propagation model case

Full size image
Fig. 12
figure 12

Position estimation error with DQUKF in linear velocity propagation model case

Full size image

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
figure 13

Attitude estimation error with DQUKF in linear velocity measurement case

Full size image
Fig. 14
figure 14

Position estimation error with DQUKF in linear velocity measurement case

Full size image

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.