Hybrid Nonlinear Observers for Inertial Navigation Using Landmark Measurements

# Hybrid Nonlinear Observers for Inertial Navigation Using Landmark Measurements

Miaomiao Wang and Abdelhamid Tayebi This work was supported by the National Sciences and Engineering Research Council of Canada (NSERC). A preliminary and partial version of this work was presented in [1].M. Wang is with the Department of Electrical and Computer Engineering, Western University, London, ON N6A 3K7, Canada (e-mail: mwang448@uwo.ca).A. Tayebi is with the Department of Electrical and Computer Engineering, Western University, London, ON N6A 3K7, Canada, and also with the Department of Electrical Engineering, Lakehead University, Thunder Bay, ON P7B 5E1, Canada (e-mail: atayebi@lakeheadu.ca).
###### Abstract

This paper considers the problem of attitude, position and linear velocity estimation for rigid body systems relying on landmark measurements. We propose two hybrid nonlinear observers on the matrix Lie group , leading to global exponential stability. The first observer relies on fixed gains, while the second one uses variable gains depending on the solution of a continuous Riccati equation (CRE). These observers are then extended to handle biased angular velocity measurements. Both simulation and experimental results are presented to illustrate the performance of the proposed observers.

Inertial navigation system (INS); Hybrid observers; Landmark measurements; Pose and linear velocity estimation

## I Introduction

The development of reliable attitude, position and linear velocity estimation algorithms for inertial navigation systems is instrumental in many applications, such as autonomous underwater and ground vehicles, and unmanned aerial vehicles. It is well known that the attitude of a rigid body can be estimated using body-frame observations of some known inertial vectors obtained, for instance, from an inertial measurement unit (IMU) equipped with a gyroscope, an accelerometer and a magnetometer [2, 3, 4]. The position and linear velocity can be obtained, for instance, from a Global Positioning System (GPS)[5, 6, 7]. As a matter of fact, IMU-based nonlinear attitude observers rely on the fact that the accelerometer provides a measurement of the gravity vector in the body-fixed frame, which is true only in the case of negligible linear accelerations. In applications involving accelerated rigid body systems, a typical solution consists in using linear velocity measurements together with IMU measurements with the so-called velocity-aided attitude observers [8, 9, 10, 11, 12]. However, these observers are not easy to implement in GPS-denied environments (e.g., indoor applications), where it is challenging to obtain the linear velocity.

On the other hand, the pose (position and orientation) can be obtained using estimators relying on inertial-vision systems consisting of an IMU and a stereo vision system attached to the rigid body [13]. Most of the existing pose estimators are typically filters of the Kalman-type. Recently, nonlinear geometric observers on Lie groups have made their appearance in the literature [14, 15]. Nonlinear pose observers designed on using landmark and group velocity measurements have been considered in [16, 17, 18, 19]. However, these observers are shown to guarantee almost global asymptotic stability (AGAS), i.e., the pose error converges to zero from almost all initial conditions except from a set of Lebesgue measure zero. Nonlinear hybrid observers evolving on with global asymptotic and exponential stability guarantees have been proposed in [20] and [21], respectively.

It is important to point out that the dynamics of the attitude, position and linear velocity are not (right or left) invariant, and hence, the extension of the existing invariant observers designed on to the estimation problem considered in this work is not trivial. Moreover, in practice, it is difficult to obtain the linear velocity using low-cost sensors, in GPS-denied environments. Therefore, developing estimation algorithms that provide the attitude, position and linear velocity, with strong stability guarantees, is of great importance (from theoretical and practical point of views) for inertial navigation systems.

In the present work, we formulate the estimation problem for inertial navigation systems using the matrix Lie group introduced in [22]. Then, we propose nonlinear geometric hybrid observers, relying on landmark measurements, for the estimation of the pose, linear velocity and gyro-bias. The proposed observers are endowed with exponential stability guarantees and enjoy and interesting decoupling property that will be detailed later. To the best of our knowledge, there are no results in the literature achieving such strong stability properties for the estimation problem at hand. In fact, most of the existing results in the literature rely on linearizations and the use of Kalman filtering approaches, see for instance [23, 24, 25]. Recently, some interesting results dealing with the estimation problem for inertial navigation systems, taking into account the geometric properties of the system, leading to local stability results, have been proposed in [22] and [26]. In fact, an invariant Extended Kalman Filter (iEKF), using a geometric error, has been proposed in [22], and a Riccati-based geometric pose, linear velocity and gravity direction observer has been proposed in [26].

Note that, in our preliminary work [1], we proposed a fixed-gain hybrid observer for inertial navigation systems in the gyro-bias-free case. In this paper, we provide a comprehensive hybrid observer design methodology for inertial navigation systems, with fixed and variable gains, using bias-free and biased angular velocity measurements. Moreover, experimental results are presented to illustrate the performance of the proposed observers.

