Globally Convergent Observer for the Rigid Body System on SE(3) in Presence of Intermittent Measurements

We propose a globally asymptotically convergent hybrid observer for the rigid body rotation and translation system evolving on the special Euclidean group SE(3) in the presence of intermittent measurements of the pose and continuous measurements of the velocities. We embed the system into an ambient Euclidean space and design an observer on the Euclidean space for global convergence. We perform numerical simulation of the proposed observer to show convergence. We also perform simulation using data collected from an Intel Realsense T265 camera.


I. INTRODUCTION
This paper deals with designing observers for the system modelling translation and rotation of a rigid body in three dimensional space. The problem of nonlinear observer design for rotational kinematics has been extensively studied in the literature [1], [2], [3], [4]. The authors in those papers assume a continuous-time system with continuous measurements of the pose. However, due to the digital nature of sensors used in robotics, measurements are usually of a discrete nature while the system is modelled by an ordinary differential equation, hence being continuous. This requires designing a nonlinear observer for a continuous-time system which can handle measurements available at intermittent instants of time. Some recent results considering such a system with intermittent measurements can be seen in [5], [6], [7], [8], [9], and [10].
For the system under consideration, measurements are usually available from various sensors at varying frequency. Often, sensors such as IMUs and accelerometers have a considerably higher frequency of measurement as compared to sensors such as camera or lidars. As an example, the EuRoC The associate editor coordinating the review of this manuscript and approving it for publication was Haibin Sun . dataset [11] which contains information about the flight of a quadcopter, contains IMUs which measure angular velocity at 200Hz while cameras, which are used to construct the pose measurement of the robot, operate at a frequency of 20Hz. Due to such a high ratio of frequencies, we assume that the measurements of the angular and linear velocity to be continuous in time while the measurements of the pose to be available intermittently for our system.
For the rigid body rotation and translation system with intermittent measurements, an almost global asymptotic observer has been designed in [8] on the special orthogonal group, SO(3), while a local exponential observer is designed on the special Euclidean group, SE(3) in [10]. As can be seen from these works, prior research in the field of design of estimators for continuous-time rigid body kinematics on SE(3) is restricted to designing observers on the special Euclidean group. This approach results in an inability to design globally asymptotic observers on SE(3) due to the topology of SO(3) [12]. To design globally convergent observers for systems evolving on SE(3), we design a globally convergent observer in R 3×3 × R 3 which is an ambient Euclidean space of SE(3). This observer system tracks the pose of the system evolving on SE(3) and does not suffer from the topological obstruction. The technique of designing observers in ambient Euclidean space is well accepted as in [13], allowing the design of globally convergent observers while avoiding any topological obstruction of the original state space.
Since the authors in [8] and [10] consider a similar system and, to the best of our knowledge, their observers are the best performing observers in the literature, we compare our results with their observers. Since the presence of a repeller set in the observers proposed in [8] and [10] degrades their performance in a neighborhood of the repeller set, we compare the performance in this neighborhood to our observer. Since our observer exhibits global asymptotic convergence to the system when it starts from SE(3), the performance improvement over the previously proposed observers is significant.
This paper is structured as follows. Section II contains preliminary information about the problem. The notation used and the system and measurements considered are presented in this section. Section III presents the theorems pertaining to local asymptotic stability of the system. This section also contains results regarding global convergence on SE(3). Sections IV and V contain simulations(numerical and experimental) of the system and the comparison with observers present in the literature.

