# A Nonlinear Observer for Free-Floating Target Motion using only Pose Measurements

In this paper, we design a nonlinear observer to estimate the inertial pose and the velocity of a free-floating non-cooperative satellite (Target) using only relative pose measurements. In the context of control design for orbital robotic capture of such a non-cooperative Target, due to lack of navigational aids, only a relative pose estimate may be obtained from slow-sampled and noisy exteroceptive sensors. The velocity, however, cannot be measured directly. To address this problem, we develop a model-based observer which acts as an internal model for Target kinematics/dynamics and therefore, may act as a predictor during periods of no measurement. To this end, firstly, we formalize the estimation problem on the SE(3) Lie group with different state and measurement spaces. Secondly, we develop the kinematics and dynamics observer such that the overall observer error dynamics possesses a stability property. Finally, the proposed observer is validated through robust Monte-Carlo simulations and experiments on a robotic facility.

## Authors

• 1 publication
• 4 publications
• 1 publication
• 9 publications
01/05/2021

### Nonlinear Filter for Simultaneous Localization and Mapping on a Matrix Lie Group using IMU and Feature Measurements

Simultaneous Localization and Mapping (SLAM) is a process of concurrent ...
11/11/2020

### Docking two multirotors in midair using relative vision measurements

Modular robots have been rising in popularity for a variety of applicati...
04/19/2019

### Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation

Legged robots require knowledge of pose and velocity in order to maintai...
05/16/2022

### An Experimental Comparison of Floating Base Estimators for Humanoid Robots with Flat Feet

Extended Kalman filtering is a common approach to achieve floating base ...
03/09/2020

### A Mathematical Framework for IMU Error Propagation with Applications to Preintegration

To fuse information from inertial measurement units (IMU) with other sen...
05/14/2020

### Robust On-Manifold Optimization for Uncooperative Space Relative Navigation with a Single Camera

Optical cameras are gaining popularity as the suitable sensor for relati...
12/19/2017

### Flexible Stereo: Constrained, Non-rigid, Wide-baseline Stereo Vision for Fixed-wing Aerial Platforms

This paper proposes a computationally efficient method to estimate the t...
##### This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

## I Introduction

The estimation of motion parameters is key to several Cartesian control methods for robots and vehicles. For regulation problems, a pose estimate using a kinematic observer is sufficient. However, for tracking problems of the kind where the motion of the desired frame is time-varying, an additional velocity measurement is required in the commonly known PD+ controllers [1]. In the context of On-Orbit Servicing (OOS, see Fig. 1), the control objective is to track a grasping frame on a free-floating satellite (right) with a manipulator-equipped spacecraft (left). In the specific case that such a satellite is also non-cooperative (Target), the available measurement is often a relative pose which is computed using an exteroceptive sensor like a camera. Therefore, the control design is negatively affected by the lack of in-situ proprioceptive sensors, like an Inertial Measurement Unit (IMU), to measure the Target’s velocity, thereby motivating the need for an observer of the Target’s motion states.

Pertaining only to attitude estimation, in [2]

, the Multiplicative Extended Kalman Filter (M-EKF) was proposed which dealt with the orientation manifold by performing measurement update in the tangent space of the quaternion group. In

[3], the authors developed a theoretical treatment of Lie group observers that adhered to symmetries in kinematics and possessed autonomous error dynamics. In [4] and [5], this foundation was used to develop an invariant EKF which was proved to be locally exponentially stable. In [6], the invariant observer theory was enhanced and the autonomous error dynamics derived in [3] was corroborated. The approaches by both [3] and [7] propose an internal model (pre-observer) which possesses a geometric structure which is identical to the actual kinematic system. In [8], the Continuous-Discrete-EKF was developed which formalized filtering on Lie group manifolds by making innovation updates in the tangent space. In all these approaches, however, the estimation problem was limited to the system kinematics with an assumption that the proprioceptive sensor, IMU, provides velocity measurements.