The rest of this paper is organized as follows: Section II introduces some preliminary notions that will be used throughout this paper. Section III is devoted to the design of the hybrid nonlinear observers in the angular velocity bias-free case. These results are extended to address the problem of biased angular velocity in Section IV. Simulation and experimental results are presented in Section V and Section VI, respectively.

## Ii Preliminary Material

### Ii-a Notations

The sets of real, non-negative real and natural numbers are denoted as , and , respectively. We denote by the -dimensional Euclidean space, and denote by the set of -dimensional unit vectors. Given two matrices, , their Euclidean inner product is defined as . The Euclidean norm of a vector is defined as , and the Frobenius norm of a matrix is given by . The -by- identity matrix is denoted by . For each , we define as the set of all eigenvectors of , and as the eigenbasis set of . Let be the -th eigenvalue of , and and be the minimum and maximum eigenvalue of , respectively.

Let be an inertial frame and be a frame attached to a rigid body moving in 3-dimensional space. Define as the rotation of frame with respect to frame , where . Let and be the position and linear velocity of the rigid-body expressed in the inertial frame , respectively. We consider the following extended Special Euclidean group of order 3 proposed in [22]: , which is defined as . The map is defined by [22]

 T(R,v,p) =⎡⎢⎣Rvp01×31001×301⎤⎥⎦.

For every , one has the inverse of as . Denote as the tangent space of at point . The Lie algebra of , denoted by , is given by

 se2(3):= {U=[Ωαv02×302×302×3]∈R5×5∣∣∣ Ω∈so(3),v,α∈R3},

where denoting the Lie algebra of . Let be the vector cross-product on and define the map such that . Define the inverse isomorphism of as such that . For any , we define as the normalized Euclidean distance on with respect to the identity , such that . Let the map represents the well-known angle-axis parametrization of the attitude, which is given by with being the rotation angle and the rotational axis. For any matrix , define as the anti-symmetric projection of , such that . For a matrix , we define . Then, one has the identity . Let denote the projection of on the Lie algebra , such that, for all , one has . For all and , one has

 P⎛⎜ ⎜⎝⎡⎢ ⎢⎣A1a2a3a⊤4a6a7a⊤5a8a9⎤⎥ ⎥⎦⎞⎟ ⎟⎠=⎡⎢⎣Pa(A1)a2a301×30001×300⎤⎥⎦. (1)

Given a rigid body with configuration , for all , the adjoint map is given by . For all , one can verify that .

### Ii-B Hybrid Systems Framework

Define a hybrid time domain as a subset in the form

 E=J−1⋃j=0([tj,tj+1]×{j}),