II. PRELIMINARIES
Let {I} denote the inertial frame and {B} the body fixed frame. We denote the estimate of the state A byĀ. The state of the system after a discrete jump is denoted by A + . The matrix representation of the cross product with a vector v is denoted by v × : (3) is the set of 3 × 3 skew symmetric matrices. Denote by SO(3) the special orthogonal group on R 3 . Denote by SE(3) = SO(3) R 3 the special Euclidean group on R 3 where the product is regarded as a Cartesian product of the sets and a Semidirect product of the groups. The Euclidean inner product of two matrices in R m×n is denoted by ·, · : R m×n × R m×n → R such that for all A, B ∈ R m×n , A, B = trace(A T B). The Euclidean norm of a matrix A ∈ R m×n is denoted as A = √ A, A . Consider the rigid body rotation and translation systeṁ where R ∈ SO(3) ⊂ R 3×3 denotes the rotation of frame {B} with respect to frame {I} and p ∈ R 3 is the position of the body in frame {I}. The vector ∈ R 3 denotes the angular velocity of the body in frame {B} and v ∈ R 3 denotes the linear velocity of the body in frame {B}.
We assume that continuous measurements of and v are available, denoted by m and v m , respectively. We also assume that discrete measurements of R and p are available with the following assumption: Assumption 1: The measurements of R and p are available at intermittent instants of time t j , j ∈ N such that t j is an increasing sequence for j ∈ N and there exists T M > 0 such that t j+1 − t j ≤ T M for all j ∈ N with the first measurement available at time t 1 ≤ T M .
We have assumed availability of measurements of R and p. Alternatively, the locations of predetermined markers in the environment may be available. In this case, let p i ∈ R 3 , i = 1, . . . , n denote the location of n such markers in {I}. Let y i ∈ R 3 denote the measurement of marker at p i in {B}. Define r i = (p i , 1) and b i = (y i , 1) the corresponding homogeneous coordinates. We assume n > 2. If n = 2, a third marker can be generated as p 1 ×p 2 , with the corresponding measurement as y 1 ×y 2 . Then the measurements are related by the equation Define r = r 1 r 2 . . . r n and b = b 1 b 2 . . . b n . Then, the measured value of X can be given by The presence of both continuous and discrete measurements requires us to consider a hybrid system for the observer. The measurements of the state at the discrete instants of time are used to correct any errors between the estimated state arrived at using a continuous update law and the true value of the state.
The following lemma will be used later in the proof of theorems.

then A is invertible and the norm of its inverse satisfies
Proof: Define T := R T A such that I −T = A−R < 1. From Corollary 5.6.16 in [14], we see that T is invertible and the inverse can be written as T −1 = ∞ k=0 (I − T ) k . Taking norm on both sides of the equality, repeatedly applying the triangle inequality and using the submultiplicativity of the Euclidean norm, where we have substituted I = √ 3 for the Euclidean norm. Substituting T = R T A and using the property RB = B for all B ∈ R 3×3 and R ∈ SO(3), we prove the lemma.

