Adaptive pose and inertial parameters estimation of free-floating tumbling space objects using dual vector quaternions

Abstract

The robust parameter estimation of unknown space objects is essential to the on-orbit servicing missions. Based on the adaptive filtering techniques along with the dual quaternions modeling methods for pose estimation, this article proposes a dual vector quaternions–based extended Kalman filter and a dual vector quaternions–based adaptive fading factors extended Kalman filter to estimate the parameters of a free-floating tumbling space target. Using the dual vector quaternions to model the kinematics and dynamics of the system, the representation of the model is concise and compact. Also, the translational and rotational coupled effects are considered. In addition, the estimation algorithm is designed by the innovation-based multiple adaptive fading factors. As a result, the dual vector quaternions–based adaptive fading factors extended Kalman filter is robust against the faulty measurements which may lead to divergence of the traditional extended Kalman filter. As far as the authors know, both the designed filters are the first pose and inertial parameters estimation algorithms based on dual vector quaternions, and the dual vector quaternions–based adaptive fading factors extended Kalman filter is the first robust dual vector quaternions–based parameters estimating method. Finally, the proposed dual vector quaternions–based extended Kalman filter and dual vector quaternions–based adaptive fading factors extended Kalman filter are validated by mathematical simulations, and the dual vector quaternions–based adaptive fading factors extended Kalman filter is compared with the dual vector quaternions–based extended Kalman filter to show its robust performances.

Introduction

There have been growing attentions to the on-orbit servicing (OOS) missions which require the autonomous rendezvous and docking (AR&D) technology.^1–4 In these OOS missions, the accurate estimation of parameters of the target spacecraft is essential to the AR&D. Especially for the free-floating uncooperative targets, the AR&D becomes much more difficult because the guidance, navigation, and control (GNC) system of the chaser spacecraft cannot rely on any assistance from the uncooperative target spacecraft. In addition, uncooperative targets are tumbling in space, which also bring challenges for the AR&D. Consequently, the vision-based sensors are widely used in this field to obtain the relative pose measurements, which are further utilized to output the pose estimation of the uncooperative targets by image processing techniques.^5–7 However, these methods do not take the kinematics and dynamics of the space objects into account, which results in huge computational burden and disregard of the space environment. Also, the methods merely rely on the image processing techniques, so they can only estimate the pose parameters. Because of the lack of kinematics and dynamics equations of the target, the inertial parameters (e.g. ratios of the moments of inertia of the target and the location of the center of mass) cannot be estimated as well.

Regarding the defects of the methods, some researchers take the kinematics and dynamics of the target into consideration, and a number of parameter estimations methods have been proposed. These researches utilize the measurements from the vision-based sensors, combine with the kinematics and dynamics equations of the target, and use the extended Kalman filter (EKF)-based parameter estimating method to estimate the parameters of the uncooperative targets. Lefferts et al. ⁸ first proposed the error quaternions to represent the attitude kinematics of a space object and designed an error quaternion–based EKF to estimate the attitude parameters of the object. However, this work does not consider the attitude dynamics of the object, which lead to a lack of inertial parameters estimation. Also, the estimation method proposed in Lefferts et al. ⁸ does not utilize the relative translational motion equation, which is the essence of the relative position estimation. As a result, it cannot be called the pose estimation method and only can be regarded as the attitude estimation algorithm. Based on the error quaternions–based EKF, Aghili and coworkers^9,10 consider the attitude dynamics and relative translational motion equations which, as far as the authors know, lead to the first pose and inertial parameters estimation of an free-floating tumbling uncooperative target in space. Using the measured attitude and range information from the light detection and ranging (LIDAR) system, Aghili and colleagues,^9,10 have proposed an EKF-based estimation method to output the pose parameters, the ratios of the moments of inertia of the target, and the location of the center of mass in the same time.

A similar research in Song et al. ¹¹ using the measurements from a monocular camera is also based on the same method by Aghili ⁹ and Aghili and Parsa. ¹⁰ All the works mentioned above utilize the traditional error quaternions–based attitude kinematics and traditional attitude dynamics to model the relative attitude, and use the relative translational motion equations to describe the relative motion, which do not take the rotational and translational coupled effect into consideration. In this way, all the methods mentioned above can only be used in the scenarios which has weak translational and rotational coupled effects. Also, because the rotation and the translation are coupled, which means the relative linear motion between the two spacecraft depends on the angular motion, the position parameters can only be estimated when the attitude parameters are converged.^12,13 These researches designed two separated estimators, in which the position estimation is turned on when the attitude estimation is done. In this way, the attitude parameters estimation and the position parameters estimation cannot be conducted in the same time, which will decrease the estimation efficiency. In addition, using two separated estimators will introduce more nonlinear factors, which make the computation become more complex and, to some extent, make the filters diverged. Also, utilizing the traditional rotation and translation modeling method is not compact and concise.

Aimed to overcome the defects of the traditional parameter estimation methods, some researchers have utilized the dual quaternions to model the rotation and translation motion recently. Unlike the traditional modeling methods which separate the attitude and position modeling, the dual quaternions modeling method takes the translational and rotational coupled effects into consideration and combines the rotational parameters and the translational parameters into one dual quaternions, which is a much more compact representation compared to the traditional quaternions kinematics, dynamics, and translational motion modeling. Also, the dual quaternion–based representations have the similar form of the traditional ones, which is one of the most appealing feature of the dual quaternion–based method. ¹⁴ Brodsky and Shoham ¹⁵ describe the dual quaternion–based rigid body kinematics and dynamics modeling in detail. Goddard ¹⁶ and Zu et al. ¹⁷ use the dual quaternion–based kinematics and the EKF to estimate the pose parameters of a target in space. Filipe et al. ¹⁴ utilize the error dual quaternions to model the targets kinematics and then make the pose parameters estimation with the EKF. However, all the researches mentioned above only utilize the dual quaternion–based kinematics to accomplish the pose parameter estimation, and none of them uses the dual quaternion dynamics to estimate the space targets. As a result, none of the former works accomplishes the inertial parameters estimation together with the dual quaternion–based pose parameter estimation.

Also, all the above-mentioned methods utilize the conventional EKF estimation frame. Although its effectiveness has been proven by multiple spacecraft missions, the conventional EKF-based estimating algorithms have their innate defects. In fact, the complex space environment will sometimes cause the sensors fail to give the correct measurements. As a result, the standard EKF is not robust against faults ¹⁸ when using the parameters estimation in space, and the algorithm sometimes diverges or has a low accuracy.^18,19 To deal with this defect, some researchers developed the adaptive estimating techniques, which can be classified into three categories, namely, the multiple model–based adaptive estimation (MMAE),^20,21 the innovation-based adaptive estimation (IAE),^18,19,22 and the residue-based adaptive estimation (RAE). ²³ Among these researches, some of them estimate the uncertain process noise and the measurements noise,^20,21,23 and some of them scale the noise covariance matrices by single or multiple factors to make the estimation robust.^18,19,22 However, as far as the authors know, all the robust filtering techniques are utilized by the traditional translational and rotational motion models and none of them have been introduced to the parameters estimations based on the novel dual quaternion modeling method.

In this article, a dual vector quaternions–based extended Kalman filter (DVQ-EKF) and a dual vector quaternions–based adaptive fading factors extended Kalman filter (DVQ-AFFEKF) are presented to estimate the pose and inertial parameters of a free-floating tumbling space target. In the proposed algorithms, not only the dual vector quaternions–based kinematics is used but also the dual vector quaternions–based dynamics is derived and utilized to estimate the inertial parameters. As far as the authors know, both of the designed methods are the first pose and inertial parameters estimation algorithm based on dual vector quaternions. Unlike the dual quaternion–based dynamics proposed in Filipe and colleagues,^24–27 the dual vector quaternions–based dynamics proposed in this article decreases the dimension of the dual quaternions from 8-by-8 to 6-by-6, which only utilizes the vector parts of them in the dynamic equations to have a more concise representation format and a lower computational burden. In addition, the designed DVQ-AFFEKF utilized the IAE method to generate multiple fading factors to tuning the Kalman filter gain to against the influences caused by the faulty measurements. Based on the proposed algorithms, not only the pose parameters but also the inertial parameters of the free-floating tumbling uncooperative target can be estimated, which is impossible in the former researches.^14,24–28 Besides, considering the DVQ-AFFEKF will have more computational burden, prior to using the designed DVQ-AFFEKF, an innovation-based ${\chi }^2$ fault detection is first introduced in the algorithm. After the fault detection, only when the system is regarded as faulty will use the designed DVQ-AFFEKF, otherwise the DVQ-EKF will be used to estimate the parameters to accelerate the parameter estimation speed. As a result, the algorithms presented in this article are “all parameters” estimation algorithm without touching the target and can be utilized in various AR&D missions to provide a better prior knowledge for the following route plan and capture procedures.

This article is organized as follows. Section “Mathematical preliminaries” gives the mathematical preliminaries of the work. Then, the mission background is given in section “Model kinematics and dynamics based on dual vector quaternions.” Also, the dual quaternion–based kinematic equations, the dual error quaternions–based kinematic equations, and the dual quaternion–based dynamic equations are proposed and derived (only dynamics) in section “Model kinematics and dynamics based on dual vector quaternions.” In section “Pose parameters and inertial parameters estimation based on DVQ-EKF and DVQ-AFFEKF,” the DVQ-EKF and DVQ-AFFEKF are designed. In section “Simulation,” the simulations are carried out and discussed. Finally, in section “Conclusion,” the conclusions are provided.

Mathematical preliminaries

The definitions of the quaternions, dual quaternions, and dual vector quaternions are presented in sections “Quaternion” and “Dual quaternions and dual vector quaternions,” respectively. Also, some useful lemmas which will be utilized later in this article are shown. More details can be found in Filipe et al. ¹⁴ and Wang and Sun. ²⁹

Quaternions

The quaternions has the following form

$$\mathit{q}=(q_0,q_1,q_2,q_3)=q_0+q_1\mathit{i}+q_2\mathit{j}+q_3\mathit{k}$$

(1)

where $q_0$ is the scalar part, and $q_1\mathit{i}+q_2\mathit{j}+q_3\mathit{k}$ is the vector part. If $\Vert q\Vert =1$ , the quaternion is called the unit quaternion which can be used to represent the attitude kinematics of a rigid body, and the unit quaternion satisfies the equation $q_0^2+q_1^2+q_2^2+q_3^2=1$ . When representing the attitude kinematics of a rigid body, the unit quaternion is represented as $q=(\begin{array}{cc}\sin \frac{\phi }{2}\hfill & \cos \frac{\phi }{2}\hfill \end{array})$ , where $\phi$ is the angle around the rotation axis $n$ . The conjugate of $q$ is defined as $(q_0,-q_1,-q_2,-q_3)$ .