Pertinent to observers which include motion dynamics, [9] developed a globally convergent angular velocity observer using only orientation measurements, and used the natural energy function on the momentum as a Lyapunov candidate which resulted in a quadratic stable internal observer. [10] and [11] developed a nonlinear observer which estimated pose and a velocity and further demonstrated stability. Both these observers, however, used pose and velocity measurements. For non-cooperative targets, [12] developed a computationally efficient discrete EKF but the design did not exploit symmetries in motion and it is also not trivial to derive the region of attraction in an EKF. In contrast to [9]

, we propose an alternative approach to compute a vector difference using a push-forward vector operation. Furthermore, since the subject of this paper is concerned with a non-cooperative Target and velocity measurement is unavailable, the well-formalized theory of autonomous error dynamics in

[3], [6] and [7] cannot be used directly and additionally, the observers in [10] and [11] are not applicable. Therefore, in this paper, we address the problem of estimating the inertial pose and body velocity of a non-cooperative free-floating Target using only the relative noisy pose measurements.

The contributions of this paper are threefold. Firstly, we propose an observer which extends the existing concepts of kinematics symmetry in Lie group observer theory and uses additional properties of rigid body motion dynamics. To this end, we formalize the estimation problem on Lie groups with different state and measurement spaces and derive a novel left-invariant error formulation which narrows down observer analysis to the state-space error. The kinematics and dynamics part of the observer are designed such that it also acts as a predictor. Secondly, through Lyapunov analysis, we show that the observer error dynamics has almost-global uniform asymptotic stability. Through a reformulation of dynamics equations and exploitation of the skew-symmetric property in rigid body motion, we are able to simplify the stability analysis. Finally, we validate the proposed observer with

Monte-Carlo simulations and experimental validation.

The note is organized as follows. In sec II, the underlying concepts of mechanical system modeling in , which are relevant to this text, are provided. Following this, in sec. III, the problem is formalized on Lie group with a general measurement model. In sec. IV, the proposed observer equations are derived and stability is proved for the observer parameters. This is followed by validation of the proposed method using robust Monte-Carlo simulations and experiments on OOS-SIM [13] (see Fig. 1) in sec. V and sec. VI respectively. Finally, the conclusions and future scope of work are laid out in sec. VII. All the Lemmas that are used in the paper have been provided in the Appendix.

## Ii Mechanical Systems on SE(3) group

Fig. 1 is a representative scenario for Target tracking using a manipulator-equipped spacecraft wherein, , , , and indicate the inertial, Target center-of-mass (CoM), the grasping, and the end-effector-mounted camera frames respectively. Before introducing the kinematics and dynamics, we describe the notation concerning mechanical systems on which is used in this paper.

### Ii-a Notations and definitions

The pose (configuration) of a rigid body in space is given as a matrix Lie group representation called and is written as , whose identity is , where

is a square identity matrix of dimension

. A pose between two frames is represented with the subscript of the lowercase letters of both frames. For instance, the pose of relative to is . The tangent space of a given pose at identity is the velocity field (Lie Algebra) of the pose and is also a matrix group which may be expressed in either the body () or a spatial () frame. In the following text, all velocity quantities are body velocities. Analogously, the cotangent space at identity, denoted as , is the space of generalized forces. An velocity is given as , where indicates the skew-symmetric matrix for the vector and, and are angular and linear velocities respectively. is isomorphic to , , and in -form, is written as . Since and corresponding isomorphisms refer to different notations of the same quantity, they are used interchangeably for simplicity of notation in this paper. Poses and velocities with one subscript indicate that they are referenced relative to , for instance, are pose and velocity of the Target CoM relative to .

###### Def. 1

pose reconstruction formula: For a pose and body velocity, , . The superscript denotes body velocity.

The Adjoint action, , of a pose, , transforms velocities between spatial and body frames as where , see [14]. There also exists an adjoint map of the Lie Algebra onto itself, which is the differential of the Ad map, for . The codajoint action is defined as . The SE(3) Lie group and its algebra are endowed with a local (almost global) diffeomorphism map and has been defined explicitly in Lemma 1 in the Appendix.

The following two group operations are pointed out and will be used in sec. III to derive the measurement model.

###### Def. 2

Lie group action: A Lie group action of a pose on another group element , is a left and/or right translation operation(s), given as , , and , respectively.

###### Def. 3