for some finite sequence , with the “last” interval possibly in the form or . On each hybrid time domain there is a natural ordering of points : if and . Given a manifold , we consider the following hybrid system [27]:

 H:   {˙x  ∈F(x),x∈Fx+∈G(x),x∈J (2)

where the flow map describes the continuous flow of on the flow set ; the jump map describes the discrete flow of on the jump set . A hybrid arc is a function , where is a hybrid time domain and, for each fixed , is a locally absolutely continuous function on the interval . Note that denotes the value after a jump, namely, . For more details on dynamic hybrid systems, we refer the reader to [27, 28] and references therein.

### Ii-C Kinematics and Measurements

Consider the following kinematics of a rigid body navigating in 3D space:

 ˙R =Rω×, (3) ˙p =v, (4) ˙v =→g+Ra, (5)

where denotes the gravity direction and denotes the gravity constant, denotes the angular velocity expressed in body-frame, and is the body-frame “apparent acceleration” capturing all non-gravitational force applied to the rigid body expressed in body frame. We assume that and are available for measurement. In this paper, we consider the configuration of the rigid body represented by an element of the matrix Lie group . Let us introduce the nonlinear map , such that the kinematics (3)-(5) can be rewritten in the following compact form

 ˙X=f(X,ω,a):=⎡⎢⎣Rω×→g+Rav01×30001×300⎤⎥⎦. (6)

Consider a family of landmarks available for measurements, and let be the position of the -th landmark expressed in the inertial frame . The landmark measurements expressed in the body frame are denoted as

 yi:=R⊤(pi−p),i=1,2,⋯,n. (7)

Practically, vision systems do not provide the 3D landmark positions directly. However, one can, for instance, construct the landmark positions using stereo bearing measurements obtained from a stereo vision system, for example [29, Eq. (26)]. Let for all be the new inertial reference vectors with respect to the inertial frame , and be their measurements expressed in the body frame . From (7), one has

 bi=h(X,ri):=X−1ri,i=1,2,⋯,n. (8)

Note that, the Lie group action is a right group action in the sense that for all and , one has For later use, we define and .

###### Assumption 1.

Assume that there exist at least three non-collinear landmarks among the measurable landmarks.

It should be noted that Assumption 1 is common in pose estimation on using landmark measurements [17, 16, 18, 19, 20, 21]. Define the matrix with , and . The matrix can be rewritten as with . Given three non-collinear landmarks, it is always possible to guarantee that the matrix is positive semidefinite and has no more that one zero eigenvalues through an appropriate choice of the gains . Some useful properties are given in the following lemma.

###### Lemma 1.

[21] Let be a positive semi-definite matrix under Assumption 1. Consider the map defined as:

 ΔM(u,v):=u⊤(tr(Mv)I3−Mv)u, (9)

where and . Define the constant scalar

 Δ∗M:=minv∈E(M)maxu∈UΔM(u,v). (10)

Then, the following results hold:

• Let be a superset of (i.e., ), then the following inequality holds:

 Δ∗M≥⎧⎪ ⎪⎨⎪ ⎪⎩23λM1%ifλM1=λM2=λM3>0min{2λM1,λM3}if λM1=λM2≠λM3>0tr(M)−λMmaxif λMi≠λMj≥0,i≠j.
• Let be a matrix such that , and let be a set that contains any three orthogonal unit vectors in , then the following inequality holds:

 Δ∗M≥23(tr(M)−2λMmax).

## Iii Hybrid Observers Design Using Bias-free Angular Velocity

### Iii-a Continuous observer and undesired equilibra

Let be the estimate of the state , where denotes the estimate of the attitude , denotes the estimate of the linear velocity and denotes the estimate of the position . Define the right-invariant estimation error as with and .

Consider the following time-invariant continuous observer

 ˙^X=f(^X,ω,a)−Δ^X, (11) Δ:=−AdXc(P(X−1c(r−^Xb)Knr⊤X−⊤cK)) (12)

where and with defined before. The gain parameters are given by

 Kn=diag(k1,⋯,kn),K=⎡⎢⎣kRI303×103×101×30001×3kvkp⎤⎥⎦, (13)

with .

###### Remark 1.

Note that the proposed continuous observer is designed on the matrix Lie group directly, which is different from most of the existing Kalman-type filters. The observer has two parts: the term relying on the measurements of and , and an innovation term designed in terms of the estimated state and landmarks measurements.

###### Remark 2.

A homogeneous transformation matrix is introduced in the innovation term , which intends to transform the inertial vectors to a specific frame. Considering the transformation , the innovation term defined in (12 ) can be simplified as with . Choosing , leads to a nice decoupling property in the closed loop dynamics, which will be discussed later. Similar techniques can be founded in [20, 21].

Let for all . From the definitions of and , one obtains

 X−1c(r−^Xb)Knr⊤X−⊤c=X−1cn∑i=1ki(ri−^Xbi)r⊤iX−⊤c =⎡⎢⎣∑ni=1ki~yi(pi−pc)⊤03×1∑ni=1ki~yi01×30001×300⎤⎥⎦,

where we made use of the fact . Then, from the definitions of , the Adjoint map and the projection map , the expression of defined in (12) becomes

 Δ =−⎡⎢⎣kRPa(ΔR)kvΔpkpΔp−kRPa(ΔR)pc01×30001×300⎤⎥⎦ (14)

where and . For the sake of simplicity, let us define the new position estimation error . In view of (6), (11) and (14), one has the following closed-loop system:

 ⎧⎪ ⎪⎨⎪ ⎪⎩˙~R =−~R(kRPa(M~R))˙~pe=−kpkc~pe+~v˙~v =−kvkc~pe+(I3−~R)→g (15)

where we made use of the facts that and . It is clear that if and only if . Note that the geometric errors and considered in this paper are different from the linear errors (i.e., and ) considered in the classical EKF-based navigation filters. The modified geometric errors we considered, lead to an interesting decoupling property for the closed-loop system, where the dynamics of are not dependent on and as shown in the first equation of (15).

###### Proposition 1.

Consider the closed-loop dynamics (15). Let be the set of undesired equilibrium points (i.e., all the equilibrium points except ) of the closed-loop dynamics, which is given by

 Ψ:= {gkckvT(~R,~v,~pe)∈SE2(3)|~R=Ra(π,u),u∈E(M), ~pe=gkckv(I−~R)e3,~v=kckp~pe}. (16)
###### Proof.

The proof of Proposition 1 is straightforward. For the first identity , one has [30, Lemma 2] , while the set is the set of undesired equilibrium points of the rotational error dynamics. Substituting into the identities and , one can easily verify (16) from (15) .

###### Remark 3.

From the dynamics of in (15), it is easy to verify that the set is AGAS. It is important to mention that, due to the topology of the Lie group as pointed out in [31], it is impossible to achieve robust and global stability results with smooth (or even discontinuous) state observers [30, 32]. Hence, the best result one can achieve with the continuous observer (11)-(12) is AGAS. This motivates the design of hybrid observers leading to robust and global stability results as shown in the next section.

### Iii-B Fixed-gain hybrid observer design

Define the following real-valued cost function

 Φ(^X,r,b):=12n∑i=1ki∥(ri−rc)−^X(bi−bc)∥2 (17)

where and with . From the definitions of and , one can rewrite as Given a non-empty and finite transformation set , let us define a real-valued function as

 μQ(^X,r,b):=Φ(^X,r,b)−minXq∈QΦ(X−1q^X,r,b). (18)

The flow set and jump set are defined as follows:

 Fo :={^X∈SE2(3) | μQ(^X,r,b)≤δ}, (19) Jo :={^X∈SE2(3) | μQ(^X,r,b)≥δ}, (20)

with some , and the set given by

 Q:= {X=T(R,v,p)|R=Ra(θ,u),θ∈(0,π] u∈U,p=(I3−R)pc,v=0}. (21)

where the set of unit vectors will be designed latter. The sets and are closed, and . We propose the following hybrid observer:

where and the map is defined by

 γ(^X):={Xq∈Q|Xq=argminXq∈QΦ(X−1q^X,r,b)}. (24)

We define the extended space and state as and , respectively. In view of (15), and (21)-(24), one obtains the following hybrid closed-loop system:

 Hc1:{˙xc1  =F1(xc1)xc1∈Fc1xc1+=G1(xc1)xc1∈Jc1 (25)

with , , and

 F1(xc1)=⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣f(^X,ω,a)−Δ^X−~R(kRPa(M~R))−kckp~pe+~v−kckv~pe+(I−~R)→g1⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦,G1(xc1)=⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣X−1q^X~RRq~pe~vt⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦.

where we made use of the facts: , and . Note that the sets are closed, and . Note also that the closed-loop system (25) satisfies the hybrid basic conditions of [27] and is autonomous by taking and as functions of .

The main idea behind our hybrid observer is the introduction of a resetting mechanism to avoid the undesired equilibrium points of the closed-loop system (25) in the flow set , i.e., all the undesired equilibrium points of the closed-loop system belong to the jump set . The innovation term and the transformation set are designed to guarantee a decrease of a Lyapunov function in both flow set and jump set .

###### Proposition 2.

Consider the hybrid dynamics defined in (25). Choose as per Lemma 1 for the transformation set in (21). Then, there exists a constant as per Lemma 1 such that for all , one has .

See Appendix -A for the proof. Proposition 2 provides a choice for the hysteresis gap , ensuring that the set of undesired equilibrium points of the flow dynamics of (25) is a subset of the jump set .

Let us define the set . Now, one can state one of our main results.

###### Theorem 1.

Consider the hybrid closed-loop system (25). Suppose that Assumption 1 holds. Let , and choose the set as per Lemma 1 and with defined in (10). Then, the number of discrete jumps is finite and the set is uniformly globally exponentially stable.

See Appendix -B

###### Remark 4.

In view of (14), (21), (22) and (24), the proposed hybrid observer can be explicitly expressed, in terms of the available measurements, as follows:

 ˙^R=^Rω×+kRPa(ΔR)^R˙^p=^v+kRPa(ΔR)(^p−pc)+kpΔp˙^v=→g+^Ra+kRPa(ΔR)^v+kvΔp⎫⎪ ⎪ ⎪⎬⎪ ⎪ ⎪⎭^X∈Fo ^R+=R⊤q^R^p+ =R⊤q(^p−(I3−Rq)pc)^v+ =R⊤q^v⎫⎪ ⎪⎬⎪ ⎪⎭^X∈Jo

where , and are defined in (14).

### Iii-C Variable-gain hybrid observer design

In this subsection we provide a different version of the hybrid observer using variable gains relying on the solution of a continuous Riccati equation. Let us define the following gain map inspired by [19], such that for all and , one has

 PK⎛⎜ ⎜⎝⎡⎢ ⎢⎣A1a2a3a⊤4a6a7a⊤5a8a9⎤⎥ ⎥⎦⎞⎟ ⎟⎠=⎡⎢⎣kRPa(A1)Kva2Kpa301×30001×300⎤⎥⎦. (26)

where with and to be designed. The, we propose the following hybrid observer.

 Ho2:{˙^X=f(^X,ω,a)−Δ^X