Also, the mapping between quaternions and the real number can be represented as $[·]:Q^4\to R^4$ . Under this mapping, the quaternion multiplication and cross product can be computed in the real number domain as follows

$$[\mathit{a}\times \mathit{b}]=[\mathit{a}\times ][\mathit{b}]$$

(2)

and

$$[\mathit{ab}]=[\mathit{a}{]}^{\mathit{L}}[\mathit{b}]$$

(3)

where $[\mathit{a}\times ]=\left[\begin{array}{cc}0\hfill & 0_{1\times 3}\hfill \\ \multicolumn{1}{c}{0_{3\times 1}}& [\overline{\mathit{a}}\times ]\hfill \end{array}\right]$ , $[\overline{\mathit{a}}\times ]=\left[\begin{array}{ccc}0\hfill & -a_3\hfill & a_2\hfill \\ \multicolumn{1}{c}{a_3}& 0\hfill & -a_1\hfill \\ \multicolumn{1}{c}{-a_2}& a_1\hfill & 0\hfill \end{array}\right]$ , and $[\mathit{a}{]}^{\mathit{L}}=[\begin{array}{cc}[\mathit{a}{]}_{4\times 1}^{\mathit{L}}\hfill & [\mathit{a}{]}_{4\times 3}^{\mathit{L}}\hfill \end{array}]$ , $[\mathit{a}{]}_{4\times 1}^{\mathit{L}}=[\begin{array}{c}{a}_0\hfill \\ \multicolumn{1}{c}{\overline{\mathit{a}}}\end{array}]$ , ${[a]}_{4\times 3}^L=\left[\begin{array}{c}-a^T\\ a_0{I}_{3\times 3}+[\overline{a}\times ]\end{array}\right]$

Dual quaternions and dual vector quaternions

The dual quaternions has the following format

$$\hat{\mathit{q}}=\mathit{q}+\mathbf{\epsilon }\mathit{q}\prime$$

(4)

where $q$ and $q\prime$ are quaternions with the dual operator ${\epsilon }^2=0$ and $\epsilon \ne 0$ . In particular, if both the real part and the dual part of a dual quaternion are unit quaternions, the dual quaternion is called the unit dual quaternion.

The dual vector quaternions is defined only to use the vector part of each quaternion, and the dual vector quaternions can be represented as follows

$$\overline{\hat{\mathit{q}}}=\overline{\mathit{q}}+\mathbf{\epsilon }\overline{\mathit{q}}\prime$$

(5)

where $\overline{q}$ and $\overline{q}\prime$ are the vector parts of the quaternions $q$ and $q\prime$ .

Given two dual quaternions ${\hat{q}}_1$ , ${\hat{q}}_2$ and two dual vector quaternions ${\overline{\hat{q}}}_1$ , ${\overline{\hat{q}}}_2$ , the basic operations are as follows:

1. Addition

$${\hat{\mathit{q}}}_1+{\hat{\mathit{q}}}_2=({\mathit{q}}_1+{\mathit{q}}_2)+\mathbf{\epsilon }\mathbf{(}{\mathit{q}}_1\prime \mathbf{+}{\mathit{q}}_2\prime \mathbf{)}$$

(6)

$${\overline{\hat{\mathit{q}}}}_1+{\overline{\hat{\mathit{q}}}}_2=\overline{{\hat{\mathit{q}}}_1+{\hat{\mathit{q}}}_2}$$

(7)

2. Dual quaternion multiplication

$${\hat{\mathit{q}}}_1{\hat{\mathit{q}}}_2=({\mathit{q}}_1{\mathit{q}}_2)+\mathbf{\epsilon }\mathbf{(}{\mathit{q}}_1{\mathit{q}}_2\prime \mathbf{+}{\mathit{q}}_1\prime {\mathit{q}}_2\mathbf{)}$$

(8)

$${\overline{\hat{\mathit{q}}}}_1{\overline{\hat{\mathit{q}}}}_2=\overline{{\hat{\mathit{q}}}_1{\hat{\mathit{q}}}_2}$$

(9)

3. Dual quaternion conjugate

$${\hat{\mathit{q}}}^{*}={\mathit{q}}^{*}+\mathbf{\epsilon }{\mathit{q}}^{\prime \mathbf{*}}$$

(10)

$${\overline{\hat{\mathit{q}}}}^{*}=\overline{{\hat{\mathit{q}}}^{*}}$$

(11)

4. Dual quaternion swap

$${\hat{\mathit{q}}}^{\mathit{s}}=\mathit{q}\prime +\mathbf{\epsilon }\mathit{q}$$

(12)

$${\overline{\hat{\mathit{q}}}}^{\mathit{s}}=\overline{{\hat{\mathit{q}}}^{\mathit{s}}}$$

(13)

Also, like the mapping between the quaternion and the real number, the dual quaternion and the dual vector quaternion also have a mapping with the real matrix

$$[\hat{\mathit{q}}]=[\begin{array}{cccccccc}{q}_0\hfill & q_1\hfill & q_2\hfill & q_3\hfill & q_0\prime \hfill & q_1\prime \hfill & q_2\prime \hfill & q_3\prime \hfill \end{array}]$$

(14)

$$[\overline{\hat{\mathit{q}}}]=\left[\begin{array}{cccccc}{q}_1\hfill & q_2\hfill & q_3\hfill & q_1\prime \hfill & q_2\prime \hfill & q_3\prime \hfill \end{array}\right]$$

(15)

Similar to equation (2), the multiplication between two dual quaternions in real number domain can be represented as

$$[{\hat{\mathit{q}}}_1{\hat{\mathit{q}}}_2]=[{\hat{\mathit{q}}}_1{\mathbf{]}}^L[{\hat{\mathit{q}}}_2]$$

(16)

$$[{\overline{\hat{\mathit{q}}}}_1{\overline{\hat{\mathit{q}}}}_2]=\overline{[{\hat{\mathit{q}}}_1{\hat{\mathit{q}}}_2]}$$

(17)

where $[{\hat{\mathit{q}}}_1{]}^{\mathit{L}}=\left[\begin{array}{cc}{[{\mathit{q}}_1]}^{\mathit{L}}\hfill & 0_{4\times 4}\hfill \\ \multicolumn{1}{c}{{[{\mathit{q}}_1\prime ]}^{\mathit{L}}}& {[{\mathit{q}}_1]}^{\mathit{L}}\hfill \end{array}\right]$ , $[{\mathit{q}}_1{]}^{\mathit{L}}$ and $[{\mathit{q}}_1\prime {]}^{\mathit{L}}$ have the same definition as mentioned above in quaternion operation.

Also, the cross product between two dual quaternions can be expressed in real number domain as follows

$$[{\hat{\mathit{q}}}_1\times {\hat{\mathit{q}}}_2]=[{\hat{\mathit{q}}}_1\times ][{\hat{\mathit{q}}}_2]$$

(18)

$$[{\overline{\hat{\mathit{q}}}}_1\times {\overline{\hat{\mathit{q}}}}_2]=\overline{[{\hat{\mathit{q}}}_1\times {\hat{\mathit{q}}}_2]}$$

(19)

where $[{\hat{\mathit{q}}}_i\times ]=\left[\begin{array}{cc}[{\mathit{q}}_i\times ]\hfill & 0_{4\times 4}\hfill \\ \multicolumn{1}{c}{[{\mathit{q}}_i\prime \times ]}& [{\mathit{q}}_i\times ]\hfill \end{array}\right]$ .

Model kinematics and dynamics based on dual vector quaternions

Model overview

Figure 1 illustrates the chaser satellite and the free-floating uncooperative target as rigid bodies operating in a nearby orbit. $\left\{\mathrm{I}\right\}$ is assumed to be the inertial frame, and $\left\{\mathrm{B}\right\}$ is assumed to be the body frame of the target. Particularly, the origin of $\left\{\mathrm{B}\right\}$ is located at the center of mass of the target. Coordinate $\{\mathrm{B}\prime \}$ is assumed to be fixed to the targets capturing fixture. In this article, it is assumed that $\{\mathrm{B}\prime \}$ and $\left\{\mathrm{B}\right\}$ have the same orientation, so there is only a translation distance $\rho$ between the origins of $\left\{\mathrm{B}\right\}$ and $\{\mathrm{B}\prime \}$ .

Figure 1.

Chaser satellite and the free-floating uncooperative target as rigid bodies operating in a nearby orbit.

Then, the rotation of $\left\{\mathrm{B}\right\}$ with respect to $\left\{\mathrm{I}\right\}$ is represented by unit quaternion $q_{B/I}$ , and the distance between the origin of $\left\{\mathrm{B}\right\}$ and $\left\{\mathrm{I}\right\}$ is $r_{B/I}$ along with the distance between it is $r_m$ . Also, this article assumes that the angular velocity of the target with respect to $\left\{\mathrm{I}\right\}$ is $\mathit{\omega }$ , and the linear velocity is $v$ .

Model kinematics based on dual quaternions

As mentioned in section “Model kinematics based on dual error vector quaternions,” the attitude of $\left\{\mathrm{B}\right\}$ with respect to $\left\{\mathrm{I}\right\}$ can be represented as ${\mathit{q}}_{\mathit{B}/I}$ , and the position of $\left\{\mathrm{B}\right\}$ with respect to $\left\{\mathrm{I}\right\}$ is ${\mathit{r}}_{\mathit{B}/I}$ . From Filipe and Tsiotras, ²⁴ it is known that the pose of it can be represented compactly by an unit dual quaternion

$$\begin{array}{c}\multicolumn{1}{c}{{\hat{\mathit{q}}}_{\mathit{B}/I}={\mathit{q}}_{\mathit{B}/I}+\mathbf{\epsilon }{\mathit{q}\prime }_{\mathit{B}\mathbf{/}\mathbf{I}}}\\ ={\mathit{q}}_{\mathit{B}/I}+\mathbf{\epsilon }\frac{1}{2}{\mathit{q}}_{\mathit{B}\mathbf{/}\mathbf{I}}{\mathit{r}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathbf{B}}\hfill \\ ={\mathit{q}}_{\mathit{B}/I}+\mathbf{\epsilon }\frac{1}{2}{\mathit{r}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathbf{I}}{\mathit{q}}_{\mathit{B}\mathbf{/}\mathbf{I}}\hfill \end{array}$$

(20)