III. HYBRID OBSERVER
We propose the following hybrid observer system for the rigid body rotation and translation system (1): where the observer statesR ∈ R 3×3 andp ∈ R 3 are estimates to R and p, respectively. The discrete jumps in equations (2c) VOLUME 10, 2022 and (2d) occur at times t = t j , j ∈ N when the measurements of R and p are available.
The following theorem proposes local convergence of the observer system defined in (2) to the system (1): Theorem 1: Suppose that Assumption 1 holds, and for all µ > 0, p(t) exp(−µt) → 0 as t → ∞, and let where the rigid body system is given in equation (1), and the observer system is given in equation (2). Then, with 0 < k e < 2 and 0 < k p < 2, the error term E R (t) converges exponentially to 0 and E p (t) converges Using the defined error terms E R and e p , the observer jump equations (2c) and (2d) can be simplified as where we have substituted the measurement models. Using the above equations and the error terms E R and e p , the following error equations are arrived at by differentiating the error terms with respect to time and substituting equations (1) and (2):Ė where E + R and e + p are the error between the state and the estimate after the jump at t = t j , j ∈ N.
Define the function V 1 as Differentiating equation (7) with respect to time along the trajectory of system (6), we see thaṫ 2 , this leads to E R (t) being constant over the same intervals. Substituting the error jump map (6c) into (7) for the value of V 1 after the jump, we see that which decreases at the discrete jumps since 0 < k p < 2.
Similarly, considering a function V 2 for the error term e p defined as and using equation (6b), we see that the derivative of V 2 with respect to time along the trajectory of the system (6) is 0 for all t ∈ (t j , t j+1 ), j ∈ N. Hence, V 2 (e p (t)) is constant over the interval t ∈ (t j , t j+1 ) for every j ∈ N. Since V 2 (e p (t)) = e p (t) 2 , this leads to e p (t) being constant over the same intervals. The value of the function after the jump, arrived at using (6d), decreases from that before the jump since 0 < k e < 2. The hybrid observer system equations (2b) and (2d) contain the termsR −1 and ((1 − k p )R T + k p R T ) −1 , respectively. Note that the second term here is (R + ) −1 . For the observer system to be defined for all t ≥ 0, we now prove thatR(t) andR + (t) are invertible given that E R (0) < 0.99.
Consider first the case where t ≤ t 1 . Since E R (0) < 0.99 according to the choice of the initial condition and the fact that E R (t) is constant for all t ≤ t 1 ,R(t) is invertible for all t ≤ t 1 from Lemma 1. For the discrete time kinematics, note that R(t) −R + (t) = E + R (t) = |1 − k p | E R (0) < 0.99, since 0 < k p < 2 as specified in the theorem. Hencē R + (t 1 ) is invertible from Lemma 1. SinceR + (t 1 ) is the initial condition of the ordinary differential equation governing the update of the state in time t ∈ (t 1 , t 2 ), and E + R (t 1 ) < 0.99, it follows thatR(t) is invertible for all t ∈ (t 1 , t 2 ). Also, We now show exponential stability of the error terms E R and e p . From the jump map (8), we can see that V 1 (E + R (t j )) = (1 − k p ) 2j V 1 (E R (0)) for all j ∈ N. Defining j t the number of jumps till time t ≥ 0, and using the property that V 1 (E R (t)) is constant over the interval t ∈ (t j , t j+1 ) for every j ∈ N, and noting that j t + 1 ≥ t/T M for all t ≥ 0 from Assumption 1 and |1 − k p | < 1, where Similarly, using the jump map (10) and the fact that V 2 (e p (t)) is constant over the interval t ∈ (t j , t j+1 ) for every j ∈ N, where Substituting V 1 (E R ) from equation (7) and V 2 (e p ) from equation (9) in the above equations, we get that for all t ≥ 0. Hence, E R (t) → 0 and e p (t) → 0 exponentially fast as t → ∞.
We write E p defined in equation (3) in terms of E R and e p defined in equations (3) and (4) as Taking norm on both sides of the equality, and from equations (13) and (14), we have for all t ≥ 0, where R (t) −T < 100 √ 3 from Lemma 1 and the fact that R(t) −R(t) ≤ R(0) −R(0) < 0.99, and µ > 0 is chosen such that µ − µ 1 < 0. Taking limit on both sides of the inequality as t → ∞, and using the assumption lim t→∞ p(t) exp(−µt) = 0 for all µ > 0, we get that E p → 0 as t → ∞.
The assumption in the statement of the theorem, i.e. p(t) exp(−µt) → 0 as t → ∞ for all µ > 0 includes the case where each component of p(t) is a polynomial in t.
Note that here we need existence of finite T M for existence of µ 1 and µ 2 such that the exponential bounds on V 1 and V 2 can be defined. However, we do not need the information about the value of T M to design the observer. If the value of T M is known, the assumption on the position can be further relaxed as follows: Corollary 1: Suppose that Assumption 1 holds, and for some µ, c > 0, p(t) exp(−µt) → c as t → ∞, and let where the rigid body system is given in equation (1), and the observer system is given in equation (2). Then, with 1−exp(−µT M ) < k p < 1+exp(−µT M ) and 0 < k e < 2, the error term E R (t) converges exponentially to 0 and E p (t) converges asymptotically to 0 as t → ∞ for all (R(0),p(0)) ∈ The part of the proof till local exponential convergence of E R (t) and e p (t) defined in equations (3) and (4) to 0 as t → ∞ is not affected by the change in the assumption on the bounds on p(t) . Hence, it follows from the corresponding part of Theorem 1.
Writing E p in terms of E R and e p as defined in equation (15) and taking norm on both sides of the equality, we have from equation (16) for all t ≥ 0, ). The choice of k p ensures that µ − µ 1 < 0. Taking limit on both sides as t → ∞, and using the assumption lim t→∞ p(t) exp(−µt) = c for the given µ, we get that E p → 0 as t → ∞.
The following modification to Theorem 1 enlarges the region of convergence.
Theorem 2: Suppose that Assumption 1 holds, and for all µ > 0, p(t) exp(−µt) → 0 as t → ∞, and let where the rigid body system is given in equation (1), and the observer system is given in equation (2). Given any initial the error term E R (t) converges exponentially to 0 and E p (t) converges asymptotically to 0 as t → ∞. Proof: The bounds on k p in (17) are a subset of the bounds on k p of Theorem 1. The initial condition and the bounds on the position also follow from Theorem 1. Hence, convergence for the initial condition R(0) −R(0) < 0.99 has been shown in Theorem 1.
Consider the case where the givenR(0) is such that α := The invertibility ofR(0) implies that ofR(0)R(0) T which implies the invertibility ofR(t)R(t) T for all 0 ≤ t < t 1 which implies the invertibility ofR(t) for all 0 ≤ t < t 1 . By (17), we have that Hence,R + (t 1 ) is invertible andp + is well defined. Note that there is no change in invertibility ofR(t) for t ≤ t 1 . After the first jump, since E + R (t 1 ) < 0.99, Theorem 1 and the time-invariant nature of the system (1) and system (2) ensures that the error term E R (t) converges exponentially to 0 and E p (t) converges asymptotically to 0 as t → ∞.
Under the assumptions on the position and bounds on k e as specified in Theorem 2, as k p gets closer to 1, the region of convergence of the observer (2) to the system (1) converges to the set (A, b) ∈ R 3×3 × R 3 | det(A) = 0 , which can be easily seen by re-writing (17) as for all k p near 1 but not equal to 1. Since the right-hand side of the above inequality diverges to infinity as k p → 1, the claims follows. Based on Theorem 2, we present a globally convergent observer if the observer system starts from SO(3) R 3 : Theorem 3: Suppose that Assumption 1 holds, and for all µ > 0, p(t) exp(−µt) → 0 as t → ∞, and let VOLUME 10, 2022 where the rigid body system is given in equation (1), and the observer system is given in equation (2). Then, with 0 < k e < 2 and 0.75 < k p < 1.25, the error term E R (t) converges exponentially to 0 and E p (t) converges asymptotically to 0 as t → ∞ for all (R(0),p(0)) ∈ SO(3) R 3 .
Proof: Note that det(R(0)) = 0. Hence it satisfies conditions of Theorem 2. Since R(0) −R(0) ≤ 2 √ 2 and 0.75 < k p < 1.25, k p and k e satisfy the bounds as defined in Theorem 2. From Theorem 2, the errors E R (t) and E p (t) converge to 0 as t → ∞. Moreover, the rate of convergence of E R (t) to 0 is exponential. Hence, the observer exhibits global convergence.
The initial state of the observer is chosen by the designer. According to Theorem 3, irrespective of the initial state of the rigid body system, any choice of the initial state of the observer in SO(3) R 3 leads to convergence of the observer to the state of the rigid body. Hence, the result in Theorem 3 is global.
We can modify Theorem 2 and Theorem 3 by relaxing the condition on the bound of the position as in Corollary 1. The proof of the corollary and the theorem presented below follows from that of Theorem 2 and Theorem 3 with a similar modification as in Corollary 1. We leave the proof to the reader.
Corollary 2: Suppose that Assumption 1 holds, and for some µ, c > 0, p(t) exp(−µt) → c as t → ∞, and let where the rigid body system is given in equation (1), and the observer system is given in equation (2). Given any initial state (R(0),p(0)) ∈ (A, b) ∈ R 3×3 × R 3 | det(A) = 0 , with 0 < k e < 2 and k L p < k p < k U p where the bounds k L p and k U p are defined as the error term E R (t) converges exponentially to 0 and E p (t) converges asymptotically to 0 as t → ∞. Proof: Omitted.
The following theorem is a global result extending Theorem 3.
Proof: Omitted. Remark 1: To study the effect of varying the gains k p and k e on the observer dynamics, we note that µ 1 and µ 2 as defined in equations (11) and (12) are a function of log e |1 − k p | and log e |1 − k e |, respectively. Consequently, as k p and k e are chosen closer to 1, the convergence of the observer to the measurement is faster. From equations (5), we see that the farther k p and k e are from 1, the higher their noise rejection effect is on the observer state.
Remark 2: We note that the system gains can be updated at every t i , i ∈ N due to the structure of the observer. Hence, the designer may choose a varying gains strategy. An example of this is gains being chosen close to 1 initially, such that the bounds of Theorem 3 are satisfied, and then reducing the gains such that the system starts rejecting noise in the measurements. For an application of this scheme, see Sections IV and V.