Lie algebra Automorphism: Given and a Lie group operation such that , if , then , for . In other words, if is a group operation, is its corresponding Lie algebra transformation.

### Ii-B Kinematics and Dynamics

The Target (right) in Fig. 1 is assumed to be a rigid body with fixed inertia and its configuration space is the pose of . For a rigid body with inertia , the Euler-Poincaré [15, th. 6.1, iii] equation of motion is,

where and . Applying (1) to a free-floating Target with , we get the equation of motion for inertia, , as,

The kinematics for the Target are given by Def. 1 as follows,

 Kinematics ˙gt=gt[Vbt]∧, gt≡(Rt,rt) (3)

In the analysis that follows, we use the following definitions. The set of singular values for

are given as, , where

is the set of eigenvalues of

. and refer to the lowest and highest singular value of respectively. Additionally, the operator norm, . For a vector , such that implies , which means that is continuous and implies boundedness. refer to standard logical substitutes for because/therefore. refers to the inner product of the two arguments. indicates a vector of ones.

## Iii Problem formulation

In this section, the problem of estimating Target states, is formalized. This problem is abstractly illustrated in Fig. 2. Illustrated on the left are the Lie group configuration-space trajectories of the true state (, solid) and observer’s estimate (, dashed) respectively. denotes the state estimation error between and . On the right are the measurement-space trajectories for the actual measurement (, solid) and estimated measurement (, dashed). denotes the measurement error which is the residual between and . Between configuration-space and measurement-space, there is a transformation, , obtained through Lie group actions (see Def. 2). This means that, given , we can obtain and in the measurement space. The poses, and are associated to their Lie algebra respectively. The estimation problem in this paper is to use the measurement to reconstruct the states which include the pose () and its Lie algebra (). For this, we seek a transformation of errors from measurement-space to the configuration-space so that we can design the observer only in terms of the latter for simplicity.

### Iii-a Configuration-space error

First, we define a configuration-space error and a corresponding Lie algebra error as follows. In Fig. 2, the estimation error in configuration-space, , is defined as, . Note that is a left-invariant error formulation [7, eq. 6]. Using the logarithm mapping defined in Lemma 1, we obtain an error term as and , where and are the orientation and translational errors respectively.

### Iii-B Measurement-space error

In this subsection, by using Lie group theory concepts, we establish a relationship between configuration-space errors () and a similar measurement-space error () so that in the rest of the text we can preclude measurement-space for simplicity.

To this end, we describe any general measurement model for as a composite Lie group action (Def. 2) which may contain both and actions, given by a transformation . Let us assume that there exist both, left and right actions on the state such that . Using this, a left-invariant measurement pose error is defined as . A Lie algebra error is obtained using Lemma 1, such that . In the next step, we derive explicit forms of the relationship between errors. Through rearrangement, we get,

 η=grΔhg−1r (4)

The expression in (4) is exactly the operation (see Def. 3) of the action of contained in . Hence, undergoes an transformation.

From Fig. 1, the camera measurement pose is while the rigid body state is . In this specific case, from kinematics we obtain, and . Applying the aforementioned transformations in (4) and (5), we get,

where . It can be seen that using (6), the measurement-space errors have been transformed to the configuration-space for the case in Fig. 1. Hence, in the rest of the paper, we perform analysis only with respect to the configuration-space errors which are obtained using (6).

## Iv Observer design

### Iv-a Kinematics observer

Let us consider the kinematic part of the observer with the same geometric structure as (3) with for corresponding estimates. This is obtained by applying Def. 1 with an error injection term as follows,

where, is the observer kinematic gain which is determined through stability analysis in sec. IV.

### Iv-B Error kinematics

In this section, the observer error kinematics are derived. The observer velocity error, , is defined as . Taking the time derivative of the pose error, , we get,

In parlance with the nomenclature presented in [14, §4], the use of the mapping for the error implies, the proposed observer is the Logarithm feedback variant.

### Iv-C Velocity error dynamics