where ${\hat{\mathit{q}}}_{\mathit{B}/I}$ is the unit dual quaternion of $\left\{\mathrm{B}\right\}$ with respect to $\left\{\mathrm{I}\right\}$ , and ${\mathit{r}}_{X/Y}^Z=(\begin{array}{cc}0\hfill & \overline{{\mathit{r}}_{X/Y}^Z}\hfill \end{array})$ , ${\overline{\mathit{r}}}_{\mathit{X}/Y}^{\mathit{Z}}=(\begin{array}{ccc}{\overline{r}}_{X/Y,x}^Z\hfill & {\overline{r}}_{X/Y,y}^Z\hfill & {\overline{r}}_{X/Y,z}^Z\hfill \end{array})$ is the position of $\left\{\mathrm{X}\right\}$ with respect to $\left\{\mathrm{Y}\right\}$ expressed in $\left\{\mathrm{Z}\right\}$ in quaternion format.

Also, the motion between $\left\{\mathrm{B}\right\}$ and $\left\{\mathrm{I}\right\}$ can also be represented by an unit dual quaternion. Using the definition of $\mathit{\omega }$ and $v$ defined in “Model kinematics based on dual error vector quaternions,” a new dual quaternion is defined as the follows to represent the motion between $\left\{\mathrm{B}\right\}$ and $\left\{\mathrm{I}\right\}$