IV. NUMERICAL SIMULATION
The simulation for the observer error system (6) is shown in Figure 1a. We assume that the initial state is (R,p) = (exp(πe 3 × /3), 0 3×1 ) ∈ SO(3) R 3 . The angular velocity is assumed to be of the form (cos(t), sin(t), cos(2t)) and the linear velocity (sin(t) cos(t), sin(2t), cos(t/5)). The measurements are assumed to arrive randomly between 0.5s to 2s. It can be seen that the observer system (2) converges exponentially fast for the choice of k p = 0.8 and k e = 0.5.
We compare the proposed observer (2) with the system designed in [8]. The major improvement of our paper over existing ones in the literature is the global convergence result, hence we choose the initial position for the estimate near the boundary of the repeller set in [8]. The simulation results can be seen in Figure 1b. We see that the proposed observer (2) does not suffer as the observer proposed by [8].
We also compare the same with the fixed-gain observer system designed in [10]. Note that the authors in [10] consider a system with the measurement of acceleration and angular velocity as compared to our proposed system with measurements of linear and angular velocity. To account for this, we calculate the acceleration corresponding to the velocity trajectory [sin(t) cos(t), sin(2t), cos(t/5)] T and use this acceleration in the simulation of the observer proposed in [10] while using the velocity in our proposed observer. The measurements are assumed to arrive randomly between 0.5s to 2s. The simulation results can be seen in Figure 1c. Since the choice of gains decides the size of the attractor set in [10], the proposed observer (2) improves upon the results presented by [10] due to its global nature.
We also simulate the system in the presence of noisy measurements. The noise is assumed to be Gaussian with a mean of 0 and a standard deviation of 0.1. The measurements are assumed to arrive randomly between 0.5s to 2s. The initial condition is assumed to be (R,p) = (exp(πe 3 × /3), 0 3×1 ) ∈ SO(3) R 3 . To show the effect of k p and k e values on the results, we choose two sets of values, k p = 0.65 and k e = 0.3, and k p = 1 and k e = 1. As the bounds specified in Theorem 3  correspond to the worst case scenario, we are permitted to use k p = 0.65 here since E R (0) = 1.41. The simulation of the system can be seen in Figure 2a.
We implement the varying k p and k e scheme as explained in Remark 2. We choose the same values of k p and k e as in the previous simulation, and change the value of k p and k e to 0.1 after 10 measurements. The effects of this can be seen in Figure 2b. This provides the designer of the control law greater flexibility in designing a robust observer system.

V. EXPERIMENTAL SIMULATION
We perform a similar simulation as in Section V with data collected from a real system. We measure R and p using an Intel Realsense T265 camera and the ground truth is measured using a high accuracy motion tracking system OptiTrack.
The time difference between two consecutive measurements of angular velocity and linear velocity vary from 10 −5 s to 0.043s and that of the pose are around 1s. Since the initial estimate error is unknown, we choose k p = 0.8 and k e = 0.2 initially. We update the value of k p to 0.2 while keeping k e constant after one epoch according to Remark 2. The simulation results for the observer are shown in Figure 2c. It can be seen that the proposed observer gives a better estimate of the true state as compared to the measurement values due to its hybrid nature.

VI. CONCLUSION
In this paper, a hybrid measurement triggered observer to the rigid body rotation and translation kinematics problem is designed. It can be seen that the convergence of the rotation estimate of the state to the rotation of the body is exponential globally and the convergence of the position estimate to the position is asymptotic globally when the initial estimate is chosen from SO(3) R 3 . Also, the improvements over the results of [8] and [10] are substantial since our proposed observer tracks the system and the error converges to 0 globally.