Before describing the equations of the dynamics observer, we first motivate the formulation by pointing out the following concept. For stability analysis, we need the velocity error dynamics from the equations of motion. In order to obtain this, the Target velocity () and observer velocity () have to be compared as a valid vector operation. For a pose-error , is the push-forward which transforms the velocity to the actual Target body frame as . Hence, with the use of the push-forward and referring both velocities on the same point in , we obtain a correct vector comparison between transformed observer velocity, and Target velocity, . Note that, this is evident in the velocity error in (8) which takes the form, .

Following the discussion above, we compute the velocity error dynamics by taking the time-derivative of . To this end, the time-derivative of is computed using [14, Lemma ] and (8) and error dynamics are written as,

Substituting for and using the properties, and in (10), we get

Therefore, the observer design is now limited to determining such that the observer exhibits a stability property.

### Iv-D Dynamics observer

The observer dynamic equations of motion are proposed with a geometric structure similar to (1) and velocity as,

where is a design input force which is determined by Lyapunov stability analysis. It is also worth pointing out that the resultant system consisting of (7) and (12) is an internal observer (see [3], [6]), which means that the time-evolution of the system in absence of measurement is like that of a rigid body and hence the observer can be used as a predictor.

Substituting (12) in (11), we get,

Applying Lemma 6 to the first two terms in R.H.S, we obtain,

The observer error dynamics can be written together for kinematics in (9) and dynamics in (14) as,

 ddt[ϵVbe]=[−Br(ϵ)K1Br(ϵ)06,6Λ−1tC(Vbt,Vbe)]A[ϵVbe]−[06,6Λ−1t]Bf0 (15)

In the following, and are determined using Lyapunov stability analysis for the proposed nonlinear observer.

###### Theorem 1

Main result: For a free-floating rigid body whose motion equations are given by (3) and (2), and an observer given by (7) and (12), the observer error, is almost globally uniformly asymptotically stable about the origin for design parameters , and observer kinematic gain, and dynamic input such that,

Proof: The proof is split into two parts. In the first part, uniform asymptotic stability is proved and in the latter part, the constraint on the matrix is determined so that the rotational singularity in the map is not encountered along trajectories. The latter part refers to the item 4 in theorem 1 and ensures almost-global stability of the observer error system in (15). Choosing the Lyapunov candidate as , where such that for an open connected region , and there exist bounds as,

 12σ––(P)||x||2α––(||x||)≤W≤12¯¯¯σ(P)||x||2¯¯¯α(||x||) (16)

Taking time derivative along trajectories and using observer error dynamics in (15), we get,

 ˙W=xTPAx−xTPBfo=xT[−P1Br(ϵ)K1P1Br(ϵ)06,6P2C(Vbt,Vbe)]x−xT[06,6P2]fo (17)

In the following, constraints are imposed on the design parameters in order to simplify (17). We choose, so that we can apply Lemma 3 to the block matrix position in the first term. The Lemma 7 is applied to the block matrix position in the first term so that the term with vanishes. Furthermore, we set the input which leads to a cancellation of coupled terms, ( and ) that follow in eq.(18). Hence, we get,

 ˙W= xT[−k1p1I6,6p1Br(ϵ)−p1Br(ϵ)T06,6]x (18) = −k1p1||ϵ||2≤0 (19)

Hence, from (19), we first conclude that the observer error dynamics in (15) is uniformly stable. In order to prove uniform asymptotic stability of the state , we use Matrosov’s theorem (see [1] for application) from theorem 2 (in Appendix). We choose an auxiliary function, , where .

Using Lemma 8 which guarantees a bounded observer error , we deduce that

 β––||x||2≤|W|≤¯¯¯β||x||2 (20)

where . Hence, is bounded.

Taking time derivative of along trajectories, setting and as defined in theorem 1, we obtain,

 ˙W=xT(PA+ATPQ1)x−xTPBfo−fToBTPx=xTQ1x−2p1VbTeBr(ϵ)Tϵ+p1ϵTP−12Br(ϵ)Tϵ=xT(Q1−Q2)x (21)

where . Continuity and boundedness of follows from the conclusion in Lemma 10, which was derived by systematically proving these two properties for all the terms in (21).