$${\hat{\mathit{\omega }}}_{\mathit{B}/I}^{\mathit{B}}={\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{+}\mathbf{\epsilon }\mathbf{\omega }{\prime }_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{=}{\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{+}\mathbf{\epsilon }{\mathbf{v}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{=}\mathbf{\omega }\mathbf{+}\mathbf{\epsilon }\mathit{v}$$

(21)

where ${\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{=}\mathbf{(}\begin{array}{cc}0\hfill & {\overline{\mathbf{\omega }}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\hfill \end{array}\mathbf{)}$ , ${\overline{\mathit{\omega }}}_{\mathit{B}/I}^{\mathit{B}}=(\begin{array}{ccc}{\overline{\mathit{\omega }}}_{B/I,x}^B\hfill & {\overline{\mathit{\omega }}}_{B/I,y}^B\hfill & {\overline{\mathit{\omega }}}_{B/I,z}^B\hfill \end{array})$ is the angular velocity of $\left\{\mathrm{B}\right\}$ with respect to $\left\{\mathrm{I}\right\}$ expressed in $\left\{\mathrm{B}\right\}$ , ${\mathit{v}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}=(\begin{array}{cc}0\hfill & {\overline{\mathit{v}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\hfill \end{array})$ , ${\overline{\mathit{v}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}=(\begin{array}{ccc}{\overline{v}}_{B/I,x}^B\hfill & {\overline{v}}_{B/I,y}^B\hfill & {\overline{v}}_{B/I,z}^B\hfill \end{array})$ is the linear velocity of the origin of $\left\{\mathrm{B}\right\}$ with respect the origin of {I} expressed in {B}.

From Filipe and Tsiotras, ²⁴ the dual quaternion–based kinematic equation can be represented as follows

$$2\stackrel{·}{\hat{\mathit{q}}}={\hat{\mathit{\omega }}}_{\mathit{B}/I}^{\mathit{I}}{\hat{\mathit{q}}}_{\mathit{B}/I}={\hat{\mathit{q}}}_{\mathit{B}/I}{\hat{\mathit{\omega }}}_{\mathit{B}/I}^{\mathit{B}}$$

(22)

Note that the above dual quaternion–based kinematic equation has the similar format as the traditional quaternion–based kinematic equation in Markley and colleagues.^8,30

Model kinematics based on dual error vector quaternions

When using the traditional four-element quaternion to represent the attitude kinematics, as Filipe et al. ¹⁴ mentioned, it will have a $4\times 4$ covariance matrix of the four quaternions, which leads to a heavy computational burden. However, if the unit quaternion is used to represent the attitude kinematics, because of the unit norm constraint, using only three-element quaternion can represent the attitude kinematics, and only a $3\times 3$ covariance matrix needs to be computed, especially when using the EKF to do the further estimation. Hence, to reduce the computation load and use the dual quaternion form to represent the kinematics of the target, the dual error vector quaternions–based kinematics of the target was first proposed by Filipe et al. ¹⁴ as follows

$$\frac{\mathit{d}\mathbf{\delta }\overline{\hat{\mathit{q}}}}{\mathit{dt}}=-\frac{1}{2}\overline{\widehat{\tilde{\mathit{\omega }}}}\mathbf{\delta }\overline{\hat{\mathit{q}}}\mathbf{+}\frac{1}{2}\mathbf{\delta }\overline{\hat{\mathit{q}}}\overline{\hat{\mathit{\omega }}}$$

(23)

where $\overline{\widehat{\tilde{\mathit{\omega }}}}$ is the estimate of $\overline{\hat{\mathit{\omega }}}$ , $\mathbf{\delta }\overline{\hat{\mathit{q}}}$ is the dual error quaternion, which can be represented as $\mathbf{\delta }\overline{\hat{\mathit{q}}}\mathbf{=}{\overline{\widehat{\tilde{\mathit{q}}}}}^{\mathbf{*}}\overline{\hat{\mathit{q}}}\mathbf{=}\mathbf{\delta }\overline{\mathit{q}}\mathbf{+}\frac{1}{2}\mathbf{\delta }{\overline{\mathit{r}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{I}}\mathbf{\delta }\overline{\mathit{q}}\mathbf{=}\mathbf{\delta }\overline{\mathit{q}}\mathbf{+}\frac{1}{2}\mathbf{\delta }\overline{\mathit{q}}\mathbf{\delta }{\overline{\mathit{r}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}$ , ${\overline{\widehat{\tilde{\mathit{q}}}}}^{*}$ is vector part of the conjugate of the estimation of $\overline{\hat{\mathit{q}}}$ , $\delta \overline{\mathit{q}}$ is the error quaternion of $\left\{\mathrm{B}\right\}$ with respect to $\left\{\mathrm{I}\right\}$ , $\mathbf{\delta }{\overline{\mathit{r}}}_{\mathit{X}\mathbf{/}\mathbf{Y}}^{\mathit{Z}}\mathbf{=}{\overline{\mathit{r}}}_{\mathit{X}\mathbf{/}\mathbf{Y}}^{\mathit{Z}}-{\overline{\tilde{\mathit{r}}}}_{\mathit{X}\mathbf{/}\mathbf{Y}}^{\mathit{Z}}$ is the error position of $\left\{\mathrm{X}\right\}$ with respect to $\left\{\mathrm{Y}\right\}$ expressed in $\left\{\mathrm{Z}\right\}$ in quaternion format, and ${\overline{\tilde{\mathit{r}}}}_{\mathit{X}/Y}^{\mathit{Z}}$ is the vector part of the estimation of ${\mathit{r}}_{X/Y}^Z$ .

Using equation (23), one can have the dual error vector quaternions–based kinematics of a target to reduce the computation load. However, as Markley ³⁰ mentioned, all the three-dimensional attitude representations are singular of some certain attitudes. In fact, in Markley and colleagues,^8,30 it is obvious that the attitude between the true attitude and the estimated attitude is bigger than $180°$ . Also this defect is proposed by Filipe et al. ¹⁴ Nevertheless, when using the kinematic equations to perform a parameter estimation task, the error should be small enough if the estimation method is correct, and the EKF should be converged if one used the EKF as the estimation method. In this way, the error between the true attitude and the estimated one is small enough to guarantee this dual error vector quaternions–based kinematic representation will not have the singularity problem.

Model dynamics based on dual vector quaternions

Compared to much research work on dual quaternion–based kinematics, the dual quaternion–based dynamics is given less attention. Brodsky and Shoham ¹⁵ proposed a general dual quaternion–based dynamics for the rigid body, and Wang and Sun ²⁹ proposed a dual quaternion–based dynamics for a space target. However, both of these two dual quaternion–based dynamics used a dual operator $\frac{d}{d\epsilon }$ , which has the properties that $\frac{d}{d\epsilon }(\mathit{q}+\mathbf{\epsilon }\mathit{q}\prime \mathbf{)}\mathbf{=}\mathit{q}\prime$ , and $(\frac{d}{d\epsilon }{)}^2=0$ . When this operator is used, it has two main drawbacks. First of all this operator makes the later operation complex, especially when doing the matrix inverse operation. ²⁸ Second, using this operator makes the mapping between the dual number and the real number much more difficult. In the literature,^24–28 8-by-8 symmetric matrix is utilized to represent the model dynamics. However, the 8-by-8 symmetric matrix will cost a high computational load, and the scalar part of each quaternion in the dual quaternions is zero in this case. To overcome these drawbacks, this article only utilizes the vector parts of the dual quaternions to represent the model dynamics to have a more concise representation and lower computational burden.

According to Wang and Sun, ²⁹ the dual quaternion–based dynamic equations are analogous to the traditional attitude dynamic equations as follows

$$\frac{\mathit{d}{\hat{\mathit{H}}}_{\mathit{B}/I}^{\mathit{B}}}{\mathit{dt}}+{\hat{\mathit{\omega }}}_{\mathit{B}/I}^{\mathit{B}}\times {\hat{\mathit{H}}}_{\mathit{B}/I}^{\mathit{B}}=\widehat{{\mathit{f}}^B}$$

(24)

where ${\hat{\mathit{H}}}_{\mathit{B}/I}^{\mathit{B}}={\mathit{mv}}_{\mathit{B}/I}^{\mathit{B}}+\mathbf{\epsilon }\mathbf{(}{\mathit{J}}_{\mathbf{B}}\mathbf{\otimes }{\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{)}$ , ${\mathit{J}}_B=\left[\begin{array}{cc}0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& {\overline{\mathit{J}}}_{\mathit{B}}\hfill \end{array}\right]$ , $\overline{{\mathit{J}}_B}=\left[\begin{array}{ccc}{J}_{\mathrm{Bx}}\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& J_{\mathrm{By}}\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & J_{\mathrm{Bz}}\hfill \end{array}\right]$

$J_{\mathrm{Bx}}$ , $J_{\mathrm{By}}$ , $J_{\mathrm{Bz}}$ are the inertia tensor of the main axis of the target in the body frame. $\widehat{{\mathit{f}}^B}={\mathit{f}}^B+\mathbf{\epsilon }{\mathbf{\tau }}^{\mathit{B}}$ , ${\mathit{f}}^B=(\begin{array}{cc}0\hfill & {\overline{\mathit{f}}}_{\mathit{B}}\hfill \end{array})$ is the total external moment force in translation, and $\overline{{\mathit{f}}^B}=(\begin{array}{ccc}{\overline{f}}_x^B\hfill & {\overline{f}}_y^B\hfill & {\overline{f}}_z^B\hfill \end{array})$ . ${\mathbf{\tau }}^{\mathit{B}}=(\begin{array}{cc}0\hfill & {\overline{\tau }}_{\mathit{B}}\hfill \end{array})$ is the total external moment force in rotation, and ${\overline{\tau }}_{\mathit{B}}=(\begin{array}{ccc}{\overline{\tau }}_{\mathrm{Bx}}\hfill & {\overline{\tau }}_{\mathrm{By}}\hfill & {\overline{\tau }}_{\mathrm{Bz}}\hfill \end{array})$ . Extending equation (24), we get

$$\begin{array}{c}\frac{d(m{\mathit{v}}_{\mathit{B}/I}^{\mathit{B}}+\mathbf{\epsilon }\mathbf{(}{\mathit{J}}_{\mathbf{B}}\mathbf{\otimes }{\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{)}\mathbf{)}}{\mathrm{dt}}+\hfill \\ \multicolumn{1}{c}{({\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{+}\mathbf{\epsilon }{\mathit{v}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{)}\mathbf{\times }\mathbf{(}\mathbf{m}{\mathit{v}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{+}\mathbf{\epsilon }\mathbf{(}\mathbf{(}{\mathit{J}}_{\mathbf{B}}\mathbf{\otimes }{\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{)}\mathbf{)}}\\ \multicolumn{1}{c}{={\mathit{f}}^B+\mathbf{\epsilon }{\mathbf{\tau }}^{\mathit{B}}}\end{array}$$

(25)

$$\begin{array}{c}m{\stackrel{·}{\mathit{v}}}_{\mathit{B}/I}^{\mathit{B}}+\mathbf{\epsilon }\mathbf{(}{\mathit{J}}_{\mathbf{B}}\mathbf{\otimes }{\stackrel{·}{\mathbf{\omega }}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{)}\mathbf{+}\mathbf{(}{\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{\times }\mathbf{m}{\mathit{v}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{)}\mathbf{+}\hfill \\ \multicolumn{1}{c}{\mathbf{\epsilon }\mathbf{(}{\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{\times }\mathbf{(}{\mathit{J}}_{\mathbf{B}}\mathbf{\otimes }{\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{)}\mathbf{+}{\mathit{v}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{\times }\mathbf{(}\mathbf{m}{\mathit{v}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{)}\mathbf{)}}\\ \multicolumn{1}{c}{={\mathit{f}}^B+\mathbf{\epsilon }{\mathbf{\tau }}^{\mathit{B}}}\end{array}$$

(26)

$$\begin{array}{c}(m{\stackrel{·}{\mathit{v}}}_{\mathit{B}/I}^{\mathit{B}}+{\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{\times }\mathbf{m}{\mathit{v}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{)}\mathbf{+}\hfill \\ \multicolumn{1}{c}{\mathbf{\epsilon }\mathbf{(}{\mathit{J}}_{\mathbf{B}}\mathbf{\otimes }{\stackrel{·}{\mathbf{\omega }}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{+}{\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{B}}^{\mathit{B}}\mathbf{\times }\mathbf{(}{\mathit{J}}_{\mathbf{B}}\mathbf{\otimes }{\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{)}\mathbf{)}}\\ \multicolumn{1}{c}{={\mathit{f}}^B+\mathbf{\epsilon }{\mathbf{\tau }}^{\mathit{B}}}\end{array}$$

(27)

from equations (25)–(27), it is obvious that the translational motion (the real part) is coupled with the rotation. However, the rotation (the dual part) is not coupled with the translational motion.

Define matrix $\mathit{A}=\left[\begin{array}{cc}{\mathit{A}}_{11}\hfill & {\mathit{A}}_{12}\hfill \\ \multicolumn{1}{c}{{\mathit{A}}_{21}}& {\mathit{A}}_{22}\hfill \end{array}\right]$ , ${\mathit{A}}_{11}=\left[\begin{array}{cc}0\hfill & 0_{1\times 3}\hfill \\ \multicolumn{1}{c}{0_{3\times 1}}& m{\mathit{I}}_{3\times 3}\hfill \end{array}\right]$ , ${\mathit{A}}_{12}=0_{4\times 4}$ , ${\mathit{A}}_{21}=0_{4\times 4}$ , ${\mathit{A}}_{22}=\left[\begin{array}{cc}0\hfill & 0_{1\times 3}\hfill \\ \multicolumn{1}{c}{0_{3\times 1}}& {\overline{\mathit{J}}}_{\mathit{B}}\hfill \end{array}\right]$ .

We get ${\hat{\mathit{H}}}_{\mathit{B}/I}^{\mathit{B}}=\mathit{A}\otimes ({\hat{\mathit{\omega }}}_{\mathit{B}/I}^{\mathit{B}}{)}^{\mathit{s}}$ . Hence, equation (24) can be written as

$$\frac{\mathit{dA}\otimes {({\hat{\mathit{\omega }}}_{\mathit{B}/I}^{\mathit{B}})}^{\mathit{s}}}{\mathit{dt}}+{\hat{\mathit{\omega }}}_{\mathit{B}/I}^{\mathit{B}}\times (\mathit{A}\otimes ({\hat{\mathit{\omega }}}_{\mathit{B}/I}^{\mathit{B}}{)}^{\mathit{s}})=\widehat{{\mathit{f}}^B}$$

(28)

If the external control torque is zero and the disturbance torque can be ignored, equation (28) can be written as

$$\frac{\mathit{dA}\otimes {({\hat{\mathit{\omega }}}_{\mathit{B}/I}^{\mathit{B}})}^{\mathit{s}}}{\mathit{dt}}+{\hat{\mathit{\omega }}}_{\mathit{B}/I}^{\mathit{B}}\times (\mathit{A}\otimes ({\hat{\mathit{\omega }}}_{\mathit{B}/I}^{\mathit{B}}{)}^{\mathit{s}})=\hat{0}$$

(29)

which equals to

$$\frac{\mathit{d}{({\hat{\mathit{\omega }}}_{\mathit{B}/I}^{\mathit{B}})}^{\mathit{s}}}{\mathit{dt}}={\mathit{A}}^{-1}\otimes (-{\hat{\mathit{\omega }}}_{\mathit{B}/I}^{\mathit{B}}\times (\mathit{A}\otimes ({\hat{\mathit{\omega }}}_{\mathit{B}/I}^{\mathit{B}}{)}^{\mathit{s}}))$$

(30)

equation (30) is the dual quaternion–based dynamics of the target when the rotational angular velocity and the translational velocity of the body frame $\left\{\mathrm{B}\right\}$ are represented with respect to the inertial frame $\left\{\mathrm{I}\right\}$ .

Note that the scalar parts of the quaternions in each dual quaternions are zero, we can rewrite equation (30) only by the vector parts of each quaternions as follows

$$\frac{\mathit{d}{({\overline{\hat{\mathbf{\omega }}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{)}}^{\mathit{s}}}{\mathit{dt}}={\overline{\mathit{A}}}^{-1}\otimes (-{\overline{\hat{\mathbf{\omega }}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{\times }\mathbf{(}\overline{\mathit{A}}\mathbf{\otimes }\mathbf{(}{\overline{\hat{\mathbf{\omega }}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}{\mathbf{)}}^{\mathit{s}}\mathbf{)}\mathbf{)}$$

(31)

where $\overline{\mathit{A}}=\left[\begin{array}{cc}{\overline{\mathit{A}}}_{11}\hfill & {\overline{\mathit{A}}}_{12}\hfill \\ \multicolumn{1}{c}{{\overline{\mathit{A}}}_{21}}& {\overline{\mathit{A}}}_{22}\hfill \end{array}\right]$ , ${\overline{\mathit{A}}}_{11}=m{\mathit{I}}_{3\times 3}$ , ${\overline{\mathit{A}}}_{12}=0_{3\times 3}$ , ${\overline{\mathit{A}}}_{21}=0_{3\times 3}$ , ${\overline{\mathit{A}}}_{22}=\overline{{\mathit{J}}_B}$ . Also, define the $\mathrm{operation}\overline{\otimes }$ and $\overline{\odot }\mathrm{as}$ follows

$$\overline{\mathit{A}}\otimes \overline{\hat{\mathit{a}}}=({\overline{\mathit{A}}}_{11}\overline{\odot }\overline{\mathit{a}}+{\overline{\mathit{A}}}_{12}\overline{\odot }\overline{\mathit{a}}\prime )+\mathbf{\epsilon }\mathbf{(}{\overline{\mathit{A}}}_{21}\overline{\odot }\overline{\mathit{a}}\mathbf{+}{\overline{\mathit{A}}}_{22}\overline{\odot }\overline{\mathit{a}}\prime \mathbf{)}$$

(32)

$$\overline{\mathit{A}}\overline{\odot }\overline{\mathit{a}}=\overline{\mathit{A}}[\overline{\mathit{a}}]$$

(33)

Note that although equation (31) has the same format as equation (30), which is the one mentioned in Filipe and colleagues,^24–27 equation (31) is the dual vector quaternions representation of the model dynamics and it decreased the dimension from 8-by-8 to 6-by-6, which decreased the computational load and had a more concise format. In addition, because only the dual vector quaternions are used, the multiplication of the matrix and the vector quaternions became the real matrix multiplication between a 3-by-3 matrix and a 3-by-1 column vector, which also simplified the computational procedure compared to the one mentioned in Filipe and colleagues.^24–27

Pose parameters and inertial parameters estimation based on DVQ-EKF and DVQ-AFFEKF

The DVQ-EKF and DVQ-AFFEKF for free-floating space objects pose parameters and inertial parameters estimation are designed in this section. First of all, the designed algorithms utilize the dual error quaternion–based kinematic equations and dual quaternion–based dynamic equations derived in section “Model kinematics and dynamics based on dual vector quaternions” as the base of the state equations. Then, the measured targets attitude and origin position of $\{\mathrm{B}\prime \}$ with respect to the inertial frame $\left\{\mathrm{I}\right\}$ are assumed to be obtained from a vision-based sensor (with noise).^9,10 The DVQ-EKF and DVQ-AFFEKF proposed in this article not only estimate the pose of the target but also estimate the inertial parameters, such as the ratios of the moments of inertia of the target and the location of the center of mass. Because without other information, the accurate inertial tensor values cannot be estimated,^9,10 the parameters estimated by this algorithm can, to some extent, be regarded as the full parameters estimation method without touching the target.

Besides, considering the DVQ-AFFEKF will have more computational burden, prior to using the designed DVQ-AFFEKF, a residue-based ${\chi }^2$ fault detection is first introduced in the algorithm. After the fault detection, only when the system is regarded as faulty will use the designed DVQ-AFFEKF, otherwise the DVQ-EKF will be used to estimate the parameters to enhance the parameters estimation speed.

This section will discuss the details of the designed DVQ-EKF and DVQ-AFFEKF.

Observation equations and linearization

Assuming the vision-based sensors and image processing system can give the relative attitude and origin position of the grapple fixture (frame $\{\mathrm{B}\prime \}$ ) with respect to $\left\{\mathrm{I}\right\}$ (with noise).^9,10,24 The observation equations can be written as

$$Z=\left[\begin{array}{c}{\mathit{q}}_m\hfill \\ \multicolumn{1}{c}{r_m}\end{array}\right]+v$$

(34)

where $r_m$ is the origin position of $\{\mathrm{B}\prime \}$ with respect to $\left\{\mathrm{I}\right\}$ , and ${\mathit{q}}_{\mathit{m}}$ is the relative attitude of $\{\mathrm{B}\prime \}$ with respect to $\left\{\mathrm{I}\right\}$ , and $v$ is the measurements noise, which is assumed to be the white noise with the covariance matrix $R=E[v{v}^T]$ .

Using the same technique in Aghili and Parsa, ¹⁰ the $q_m$ can be represented as

$${\mathit{q}}_{\mathit{m}}\stackrel{.}{=}{\tilde{\mathit{q}}}_{\mathit{B}/I}+\mathit{Q}({\tilde{\mathit{q}}}_{\mathit{B}/I})\mathbf{\delta }{\overline{\mathit{q}}}_{\mathit{B}\mathbf{/}\mathbf{I}}$$

(35)

where

$$\mathit{Q}({\tilde{\mathit{q}}}_{\mathit{B}/I})=\left[\begin{array}{c}-{\overline{\tilde{\mathit{q}}}}_{\mathit{B}/I}^{\mathit{T}}\hfill \\ \multicolumn{1}{c}{{\tilde{\mathit{q}}}_{\mathit{B}/I,0}{I}_{3\times 3}+[{\overline{\tilde{\mathit{q}}}}_{\mathit{B}/I}\times ]}\end{array}\right]$$

(36)

Also, from Figure 1, one can get

$${\mathit{r}}_{\mathit{m}}={\overline{\mathit{r}}}_{\mathit{B}\mathbf{/}\mathbf{I}}+\mathit{A}({\mathit{q}}_{\mathit{B}/I})\overline{\rho }$$

(37)

where $A(q)$ is the rotation matrix between $\{\mathrm{B}\prime \}$ and $\left\{\mathrm{I}\right\}$ , and ${\mathit{q}}_{\mathit{m}}=\mathit{q}$ , if one only gets the vector part, then ${\mathit{q}}_{\mathit{m}}=\overline{\mathit{q}}$ .

Equation (37) can be extended to the Taylor first-order extension as

$${\mathit{r}}_m\stackrel{.}{=}{\overline{\mathit{r}}}_{\mathit{B}/I}+[\mathit{A}({\tilde{\mathit{q}}}_{\mathit{B}/I})({\mathit{I}}_{3\times 3}+2[\mathbf{\delta }{\overline{\mathit{q}}}_{\mathit{B}\mathbf{/}\mathbf{I}}\mathbf{\times }\mathbf{]}\mathbf{)}\mathbf{]}\overline{\mathbf{\rho }}$$

(38)

where ${\tilde{\mathit{q}}}_{\mathit{B}/I}$ is the estimated value of ${\mathit{q}}_{\mathit{B}/I}$ . ${\overline{\mathit{q}}}_{\mathit{B}/I}={\overline{\tilde{\mathit{q}}}}_{\mathit{B}/I}\mathbf{\delta }{\overline{\mathit{q}}}_{\mathit{B}\mathbf{/}\mathbf{I}}\mathbf{=}\mathbf{[}{\overline{\tilde{\mathit{q}}}}_{\mathit{B}\mathbf{/}\mathbf{I}}{\mathbf{]}}^{\mathit{L}}\mathbf{[}\mathbf{\delta }{\overline{\mathit{q}}}_{\mathit{B}\mathbf{/}\mathbf{I}}\mathbf{]}$ , ${\overline{\tilde{q}}}_{B/I}$ is the vector part of ${\tilde{\mathit{q}}}_{\mathit{B}/I}$ .

Also,

$${\overline{\mathit{r}}}_{\mathit{B}/I}={\overline{\mathit{r}}}_{\mathit{B}/I}^I=2({\overline{\tilde{\mathit{q}}}}_{\mathit{B}\mathbf{/}\mathbf{I}}\mathbf{\delta }{\overline{\hat{\mathit{q}}}}_{\mathit{B}\mathbf{/}\mathbf{I}}\mathbf{)}\prime {\overline{\mathit{q}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathbf{*}}\mathbf{\delta }{\overline{\tilde{\mathit{q}}}}_{\mathit{B}\mathbf{/}\mathbf{I}}$$

(39)

where ${\overline{\widetilde{\hat{\mathit{q}}}}}_{\mathit{B}/I}$ is the estimated value of ${\overline{\hat{\mathit{q}}}}_{\mathit{B}/I}$ .

Combining equations (38) and (39), the observation equations can be written as

$$Z=h(X)=\left[\begin{array}{c}{h}_1(X)\hfill \\ \multicolumn{1}{c}{h_2(X)}\end{array}\right]$$

(40)

where $h_1(X)={\tilde{\mathit{q}}}_{\mathit{B}/I}+\mathit{Q}({\tilde{\mathit{q}}}_{\mathit{B}/I})\mathbf{\delta }{\mathit{q}}_{\mathit{B}\mathbf{/}\mathbf{I}}$ , and $h_2(X)=2({\overline{\widetilde{\hat{\mathit{q}}}}}_{\mathit{B}/I}\mathbf{\delta }{\overline{\hat{\mathit{q}}}}_{\mathit{B}\mathbf{/}\mathbf{I}}\mathbf{)}\prime {\overline{\mathit{q}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathbf{*}}\mathbf{\delta }{\overline{\tilde{\mathit{q}}}}_{\mathit{B}\mathbf{/}\mathbf{I}}\mathbf{+}\mathbf{[}\mathit{A}\mathbf{(}{\tilde{\mathit{q}}}_{\mathit{B}\mathbf{/}\mathbf{I}}\mathbf{)}\mathbf{(}{\mathit{I}}_{3\mathbf{\times }3}\mathbf{+}2\mathbf{[}\mathbf{\delta }{\overline{\mathit{q}}}_{\mathit{B}\mathbf{/}\mathbf{I}}\mathbf{\times }\mathbf{]}\mathbf{)}\mathbf{]}\overline{\mathbf{\rho }}$ . Define the state vector of the system as

$$X=[\begin{array}{cccc}{\overline{\hat{\mathit{q}}}}_{\mathit{B}/I}\hfill & {\overline{\hat{\mathbf{\omega }}}}_{\mathit{B}/I}\hfill & \overline{\hat{\mathit{p}}}\hfill & \overline{\hat{\mathbf{\rho }}}\hfill \end{array}]$$

(41)

where ${\overline{\hat{\mathit{q}}}}_{\mathit{B}/I}$ is the vector part of ${\hat{\mathit{q}}}_{\mathit{B}/I}$ , ${\overline{\hat{\mathbf{\omega }}}}_{\mathit{B}\mathbf{/}\mathbf{I}}$ is the vector part of ${\hat{\mathit{\omega }}}_{\mathit{B}/I}$ , $\overline{\hat{\mathit{p}}}$ is the vector part of $\hat{\mathit{p}}$ , $\hat{\mathit{p}}=\mathit{p}+\mathbf{\epsilon }0$ , $\mathit{p}=\left[\begin{array}{cccc}0\hfill & p_x\hfill & p_y\hfill & p_z\hfill \end{array}\right]$ is the ratios of the moments of inertia of the target in quaternion form, and similar to Aghili, ⁹ $\mathit{p}=\left[\begin{array}{ccc}{p}_x=\frac{J_{\mathrm{By}}-J_{\mathrm{Bz}}}{J_{\mathrm{Bx}}}\hfill & p_y=\frac{J_{\mathrm{Bz}}-J_{\mathrm{Bz}}}{J_{\mathrm{By}}}\hfill & p_x=\frac{J_{\mathrm{Bx}}-J_{\mathrm{By}}}{J_{\mathrm{Bz}}}\hfill \end{array}\right]$ , $\overline{\hat{\mathbf{\rho }}}$ is the vector part of $\hat{\rho }$ , $\hat{\rho }=\mathbf{\rho }\mathbf{+}\mathbf{\epsilon }0$ , is the location of the center of mass in quaternion form.

The linearization of the observation equations is

$$\begin{array}{c}H(X)=\frac{\partial h}{\partial X}=\hfill \\ \multicolumn{1}{c}{\left[\begin{array}{cccc}\mathit{Q}({\tilde{\mathit{q}}}_{\mathit{B}/I})\hfill & 0_{4\times 3}\hfill & 0_{4\times 12}\hfill & 0_{4\times 6}\hfill \\ \multicolumn{1}{c}{-2\mathit{A}({\tilde{\mathit{q}}}_{\mathit{B}/I})[\overline{\rho }\times ]}& \mathbf{\Gamma }\mathbf{(}{\tilde{\mathit{q}}}_{\mathit{B}\mathbf{/}\mathbf{I}}\mathbf{)}\hfill & 0_{3\times 15}\hfill & 2\mathit{A}({\tilde{\mathit{q}}}_{\mathit{B}/I})(\mathbf{\delta }{\overline{\mathit{q}}}_{\mathit{B}\mathbf{/}\mathbf{I}}\mathbf{\times }\mathbf{)}\hfill \end{array}\right]}\end{array}$$

(42)

where $\mathbf{\Gamma }\mathbf{(}{\tilde{\mathit{q}}}_{\mathit{B}\mathbf{/}\mathbf{I}}\mathbf{)}\mathbf{=}2\mathbf{(}{\overline{\widetilde{\hat{\mathit{q}}}}}_{\mathit{B}\mathbf{/}\mathbf{I}}\mathbf{)}\prime \mathbf{\delta }{\overline{\tilde{\mathit{q}}}}_{\mathit{B}\mathbf{/}\mathbf{I}}{\overline{\tilde{\mathit{q}}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathbf{*}}\mathbf{=}2\mathit{A}\mathbf{(}{\tilde{\mathit{q}}}_{\mathit{B}\mathbf{/}\mathbf{I}}\mathbf{)}$ .

Design of the DVQ-EKF

Depending on the chosen states by equation (41), the dual error quaternion–based kinematic equations and the dual quaternion–based dynamic equations drives in section “Model kinematics and dynamics based on dual vector quaternions,” this chapter drives the state equations and the linearization form of them.

Supposing the inertial parameters of the free-floating space target is unchangeable, which leads to

$$\stackrel{·}{\overline{\hat{\mathit{p}}}}=\overline{\hat{0}}$$

(43)

$$\stackrel{·}{\overline{\hat{\mathit{\rho }}}}=\overline{\hat{0}}$$

(44)

Now, combining equations (23), (41), (43), and (44) and taking the vector parts of the chosen states, the state equations can be written as

$$\stackrel{·}{X}=f(X)+N(w)$$

(45)

where $N(\mathit{\omega })$ is the process noise of the state equations.

Because for a certain free-floating space object the disturbances to the translational motion and the rotational motion are limited and can be regarded as the white noise, the $N(\mathit{\omega })$ can be written as: $N(\mathit{\omega })=\left[\begin{array}{c}{0}_{6\times 1}\hfill \\ \multicolumn{1}{c}{w_{6\times 1}}\\ \multicolumn{1}{c}{0_{6\times 1}}\\ \multicolumn{1}{c}{0_{6\times 1}}\end{array}\right]$ , and the white noise disturbances satisfy the equation $E(w{w}^T)={\mathit{I}}_{6\times 6}{\sigma }^2$ .

For convenience, we rewrite equation (23) which is the dual error vector quaternion–based kinematic equation as follows

$$\frac{\mathit{d}\mathbf{\delta }\stackrel{·}{\overline{\hat{\mathit{q}}}}}{\mathit{dt}}=-\frac{1}{2}\overline{\widehat{\tilde{\mathit{\omega }}}}\mathbf{\delta }\overline{\hat{\mathit{q}}}\mathbf{+}\frac{1}{2}\mathbf{\delta }\overline{\hat{\mathit{q}}}\overline{\hat{\mathbf{\omega }}}$$

and

$$\overline{\widehat{\tilde{\mathit{\omega }}}}=\overline{\hat{\mathbf{\omega }}}-\mathbf{\delta }\overline{\hat{\mathbf{\omega }}}$$

(46)

substituting (46) into (23), and using the same linearization technique in Lefferts et al., ⁸ one can obtain

$${\mathit{f}}_1=-\overline{\hat{\mathbf{\omega }}}\mathbf{\times }\mathbf{\delta }\overline{\hat{\mathit{q}}}\mathbf{+}\frac{1}{2}\mathbf{\delta }\overline{\hat{\mathbf{\omega }}}$$

(47)

Depending on equation (31), the vector part of the dual quaternion–based dynamic equation is

$${\mathit{f}}_2=\frac{\mathit{d}{({\overline{\hat{\mathbf{\omega }}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{)}}^{\mathit{s}}}{\mathit{dt}}={\overline{\mathit{A}}}^{-1}\otimes (-{\overline{\hat{\mathbf{\omega }}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{\times }\mathbf{(}\overline{\mathit{A}}\mathbf{\otimes }\mathbf{(}{\overline{\hat{\mathbf{\omega }}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}{\mathbf{)}}^{\mathit{s}}\mathbf{)}\mathbf{)}$$

(48)

Representing equation (48) in real domain, one can obtain

$$\frac{d{({\overline{\hat{\mathbf{\omega }}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{)}}^{\mathit{s}}}{\mathrm{dt}}=\left[\begin{array}{c}-({\overline{\hat{\mathbf{\omega }}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{\times }\mathbf{)}{\overline{\mathit{v}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\hfill \\ \multicolumn{1}{c}{{{\overline{\mathit{J}}}_{\mathit{B}}}^{-1}(-{\overline{\hat{\mathbf{\omega }}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{\times }\mathbf{(}{\overline{\mathit{J}}}_{\mathbf{B}}{\overline{\hat{\mathbf{\omega }}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{)}\mathbf{)}}\end{array}\right]$$

(49)

According to Aghili and colleagues,^9,10 the dual part of equation can be written by the inertial parameters as

$${\overline{{\mathit{J}}_B}}^{-1}(-{\overline{\hat{\mathbf{\omega }}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{\times }\mathbf{(}{\overline{\mathit{J}}}_{\mathit{B}}{\overline{\hat{\mathbf{\omega }}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{)}\mathbf{)}\mathbf{=}\mathbf{\left[}\begin{array}{c}{\mathbf{p}}_{\mathbf{x}}{\mathbf{\omega }}_{\mathbf{y}}{\mathbf{\omega }}_{\mathbf{z}}\hfill \\ \multicolumn{1}{c}{{\mathbf{p}}_{\mathbf{y}}{\mathbf{\omega }}_{\mathbf{x}}{\mathbf{\omega }}_{\mathbf{z}}}\\ \multicolumn{1}{c}{{\mathbf{p}}_{\mathbf{z}}{\mathbf{\omega }}_{\mathbf{y}}{\mathbf{\omega }}_{\mathbf{x}}}\end{array}\mathbf{\right]}$$

(50)

Taking equations (30), (31), (43), (44), and (23), the linearized state equations can be represented as

$$F_{24\times 24}=\frac{\partial f}{\partial X}=\left[\begin{array}{cccc}-({\overline{\hat{\mathbf{\omega }}}}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{\times }\mathbf{)}\hfill & \frac{1}{2}{\mathit{I}}_{6\times 6}\hfill & 0_{6\times 6}\hfill & 0_{6\times 6}\hfill \\ \multicolumn{1}{c}{0_{6\times 6}}& \mathit{A}\hfill & \mathit{B}\hfill & 0_{6\times 6}\hfill \\ \multicolumn{1}{c}{0_{6\times 6}}& 0_{6\times 6}\hfill & 0_{6\times 6}\hfill & 0_{6\times 6}\hfill \\ \multicolumn{1}{c}{0_{6\times 6}}& 0_{6\times 6}\hfill & 0_{6\times 6}\hfill & 0_{6\times 6}\hfill \end{array}\right]$$

(51)

where $\mathit{A}=\left[\begin{array}{cc}{\mathit{A}}_{21}\hfill & {\mathit{A}}_{22}\hfill \\ \multicolumn{1}{c}{(\mathit{v}\times )}& -(\mathbf{\omega }\mathbf{\times }\mathbf{)}\hfill \end{array}\right]$ , ${\mathit{A}}_{21}=\left[\begin{array}{ccc}0\hfill & p_x{\mathit{\omega }}_z\hfill & p_x{\mathit{\omega }}_y\hfill \\ \multicolumn{1}{c}{p_y{\mathit{\omega }}_z}& 0\hfill & p_y{\mathit{\omega }}_x\hfill \\ \multicolumn{1}{c}{p_z{\mathit{\omega }}_y}& p_z{\mathit{\omega }}_x\hfill & 0\hfill \end{array}\right]$ , ${\mathit{A}}_{22}=0_{3\times 3}$ ,

$$\mathit{B}=\left[\begin{array}{cc}{\mathit{B}}_{21}\hfill & 0_{3\times 3}\hfill \\ \multicolumn{1}{c}{0_{3\times 3}}& 0_{3\times 3}\hfill \end{array}\right],{\mathit{B}}_{21}=\left[\begin{array}{ccc}{\mathit{\omega }}_y{\mathit{\omega }}_z\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& {\mathit{\omega }}_x{\mathit{\omega }}_z\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & {\mathit{\omega }}_x{\mathit{\omega }}_y\hfill \end{array}\right].$$

Depending on equation (45), the linearized discrete-time state equations can be written as

$$\delta X_{k+1}={\mathrm{\Phi }}_k\delta X_k+w_k$$

(52)

where ${\mathrm{\Phi }}_k=e^{F\mathrm{\Delta }T}\stackrel{.}{=}I+F\mathrm{\Delta }T$ , $\mathrm{\Delta }T$ is the time interval.

In addition, the covariance of the discrete-time process noise can be written as

$$Q_k=C_k{D}_k^{-1}$$

(53)

where $\left(\begin{array}{c}{C}_k\hfill \\ \multicolumn{1}{c}{D_k}\end{array}\right)=\exp \left\{\left(\begin{array}{cc}F\hfill & Q_c\hfill \\ \multicolumn{1}{c}{0}& -F^T\hfill \end{array}\right)\mathrm{\Delta }T\right\}\left(\begin{array}{c}0\hfill \\ \multicolumn{1}{c}{I}\end{array}\right)$ , and $Q_c=\mathrm{diag}\{\begin{array}{cccc}{0}_{6\times 6}\hfill & \begin{array}{cc}{\sigma }_{\mathit{\omega }}^2\hfill & {\sigma }_v^2\hfill \end{array}\hfill & 0_{6\times 6}\hfill & 0_{6\times 6}\hfill \end{array}\}$ is the power spectral density of the processing noise $N(\mathit{\omega })$ .

Now one can design the DVQ-EKF based on the above derived equations in the following three steps:

• 1. The time update of the states $X$ can be represented as the following equations according to Gordon et al. ³¹

$$X_{K/K-1}={\mathrm{\Phi }}_k{X}_{k-1/k-1}$$

(54)

$$P_{k/k-1}={\mathrm{\Phi }}_k{P}_{k-1/k-1}{\mathrm{\Phi }}_k^T+Q_k$$

(55)

• 2. The measurement update can be represented as follows

$$K_k=P_{k/k-1}{H}_k^T(H_k{P}_{k/k-1}{H}_k^T+R_k{)}^{-1}$$

(56)

$$\delta X_k=K_k(Z_k-h(X_{k/k-1}))$$

(57)

$$P_k=[I-K_k{H}_k]P_{k/k-1}$$

(58)

• 3. The state update is an error-state update, namely, $\delta X=[\begin{array}{cccc}\mathbf{\delta }{\overline{\hat{\mathit{q}}}}_{\mathit{B}\mathbf{/}\mathbf{I}}\hfill & \mathbf{\delta }{\overline{\hat{\mathbf{\omega }}}}_{\mathit{B}\mathbf{/}\mathbf{I}}\hfill & \mathbf{\delta }\overline{\hat{\mathit{p}}}\hfill & \mathbf{\delta }\overline{\hat{\mathbf{\rho }}}\hfill \end{array}]$ , and can be represented as:

Update of the dual quaternions

$${\hat{\mathit{q}}}_{\mathit{B}/I,\mathit{k}}={\hat{\mathit{q}}}_{\mathit{B}/I,\mathit{k}-1}\mathbf{\delta }{\hat{\mathit{q}}}_{\mathit{B}\mathbf{/}\mathbf{I}\mathbf{,}\mathit{k}}$$

(59)

where $\mathbf{\delta }{\hat{\mathit{q}}}_{\mathit{B}\mathbf{/}\mathbf{I}\mathbf{,}\mathit{k}}\mathbf{=}\mathbf{\delta }{\mathit{q}}_{\mathit{B}\mathbf{/}\mathbf{I}\mathbf{,}\mathit{k}}\mathbf{+}\mathbf{\epsilon }\mathbf{\delta }\mathit{q}{\prime }_{\mathit{B}\mathbf{/}\mathbf{I}\mathbf{,}\mathit{k}}$ , $\mathbf{\delta }{\mathit{q}}_{\mathit{B}\mathbf{/}\mathbf{I}\mathbf{,}\mathit{k}}\mathbf{=}\mathbf{\left(}\begin{array}{cc}\sqrt{1-{\mathbf{\Vert }\mathbf{\delta }{\overline{\mathit{q}}}_{\mathit{B}\mathbf{/}\mathbf{I}\mathbf{,}\mathit{k}}\mathbf{\Vert }}^2}\hfill & \mathbf{\delta }{\overline{\mathit{q}}}_{\mathit{B}\mathbf{/}\mathbf{I}\mathbf{,}\mathit{k}}\hfill \end{array}\mathbf{\right)}$ , $\delta {\mathit{q}}_{\mathit{B}/I,\mathit{k}}\prime =\left(\begin{array}{cc}\frac{-\mathbf{\delta }{\overline{\mathit{q}}}_{\mathit{B}\mathbf{/}\mathbf{I}\mathbf{,}\mathit{k}}\mathbf{\delta }{\overline{\mathit{q}}\prime }_{\mathit{B}\mathbf{/}\mathbf{I}\mathbf{,}\mathit{k}}^{\mathbf{*}}}{\sqrt{1-{\Vert \mathbf{\delta }{\overline{\mathit{q}}}_{\mathit{B}\mathbf{/}\mathbf{I}\mathbf{,}\mathit{k}}\Vert }^2}}\hfill & \mathbf{\delta }{\overline{\mathit{q}}\prime }_{\mathit{B}\mathbf{/}\mathbf{I}\mathbf{,}\mathit{k}}\hfill \end{array}\right)$ .

Update of the other states

$$X_{k/k}=X_{k-1/k-1}+\delta X_k$$

(60)

Remark 1. The linearized state equation (51) has a similar form as the one presented in Aghili; ⁹ however, equation (51) is a dual vector quaternion–based representation in which the coupled translational and rotational motion are represented by the specific matrices.

Remark 2. The measurements can be provided by the vision-based sensors, such as LIDAR system in Aghili ⁹ and cameras in Song et al., ¹¹ and pre-processed by image processing algorithms in Filipe et al. ³² to output the relative attitude and range measurements.

Remark 3. Because the estimation algorithm proposed by this article is a multiplicable dual quaternion EKF, the dual quaternion is updated different from the other states, see equations (59) and (60).

Remark 4. The designed DVQ-EKF is a full parameter estimator which not only estimates the pose parameters but also estimates the inertial parameters.

Design of the DVQ-AFFEKF

The DVQ-EKF is based on the conventional EKF frame which is not robust to measurement uncertainties, model uncertainties, and the linearization errors. Since the space environment is complex and the free-floating tumbling target is hard to model and measure accurately, a more robust parameter estimating algorithms needs to be developed. Here, a DVQ-AFFEKF which is based on the IAE information to modify the Kalman gain in each measurement updating step is proposed.

The scaling factors matrix based on the IAE is designed to modify the Kalman gain in DVQ-EKF together. First, the concept of the innovation is given as the following equation

$$v_k=Z_k-h(X_{k/k-1})$$

(61)

In addition, define

$${\eta }_k=\frac{1}{k}\sum _{i=1}^k{v}_{k,i}{v}_{k,i}^T$$

(62)

to be the residue sequence.

At the very first, a residue-based ${\chi }^2$ fault detection is first introduced in the algorithm.

First, two hypotheses are given:

$H_0$ : no faults occur in the parameters estimation procedure;

$H_1$ : there are faults existing in the system.

The IAE-based statistical function to detect whether a fault occurs is expressed as follows

$${\mathrm{\Delta }}_k={\eta }_k^T[H_k{P}_{k/k-1}{H}_k^T+R_k{]}^{-1}{\eta }_k$$

(63)

According to Hajiyev and Soken, ¹⁹ the above statistical function has ${\chi }^2$ distribution with $n$ degrees of freedom where $n$ is the dimension of the residue vector.

Using the same conventional fault detection method proposed in Soken et al., ¹⁸ the fault detection procedures can be represented as follows:

1. Select the level of significance as $\xi$ , which means $P\{{\chi }^2>{\chi }_{\xi ,n}^2\}=\xi$ , where $0<\xi <1$ , and ${\chi }_{\xi ,n}^2$ can be found.

2. Checking whether a fault occurs:

   $H_0$ : ${\mathrm{\Delta }}_k\le {\chi }_{\xi ,n}^2$ , $\forall k$

   $H_1$ : ${\mathrm{\Delta }}_k>{\chi }_{\xi ,n}^2$ , $\exists k$

After the fault detection is completed, the DVQ-EKF or DVQ-AFFEKF will be operated depending on which hypothesis is accepted. If $H_0$ is accepted, the DVQ-EKF will be in use; otherwise, the DVQ-AFFEKF will accomplish the parameters estimation task.

It can be derived that the relationship between the innovation sequence and the system matrices if the system reaches its optimal estimation ²³

$$\eta =H_k{P}_{k/k-1}{H}_k^T+R_k$$

(64)

However, when the measurements are inaccurate, equation (64) will no longer be satisfied, which means the estimation is not optimal but suboptimal instead. In this way, the Kalman filter gain should be modified to output a desirable estimation.

Considering equation (64), if the measurements have some faults, this equation will not be satisfied anymore. By introducing the scaling factors matrix $\Upsilon$ , the modified equation can be represented as follows

$$\eta =H_k{P}_{k/k}{H}_k^T+\Upsilon R_k$$

(65)

then, $\Upsilon$ can be computed as

$$\Upsilon =[\eta -H_k{P}_{k/k}{H}_k^T]R_k^{-1}$$

(66)

Also, to ensure the components in $\Upsilon$ is no less than one, the modified $\Upsilon$ can be represented as

$$\Upsilon *=\mathrm{diag}({\gamma }_1^{*},{\gamma }_2^{*}\dots {\gamma }_n^{*})$$

(67)

where ${\gamma }_i^{*}=\max \{1,{\Upsilon }_{\mathrm{ii}}\}$ .

As a result, the modified measurements noise can be represented as

$$R_k^{*}=\Upsilon *R_k$$

(68)

According to Soken and Sakai, ³³ because the DVQ-AFFEKF is utilized in the “full parameters” estimation of the uncooperative target, the algorithm is sensitive to the unknown errors of the measurements. In order to have a good stability especially dealing with the unknown initial values of the system, a low path filter is used here

$$R_k^{*}=\alpha R_k^{*}+(1-\alpha )R_k^{*}$$

(69)

where $\alpha$ is the designed parameter.

Then, using the scaled $R_k^{*}$ , the time updated error covariance matrix and the Kalman gain can be modified as

$$K_k^{*}=P_{k/k-1}{H}_k^T(H_k{P}_{k/k-1}{H}_k^T+R_k^{*}{)}^{-1}$$

(70)

Then, by updating algorithm by equations (59) and (60), the measurement updated states and error covariance matrix can be computed as $X_{k/k}^{*}$ and $P_{k/k}^{*}$ .

Figure 2 illustrates the architecture of the proposed algorithms.

Figure 2.

Algorithms architecture for the DVQ-EKF and DVQ-AFFEKF under the fault detection.

Simulation

The mathematical experimental results are presented in this section to show that the designed algorithms are valid and efficient. First, the service satellite utilizes the measurement data pre-processed by the vision-based sensors.^9,10,26 Then, from the designed DVQ-EKF and DVQ-AFFEKF, the estimations of the states can be achieved using the MATLAB experiments.

The center of mass of the target is ${\overline{\rho }}_s=(0.5,0.5,0.5{)}^{T}m$ , and the moment of inertia of the target is ${\overline{\mathit{J}}}_B=\mathrm{diag}(7,7,4)\mathrm{kg}/{\mathrm{m}}^2$ . Therefore, the ratio of the moments of inertia is ${\mathit{p}}_s=(3/7,-3/7,0{)}^T$ . Also, the initial angular velocity of the target is ${\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}\mathbf{,}\mathit{s}}^{\mathit{B}}\mathbf{(}0\mathbf{)}\mathbf{=}\mathbf{(}0\mathbf{.}1\mathbf{,}0\mathbf{.}1\mathbf{,}0\mathbf{.}1{\mathbf{)}}^{\mathbf{T}}\mathrm{rad}/\mathrm{s}$ , and the initial relative translational velocity is ${\mathit{v}}_{\mathit{B}/I,\mathit{s}}^{\mathit{B}}(0)=(0.01,0.01,0.01{)}^T\mathrm{m}/\mathrm{s}$ .

Because both the service satellite and the target are operating in the space, their motions are affected by the environmental disturbances. Considering the limited AR&D period and the operating environment, the disturbances are considered to be the white noise with the following covariances

$${\sigma }_{\mathit{\omega }}^2=0.5\times {10}^{-3}\mathrm{rad}/{\mathrm{s}}^2$$

(71)

$${\sigma }_v^2=1\times {10}^{-3}\mathrm{m}/{\mathrm{s}}^2$$

(72)

which represents the effects of the disturbances to the rotational and translational motion, respectively. Based on the above conditions, the time-varied system states are generated by the MATLAB Simulink off-line.

Providing that the vision-based sensors have an update frequency at 2Hz, which can be obtained by most of the common vision-based devices. The measurements noise matrix $R$ is

$$R=\mathrm{diag}((0.01)I_{4\times 4},(0.1)I_{4\times 4}m)$$

(73)

which represents the measurements inaccuracy of the vision-based system.

Also, in order to start the estimating algorithm, the initial value of the measurements are set as follows

$${\mathit{q}}_{\mathit{B}/I,\mathit{m}}(0)=(1,0,0,0{)}^T$$

(74)

and

$${\mathit{r}}_{\mathit{B}/I,\mathit{m}}^{\mathit{I}}(0)=(50,50,50{)}^{T}m$$

(75)

which represents the initial distance between the center of mass of the servicing satellite and the grapple fixture on the target.

In order to start the simulation, the initial values of the variables which form the dual vector quaternions are given as follows (Table 1).

Table 1.

Initial values of the variables which form the dual vector quaternions.

Variable	Initial value
${\mathit{q}}_{\mathit{B}/I}$	${\mathit{q}}_{\mathit{B}/I}(0)=(1,0,0,0{)}^T$
${\mathit{r}}_{\mathit{B}/I}^{\mathit{I}}$	${\mathit{r}}_{\mathit{B}/I}^{\mathit{I}}(0)=(0,50,50,50{)}^{T}m$
${\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}$	${\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}\mathbf{(}0\mathbf{)}\mathbf{=}\mathbf{(}0\mathbf{,}0\mathbf{,}0\mathbf{,}0{\mathbf{)}}^{\mathbf{T}}\mathrm{rad}/\mathrm{s}$
${\mathit{v}}_{\mathit{B}/I}^{\mathit{B}}$	${\mathit{v}}_{\mathit{B}/I}^{\mathit{B}}(0)=(0,0,0,0{)}^T\mathrm{m}/\mathrm{s}$
$\mathit{p}$	$\mathit{p}(0)=(0,1,-1,0{)}^T$
$\mathbf{\rho }$	$\mathbf{\rho }\mathbf{(}0\mathbf{)}\mathbf{=}\mathbf{(}0\mathbf{,}0\mathbf{,}0\mathbf{,}0{\mathbf{)}}^{\mathbf{T}}\mathbf{m}$

Also, the initial state covariance matrix is $P(0)=I_{24\times 24}$ , and the total simulation time is $T=200\mathrm{s}$ .

For fault detection process, the ${\chi }_{\xi ,n}^2$ is taken as 14.07 with a freedom of 7 and reliability of 95%. If faults occur, the faults are formed by adding a constant value to the related terms in the system equations, namely adding a constant value to Q if the modeling of the state equations is not accurate, adding a constant value to F if the linearization of the state equation is not accurate, and adding a constant value to R if the measurements are incorrect.

Case 1: no measurement faults happen

In this case, no measurement faults will be considered. As a result, only the designed DVQ-EKF is used in the parameter estimation.

Figures 3 and 4 show the estimated relative attitude and the estimation errors between the real value and the estimated values by DVQ-EKF. From the simulation, one can find that after 200 s, the estimation error is less than 0.005 for DVQ-EKF.

Figure 3.

The estimation of the relative attitude quaternion ${\mathit{q}}_{\mathit{B}/I}$ .

Figure 4.

The estimation of the relative attitude error quaternion $e{\mathit{q}}_{\mathit{B}/I}$ .

Figures 5 and 6 show the estimated relative angular velocity ${\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}$ represented in the body frame and the estimation errors between the real value and the estimated values. From the simulation, one can find that when the algorithm is converged, the estimation error is less than 0.02 rad/s.

Figure 5.

The estimation of the relative angular velocity ${\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}$ .

Figure 6.

The estimation of the relative error angular velocity $e{\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}$ .

Figures 7 and 8 show the estimated relative translation velocity ${\mathit{v}}_{\mathit{B}/I}^{\mathit{I}}$ represented in the inertial frame and the estimation errors between the real value and the estimated values. From the simulation, one can find that when the algorithm is converged, the estimation error is less than 0.1 m/s. The reason for the estimation error in the translational velocity is that the translational motion is affected by the rotational motion. As a result, before the angular velocity converged, the translational velocity is oscillating. However, when the angular velocity began to converge, the amplitude of the oscillation decreased. In addition, with the simulation running in longer time, the estimation error will also decrease. Also, the AR&D requires a very small relative speed before starting the parameter estimation procedure, and the relative translational velocity can be measured by accurate individual velocity-only sensors which can be used to correct the parameter estimation results. As a result, although there is an 0.1 m/s estimation error in this simulation, the result is acceptable in the AR&D task.

Figure 7.

The estimation of the relative translational velocity ${\mathit{v}}_{\mathit{B}/I}^{\mathit{I}}$ .

Figure 8.

The estimation of the relative error translational velocity $e{\mathit{v}}_{\mathit{B}/I}^{\mathit{I}}$ .

Figures 9 and 10 show the estimated relative distance of the center of mass expressed in the inertial frame ${\mathit{r}}_{\mathit{B}/I}^{\mathit{I}}$ and the estimation errors between the real value and the estimated values. From the simulation, one can find that when the algorithm is converged, the estimation error is less than 0.7 m. This error is caused by the translational velocity. With a higher accuracy of the translational velocity estimation, the error of the ${\mathit{r}}_{\mathit{B}/I}^{\mathit{I}}$ estimation will decrease. Also, when compared with the system initial relative distance of the center of mass, the estimation is very small and can be accepted by the AR&D task.

Figure 9.

The estimation of the relative distance of the center of mass ${\mathit{r}}_{\mathit{B}/I}^{\mathit{I}}$ .

Figure 10.

The estimation of the error relative distance of the center of mass $e{\mathit{r}}_{\mathit{B}/I}^{\mathit{I}}$ .

Figures 11 and 12 show the estimated distance between the grapple fixture and the center of mass of the target expressed in the body frame $\mathbf{\rho }$ and the estimation errors between the real value and the estimated values. From the simulation, one can find that when the algorithm is converged, the estimation error is less than 0.1 m.

Figure 11.

The estimation of distance between the grapple fixture and the center of mass of the target $\mathbf{\rho }$ .

Figure 12.

The estimation of error distance between the grapple fixture and the center of mass of the target $e\mathbf{\rho }$ .

Figures 13 and 14 show the estimated proportion of the moment of inertia of the target $\mathit{p}$ and the estimation errors between the real value and the estimated values. From the simulation, one can find that when the algorithm is converged, the estimation error is less than 0.03. This is a very good result which represents that the designed DVQ-EKF is sufficient for the inertial parameters estimation besides the pose estimation and can be utilized in a number of AR&D operations to provide a prior knowledge of the inertial characteristics of an unknown target.

Figure 13.

The estimated ratios of the moment of inertia of the target $\mathit{p}$ .

Figure 14.

The error of the estimated ratios of the moment of inertia of the target.

Figure 15 shows the norm of the covariance of the error of the state $\Vert P\Vert$ varying with time. From Figure 8, one can find that $\Vert P\Vert$ is less than 0.2, which means the designed estimation algorithm has a good convergence after 200 s. Actually, one can set a specific threshold of $\Vert P\Vert$ to stop the simulation as represented in Aghili. ⁹ Here, in order to show the time-varying characteristic of $\Vert P\Vert$ , the threshold is not given.

Figure 15.

The norm of the covariance of the error of the state $\Vert P\Vert$ varying with time.

Case 2: measurement faults happen

In this case, the measurement faults will be taken into consideration. The measurement faults are formed by adding random disturbances to the measurements noise matrix $R$ . Supposing that in the mathematical simulation, the measurement faults for the attitude quaternions $q$ is formed by adding a new zero mean white noise with a covariance of 0.5, and the measurements faults for the relative distance is formed by adding a new zero mean white noise with a covariance of $I_{7\times 7}$ between the 5th simulation step and the 15th simulation step. As can be seen from Figure 16, because of the bad initial guess of the non-cooperative target and the faulty measurement in the very beginning, the DVQ-EKF is diverged when the simulation ends.

Figure 16.

The norm of the covariance of the error of the state $\Vert P\Vert$ when measurements faults occur varying with time.

In this case, the DVQ-AFFEKF will be used to have a robust estimation. The results are discussed and shown below.

Figures 17–27 show the results of the DVQ-AFFEKF. In the simulation results, it can be found that every estimation has some fluctuations in the beginning when compared to the results in case 1. This is because the DVQ-AFFEKF is dealing with the measurement faults and forcing the estimation to converge to the accurate value. Also, in the DVQ-AFFEKF, the filter is changing from DVQ-EKF to DVQ-AFFEKF by the innovation fault detection, and it can be seen from the simulation results that using the DVQ-AFFEKF, the estimated parameters can converge to acceptable values after the simulation is done. Also, from Figures 21 and 22, it can be seen that the error of the relative translational velocity is bigger than the one in case 1. This is also the result of the translational and rotational coupled effects together with the measurement faults.

Figure 17.

The estimation of the relative attitude quaternion ${\mathit{q}}_{\mathit{B}/I}$ under measurements faults.

Figure 18.

The estimation of the relative attitude error quaternion $e{\mathit{q}}_{\mathit{B}/I}$ under measurements faults.

Figure 19.

The estimation of the relative angular velocity ${\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}$ under measurements faults.

Figure 20.

The estimation of the relative error angular velocity $e{\mathbf{\omega }}_{\mathit{B}\mathbf{/}\mathbf{I}}^{\mathit{B}}$ under measurements faults.

Figure 21.

The estimation of the relative translational velocity ${\mathit{v}}_{\mathit{B}/I}^{\mathit{I}}$ under measurements faults.

Figure 22.

The estimation of the relative error translational velocity $e{\mathit{v}}_{\mathit{B}/I}^{\mathit{I}}$ under measurements faults.

Figure 23.

The estimated ratios of the moment of inertia of the target $\mathit{p}$ under measurements faults.

Figure 24.

The error of the estimated ratios of the moment of inertia of the target under measurements faults.

Figure 25.

The estimation of distance between the grapple fixture and the center of mass of the target $\mathbf{\rho }$ under measurements faults.

Figure 26.

The estimation of error distance between the grapple fixture and the center of mass of the target $e\mathbf{\rho }$ under measurements faults.

Figure 27.

The norm of the covariance of the error of the state $\Vert P\Vert$ under measurements faults.

It can also be found that the norm of the covariance of the error of the states by the DVQ-AFFEKF is larger than the one in the DVQ-EKF. This is because even though the DVQ-AFFEKF is a robust algorithm which can deal with the measurement faults to force the algorithm to converge, it will have more bias than the DVQ-EKF without measurement faults. However, from Figures23–26, the estimations of the inertial parameters (i.e. the proportion of the moments of the inertial tensor and the location of the center of mass of the target) are quite accurate. This result also reflects the robustness of the designed DVQ-AFFEKF in the inertial parameter estimation.

Conclusion

This article presents a novel DVQ-EKF and a novel DVQ-AFFEKF to estimate the pose and inertial parameters of a free-floating tumbling space target. First, the model kinematics by dual vector quaternions are reviewed, and the new model dynamics based on the dual vector quaternions which have a more concise format and a lower computational burden compared to the former research are derived. Second, based on the new model and the measurements from a vision-based system, the DVQ-EKF is proposed to estimate the pose and inertial parameters of a free tumbling space target without touching. In this estimation algorithm, the relative translational and rotational position and speed are estimated in the same time. Also, the inertial parameters (e.g. ratios of the moments of inertia of the target and the location of the center of mass) are represented in the dynamics equations to make them can be estimated.

Then, a robust DVQ-AFFEKF is designed to estimate the parameters when fault occurs during the estimation procedure. In the designed DVQ-AFFEKF, a fault detection step is first operated to determine whether DVQ-AFFEKF will be run. If it runs, the fading factors will be computed based on the innovation information, and the Kalman gain will be modified to output a more accurate estimation compared with the DVQ-EKF. Finally, mathematical experiments are carried out to illustrate the estimation accuracy of the proposed algorithms and the robust performances by DVQ-AFFEKF. Based on the initial conditions, the results show that the pose and inertial parameter estimation algorithm proposed by this article can converge in a proper time and have an acceptable accuracy in the meantime. When a fault occurs, the DVQ-AFFEKF is much more robust than the DVQ-EKF and can provide a reliable result. The designed DVQ-EKF and DVQ-AFFEKF can be used in a number of AR&D missions to provide a good prior knowledge of the unknown targets.