Hybrid Nonlinear Observers for Inertial Navigation Using Landmark Measurements
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.
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 bodyframe 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, IMUbased nonlinear attitude observers rely on the fact that the accelerometer provides a measurement of the gravity vector in the bodyfixed 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 socalled velocityaided attitude observers [8, 9, 10, 11, 12]. However, these observers are not easy to implement in GPSdenied 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 inertialvision 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 Kalmantype. 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 lowcost sensors, in GPSdenied 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 gyrobias. 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 Riccatibased geometric pose, linear velocity and gravity direction observer has been proposed in [26].
Note that, in our preliminary work [1], we proposed a fixedgain hybrid observer for inertial navigation systems in the gyrobiasfree case. In this paper, we provide a comprehensive hybrid observer design methodology for inertial navigation systems, with fixed and variable gains, using biasfree 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 biasfree 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
Iia Notations
The sets of real, nonnegative 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 3dimensional space. Define as the rotation of frame with respect to frame , where . Let and be the position and linear velocity of the rigidbody 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]
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
where denoting the Lie algebra of . Let be the vector crossproduct 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 wellknown angleaxis parametrization of the attitude, which is given by with being the rotation angle and the rotational axis. For any matrix , define as the antisymmetric 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
(1) 
Given a rigid body with configuration , for all , the adjoint map is given by . For all , one can verify that .
IiB Hybrid Systems Framework
Define a hybrid time domain as a subset in the form
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]:
(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.
IiC Kinematics and Measurements
Consider the following kinematics of a rigid body navigating in 3D space:
(3)  
(4)  
(5) 
where denotes the gravity direction and denotes the gravity constant, denotes the angular velocity expressed in bodyframe, and is the bodyframe “apparent acceleration” capturing all nongravitational 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
(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
(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
(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 noncollinear 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 noncollinear 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 semidefinite matrix under Assumption 1. Consider the map defined as:
(9) 
where and . Define the constant scalar
(10) 
Then, the following results hold:

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

Let be a matrix such that , and let be a set that contains any three orthogonal unit vectors in , then the following inequality holds:
Iii Hybrid Observers Design Using Biasfree Angular Velocity
Iiia 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 rightinvariant estimation error as with and .
Consider the following timeinvariant continuous observer
(11)  
(12) 
where and with defined before. The gain parameters are given by
(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 Kalmantype 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
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
(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 closedloop system:
(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 EKFbased navigation filters. The modified geometric errors we considered, lead to an interesting decoupling property for the closedloop system, where the dynamics of are not dependent on and as shown in the first equation of (15).
Proposition 1.
Consider the closedloop dynamics (15). Let be the set of undesired equilibrium points (i.e., all the equilibrium points except ) of the closedloop dynamics, which is given by
(16) 
Proof.
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.
IiiB Fixedgain hybrid observer design
Define the following realvalued cost function
(17) 
where and with . From the definitions of and , one can rewrite as Given a nonempty and finite transformation set , let us define a realvalued function as
(18) 
The flow set and jump set are defined as follows:
(19)  
(20) 
with some , and the set given by
(21) 
where the set of unit vectors will be designed latter. The sets and are closed, and . We propose the following hybrid observer:
(22)  
(23) 
where and the map is defined by
(24) 
We define the extended space and state as and , respectively. In view of (15), and (21)(24), one obtains the following hybrid closedloop system:
(25) 
with , , and
where we made use of the facts: , and . Note that the sets are closed, and . Note also that the closedloop 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 closedloop system (25) in the flow set , i.e., all the undesired equilibrium points of the closedloop 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.
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.
Proof.
See Appendix B
IiiC Variablegain 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
(26) 
where with and to be designed. The, we propose the following hybrid observer.