Furthermore, In the set , applying limits to (21), we obtain,

 ⇒lim˙W→0˙W=lim||ϵ||→0˙W=−VbTe(ΛtBr(06,1)−(C(Vbt,Vbe)TP2+P2C(Vbt,Vbe))Vbe (22)

Using the definition of from Lemma 4, . Employing the inner product property of Lemma 7 (see (31)) to cancel out terms with , we obtain,

 lim˙W→0˙W=−VbTeΛtVbe≤−σ––(Λt)||Vbe||2⇒lim˙W→0|˙W|≥σ––(Λt)||Vbe||2 (23)

We conclude therefore that is bounded and sign-definite (negative for non-zero values of ) in the set .

The conclusions from (16), (19), (20) match conditions in Matrosov’s theorem. For the condition , the two conditions in Lemma 11 are satisfied by Lemma 10 and (23). Since, the error dynamics in (15) is bounded for , from the Matrosov’s theorem in Lemma 11, we conclude uniform asymptotic stability of the state about the origin.

Topological drawbacks preclude global stability in SE(3) due to the ambiguity in rotation. Readers are referred to works by [10], [16] and [17] where this problem is discussed and applied. In order to achieve, almost-global stability, a condition on the minimum is derived next. In this part of the analysis, all functions with a subfix denote the rotational component of the corresponding function.

Let us define a sublevel set, , which is the state-space at the least value of at singularity (). Since, we proved that is non-increasing and furthermore, , and choice of are decoupled from the translational part, is a positive invariant set. If the upper bound of for time is restricted within the aforesaid sublevel set , it is sufficient to ensure that the observer trajectories never encounter the rotational singularity. This can be written as,

 12(p1||ψt(0)||2+p22¯¯¯σ(It)||ωbe(0)||2)p22¯¯¯σ(It)||ωbe(0)||2π2−||ψ(0)||2 (24)

(24) provides a sufficient condition to ensure that the observer error dynamics in (15) have almost-global uniform asymptotic stability.

## V Monte-Carlo Simulation results

The proposed observer was implemented for estimating the states of an inactive tumbling satellite (ENVISAT, [18]). In such a scenario, the motion states as well as the the inertia, , are subject to uncertainties (detailed in Table I). These uncertainties, together with the exteroceptive sensor (camera) noise, make estimation robustness a key criteria in determining practical use. The camera noise was simulated as a concentrated Gaussian [8] with in the tangent space and the noisy measurement was obtained as with sampling time . In order to validate the robustness of the proposed observer, Monte-Carlo simulations were performed by varying the set within the uncertainty bounds as specified in Table I. In all the simulations, the observer was initialized only to zero initial conditions and the gains were computed from parameters and which are declared in Table I. In the description below, is used to show position and orientation errors.

Fig. 4 shows the convergence of all motion states: the configuration pose and velocity , for the simulation run. The velocity (top row), , of the Target is shown to converge after an initial transient period. The configuration pose, , which is plotted as position and quaternion, converged to position-error norm, and angular-error norm, . Additionally, in Fig. 4, from the histogram of component-wise errors for velocities (top row) and pose (bottom row), we can infer that despite the introduced uncertainty, the observer converges to a bounded error. The results tabulated in Table II summarize these histogram results and it can be seen that the maximum error-norm of both, position and orientation, are and respectively, which are better than the corresponding metrics for the noisy measurements (, respectively). These results validate the design and additionally prove robustness of the observer which was designed using theorem 1.

## Vi Experimental validation

The proposed observer was implemented at the OOS-SIM facility at DLR to estimate states of an axially spinning satellite. The satellite inertia was simulated using the facility’s client dynamics with , and . The satellite was spun about its dominant axis with for each time. The satellite was observed using a end-effector camera as shown in Fig. 1 and an image-processing algorithm was used to provide pose-measurements to the observer at . Firstly, the convergence of the estimated towards ground truth is demonstrated in Fig. 5

. For practical purposes, a heuristic threshold-based outlier rejection scheme was implemented to avoid using unlikely pose-estimates from the image-processing algorithm. However, despite this, we observe that due to extremely noisy pose measurements, the estimation degrades and fluctuates about the true value. Furthermore, for the fastest case

, we demonstrate the convergence of the observer state to the ground truth within from initial conditions of the observer in Fig. 6. This concludes the experimental validation of the proposed observer.