Nonlinear Observers Design for Vision-Aided Inertial Navigation Systems
11 Nonlinear Observers Design for Vision-AidedInertial Navigation Systems
Miaomiao Wang, Soulaimane Berkane, and Abdelhamid Tayebi
Abstract —This paper deals with the simultaneous estimationof the attitude, position and linear velocity for vision-aidedinertial navigation systems. We propose a nonlinear observeron SO (3) × R relying on body-frame acceleration, angularvelocity and (stereo or monocular) bearing measurements of somelandmarks that are constant and known in the inertial frame.Unlike the existing local Kalman-type observers, our proposednonlinear observer guarantees almost global asymptotic stabilityand local exponential stability. A detailed uniform observabilityanalysis has been conducted and sufficient conditions are de-rived. Moreover, a hybrid version of the proposed observer isprovided to handle the intermittent nature of the measurementsin practical applications. Simulation and experimental resultsare provided to illustrate the effectiveness of the proposed stateobserver. Index Terms —Nonlinear observer, Vision-aided inertial naviga-tion system (Vision-aided INS), Bearing measurements, Uniformobservability.
I. I
NTRODUCTION
The design of reliable state observers for the simultaneousestimation of the attitude (orientation), position, and linearvelocity for inertial navigation systems (INSs) is crucial inmany robotics and aerospace applications. Visual sensors,which provide a rich information about the environment, arebecoming ubiquitous in a wide range of applications frommobile augmented reality to autonomous vehicles’ navigation.A vision system together with an inertial measurement unit(IMU) forms a vision-aided INS (or visual-inertial navigationsystem). Depending on the camera type, vision-aided INSs canbe categorized as monocular vision-aided INS (single camera)or stereo vision-aided INS (two cameras in a stereo setup).Vision-aided INS is widely used in robotics, for instance,visual-inertial odometry (VIO) and Simultaneous Localizationand Mapping (SLAM) [2]–[5]. Unlike the VIO and SLAM,where the landmarks/features are unknown, the estimationproblem at hand, referred to as vision-aided INS, assumesknown landmarks in the inertial frame. Visual measurements
This work was supported by the National Sciences and EngineeringResearch Council of Canada (NSERC), under the grants NSERC-DG RGPIN2020-06270 and NSERC-DG RGPIN-2020-04759. A preliminary and partialversion of this work was presented in [1].M. Wang is with the Department of Electrical and Computer En-gineering, Western University, London, ON N6A 3K7, Canada (e-mail:[email protected]).S. Berkane is with the Department of Department of Computer Scienceand Engineering, University of Quebec in Outaouais, QC J8X 3X7, Canada(e-mail: [email protected]).A. Tayebi is with the Department of Electrical and Computer Engineering,Western University, London, ON N6A 3K7, Canada, and also with theDepartment of Electrical Engineering, Lakehead University, Thunder Bay, ONP7B 5E1, Canada (e-mail: [email protected]). of these landmarks together with IMU measurements areused to simultaneously estimate the attitude, position andlinear velocity. The proposed estimation scheme, endowedwith almost global asymptotic stability (AGAS) guarantees,is able to handle different types of measurements, includingmonocular-bearing and stereo-bearing measurements. A. Motivation and Prior Literature
The position and linear velocity of a rigid body can beobtained, for instance, from a Global Positioning System(GPS), while its attitude can be estimated from a set ofbody-frame measurements of some known inertial vectors[6]–[8]. Typically, low-cost IMU-based estimation techniquesassume that the accelerometer provides body-frame measure-ments of the gravity vector, which is not true in applicationsinvolving non-negligible accelerations. One solution to thisproblem consists in using the so-called velocity-aided attitudeobservers [9]–[11], which make use of the linear velocityand IMU measurements to estimate the attitude. Instead ofassuming the linear velocity available (as in the velocity-aidedobservers), GPS-aided navigation observers use IMU and GPSinformation to simultaneously estimate the attitude, positionand linear velocity; see, for instance, [12] and referencestherein. These estimation schemes, however, are not suitablefor implementations in GPS-denied environments ( e.g., indoorapplications).One alternative approach, that is widely adopted in GPS-denied environments, is the vision-aided INS. Most of theexisting estimation schemes for vision-aided INSs in theliterature are of Kalman-type such as the Extended KalmanFilter (EKF) and the Unscented Kalman Filter (UKF) [2],[3]. Different versions of these algorithms have been pro-posed in the literature depending on the type of vision-basedmeasurements used. Due to the nonlinearity of the vision-aided INS kinematics, these Kalman-type filters, relying onlocal linearizations, do not provide strong stability guarantees.Recently, several nonlinear observers using three-dimensional(3D) landmark position measurements, have been proposedin the literature [13]–[15]. In contrast with the standard EKFand its variants, an invariant EKF (IEKF), with provablelocal stability guarantees, has been proposed in [13]. A lo-cal Riccati-based nonlinear observer, inspired from [16], hasbeen proposed in [14]. Motivated by the work in [17], [18],hybrid nonlinear observers, with global exponential stabilityguarantees, have been proposed in [15]. The equilibrium point is stable and asymptotically attractive from almostall initial conditions except from a set of Lebesgue measure zero. a r X i v : . [ m a t h . O C ] F e b It is clear that vision systems do not directly provide3D landmark position measurements. In fact, they can beobtained from the images of a stereo-vision system usingadditional algorithms [19]. In [1], we developed nonlinearobservers for attitude, position, linear velocity and gravityvector estimation, using direct stereo-bearing measurements.A local Riccati observer for attitude, position, linear velocityand accelerometer-bias estimation, with monocular-bearingmeasurements, has been proposed in [20]. On the other hand,in practical applications, the used sensors may have differentsampling rates. For instance, the sampling rates of the visionsensors are much lower than those of the IMU, which isdue to the hardware of the vision sensors and the heavyimage processing computations. In this situation, one shouldbe careful as the stability results derived for continuous-time observers are not preserved when the measurementsare intermittent. To address this problem, hybrid nonlinearobservers have been considered in [21]–[23].
B. Contributions and Organization of the Paper
In the present paper, we propose an AGAS nonlinearobserver for the simultaneous estimation of the attitude, posi-tion and linear velocity using body-frame accelerometer andgyro measurements as well as monocular or stereo bearingmeasurements. Note that, due to the motion space topology,AGAS is the strongest result one can achieve with smoothtime-invariant observers. We also provide a hybrid version ofthe proposed observer that takes into account the sampled andintermittent nature of the visual measurements for practicalimplementation purposes. Some highlights of the contributionsof this paper are as follows:1) To the best of our knowledge, this work is the first toachieve AGAS results for vision-aided INS with bearingmeasurements. Note that the Riccati observers in [16],[20] provide only local stability guarantees. The key ideathat allowed to achieve this strong stability result is theintroduction of some auxiliary state variables allowing toappropriately design some output-driven signals leadingto linear time-varying dynamics for the translationalestimation errors.2) The proposed observer uses generic vision-based mea-surements, including stereo-bearing and monocular-bearing measurements. This is a distinct feature fromthe existing nonlinear observers which are tailored toa specific type of vision-based measurements [1], [13]–[15], [20], [23]. This is achieved by introducing someauxiliary basis vectors in the estimation procedure, allow-ing to derive a generic (possibly time-varying) innovationterm for the translational state estimation that captures thedifferent types of vision-based measurements used in thiswork.3) A detailed uniform observably analysis has been carriedout for the two types of visual landmark information(i.e., stereo-bearing measurements and monocular bearingmeasurements). Sufficient conditions on the number andlocation of the landmarks as well as the motion of thevehicle, have been derived. The most challenging analysis was the one related to the monocular vision system whichrequired a tedious proof.4) In practice, the sampling rates of the visual measurementsare much lower than those of the IMU measurements(which can be assumed continuous). In this context, wepropose a hybrid version of our nonlinear observer tohandle the discrete and intermittent nature of the visualmeasurements, which has been experimentally validatedusing the EuRoc dataset [24].The rest of this paper is organized as follows. Aftersome preliminaries in Section II, we formulate our estimationproblem in Section III. Section IV is devoted to the designof a generic nonlinear observer for vision-aided INS usingdifferent types of vision-based measurements with stabilityand observability analysis. A hybrid version of the proposedobserver, taking into account the discrete and intermittentnature of the visual measurements, is presented in Section V.Simulation and experimental results are presented in SectionsVI and VII, respectively.II. P
RELIMINARY M ATERIAL
A. Notations and Definitions
The sets of real, non-negative real, natural numbers andnonzero natural numbers are denoted by R , R ≥ , N and N > ,respectively. We denote by R n the n -dimensional Euclideanspace, and by S n the set of unit vectors in R n +1 . TheEuclidean norm of a vector x ∈ R n is denoted by (cid:107) x (cid:107) , andthe Frobenius norm of a matrix X ∈ R n × m is denoted by (cid:107) X (cid:107) F = (cid:112) tr( X (cid:62) X ) . The n -by- n identity and zeros matricesare denoted by I n and n , respectively. For a given matrix A ∈ R n × n , we define λ ( A ) as the set of all eigenvalues of A , and E ( A ) as the set of all unit-eigenvectors of A . Theminimum and maximum eigenvalues of A are, respectively,denoted by λ A min and λ A max . By blkdiag( · ) , we denote theblock diagonal matrix. Let e i denote the i -th basis vector of R n which represents the i -th column of the identity matrix I n .The Special Orthogonal group of order three, denoted by SO (3) , is defined as SO (3) := { R ∈ R , RR (cid:62) = R (cid:62) R = I , det( R ) = 1 } . The
Lie algebra of SO (3) is given by so (3) := { Ω ∈ R : Ω = − Ω (cid:62) } . Let × be the vector cross-product on R and define the map ( · ) × : R → so (3) such that x × y = x × y , for any x, y ∈ R . Let vec : so (3) → R be theinverse isomorphism of the map ( · ) × , such that vec ( ω × ) = ω for all ω ∈ R . For a matrix A ∈ R × , we denote by P a : R × → so (3) the anti-symmetric projection of A suchthat P a ( A ) := ( A − A (cid:62) ) / . Define the composition map ψ a := vec ◦ P a such that, for a matrix A = [ a ij ] ∈ R × ,one has ψ a ( A ) = [ a − a , a − a , a − a ] (cid:62) . Forany R ∈ SO (3) , we define | R | I ∈ [0 , as the normalizedEuclidean distance on SO (3) with respect to the identity I ,which is given by | R | I = tr( I − R ) / . We introduce thefollowing important orthogonal projection operator: π : S → R × that will be used throughout this paper: π ( x ) = I − xx (cid:62) , x ∈ S . (1)Note that π ( x ) is an orthogonal projection matrix whichgeometrically projects any vector in R onto the plane or- thogonal to x . Moreover, one verifies that π ( x ) is boundedand positive semi-definite, π ( x ) y = 0 × if x, y are collinear,and Rπ ( x ) R (cid:62) = π ( Rx ) for any R ∈ SO (3) . For the sakeof simplicity, the argument of the time-dependent signals isomitted unless otherwise required for the sake of clarity.Consider A ( t ) ∈ R n × n and C ( t ) ∈ R m × n as matrix-valued functions of time t , and suppose that A ( t ) and C ( t ) are continuous and bounded on R ≥ . The following definitionformulates the well-known uniform observability condition interms of the Gramian matrix Definition 1.
The pair ( A ( t ) , C ( t )) is uniformly observableif there exist constants δ, µ such that W o ( t, t + δ ) := 1 δ (cid:90) t + δt Φ (cid:62) ( τ, t ) C (cid:62) ( τ ) C ( τ )Φ( τ, t ) dτ ≥ µI n , ∀ t ≥ , (2) where Φ( τ, t ) is the transition matrix associated to A ( t ) suchthat ddt Φ( t, τ ) = A ( t )Φ( t, τ ) and Φ( t, t ) = I n . III. P
ROBLEM F ORMULATION
A. Kinematic Model
Let {I} be an inertial frame and {B} be a body-fixed frameattached to the center of mass of a rigid body. Let the rotationmatrix R ∈ SO (3) be the attitude of the frame {B} withrespect to the frame {I} . Let the vectors p ∈ R and v ∈ R denote the position and linear velocity of the rigid bodyexpressed in frame {I} , respectively. The kinematic equationsof a rigid body navigating in 3D space is given by: ˙ R = Rω × (3a) ˙ p = v (3b) ˙ v = g + Ra (3c)where g ∈ R denotes the gravity vector in frame {I} , ω ∈ R denotes the body-frame angular velocity expressedin frame {B} , and a ∈ R denotes the body-frame “apparentacceleration” capturing all non-gravitational forces applied tothe rigid body expressed in frame {B} . Assumption 1.
The measurements of the angular velocity ω and acceleration a are continuous and bounded.B. Estimation Problem for Vision-Aided INS This work focuses on the problem of attitude, positionand linear velocity estimation for INS using the body-frameacceleration and angular velocity measurements, as well asvision-based body-frame position information of a family of N ∈ N > landmarks that are constant and known in the inertialframe. Let p i denote the (constant and known) position of the i -th landmark in frame {I} , and p B i := R (cid:62) ( p i − p ) denote theposition of the i -th landmark in frame {B} . In the following,we detail the two measurement models considered in this work(see Fig. 1).1) Stereo-bearing measurements:
Let ( R c , p c ) and ( R c , p c ) denote the homogeneous transformation fromthe body-fixed frame {B} to the right camera frame {C } and left camera frame {C } , respectively. Then, the (a) (b) Fig. 1: The geometry models of vision systems: (a) monocularvision system (b) stereo vision system.model of the stereo-bearing vectors of the i -th landmarkin frame {C s } , s ∈ { , } are given as y si := p C s i (cid:107) p C s i (cid:107) = R (cid:62) cs ( p B i − p cs ) (cid:107) p B i − p cs (cid:107) , i = 1 , , . . . , N, (4)where p C s i = R (cid:62) cs ( p B i − p cs ) denotes the coordinates ofthe i -th landmark expressed in frame {C s } for s = 1 , .2) Monocular-bearing measurements:
Let ( R c , p c ) denotethe homogeneous transformation from the body-fixedframe {B} to the camera frame {C} . Then, the modelof the monocular-bearing vector of the i -th landmarkexpressed in frame {C} is given as y i := p C i (cid:107) p C i (cid:107) = R (cid:62) c ( p B i − p c ) (cid:107) p B i − p c (cid:107) , i = 1 , , . . . , N, (5)where p C i = R (cid:62) c ( p B i − p c ) denotes the coordinates of the i -th landmark expressed in frame {C} . Remark 1.
The bearing measurements of the i -th landmarkcan be easily obtained from the direct pixel measurements ( u i , v i ) (in the image plane) as y i = K − z i / (cid:107)K − z i (cid:107) ∈ S with z i = [ u i , v i , (cid:62) and K denoting the intrinsic matrixof the camera. As we can see in (4) and (5) , only partialinformation of the 3D landmark positions in frame {B} are available in the monocular-bearing and stereo-bearingmeasurements.C. Objectives Our main objectives in this work are as follows:1) Design an almost globally asymptotically stable non-linear observer for the simultaneous estimation of theattitude R ( t ) , position p ( t ) and linear velocity v ( t ) ,using the IMU measurements ( ω ( t ) , a ( t ) ) and the visualmeasurements from either (4) or (5). The observer shouldbe generic in the sense that it does not require anymodification when using any of the above mentionedmeasurements.2) Carry out a detailed uniform observability analysis forthe above mentioned visual measurements scenarios andprovide sufficient feasibility conditions depending on the number and location of the landmarks, as well as themotion of the vehicle.3) Provide a version of the observer with continuous IMUmeasurements, and sampled and intermittent visual mea-surements for practical implementation purposes. Thisis motivated by the fact that vision systems in generalprovide measurements at much lower sampling ratescompared to the IMU sampling rates.IV. A N ONLINEAR O BSERVER U SING C ONTINUOUS V ISION -B ASED M EASUREMENTS
A. Nonlinear Observer Design
Fig. 2: Structure of the proposed nonlinear observer on SO (3) × R for vision-aided INSs.To solve the vision-aided state estimation problem describedin Section III-B, we propose the following nonlinear observeron SO (3) × R : ˙ˆ R = ˆ R ( ω + ˆ R (cid:62) σ R ) × , (6a) ˙ˆ p = ˆ v + σ × R ˆ p + ˆ RK p σ y , (6b) ˙ˆ v = ˆ g + ˆ Ra + σ × R ˆ v + ˆ RK v σ y , (6c) ˙ˆ e i = σ × R ˆ e i + ˆ RK i σ y , i = 1 , , , (6d)where ˆ g = (cid:80) i =1 g i ˆ e i with g = [ g , g , g ] (cid:62) . The matrix ˆ R ∈ SO (3) denotes the estimate of the attitude R , and the vectors ˆ p ∈ R and ˆ v ∈ R denote the estimates of the position p and linear velocity v , respectively. The structure of the overallobserver is shown in Fig. 2. The attitude innovation term σ R is given as follows: σ R := k R R (cid:88) i =1 ρ i ( ˆ R (cid:62) ˆ e i ) × ( ˆ R (cid:62) e i ) = k R (cid:88) i =1 ρ i ˆ e × i e i (7)with constant scalars k R > , ρ i ≥ , i = 1 , , . The observergain matrices are designed as follows: K = P C (cid:62) ( t ) Q ( t ) , (8)with K := [ K (cid:62) p , K (cid:62) , K (cid:62) , K (cid:62) , K (cid:62) v ] (cid:62) . The matrix C ( t ) willbe defined later depending on type of visual measurementsused. The matrix P is the solution to the following CRE: ˙ P = A ( t ) P + P A (cid:62) ( t ) − P C (cid:62) ( t ) Q ( t ) C ( t ) P + V ( t ) (9)where P (0) ∈ R × is a symmetric positive definite matrix,matrices V ( t ) ∈ R × and Q ( t ) ∈ R × N are continuous, bounded and uniformly positive definite, and matrix A ( t ) ∈ R × is given as A ( t ) = − ω × I − ω × − ω × − ω × g I g I g I − ω × . (10)The matrix A ( t ) is obtained from the closed-loop translationalerror dynamics as it will be shown in the following subsection.In the traditional Kalman filter, matrices V ( t ) and Q − ( t ) arethe covariance matrices of the additive noise on the systemstate and output, respectively. The explicit design of σ y =[ σ (cid:62) y , σ (cid:62) y , . . . , σ (cid:62) yN ] (cid:62) ∈ R N and the matrix C ( t ) in the CRE(9) for each type of visual measurement described in SectionIII-B are given as follows:1) Stereo-bearing measurements:
From the stereo-bearingmeasurements defined in (4), define the vector σ yi as σ yi = (cid:88) s =1 π ( R cs y si )( ˆ R (cid:62) (ˆ p i − ˆ p ) − p cs ) (11)for all i = 1 , , . . . , N , with p i := [ p i , p i , p i ] (cid:62) = (cid:80) j =1 p ij e j , ˆ p i = (cid:80) j =1 p ij ˆ e j for all i = 1 , , . . . , N and the projection map π defined in (1). The matrix C ( t ) is given by: C ( t ) = Π − p Π − p Π − p Π Π − p Π − p Π − p Π ... ... ... ... ... Π N − p N Π N − p N Π N − p N Π N (12)with Π i := (cid:80) s =1 π ( R cs y si ) , i = 1 , . . . , N .2) Monocular-bearing measurements:
From the monocular-bearing measurements in (5), vector σ yi is designed as σ yi = π ( R c y i )( ˆ R (cid:62) (ˆ p i − ˆ p ) − p c ) (13)for all i = 1 , , . . . , N , with the map π defined in (1).The matrix C ( t ) is given by: C ( t ) = Π − p Π − p Π − p Π Π − p Π − p Π − p Π ... ... ... ... ... Π N − p N Π N − p N Π N − p N Π N (14)with Π i := π ( R c y i ) , i = 1 , . . . , N . Remark 2.
Note that the non-standard innovation term σ R in (7) relies on the inertial frame axes e i , i ∈ { , , } and theauxiliary dynamical signals ˆ e i , i ∈ { , , } . The motivationbehind this construction is as follows. Typically, the attitudecan be estimated using body-frame measurements of at leasttwo non-collinear inertial frame vectors [6]. These body-frame vector measurements can be easily constructed fromfull landmark position measurements, see for instance [23].However, in the case of body-frame bearing measurements,the problem is quite challenging since we do not have thecorresponding inertial vectors that will allow the construc-tion of an appropriate innovation term σ R . To overcome this challenge, we consider the inertial basis vectors e i andtheir corresponding body-frame vectors R (cid:62) e i . Since R (cid:62) e i isunknown, we design the adaptive auxiliary vectors ˆ e i suchthat ˆ R (cid:62) ˆ e i tends exponentially to R (cid:62) e i . This idea is somewhatsimilar to the idea of the velocity-aided attitude estimationschemes where we introduce an auxiliary variable to overcomethe lack of the acceleration in the inertial frame [9]–[11]. Remark 3.
The projection operator π ( R c y i ) projects vectorsonto the plane orthogonal to the body-frame bearing R c y i .This operator allows to eliminate, from the projected vector,the component which is collinear to R c y i . For instance, in (13) the projection of ˆ R (cid:62) (ˆ p i − ˆ p ) − p c onto the plane orthogonalto the body-frame unit vector R c y i will boil down to theprojection of ˆ R (cid:62) (ˆ p i − ˆ p ) − p B i , since the component p B i − p c ,which is parallel to R c y i , will be eliminated. This mechanism,will allow us to generate the needed terms to put the errorinjection vector σ y in the sought-after form σ y = C ( t )˜ x asit will be shown later. The projection idea, however, is notnew; it has been used in different ways in many references,for instance, [25]–[28]. Remark 4.
Note that 3D landmark positions can be al-gebraically reconstructed from stereo-bearing measurements.The main motivation for the direct use of the stereo-bearingmeasurements in our observer is related to the robustness ofthe resulting estimation algorithm. In fact, as shown in theexperimental results (Section VII), the observers relying on3D landmark position measurements may fail in situationswhere one of the cameras of the stereo-vision system losessight of the landmarks for some period of time. However, ourobserver using direct stereo-bearing measurements handlesthis situation very well by switching to a monocular bearingconfiguration.B. Error Dynamics and Stability Analysis
Define the attitude estimation error ˜ R := R ˆ R (cid:62) , and thetranslational estimation error ˜ x := [˜ p (cid:62) , ˜ e (cid:62) , ˜ e (cid:62) , ˜ e (cid:62) , ˜ v (cid:62) ] (cid:62) ∈ R with ˜ p = R (cid:62) p − ˆ R (cid:62) ˆ p , ˜ v = R (cid:62) v − ˆ R (cid:62) ˆ v and ˜ e i = R (cid:62) e i − ˆ R (cid:62) ˆ e i , i ∈ { , , } . These geometric estimation errorsare motivated from [1], [15], [23], and are different from thestandard errors used in classical EKF-based filters [2], [3]. Theinnovation term σ R in (7) can be rewritten as σ R = k R ψ a ( M ˜ R ) + Γ( t )˜ x (15)with Γ( t ) := k R [0 , ρ e × ˆ R ( t ) , ρ e × ˆ R ( t ) , ρ e × ˆ R ( t ) , ] ∈ R × , where we made use of the facts ˆ e i = ˜ R (cid:62) e i − ˆ R ˜ e i and (cid:80) i =1 ρ i e × i ˜ R (cid:62) e i = − ψ a ( M ˜ R ) with M = (cid:80) i =1 ρ i e i e (cid:62) i =diag( ρ , ρ , ρ ) . It is not difficult to show that Γ( t ) is con-tinuous and bounded, i.e., (cid:107) Γ( t ) (cid:107) F ≤ √ k R (cid:80) i =1 ρ i := c Γ .Moreover, for any distinct non-negative scalars ρ i , i = 1 , , ,the matrix M is positive semi-definite with three distincteigenvalues. From (15), one can notice that σ R has twoterms: the first term k R ψ a ( M ˜ R ) is commonly used for theestablishment of the stability proofs of the attitude estimationsubsystem; see for instance [6], [17]. The second term depend-ing on the estimation error ˜ x is an asymptotically vanishing perturbation term as it will be shown later. In view of (3), (6)and (15), one obtains the following closed-loop system: ˙˜ R = ˜ R ( − k R ψ a ( M ˜ R ) − Γ( t )˜ x ) × (16a) ˙˜ x = A ( t )˜ x − Kσ y (16b)where A ( t ) is defined in (10), and K is designed in (8) relyingon the solution P ( t ) to the CRE (9). Note that the overallclosed-loop system (16) is nonlinear and it can be seen as acascade interconnection of a time-varying linear (LTV) systemon R and a nonlinear system evolving on SO (3) . Givencontinuous and bounded matrices A ( t ) , C ( t ) , Q ( t ) , V ( t ) , with Q ( t ) , V ( t ) being uniformly positive definite and the pair ( A ( t ) , C ( t )) uniformly observable, it follows that the solution P ( t ) to the CRE (9) is well defined on R ≥ and thereexist positive constants < p m ≤ p M < ∞ such that p m I n ≤ P ( t ) ≤ p M I n for all t ≥ [29], [30]. Theorem 1.
Consider the nonlinear system (16) with σ y = C ( t )˜ x and C ( t ) being continuous and bounded. Let As-sumption 1 hold, and suppose that the pair ( A ( t ) , C ( t )) isuniformly observable. Pick k R > and three distinct scalars ρ i ≥ , i = 1 , , . Let K be given in (8) with matrices Q ( t ) and V ( t ) in (9) being continuous, bounded and uniformlypositive definite. Then, the following statements hold: i) All solutions of the closed-loop system (16) converge tothe set of equilibria given by ( I , × ) ∪ Ψ M where Ψ M := { ( ˜ R, ˜ x ) ∈ SO (3) × R | ˜ R = R α ( π, v ) ,v ∈ E ( M ) , ˜ x = 0 × } . (17)ii) The desired equilibrium ( I , × ) is locally exponen-tially stable. iii) All the undesired equilibria in Ψ M are unstable, andthe desired equilibrium ( I , × ) is almost globallyasymptotically stable.Proof. See Appendix A.Theorem 1 provides AGAS and local exponential stabilityresults for the proposed nonlinear observer. Among the inter-esting features of our observer is the fact that ˜ x is guaranteedto converge globally exponentially to zero independently fromthe dynamics of ˜ R , as long as σ y = C ( t )˜ x and the pair ( A ( t ) , C ( t )) is uniformly observable. Note that A ( t ) in (10)is continuous and bounded as long as ω ( t ) is continuousand bounded. It is worth pointing out that AGAS is thestrongest result one can aim at with a smooth vector fieldon SO (3) × R . This is due to the topological obstructionon SO (3) which consists in the fact that no continuous time-invariant vector field on SO (3) leads to a globally asymptot-ically stable equilibrium [31]. C. Observability Analysis
In this subsection, we derive sufficient conditions for theuniform observability of the pair ( A ( t ) , C ( t )) for the previ-ously mentioned two types of vision-based measurements. Animportant technical result that will be used to carry out ouruniform observability proofs is given in the following lemma: Lemma 1.
Consider a constant matrix A ∈ R n × n and a(possibly) time-varying matrix C ( t ) ∈ R m × n such that: All eigenvalues of A are real. C ( t ) is continuous and bounded.Let N = A − S be a nilpotent matrix with index s ≤ n ,where S is a diagonalizable matrix. Let O ( t ) ∈ R r × n be a matrix composed of ( r > ) row vectors of C ( t ) , C ( t ) N, . . . , C ( t ) N s − . Suppose that there exist constantscalars δ, µ > such that (cid:90) t + δt O (cid:62) ( τ ) O ( τ ) dτ > µI n , ∀ t ≥ . (18) Then, the pair ( A, C ( t )) is uniformly observable.Proof. See Appendix B.Note that the decomposition A = S + N with a nilpotentmatrix N and a diagonalizable matrix S is known as theJordan-Chevalley decomposition. It is important to mentionthat matrices N and S are uniquely determined and commute( i.e. , SN = N S ), see [32, Theorem 1]. The main advantageof Lemma 1 is to provide a relaxed condition for the uniformobservability of the pair ( A, C ( t )) when the nilpotent part of A is non-zero ( i.e. , N (cid:54) = 0 n ). Condition (18) is equivalentto the Kalman observability if A is a nilpotent matrix and C is constant. Moreover, if A is a diagonalizable matrix( i.e. , N = 0 n ), condition (18) reduces to the persistency ofexcitation (PE) requirement on C ( t ) . Note that other uniformobservability conditions were proposed in the literature suchas [33, Lemma 3.1], which involves high-order derivatives of C ( t ) , and [27, Lemma 2.7], which is more suitable when C ( t ) is the product of a PE matrix and a constant matrixguaranteeing Kalman observability.
1) Stereo-bearing measurements:
From (4), one can rewrite σ yi in (11) in terms of the estimation errors as σ yi = (cid:88) s =1 R cs π ( y si ) R (cid:62) cs ( ˆ R (cid:62) (ˆ p i − ˆ p ) − R (cid:62) ( p i − p ))= Π i ˜ p − p i Π i ˜ e − p i Π i ˜ e − p i Π i ˜ e (19)for all i = 1 , . . . , N , where we made use of the facts Π i = (cid:80) s =1 π ( R cs y si ) , p i = (cid:80) j =1 p ij e j and π ( R cs y si )( R (cid:62) ( p i − p ) − p cs ) = 0 × . From the definition of ˜ x , one obtains σ y = C ( t )˜ x with C ( t ) defined in (12). For each i = 1 , . . . , N ,the matrix Π i is positive definite if the vectors R c y i and R c y i are non-collinear. Note that for stereo vision systems, R c y i and R c y i are naturally non-collinear since all thevisible landmarks are within the limited sensing distance inpractice. Hence, we assume that matrices Π i , i = 1 , . . . , N are uniformly positive definite. Note that the matrix C ( t ) is time-varying due to the time-varying matrices Π i ( t ) , i =1 , , . . . , N . Defining the N -by- N block diagonal matrix Θ( t ) := blkdiag(Π ( t ) , Π ( t ) , . . . , Π N ( t )) , one has C ( t ) =Θ( t ) ¯ C with ¯ C = I − p I − p I − p I I − p I − p I − p I ... ... ... ... I − p N I − p N I − p N I . (20) It is easy to show that C ( t ) is continuous and boundedsince (cid:107) Θ( t ) (cid:107) F ≤ (cid:80) Ni =1 (cid:107) Π i (cid:107) F and (cid:107) Π i (cid:107) F , i = 1 , . . . , N arebounded. Lemma 2.
Consider the matrices A ( t ) defined in (10) and C ( t ) defined in (12) . Suppose that there exist three non-alignedlandmarks among the N ≥ measurable landmarks, whoseplane is not parallel to the gravity vector. Then, the pair ( A ( t ) , C ( t )) is uniformly observable.Proof. See Appendix C.
Remark 5.
The sufficient observability conditions in Lemma2 using stereo bearing measurements are mildly stronger thanthose needed for the local observers in the literature [13],[14], [23] using 3D landmark position measurements. This ismainly due to the over-parameterization of our observer withthe additional auxiliary signals ˆ e i , which is the paid pricefor the almost global asymptotic stability results achieved inTheorem 1.2) Monocular-bearing measurements: From (5), one canrewrite σ yi in (13) in terms of the estimation errors as σ yi = R c π ( y i ) R (cid:62) c ( ˆ R (cid:62) (ˆ p i − ˆ p ) − p c − ( R (cid:62) ( p i − p ) − p c ))= Π i ˜ p − p i Π i ˜ e − p i Π i ˜ e − p i Π i ˜ e (21)for all i = 1 , . . . , N with Π i = π ( R c y i ) . From the definitionof ˜ x , one obtains σ y = C ( t )˜ x with C ( t ) defined in (14). Notethat the matrix C ( t ) defined in (14) is similar to the matrix C ( t ) defined in (12), and the main difference is that the matrix Π i in (14) is only guaranteed to be positive semi-definite. Onecan also show that C ( t ) = Θ( t ) ¯ C with ¯ C defined in (20)and Θ( t ) = blkdiag(Π , Π , . . . , Π N ) being continuous andbounded. Lemma 3.
Consider the matrices A ( t ) defined in (10) and C ( t ) defined in (14) . Suppose that there exist three non-alignedlandmarks, indexed by (cid:96) , (cid:96) , (cid:96) , among the N ≥ measurablelandmarks, whose plane is not parallel to the gravity vector,and one of following statements holds: i) The camera is in motion with bounded velocity and thereexists a constant (cid:15) > such that for any time t ∗ ≥ andlandmark i ∈ { (cid:96) , (cid:96) , (cid:96) } , there exists some time t > t ∗ such that (cid:107) ( R ( t ) R c y i ( t )) × ( R ( t ∗ ) R c y i ( t ∗ )) (cid:107) ≥ (cid:15) . ii) The camera is motionless and the following matrix hasfull rank of
15 + N O (cid:48) = ¯ C MN × N N × N ∈ R (3 N +6) × (15+ N ) (22) where ¯ C defined in (20) , M = blkdiag( p − p (cid:48) , . . . , p N − p (cid:48) ) , N = [0 , , , , I , ] , N =[0 , g I , g I , g I , ] , and p (cid:48) = p + Rp c denoting theposition of the camera in the inertial frame.Then, the pair ( A ( t ) , C ( t )) is uniformly observable.Proof. See Appendix D.
Remark 6.
The proof of this lemma relies on the applicationof the technical Lemma 1. For the condition i), using the fact R ( t ) R c y i ( t ) = ( p i − p (cid:48) ( t )) / (cid:107) p i − p (cid:48) ( t ) (cid:107) , it follows that thecamera is not indefinitely moving in a straight line passingthrough one of landmarks (cid:96) , (cid:96) , (cid:96) . Note that this conditioninvolves an extra condition on the motion of the vehicle,with respect to Lemma 2, to generate sufficient informationfor uniform observability. Condition ii) also holds when thecamera is in motion and the minimum number of requiredlandmarks for the matrix O (cid:48) to have a full rank of N + 15 is N = 5 . Proposition 1.
Consider the case where the camera is motion-less with N ≥ measurable non-aligned landmarks. Supposethat none of the following scenarios hold: (a) All the landmarks are located in the same plane. (b)
There exist three non-aligned landmarks and the rest oflandmarks are located in the plane parallel to the gravityvector and contains two of these three landmarks. (c)
There exist three non-aligned landmarks and the restof the landmarks are aligned with one of these threelandmarks and the position of the camera. (d)
There exist three non-aligned landmarks and the rest ofthe landmarks are either located in the plane parallel tothe gravity vector and contains two of these three land-marks, or aligned with the third of these three landmarksand the position of the camera.Then, the matrix O (cid:48) defined in (22) has full rank.Proof. See Appendix EAll the four cases stated in Proposition 1 are summarizedin Fig 3. Note that the cases (a) and (b) are independentfrom the location of the camera. According to the Lemma3, if the matrix O (cid:48) has full rank, one can conclude that thepair ( A ( t ) , C ) is uniformly observable. Note that the stateestimation in this static case is also known as the staticPerspective-n-Point (PnP) problem [16], [34]. In this scenario,one aims at determining the pose (position and orientation)of a camera given its intrinsic parameters and a set of N correspondences between 3D points and their 2D projections.V. H YBRID O BSERVER U SING I NTERMITTENT V ISION -B ASED M EASUREMENTS
In practical applications, the IMU measurements can beobtained at a high rate, while the vision-based measurementsare often obtained at a much lower rate due to the hardwaredesign of the vision sensors and the heavy image processingcomputations. Hence, the IMU measurements can be assumedas continuous and the measurements from the vision systemsare sampled intermittently. This motivates us to redesign theproposed continuous nonlinear observer in terms of continuousIMU and intermittent vision-based measurements.
Assumption 2.
We assume that the vision-based mea-surements are available at strictly increasing time instants { t k } k ∈ N > , and there exist constants < T m ≤ T M < ∞ such that t < T M and T m ≤ t k +1 − t k ≤ T M for all k ∈ N > . This assumption implies that the time difference betweentwo consecutive vision-based measurements are lower and (a) (b)(c) (d)
Fig. 3: The four possible cases of Proposition 1 where thematrix O (cid:48) in (22) does not have full rank. The locations of N ≥ non-aligned landmarks are depicted by purple dots, andthe location of the motionless monocular camera is depictedby a black dot.upper bounded. The positive lower bound T m is required toavoid Zeno behaviors. Note that, if T m = T M , the vision-basedmeasurements are sampled periodically.Motivated by the work in [23] and making use of theframework of hybrid dynamical systems presented in [35],[36], we propose the following hybrid nonlinear observermodified from (6) as ˙ˆ R = ˆ R ( ω + ˆ R (cid:62) σ R ) × ˙ˆ p = σ × R ˆ p + ˆ v ˙ˆ v = σ × R ˆ v + ˆ g + ˆ Ra ˙ˆ e i = σ × R ˆ e i , i ∈ , , (cid:124) (cid:123)(cid:122) (cid:125) t ∈ [ t k − ,t k ] , k ∈ N > ˆ R + = ˆ R ˆ p + = ˆ p + ˆ RK p σ y ˆ v + = ˆ v + ˆ RK v σ y ˆ e + i = ˆ e i + ˆ RK i σ y (cid:124) (cid:123)(cid:122) (cid:125) t ∈{ t k } , k ∈ N > (23)where k R > , ˆ g = (cid:80) i =1 g i ˆ e i , σ R is given in (7), and thevector σ y is given in (11) for stereo-bearing measurementsand in (13) for monocular-bearing measurements. Let K :=[ K (cid:62) p , K (cid:62) , K (cid:62) , K (cid:62) , K (cid:62) v ] (cid:62) , which is designed as K = P C (cid:62) ( t )( C ( t ) P C (cid:62) ( t ) + Q − ( t )) − (24)where P is the solution to the following continuous-discreteRiccati equations (CDRE) ˙ P = A ( t ) P + P A ( t ) (cid:62) + V ( t ) , t ∈ [ t k , t k +1 ] (25a) P + = ( I − KC ( t )) P, t ∈ { t k } (25b)where A ( t ) is given by (10), P (0) is symmetric positivedefinite, and Q ( t ) ∈ R N × N , V ( t ) ∈ R × are continuous,bounded and uniformly positive definite. Then, as per [13],[37] and [23, Lemma 7], the solution P to (25) exists,and there exist constants < p m ≤ p M < ∞ such that p m I ≤ P ≤ p M I for all t ≥ if there exist positive constants µ ∈ R and δ ∈ N > such that W ho ( t j , t j + δ ) = (cid:80) j + δi = j Φ (cid:62) ( t i , t j ) C (cid:62) ( t i ) C ( t i )Φ( t i , t j ) > µI , ∀ j ∈ N > , where Φ( t, τ ) denotes the state transition matrix associated to A ( t ) . Note that W ho is the discrete version of W o in (2). Hence,the same conditions as in Lemma 2 and 3 can be derived forthe existence of the positive constants µ ∈ R and δ ∈ N > such that W ho ( t j , t j + δ ) > µI for all j ∈ N > .In view of (3), (7) and (23), one obtains the following hybridclosed-loop system H : ˙˜ R = ˜ R ( − k R ψ a ( M ˜ R ) − Γ( t )˜ x ) × ˙˜ x = A ( t )˜ x (cid:41) t ∈ [ t k , t k +1 ] (26a) ˜ R + = ˜ R ˜ x + = ( I − KC ( t ))˜ x (cid:27) t ∈ { t k } (26b)where the AGAS proof for the equilibrium ( I , × ) of H can be easily conducted by combing the proof of Theorem1 and [23, Theorem 9], and is therefore omitted here. Theproposed hybrid nonlinear observer for vision-aided INSs withintermittent vision-based measurements has been summarizedin Algorithm 1. Algorithm 1
Nonlinear observer for vision-aided INSs
Input:
Continuous IMU measurements ω and a , and inter-mittent visual measurements at a set of time instants { t k } k ∈ N > . Output: ˆ R ( t ) , ˆ p ( t ) and ˆ v ( t ) for all t ≥ for k ≥ do while t ∈ [ t k − , t k ] do ˙ˆ R = ˆ R ( ω + ˆ R (cid:62) σ R ) × /* σ R defined in (7) */ ˙ˆ p = σ × R ˆ p + ˆ v ˙ˆ v = σ × R ˆ v + (cid:80) i =1 g i ˆ e i + ˆ Ra ˙ˆ e i = σ × R ˆ e i /* for all i = 1 , , */ ˙ P = A ( t ) P + P A (cid:62) ( t ) + V ( t ) /* A defined in (10) and V ( t ) being uniformly positive definite */ end while Obtain the vector σ y and matrix C ( t ) from the visualmeasurements at time t k /*Using (11) and (12) for stereo-bearing measurements, or (13) and (14) for monocular-bearing measurements */ K = P C (cid:62) ( t k )( C ( t k ) P C (cid:62) ( t k ) + Q − ( t k )) − /* Q ( t ) being uniformly positive definite */ Compute matrices K p , K , K , K and K v from K /*using K = [ K (cid:62) p , K (cid:62) , K (cid:62) , K (cid:62) , K (cid:62) v ] (cid:62) */ ˆ R + = ˆ R ˆ p + = ˆ p + ˆ RK p σ y ˆ v + = ˆ v + ˆ RK v σ y ˆ e + i = ˆ e i + ˆ RK i σ y /* for all i = 1 , , */ P + = ( I − KC ) P end forRemark 7. Note that our proposed observer (23) is determin-istic and the gain matrix K is designed based on the CDRE (25) with any uniformly positive definite matrices V ( t ) and Q ( t ) . However, it is possible to relate (locally) the matrices V ( t ) and Q ( t ) to the measurements noise properties leadingto a local sub-optimal design of the observer gains in the spirit of the Kalman filter. As in [23], let ω and a be the noisymeasurements and replace ω by ω + n ω and a by a + n a in (3) with n ω and n a denoting the noise signals associatedto ω and a , respectively. In view of (3) , (23) and (26b) , thetime derivative of ˜ x can be approximated around ˜ x = 0 by ˙˜ x ≈ A ( t )˜ x + G t n x with n x = [ n (cid:62) ω , n (cid:62) a ] (cid:62) , A ( t ) defined in (10) , and G t = − (cid:20) ( ˆ R (cid:62) ˆ p ) × ( ˆ R (cid:62) ˆ e ) × ( ˆ R (cid:62) ˆ e ) × ( ˆ R (cid:62) ˆ e ) × ( ˆ R (cid:62) ˆ v ) × − I (cid:21) (cid:62) Moreover, replacing y i in (5) by y i + n y i for each bearingmeasurement with n y i denoting the noise signals. From (13) , σ y i can be rewritten in the form of σ y i = Π i ˜ p − (cid:80) j =1 Π i ˜ e j + (cid:107) p − p i (cid:107) Π i n y i , and σ y can be approximated around ˜ x = 0 by σ y ≈ C ( t )˜ x + M t n y with n y = [ n (cid:62) y , . . . , n (cid:62) y N ] (cid:62) and M t =blkdiag( (cid:107) ˆ p − ˆ p (cid:107) Π , . . . , (cid:107) ˆ p − ˆ p N (cid:107) Π N ) . Then, matrices V ( t ) and Q ( t ) can be related to the covariance matrices of themeasurements noise as follows: V ( t ) = G t Cov ( n x ) G (cid:62) t , Q − ( t ) = M t Cov( n y ) M (cid:62) t . (27) In practice, a small positive definite matrix can be added to V ( t ) and Q − ( t ) to guarantee that V ( t ) and Q − ( t ) areuniformly positive definite. VI. S
IMULATION RESULTS
In this simulation, we consider an autonomous vehiclemoving on the ‘8’-shape trajectory described by p ( t ) =2[sin( t ) , sin( t ) cos( t ) , (cid:62) (m) with the initial attitude R (0) = I and angular velocity ω ( t ) = [ − cos(2 t ) , , sin(2 t )] (cid:62) (rad/s).Let g = [0 , , − . (cid:62) (m/s ) be the gravity vector expressedin the inertial frame, and assume that there are N = 5 landmarks randomly selected. Different types of continuousvision-based measurements are generated using (4) and (5).For comparison purposes, 3D landmark position measurements y i = R (cid:62) ( p i − p ) , i = 1 , , . . . , N are considered. In this case,the vector σ y = [ σ (cid:62) y , σ (cid:62) y , . . . , σ (cid:62) yN ] (cid:62) ∈ R N used in (6) isdesigned as σ yi = ˆ R (cid:62) (ˆ p i − ˆ p ) − y i , i = 1 , , . . . , N (28)with ˆ p i = (cid:80) j =1 p ij ˆ e j . It follows that σ y = C ˜ x with a constantmatrix C = ¯ C and ¯ C defined in (20) .The same initial conditions are considered for each case as: ˆ R (0) = exp(0 . πu × ) with u ∈ S , ˆ v (0) = ˆ p (0) = 0 × , ˆ e i (0) = e i , i = 1 , , , and P (0) = I . The gain parametersfor each case are chosen as ρ = 0 . , ρ = 0 . , ρ = 0 . and k R = 1 for σ R in (7), and Q = 10 I N , V = 10 − I for the CRE (9). Simulation results are shown in Fig. 4. Asone can see, the estimated states from all the cases converge,after a few seconds, to the vicinity of the real states. Roughlyspeaking, with the same tuning parameters, the performancesof the proposed continuous observer with three types of vision-based measurements is quite similar. In practice, 3D landmark positions can be obtained, for instance, usingstereo cameras, and the sufficient condition for uniform observability of thepair ( A ( t ) , C ) with A ( t ) in (10) is the same as the one in Lemma 2.
3D positionStereo-BearingMonocular-Bearing time(s)
Fig. 4: Simulation results of the nonlinear observer (6) using3D landmark position, stereo-bearing and monocular-bearingmeasurements.VII. E
XPERIMENTAL R ESULTS
To further validate the performance of the proposed ob-server, two sets of experiments have been presented using thedata from the EuRoc dataset [24], where the trajectories aregenerated by a real flight of a quadrotor. This dataset includesstereo images, IMU measurements, and ground truth obtainedfrom Vicon motion capture system. More details about theEuRoC dataset can be found in [24]. Since the sampling rate ofthe stereo camera (20Hz) is much lower than that of the IMU(200Hz), the proposed hybrid observer (23) was implemented.The accelerometer and gyro measurements are compensatedusing the biases provided in the dataset. The features aretracked via the Kanade-Lucas-Tomasi (KLT) tracker [38],with the RANSAC outliers removal algorithm. Due to thelack of physical landmarks in this dataset, a set of ‘virtual’landmarks (in the inertial frame) are generated as [23]. TheIMU measurements are not continuous although obtained ata high rate. The same numerical integration methods in [23]for the estimated states are considered. The monocular-bearingmeasurements are obtained from the right camera.For comparison purposes, the IEKF developed in [13] using3D landmark position measurements has been considered. Allthe observers are concurrently executed using the same set oflandmarks and visual measurements. The initial conditions forthe estimated states are given as ˆ R (0) = exp(0 . πu × ) with u ∈ S , ˆ p (0) = ˆ v (0) = 0 × and ˆ e i (0) = e i , i ∈ { , , } and P (0) = I . The scalar gain parameters are chosen as k R = 20 , ρ = 0 . , ρ = 0 . and ρ = 0 . . For theCDRE (25), matrices V ( t ) and Q ( t ) are tuned using (27) (withan additional small identity matrix . I ) with Cov( n x ) =blkdiag(0 . I , . I ) , Cov( n y ) = 0 . I N for bear-ing measurements and Cov( n y ) = 0 . I N for 3D positionmeasurements. For the IEKF, the gain matrices are tuned asper Section V.B in [13] using the same above mentionedcovariance of the measurements noise. The results of the first experiment are shown in Fig. 5. The estimates, providedby the proposed observer and the IEKF converge, after afew seconds, to the vicinity of the ground truth with anice performance in terms of noise attenuation. The averagedposition estimation errors after sec ( i.e., at steady state) areas follows: . cm for the proposed observer with 3D positionmeasurements, . cm for the proposed observer with stereo-bearing measurements, . cm for the proposed observerwith monocular-bearing measurements and . cm for theIEKF with 3D position measurements. The proposed observerusing monocular-bearing measurements comes with the largestposition estimation error, which is mainly due to the fact thatit takes less measurement information and requires strongerconditions (on the motion of the camera and the location of thelandmarks) for uniform observability than the other settings.In the second experiment, we consider the scenario wherethe measurements of the left camera are not available after sec . In this situation, after sec , the 3D landmark posi-tions can not be constructed from a single camera, while thestereo-bearing measurements become the monocular-bearingmeasurements. The results of the second experiment are shownin Fig. 6. As one can see, the estimates, provided by theproposed observer using 3D landmark position measurementsand IEKF, diverge after sec ; while the estimates, providedby the proposed observer using stereo-bearing measurements,stay in the vicinity of the ground truth.VIII. C ONCLUSION
We addressed the problem of simultaneous estimation of theattitude, position and linear velocity for vision-aided INSs.An AGAS nonlinear observer on SO (3) × R has beenproposed using body-frame acceleration and angular velocitymeasurements, as well as body-frame stereo (or monocular)bearing measurements of some known landmarks in the inertialframe. A detailed uniform observability analysis has beencarried out for the monocular and stereo bearing measurementscases. In the stereo bearing measurements case, uniformobservability is guaranteed as long as there exist three non-aligned landmarks whose plane is not parallel to the gravityvector. In the monocular bearing case, on top of the conditionof the stereo bearing case, it is required that none of the body-frame bearings (of the three non-aligned landmarks whoseplane is not orthogonal to the gravity vector) maintains thesame direction indefinitely. In the case of a monocular bearing,with a motionless camera, which is known as the static PnPproblem, our observer provides a viable solution as longas we have five landmarks that are not in one of the fourconfigurations shown in Fig. 3.For practical implementation purposes, we proposed a hy-brid version of our nonlinear observer to handle the casewhere the IMU measurements are continuous and the bearingmeasurements are intermittently sampled. This observer hasbeen validated using the EuRoc dataset experimental data ofa real quadrotor flight. To illustrate the benefit of using bear-ing measurements over landmark position measurements, weimplemented the bearing-based and landmark-position-basedobservers in a scenario where one of the cameras loses sight x(m) y(m)
02 21 z ( m ) True trajectory3D positionStereo-BearingMonocular-BearingIEKF
3D positionStereo-BearingMonocular-BearingIEKF time(s)
Fig. 5: Experimental results using Vicon Room 1 01 of theEuRoc dataset [24].of the landmarks after some time, and the results are shown inFig. 6. As a future work, we intend to enhance our proposedobserver with the estimation of the accelerometer and angularvelocity biases while preserving the AGAS property for theoverall closed-loop system.A
PPENDIX
A. Proof of Theorem 1
Before proceeding with the proof of Theorem 1, some usefulproperties on SO (3) are given in the following lemma, whoseproof can be found in [17], [39] Lemma 4.
Consider the trajectory ˙ R = Rω × with R (0) ∈ SO (3) and ω ∈ R . Let L M ( R ) = tr(( I − R ) M ) be thethe potential function on SO (3) with M = M (cid:62) a positivesemi-definite matrix. Then, for all x, y ∈ R the followingproperties hold: λ ¯ M min | R | I ≤ L M ( R ) ≤ λ ¯ M max | R | I , (29) (cid:107) ψ a ( M R ) (cid:107) = α ( M, R ) tr(( I − R ) M ) , (30) ˙ ψ a ( M R ) = E ( M R ) ω, (31) (cid:107) E ( M R ) (cid:107) F ≤ (cid:107) ¯ M (cid:107) F , (32)
3D positionStereo-BearingMonocular-BearingIEKF time(s)
Fig. 6: Experimental results using Vicon Room 1 01 of theEuRoc dataset [24]. The measurements of the left camera arenot available after sec . where ¯ M := (tr( M ) I − M ) , M := tr( ¯ M ) I − M , E ( M R ) = (tr( M R ) I − R (cid:62) M ) , and the map α ( M, R ) :=(1 − | R | I cos (cid:104) u, ¯ M u (cid:105) ) with u ∈ S denoting the axis of therotation R and (cid:104)· , ·(cid:105) denoting the angle between two vectors. Consider the following real-valued function on R : L P (˜ x ) = ˜ x (cid:62) P − ˜ x, (33)where P ( t ) is solution to the CRE (9). Note that A ( t ) in (10) iscontinuous and bounded since ω ( t ) is continuous and bounded.By assumptions, it easy to conclude from [29], [30] that thereexist < p m ≤ p M < ∞ such that p m I ≤ P ( t ) ≤ p M I for all t ≥ . It follows that p M (cid:107) ˜ x (cid:107) ≤ L P (˜ x ) ≤ p m (cid:107) ˜ x (cid:107) . Inview of (16b) and (9), the time-derivative of L P is given by ˙ L P = ˜ x (cid:62) ( P − A + A (cid:62) P − − C (cid:62) QC + ˙ P − )˜ x = − ˜ x (cid:62) P − V P − ˜ x − ˜ x (cid:62) C (cid:62) QC ˜ x ≤ − v m p M (cid:107) ˜ x (cid:107) ≤ − λ L P (34)with λ = v m p m /p M , v m := inf t ≥ λ V ( t )min , where we madeuse of the facts C (cid:62) QC ≥ and ˙ P − = − P − ˙ P P − = − P − A − A (cid:62) P − + C (cid:62) QC − P − V P − . Hence, one has (cid:107) ˜ x ( t ) (cid:107) ≤ (cid:112) p M /p m exp( − λ t ) (cid:107) ˜ x (0) (cid:107) , which implies that ˜ x converges to zero exponentially, and ˜ x, ˙˜ x are bounded. Notethat the convergence of ˜ x is independent from the dynamics ofthe rotation. From (16), the equilibrium points of the systemare given as ( R ∗ , × ) with (cid:107) ψ a ( M R ∗ ) (cid:107) = 0 . Using thefacts ψ a ( M R ) = vec ◦ P a ( M R ) and P a ( M R ) = (
M R − R (cid:62) M ) / , it follows that (cid:107) ψ a ( M ˜ R ) (cid:107) = 0 implies, as shownin [6], that ˜ R ∈ { R ∈ SO (3) : R = R α ( π, v ) , v ∈ E ( M ) } .On the other hand, consider the following real-valued func- tion on SO (3) : L M ( ˜ R ) = tr(( I − ˜ R ) M ) , (35)whose time-derivative is given by ˙ L M = tr( − M ˜ R ( − k R ψ a ( M ˜ R ) − Γ( t )˜ x ) × ) ≤ − k R (cid:107) ψ R (cid:107) + 2 c Γ (cid:107) ˜ x (cid:107)(cid:107) ψ R (cid:107) (36)where ψ R := ψ a ( M ˜ R ) , and we made use of the facts tr( − Au × ) = (cid:104)(cid:104) A, u × (cid:105)(cid:105) = 2 u (cid:62) ψ a ( A ) for any A ∈ R × , u ∈ R , and (cid:107) Γ( t ) (cid:107) ≤ c Γ for all t ≥ .Consider the following Lyapunov function candidate: L ( ˜ R, ˜ x ) = L M ( ˜ R ) + κ L P (˜ x ) , (37)with some κ > . Let ζ := [ (cid:107) ψ R (cid:107) , (cid:107) ˜ x (cid:107) ] (cid:62) . From (34) and (36),the time-derivative of L is given by ˙ L ≤ − k R (cid:107) ψ R (cid:107) + 2 c Γ (cid:107) ˜ x (cid:107)(cid:107) ψ R (cid:107) − κ v m p M (cid:107) ˜ x (cid:107) = − ζ (cid:62) Hζ, H := (cid:20) k R − c Γ − c Γ κv m p M (cid:21) . (38)Choosing κ > c p M / (2 k R v m ) such that matrix H is positivedefinite, it follows that ˙ L ≤ and L is non-increasing.Then, making use the facts L ( ˜ R ( t ) , ˜ x ( t )) − L ( ˜ R (0) , ˜ x (0)) = (cid:82) t ˙ L ( ˜ R ( τ ) , ˜ x ( τ )) dτ ≤ − (cid:82) t ζ (cid:62) ( τ ) Hζ ( τ ) dτ , one verifies that lim t →∞ (cid:82) t ζ (cid:62) ( τ ) Hζ ( τ ) dτ exists and is finite. Since ˜ x isbounded and matrices A ( t ) , C ( t ) , Q ( t ) and P ( t ) are bounded,it is clear that ˙˜ x is bounded. From (29) and (30) in Lemma 4,one shows that ψ R is bounded. In view of (16) and (31)-(32), one can easily verify that ˙ ψ R is bounded. Thus, itfollows that the time-derivative of ζ (cid:62) Hζ is bounded, whichimplies the uniform continuity of ζ (cid:62) Hζ . Therefore, by virtueof Barbalat’s lemma, one has lim t →∞ ζ (cid:62) ( t ) Hζ ( t ) = 0 , i.e. , ( (cid:107) ψ R (cid:107) , (cid:107) ˜ x (cid:107) ) → (0 , as t → ∞ . This implies that, for anyinitial condition ( ˜ R (0) , ˜ x (0)) ∈ SO (3) × R , the solution ( ˜ R, ˜ x ) to (16) converges to the set ( I , × ) ∪ Ψ M , whichproves item (i).Next, let us prove the local exponential stability of theequilibrium ( I , × ) in item (ii). Let < ε R < λ ¯ M min anddefine the set U ε R = { ( R, x ) ∈ SO (3) × R : L ( R, x ) ≤ ε R } . From (37)-(38) with κ > c p M / (2 k R v m ) , for anyinitial condition ( ˜ R (0) , ˜ x (0)) ∈ U ε R , one has L M ( ˜ R ( t )) ≤L ( ˜ R ( t ) , ˜ x ( t )) ≤ L ( ˜ R (0) , ˜ x (0)) ≤ ε R for all t ≥ . It followsfrom (29)-(30) that | ˜ R | I ≤ ε R / λ ¯ M min < (39) (cid:37) | ˜ R | I ≤ (cid:107) ψ R (cid:107) ≤ λ W max | ˜ R | I (40)with W := (tr( M ) I − M ) = ¯ M and (cid:37) :=min ( R,x ) ∈ U εR α ( M, R ) λ W min ≥ − ε R / λ ¯ M min ) λ W min > .Let ¯ ζ := [ | ˜ R | I , (cid:107) ˜ x (cid:107) ] (cid:62) . In view of (29), (33) and (35), oneobtains α (cid:107) ¯ ζ (cid:107) ≤ L ≤ ¯ α (cid:107) ¯ ζ (cid:107) (41)where α := min { λ ¯ M min , κp M } and ¯ α := max { λ ¯ M max , κp m } .Substituting (40) into (38), one has ˙ L ≤ − k R (cid:37) | ˜ R | I + 4 (cid:113) λ W max c Γ (cid:107) ˜ x (cid:107)| R | I − κv m p M (cid:107) ˜ x (cid:107) ≤ − ¯ ζ (cid:62) ¯ H ¯ ζ, ¯ H := (cid:34) k R (cid:37) − (cid:112) λ W max c Γ − (cid:112) λ W max c Γ κv m p M (cid:35) . (42)Choosing κ > λ W max c p M / ( k R (cid:37)v m ) , one can show that bothmatrices H and ¯ H are positive definite since (cid:37) ≤ λ W min ≤ λ W max . In view of (41) and (42), one concludes (cid:107) ¯ ζ ( t ) (cid:107) ≤ (cid:112) ¯ α/α exp( − λ ¯ H min t ) (cid:107) ¯ ζ (0) (cid:107) (43)for all t ≥ , which implies that ( ˜ R, ˜ x ) convergesto ( I , × ) exponentially for any initial condition ( ˜ R (0) , ˜ x (0)) ∈ U ε R . This completes the proof of item (ii).Now, we need to show that the equilibria in Ψ M areunstable. From (34), one shows that ˜ x converges to zeroexponentially, and is independent from the dynamics of ˜ R .Then, we focus on the dynamics of (16) at (cid:107) ˜ x (cid:107) = 0 . For each v ∈ E ( M ) , let us define R ∗ v = R α ( π, v ) and the open set U δv := { ( R, x ) ∈ SO (3) × R : R = R ∗ v exp( (cid:15) × ) , (cid:107) (cid:15) (cid:107) ≤ δ, (cid:107) x (cid:107) = 0 } with δ sufficiently small. For any ( ˜ R, ˜ x ) ∈ U δv ,pick a sufficiently small (cid:15) such that ( R ∗ v ) (cid:62) ˜ R := exp( (cid:15) × ) ≈ I + (cid:15) × . Consequently, from (16a) one obtains the linearizeddynamics of (cid:15) around R ∗ v as follows: ˙ (cid:15) = − k R W v (cid:15) (44)where ψ a ( k R M R ∗ v ( I + (cid:15) × )) = k R W v (cid:15) with W v = W (cid:62) v = (tr( M R ∗ v ) I − ( M R ∗ v ) (cid:62) ) = (2 v (cid:62) M v − tr( M )) I − (2 λ Mv vv (cid:62) − M ) , and we made use of the facts M v = λ Mv v , ψ a ( M R ∗ v ) = 0 , ψ a ( M R ∗ v (cid:15) × ) = vec ◦ P a ( M R ∗ v (cid:15) × ) and P a ( M R ∗ v (cid:15) × ) = ( M R ∗ v (cid:15) × + (cid:15) × ( M R ∗ v ) (cid:62) ) = ( W v (cid:15) ) × .Since W v = W (cid:62) v and M is positive semi-definite with threedistinct eigenvalues, one verifies that − v (cid:62) W v v = − v (cid:62) M v +tr( M ) > , which implies that − W v , ∀ v ∈ E ( M ) has at leastone positive eigenvalue. Then, one can conclude that all theequilibrium points in Ψ M are unstable. Consider the subsystem ˙˜ R = ˜ R ( − k R ψ a ( M ˜ R )) × and its linearized dynamics aroundthe undesired equilibrium points R ∗ v = R α ( π, v ) , v ∈ E ( M ) in (44). For each undesired equilibrium point R ∗ v , there exist(local) stable and unstable manifolds, and the union of the sta-ble manifolds and the undesired equilibria has dimension lessthan three [32, The Stable Manifold Theorem]. Then, the set ofthe union of the stable manifolds and the undesired equilibriahas Lebesgue measure zero. It follows that the solution ˜ R ( t ) converges to I starting from all initial conditions except froma set of Lebesgue measure zero. Hence, one concludes that theequilibrium ( I , × ) of the overall system (16) is almostglobally asymptotically stable. This completes the proof. B. Proof of Lemma 1
The proof of this Lemma is motivated from [33, Lemma3.1] and [27, Lemma 2.7]. In order to show that the pair ( A, C ( t )) is uniformly observable, we are going to verify theexistence of constants δ, µ > such that z (cid:62) W o ( t, t + δ ) z = δ (cid:82) t + δt (cid:107) C ( τ )Φ( τ, t ) z (cid:107) dτ ≥ µ for all t ≥ and z ∈ S n − .Let us proceed by contradiction and assume that the pair ( A, C ( t )) is not uniformly observable, i.e., ∀ ¯ δ, ¯ µ > , ∃ t ≥ , min z ∈ S n − δ (cid:90) t +¯ δt (cid:107) C ( τ )Φ( τ, t ) z (cid:107) dτ < ¯ µ. (45)Consider a sequence { µ q } q ∈ N of positive numbers con-verging to zero, and an arbitrary positive scalar ¯ δ . Then,there must exist a sequence of time instants { t q } q ∈ N anda sequence of vectors { z q } q ∈ N with z q ∈ S n − such that δ (cid:82) t q +¯ δt q (cid:107) C ( τ )Φ( τ, t q ) z q (cid:107) dτ < ¯ µ q for any q ∈ N . Since theset S n − is compact, there exists a sub-sequence of { z q } q ∈ N which converges to a limit ¯ z ∈ S n − . Moreover, since C ( t ) is bounded and the interval of integration in (45) is fixed, itfollows from (45) that lim q →∞ (cid:90) ¯ δ (cid:107) C ( t q + τ )Φ( t q + τ, t q )¯ z (cid:107) dτ = 0 (46)by a change of integration variable. Since A = S + N and SN = N S , the state transition matrix Φ( t q + τ, t q ) can be explicitly expressed as Φ( t q + τ, t q ) =exp( Aτ ) = exp( Sτ ) exp( N τ ) . Then, (46) is equivalent to lim q →∞ (cid:82) ¯ δ (cid:107) C ( t q + τ ) exp( Sτ ) exp( N τ )¯ z (cid:107) dτ = 0 , whichimplies lim q →∞ (cid:90) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) exp( Sτ ) exp( N τ )¯ z (cid:107) dτ = 0 (47)with some < δ < ¯ δ . Consider now the following technicalresults whose proofs are given after this proof. Lemma 5.
From (47) with ¯ δ large enough, one has lim q →∞ (cid:90) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) exp( N τ )¯ z (cid:107) dτ = 0 . (48) Lemma 6.
From (48) with ¯ δ large enough, one has lim q →∞ (cid:90) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) N k ¯ z (cid:107) dτ = 0 (49) for all k = 0 , , . . . , s − . Using the fact that the matrix O ( t ) is composed of row vec-tors of C ( t ) , C ( t ) N, . . . , C ( t ) N s − , one has (cid:80) s − k =0 (cid:107) C ( t q + τ ) N k ¯ z (cid:107) ≥ (cid:107) O ( t q + τ )¯ z (cid:107) for every q ∈ N > . It follows from(48) and (49) that lim q →∞ (cid:90) t q +¯ δt q +¯ δ − δ (cid:107)O ( τ )¯ z (cid:107) dτ = lim q →∞ (cid:90) ¯ δ ¯ δ − δ (cid:107)O ( t q + τ )¯ z (cid:107) dτ ≤ lim q →∞ (cid:90) ¯ δ ¯ δ − δ (cid:88) s − k =0 (cid:107) C ( t q + τ ) N k ¯ z (cid:107) dτ = 0 . (50)From (18), one can show that (cid:82) t q +¯ δt q +¯ δ − δ (cid:107)O ( τ )¯ z (cid:107) dτ =¯ z (cid:62) ( (cid:82) t q +¯ δt q +¯ δ − δ O (cid:62) ( τ ) O ( τ ) dτ )¯ z > µ , which contradicts (50). Itimplies that (49) is not true for all k = 0 , , . . . , s − , andin turns (46) and (47) are not true when ¯ δ is large enough.Therefore, one can always find ¯ δ large enough, such that(45) does not hold. Consequently, one concludes that the pair ( A, C ( t )) is uniformly observable. It remains to prove Lemma5 and 6.
1) Proof of Lemma 5:
We are going to show that (47)implies (48) provided that ¯ δ is large enough. Since S isdiagonalizable, from [32, Theorem 1] there exists an in-vertible matrix P such that D = diag( λ , · · · , λ n ) = P − SP with λ , · · · , λ n denoting the eigenvalues of A repeated according to their multiplicity. Then, one obtains exp( Sτ ) = P exp( Dτ ) P − . Let ¯ z (cid:48) ( τ ) = P − exp( N τ )¯ z and rewrite the eigenvalues of D as λ (cid:48) < · · · < λ (cid:48) d with d denoting the number of distinct eigenvalues. Itfollows that exp( Dτ ) P − exp( N τ )¯ z = exp( Dτ )¯ z (cid:48) ( τ ) = (cid:80) di =1 exp( λ (cid:48) i τ )¯ z (cid:48) i ( τ ) with ¯ z (cid:48) i ( τ ) ∈ R for all i = 1 , . . . , d and (cid:80) di =1 ¯ z (cid:48) i ( τ ) = ¯ z (cid:48) ( τ ) . We assume that there exist a constant (cid:15) (cid:48) > and a sub-sequence of { i } ≤ i ≤ d ⊂ N > such that lim q →∞ (cid:90) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) P ¯ z (cid:48) i ( τ ) (cid:107) dτ > (cid:15) (cid:48) . (51)Then, one can always pick the largest j from the sub-sequence. For the sake of simplicity, define ν j ( τ ) = (cid:80) ji =1 exp( λ (cid:48) i τ )¯ z (cid:48) i ( τ ) . Using the facts (cid:107) C ( t q + τ ) P ν j ( τ ) (cid:107) ≤(cid:107) C ( t q + τ ) P ν d ( τ ) (cid:107) + (cid:80) di = j +1 exp( λ i τ ) (cid:107) C ( t q + τ ) P ¯ z (cid:48) i ( τ ) (cid:107) , P ν d ( τ ) = P exp( Dτ ) P − exp( N τ )¯ z = exp( Sτ ) exp( N τ )¯ z and lim q →∞ (cid:82) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) P ¯ z (cid:48) i ( τ ) (cid:107) = 0 for all j +1 ≤ i ≤ d ,from (47) one can show that lim q →∞ (cid:90) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) P ν j ( τ ) (cid:107) dτ ≤ d (cid:88) i = j +1 exp( | λ i | ¯ δ ) lim q →∞ (cid:90) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) P ¯ z (cid:48) i ( τ ) (cid:107) dτ + lim q →∞ (cid:90) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) exp( Sτ ) exp( N τ )¯ z (cid:107) dτ = 0 . (52)Let η (cid:48) ( τ ) = (cid:80) j − i =1 exp(( λ (cid:48) i − λ (cid:48) j ) τ )¯ z (cid:48) i ( τ ) such that exp( − λ (cid:48) j τ ) ν j ( τ ) = ¯ z (cid:48) j ( τ ) + η (cid:48) ( τ ) . Using the facts (cid:80) di =1 ¯ z (cid:48) i ( τ ) = ¯ z (cid:48) ( t ) = P − exp( N τ )¯ z = P − (cid:80) s − k =0 N k ¯ z and lim t →∞ exp( − at ) t b = 0 for any a, b > , one has η (cid:48) ( τ ) → as τ → ∞ . Since C ( t ) is continuous and boundedby assumption, there exist a positive constant γ (cid:48) such that γ (cid:48) = sup t ≥ (cid:107) C ( t ) P (cid:107) . Then, from (51) one can show that exp( | λ (cid:48) j | ¯ δ ) (cid:90) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) P ν j ( τ ) (cid:107) dτ ≥ (cid:90) ¯ δ ¯ δ − δ (cid:13)(cid:13) C ( t q + τ ) P exp( − λ (cid:48) j τ ) ν j ( τ ) (cid:13)(cid:13) dτ ≥ (cid:90) ¯ δ ¯ δ − δ (cid:13)(cid:13) C ( t q + τ ) P (¯ z (cid:48) j ( τ ) + η (cid:48) ( τ )) (cid:13)(cid:13) dτ ≥ (cid:90) ¯ δ ¯ δ − δ (cid:13)(cid:13) C ( t q + τ ) P ¯ z (cid:48) j ( τ ) (cid:13)(cid:13) dτ − γ (cid:48) (cid:90) ¯ δ ¯ δ − δ (cid:107) η (cid:48) ( τ ) (cid:107) dτ Choosing ¯ δ large enough such that sup τ ∈ [¯ δ − δ, ¯ δ ] (cid:107) η (cid:48) ( τ ) (cid:107) ≤ (cid:15) (cid:48) δγ (cid:48) , it follows that lim q →∞ (cid:82) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) P ν j ( τ ) (cid:107) dτ ≥ (cid:15) (cid:48) exp( −| λ (cid:48) j | ¯ δ ) > , which contradicts (52). It means thatthe sub-sequence satisfying (51) does not exist, i.e. , theassumption according to (51) does not hold. Therefore, oneobtains lim q →∞ (cid:82) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) P ¯ z (cid:48) i ( τ ) (cid:107) dτ = 0 for all i = 1 , , . . . , d . Using the fact exp( N τ )¯ z = P (cid:80) di =1 ¯ z (cid:48) i ( τ ) ,one can show that lim q →∞ (cid:82) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) exp( N τ )¯ z (cid:107) ≤ (cid:80) di =1 lim q →∞ (cid:82) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) P ¯ z (cid:48) i ( τ ) (cid:107) = 0 , which gives (48).
2) Proof of Lemma 6:
We are going to show (49) for all k = 0 , , . . . , s − from (48) with some ¯ δ large enough. Let usproceed by contradiction and assume that (49) does not holdfor any ≤ k ≤ s − , i.e. , there exist a constant (cid:15) > anda sub-sequence of { k } ≤ k ≤ s − ⊂ N such that lim q →∞ (cid:90) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) N k ¯ z (cid:107) dτ > (cid:15). (53)One can always pick the largest ¯ k such that (53) holds.Since N is a nilpotent matrix of order s ≤ n , one has exp( N τ ) = (cid:80) s − k =0 τ k k ! N k . For the sake of simplicity, define Σ k ( τ ) = (cid:80) ki =0 τ i i ! N i with k ∈ N . Using the fact (cid:107) C ( t q + τ ) Σ ¯ k ( τ )¯ z (cid:107) ≤ (cid:107) C ( t q + τ ) exp( N τ )¯ z (cid:107) + (cid:80) s − i =¯ k +1 τ i i ! (cid:107) C ( t q + τ ) N i ¯ z (cid:107) and lim q →∞ (cid:82) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) N i ¯ z (cid:107) dτ = 0 for all ¯ k + 1 ≤ i ≤ s − , from (48) one can show that lim q →∞ (cid:90) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) Σ ¯ k ( τ )¯ z (cid:107) dτ ≤ lim q →∞ (cid:90) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) exp( N τ )¯ z (cid:107) dτ + lim q →∞ (cid:88) s − i =¯ k +1 ¯ δ i i ! (cid:90) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) N i ¯ z (cid:107) dτ = 0 . (54)Let η ( τ ) = ¯ k ! τ ¯ k Σ ¯ k − ( τ )¯ z such that ¯ k ! τ ¯ k Σ ¯ k ( τ )¯ z = N ¯ k ¯ z + η ( τ ) .It is easy to show that η ( τ ) is bounded and lim t →∞ η ( τ ) = 0 .Since C ( t ) is bounded by assumption, there exist a positiveconstant γ such that γ = sup t ≥ (cid:107) C ( t ) (cid:107) . Then, one obtains ¯ k !(¯ δ − δ ) k (cid:90) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) Σ ¯ k ( τ )¯ z (cid:107) dτ ≥ (cid:90) ¯ δ ¯ δ − δ (cid:13)(cid:13)(cid:13)(cid:13) C ( t q + τ ) ¯ k ! τ ¯ k Σ ¯ k ( τ )¯ z (cid:13)(cid:13)(cid:13)(cid:13) dτ = (cid:90) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ )( N ¯ k ¯ z + η ( τ )) (cid:107) dτ ≥ (cid:90) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) N ¯ k ¯ z (cid:107) dτ − γ (cid:90) ¯ δ ¯ δ − δ (cid:107) η ( τ ) (cid:107) dτ. Choosing ¯ δ large enough such that sup τ ∈ [¯ δ − δ, ¯ δ ] (cid:107) η ( τ ) (cid:107) ≤ (cid:15) δγ , from (53) one can show that lim q →∞ (cid:82) ¯ δ ¯ δ − δ (cid:107) C ( t q + τ ) Σ ¯ k ( τ )¯ z (cid:107) dτ ≥ (cid:15) (¯ δ − δ ) ¯ k / (2¯ k !) > , which contradicts (54),which means that the sub-sequence satisfying (53) does notexist, i.e. , assumption (53) does not hold. Thus, one can showthat lim q →∞ (cid:82) δ (cid:107) C ( t q + ¯ δ − δ + τ ) N k ¯ z (cid:107) dτ = 0 for all k = 0 , , . . . , s − . Therefore, one can conclude that (49)holds for all k = 0 , , . . . , s − with some ¯ δ large enough.This completes the proof. C. Proof of Lemma 2
From the definition of A ( t ) in (10), one can rewrite A ( t ) = ¯ A + S ( t ) with a block diagonal skew symmetricmatrix S ( t ) = blkdiag( − ω × , − ω × , . . . , − ω × ) ∈ R × and a constant matrix ¯ A such that ¯ AS ( t ) = S ( t ) ¯ A . Let usintroduce the following block diagonal matrix: T ( t ) = blkdiag( R (cid:62) , R (cid:62) , . . . , R (cid:62) ) ∈ R × (55)whose dynamics are given as ˙ T ( t ) = S ( t ) T ( t ) . One can verifythat T ( t ) T (cid:62) ( t ) = I and T ( t ) ¯ A = ¯ AT ( t ) . Let ¯Φ( t, τ ) =exp( ¯ A ( t − τ )) be the state transition matrix associated to ¯ A .Using similar steps as in the proof of [15, Lemma 3], the statetransition matrix associated to A ( t ) can be written as Φ( t, τ ) = T ( t ) ¯Φ( t, τ ) T − ( τ ) , (56)with the properties: ddt Φ( t, τ ) = A ( t )Φ( t, τ ) , Φ( t, t ) = I , Φ − ( t, τ ) = Φ( τ, t ) for all t, τ ≥ , and Φ( t , t )Φ( t , t ) = Φ( t , t ) for every t , t , t ≥ .Moreover, one can rewrite C ( t ) as C ( t ) = Θ( t ) ¯ C with Θ( t ) = blkdiag(Π ( t ) , . . . , Π N ( t )) ∈ R N × N and ¯ C definedin (20). Define a new block matrix ¯ T ( t ) = blkdiag( R (cid:62) , R (cid:62) , . . . , R (cid:62) ) ∈ R N × N . (57)One can show that ¯ CT ( t ) = ¯ T ( t ) ¯ C and ¯ T (cid:62) ( t ) ¯ T ( t ) = I N .From (2) and (55)-(57), one can show that W o ( t, t + δ ) = 1 δ (cid:90) t + δt Φ (cid:62) ( τ, t ) ¯ C (cid:62) Θ (cid:62) ( τ )Θ( τ ) ¯ C Φ( τ, t ) dτ ≥ T ( t ) W (cid:48) o ( t, t + δ ) T − ( t ) , (58)where W (cid:48) o ( t, t + δ ) = δ (cid:82) t + δt ¯Φ (cid:62) ( τ, t ) ¯ C (cid:62) ¯Θ (cid:62) ( τ ) ¯Θ( τ ) ¯ C ¯Φ( τ, t ) dτ with ¯Θ( t ) = ¯ T (cid:62) ( t )Θ( t ) ¯ T ( t ) = blkdiag( ¯Π , . . . , ¯Π N ) and ¯Π i = R Π i R (cid:62) for each i = 1 , , . . . , N .Next, we are going to show that the pair ( ¯ A, ¯Θ( t ) ¯ C ) isuniformly observable. Since ¯ A is nilpotent with ¯ A = 0 ,from Lemma 1, the pair ( ¯ A, ¯Θ( t ) ¯ C ) is uniformly observableif there exist scalars δ, µ > such that condition (18)holds with O ( t ) = [( ¯Θ( t ) ¯ C ) (cid:62) , ( ¯Θ( t ) ¯ C ¯ A ) (cid:62) , ( ¯Θ( t ) ¯ C ¯ A ) (cid:62) ] (cid:62) .Since Π i is uniformly positive definite for each i =1 , , . . . , N , one obtains ¯Π i , i = 1 , , . . . , N and ¯Θ( t ) uni-formly positive definite. Then, one obtains that rank ( O ) = rank ([ ¯ C (cid:62) , ( ¯ C ¯ A ) (cid:62) , ( ¯ C ¯ A ) (cid:62) ] (cid:62) ) . Let N , N ∈ R × be thefirst three rows of ¯ C ¯ A and ¯ C ¯ A , respectively. One can showthat rank ( O ) = rank ( ¯ O ) with ¯ O := (cid:2) ¯ C (cid:62) , N (cid:62) , N (cid:62) (cid:3) (cid:62) . (59)Since there exist at least three non-aligned landmarks, for thesake of simplicity, we assume that the first three landmarksare not aligned. Define u i = [ u i , u i , u i ] (cid:62) := p − p i +1 for i = 1 , . One can easily show that u , u and g are linearlyindependent, since the plane of these three landmarks is notparallel to the gravity vector. Let N be the first nine rows of ¯ C . Applying the matrix row and column operations on matrix [ N (cid:62) , N (cid:62) , N ] (cid:62) , we obtain the following matrix: ¯ O (cid:48) = I − p I − p I − p I u I u I u I u I u I u I I g I g I g I . (60)One can show that ¯ O (cid:48) has full rank of since u , u and g are linearly independent. Then, it is straightforward to verifythat rank ( O ) = rank ( ¯ O ) ≥ rank ( ¯ O (cid:48) ) = 15 , which impliesthat the pair ( ¯ A, ¯ C ) is uniformly observable, and there existconstants δ, µ (cid:48) > such that W (cid:48) o ( t, t + δ ) ≥ µ (cid:48) I for all t ≥ . Choosing < µ < ¯ (cid:15)µ (cid:48) , from (58), it follows that W o ( t, t + δ ) ≥ T ( t ) W (cid:48) o ( t, t + δ (cid:48) ) T − ( t ) ≥ ¯ (cid:15)µ (cid:48) T ( t ) T − ( t ) ≥ µI , i.e. , the pair ( A ( t ) , C ( t )) is uniformly observable. Thiscompletes the proof. D. Proof of Lemma 3
Following similar steps as in the proof of Lemma 2, from(2) and (55)-(57), one can show that W o ( t, t + δ ) = 1 δ (cid:90) t + δt Φ( τ, t ) (cid:62) C (cid:62) ( τ ) C ( τ )Φ( τ, t ) dτ = T ( t ) W (cid:48) o ( t, t + δ ) T (cid:62) ( t ) , (61)where W (cid:48) o ( t, t + δ ) = δ (cid:82) t + δt ¯Φ (cid:62) ( τ, t ) ¯ C (cid:62) ¯Θ (cid:62) ( τ ) ¯Θ( τ ) ¯ C ¯Φ( τ, t ) dτ with ¯Θ( t ) = ¯ T (cid:62) ( t )Θ( t ) ¯ T ( t ) = blkdiag( ¯Π , . . . , ¯Π N ) and ¯Π i = R Π i R (cid:62) = I − p i − p (cid:48) (cid:107) p i − p (cid:48) (cid:107) with p (cid:48) = p + Rp c for each i = 1 , , . . . , N .Next, we are going to show that the pair ( ¯ A, ¯Θ( t ) ¯ C ) isuniformly observable. Since ¯ A is nilpotent with ¯ A = 0 , fromLemma 1, the pair ( ¯ A, ¯Θ( t ) ¯ C ) is uniformly observable if thereexist scalars δ, µ > such that inequality (18) holds with O ( t ) = [( ¯Θ( t ) ¯ C ) (cid:62) , ( ¯Θ( t ) ¯ C ¯ A ) (cid:62) , ( ¯Θ( t ) ¯ C ¯ A ) (cid:62) ] (cid:62) . To verifythe above condition, we proceed by contradiction. Assume that ∀ ¯ δ, ¯ µ > , ∃ t ≥ , min z ∈ S n − δ (cid:90) t +¯ δt (cid:107)O ( τ ) z (cid:107) dτ < ¯ µ. (62)Similar to the arguments in the proof of Lemma (1), considera sequence { µ q } q ∈ N of positive numbers converging to zero,and an arbitrary positive scalar ¯ δ . Then, there must exist asequence of time instants { t q } q ∈ N and a sequence of vectors { z q } q ∈ N with z q ∈ S n − such that for any q ∈ N one has (cid:82) t q +¯ δt q (cid:107)O ( τ ) z q (cid:107) dτ < ¯ µ q . Since S n − is compact, there existsa sub-sequence of { z q } q ∈ N which converges to a limit ¯ z ∈ S n − . Therefore, since Θ( t ) is bounded, one has lim q →∞ (cid:90) ¯ δ (cid:107)O ( t q + τ )¯ z (cid:107) dτ = 0 (63)by a change of integration variable. Consider the followingtechnical result whose proof is given at the end of this proof. Lemma 7.
Let ¯ O ( t ) = [( ¯Θ( t ) ¯ C ) (cid:62) , N (cid:62) , N (cid:62) ] (cid:62) . From (63) ,one has lim q →∞ (cid:107) ¯ O ( t q + s )¯ z (cid:107) = 0 , ∀ s ∈ [0 , ¯ δ ] . (64)Next, we consider the following casesi) Camera in motion:
Let ¯ C i = [ I , p i I , p i I , p i I , ] associated to i -th landmark. Note that (cid:96) , (cid:96) , (cid:96) are not-aligned and their plane is not parallel to the gravity vector.Inequality ¯ C ¯ z (cid:54) = 0 N × implies that there exists at leastone of ¯ C i ¯ z, i = (cid:96) , (cid:96) , (cid:96) , which is different from zero.Without loss of generality, let ¯ z (cid:48)(cid:48) := ¯ C i ¯ z (cid:54) = 0 with i ∈{ (cid:96) , (cid:96) , (cid:96) } . From (64), one has lim q →∞ (cid:107) ¯Π i ( t q + s ) ¯ C i ¯ z (cid:107) = 0 , s ∈ [0 , ¯ δ ] . (65) For the sake of simplicity, let u ( t ) := R ( t ) R c y i ( t ) ∈ S .Using the facts ¯Π i = π ( u ) and (cid:107) π ( u ) y (cid:107) = y (cid:62) π ( u ) y = − y (cid:62) ( u × ) y = (cid:107) x × y (cid:107) , for any u ∈ S , y ∈ R , itfollows from (65) that lim q →∞ (cid:107) u ( t q + s ) × ¯ z (cid:48)(cid:48) (cid:107) =0 , ∀ s ∈ [0 , ¯ δ ] . This implies that for any ¯ µ (cid:48) , there exists q ∗ such that for all q ≥ q ∗ (cid:107) u ( t q + s ) × ¯ z (cid:48)(cid:48) (cid:107) < ¯ µ (cid:48) , ∀ s ∈ [0 , ¯ δ ] . (66)Motivated by the proof of [28, Lemma 4], let u = u ( t q ) and u = u ( t q + ¯ δ ) , and choose ¯ µ (cid:48) = ( (cid:15) (cid:107) ¯ z (cid:48)(cid:48) (cid:107) ) / (4 + 2 (cid:15) ) such that (cid:107) u × ¯ z (cid:48)(cid:48) (cid:107) + (cid:107) u × ¯ z (cid:48)(cid:48) (cid:107) < ( (cid:15) (cid:107) ¯ z (cid:48)(cid:48) (cid:107) ) / (2 + (cid:15) ) .The case where (cid:107) u × u (cid:107) = 0 is trivial. Let (cid:107) u × u (cid:107) (cid:54) =0 and ¯ z (cid:48)(cid:48) = α u + α u + α u × u with constants α i ∈ R , i = 1 , , . Then, one has (cid:107) u × ¯ z (cid:48)(cid:48) (cid:107) + | u × ¯ z (cid:48)(cid:48) (cid:107) =( α + α +2 α ) (cid:107) u × u (cid:107) , where we made use of the fact (cid:107) u i × ¯ z (cid:48)(cid:48) (cid:107) = α i (cid:107) u × u (cid:107) + α (cid:107) u × u (cid:107) for i = 1 , .One can also show (cid:107) ¯ z (cid:48)(cid:48) (cid:107) ≤ α +2 α + α (cid:107) u × u (cid:107) ≤ (2 + (cid:107) u × u (cid:107) )( α + α + α ) . Hence, one obtains (cid:107) u × u (cid:107) (cid:107) u × u (cid:107) ≤ (cid:107) u × ¯ z (cid:48)(cid:48) (cid:107) + | u × ¯ z (cid:48)(cid:48) (cid:107) (cid:107) ¯ z (cid:48)(cid:48) (cid:107) < (cid:15) (cid:15) . Since the function f ( x ) = x / (2 + x ) is monotonicallyincreasing ( i.e. , ∂f /∂x > ) for all x > , one obtains (cid:107) u ( t q ) × u ( t q + ¯ δ ) (cid:107) = (cid:107) u × u (cid:107) < (cid:15) . From the definitionof u ( t ) and the fact that ¯ δ can be arbitrary large, thiscontradicts item (i) of the Lemma that for any t ∗ ≥ ,there exists t > t ∗ such that (cid:107) u ( t ) × u ( t ∗ ) (cid:107) > (cid:15) .ii) Motionless Camera:
In this case the matrix ¯ O in (64)is constant. For any u ∈ R , one shows ¯Π i u = u + ( p i − p (cid:48) )¯ z (cid:48) i with (cid:107) p i − p (cid:48) (cid:107) (cid:54) = 0 and ¯ z (cid:48) i = − ( p i − p (cid:48) ) (cid:62) u/ (cid:107) p i − p (cid:48) (cid:107) ∈ R for all i = 1 , , . . . , N .Note that identity p (cid:48) = p i implies that the camera islocated at location the i -th landmark, which does not holdsince all the landmarks are measurable by assumption.Let ¯ z (cid:48) = [¯ z (cid:48) , . . . , ¯ z (cid:48) N ] (cid:62) and z (cid:48)(cid:48) := [¯ z (cid:62) , (¯ z (cid:48) ) (cid:62) ] (cid:62) , which isnon-zero since ¯ z is non-zero. Hence, identity (64) can berewritten as lim q →∞ (cid:107)O (cid:48) z (cid:48)(cid:48) (cid:107) = 0 . (67)with O (cid:48) defined in (22), which contradicts item (ii) of theLemma that O (cid:48) has full rank of
15 + N , i.e. , (cid:107)O (cid:48) z (cid:48)(cid:48) (cid:107) > , ∀ t ≥ .Therefore, the assumption (62) does not hold. Hence, forboth cases, the pair ( ¯ A, C ( t )) is uniformly observable. Itfollows from (61) that the pair ( A ( t ) , C ( t )) is also uniformlyobservable.It remains to prove Lemma 7. Since the velocity of thecamera v (cid:48) = ddt p (cid:48) ( t ) is bounded, the derivative of O ( t p + τ )¯ z is bounded for all q ∈ N , τ ∈ [0 , ¯ δ ] , and then it followsthat (cid:107)O ( t p + τ )¯ z (cid:107) is uniformly continuous. Since everyuniformly continuous function is also Cauchy-continuous, itimplies that (cid:107)O ( t p + τ )¯ z (cid:107) is a Cauchy sequence of continu-ous functions and lim q →∞ (cid:107)O ( t p + τ )¯ z (cid:107) exists. ApplyingLebesgue theorem, one has lim q →∞ (cid:82) ¯ δ (cid:107)O ( t q + τ )¯ z (cid:107) = (cid:82) ¯ δ lim q →∞ (cid:107)O ( t q + τ )¯ z (cid:107) = 0 . Again, making use of thefact that lim q →∞ (cid:107)O ( t q + τ )¯ z (cid:107) is uniformly continuous and non-negative, it follows that lim q →∞ (cid:107)O ( t q + s )¯ z (cid:107) = 0 , ∀ s ∈ [0 , ¯ δ ] . (68)From the definition of O and (68), it follows that (cid:80) i =0 lim q →∞ (cid:107) ¯Θ( t q + s ) ¯ C ¯ A i ¯ z (cid:107) = 0 , ∀ s ∈ [0 , ¯ δ ] . Now,we are going to show that ¯ C ¯ A ¯ z = ¯ C ¯ A ¯ z = 0 N × .Let ¯ z = [¯ z (cid:62) , ¯ z (cid:62) , . . . , ¯ z (cid:62) ] (cid:62) ∈ R with ¯ z i ∈ R forall i = 1 , . . . , . Making use of the fact ¯Θ( t ) ¯ C ¯ A ¯ z =¯Θ( t ) (cid:2) ( g ¯ z + g ¯ z + g ¯ z ) (cid:62) , . . . , ( g ¯ z + g ¯ z + g ¯ z ) (cid:62) (cid:3) (cid:62) ,one can show that, if g ¯ z + g ¯ z + g ¯ z (cid:54) = 0 , the onlysolution of lim q →∞ (cid:107) ¯Θ( t q + s ) ¯ C ¯ A ¯ z (cid:107) = 0 is when theconstant vector g ¯ z + g ¯ z + g ¯ z is collinear with p i − p (cid:48) ( t q + s ) for all i = 1 , . . . , N . This does no holdsince there are at least three non-aligned landmarks byassumption. Hence, identity lim q →∞ (cid:107) ¯Θ( t q + s ) ¯ C ¯ A ¯ z (cid:107) = 0 implies ¯ C ¯ A ¯ z = 0 N × . Moreover, using the fact ¯Θ( t ) ¯ C ¯ Az = (cid:2) ( ¯Π ¯ z ) (cid:62) , . . . , ( ¯Π N ¯ z ) (cid:62) (cid:3) (cid:62) , one can show that,if ¯ z (cid:54) = 0 , the only solution of lim q →∞ (cid:107) ¯Θ( t q + s ) ¯ C ¯ A ¯ z (cid:107) = 0 is when the constant vector ¯ z is collinear with p i − p (cid:48) ( t q + s ) for all i = 1 , . . . , N . This does no hold since thereare at least three non-aligned landmarks by assumption.These identities are not satisfied since there are at leastthree non-aligned landmarks by assumption. Hence, i lim q →∞ (cid:107) ¯Θ( t q + s ) ¯ C ¯ A ¯ z (cid:107) = 0 implies ¯ C ¯ A ¯ z = 0 N × .From (59) and the facts ¯ C ¯ A ¯ z = ¯ C ¯ A ¯ z = 0 N × , one has ¯ C ¯ z (cid:54) = 0 N × . Then, from the definition of ¯ O , identity (68)can be reduced to (64). This completes the proof. E. Proof of Proposition 1
Consider the matrix O (cid:48) defined in (22) with N ≥ . Sinceall the landmarks are not aligned, for the sake of simplicity,we assume that the first three landmarks are not aligned. Let u i = [ u i , u i , u i ] (cid:62) := p − p i +1 for i = 1 , . Hence, foreach landmark p i , there exist scalars α ij , j = 1 , , suchthat p i − p = (cid:80) j =1 α ij u j + α i g . Note that if all thelandmarks are located in the same plane, one has α i = 0 forall i = 4 , . . . , N . Otherwise, one can always find three non-aligned landmarks, denoted by p , p and p , whose plane isnot parallel to the gravity vector, i.e. , u , u and g are linearlyindependent. Applying similar column and row operationsas in (60) on the matrix O (cid:48) , one obtains O (cid:48)(cid:48) in (69) ( i.e. ,rank ( O (cid:48) ) = rank ( O (cid:48)(cid:48) ) ), which can be rewritten in the form ofa block upper triangular matrix as O (cid:48)(cid:48) = (cid:20) ¯ O (cid:48) ∆0 (3 N − × M (cid:48) (cid:21) , where ¯ O (cid:48) ∈ R × is equivalent to the one defined in (60).It is obvious to show that matrix O (cid:48)(cid:48) does not have full rankif all the landmarks are located in the plane parallel to thegravity vector ( i.e. , rank ( O (cid:48) ) < ).Now, we assume that all the landmarks are not located inthe same plane, and there exist three non-aligned landmarks,denoted by p , p and p , whose plane is not parallel to thegravity. It follows that matrix O (cid:48) has full rank and is invertible.Then, one can show that any non-zero column of ∆ is linearlydependent on the columns of ¯ O (cid:48) . Hence, O (cid:48)(cid:48) has full rank of N if the matrix M (cid:48) has rank of N . Since all the landmarks are measurable by assumption, one has p (cid:48) − p i (cid:54) = 0 for all i = 1 , , . . . , N and the last N − columns of M (cid:48) are linearlyindependent. Applying the column and row operations, onecan show that matrix M (cid:48) has the same rank as matrix M (cid:48)(cid:48) defined in M (cid:48)(cid:48) = α (cid:48) ˘ p α ˘ p α ˘ p p − p (cid:48) . . . × ... ... ... ... . . . ... α (cid:48) N ˘ p N α N ˘ p N α N ˘ p N × . . . p N − p (cid:48) (70)where ˘ p ij := p i − p j , ∀ i, j = 1 , . . . , N , and we made useof the facts α i g − α i ˘ p i − α i ˘ p i = (cid:80) j =1 α ij u j + α i g − ( α i + α i )˘ p i = α (cid:48) i ˘ p i with α (cid:48) i = − (1 + α i + α i ) for all i = 4 , . . . , N .(a) Consider the case where all the landmarks are locatedin the same plane. It implies that α i = 0 for all i = 4 , . . . , N . From (70), one can show that the firstthree columns of M (cid:48)(cid:48) are linearly dependent, i.e. , α (cid:48) i ˘ p i + α i ˘ p i + α i ˘ p i = α i g = 0 × for each i = 4 , . . . , N .Hence, the rank of M (cid:48)(cid:48) is less than N .(b) Consider the case where one of the first three columns iszero. – If α i = 0 for all i = 4 , . . . , N , one has ˘ p i = − α i ( p − p ) + α i g . It follows that all the land-marks are located in the plane that contains p , p and is parallel to the gravity vector. – If α i = 0 for all i = 4 , . . . , N , one has ˘ p i = − α i ( p − p ) + α i g . It follows that all the land-marks are located in the plane that contains p , p and is parallel to the gravity vector. – If α (cid:48) i = 0 ( i.e. , α i + α i = − ) for all i = 4 , . . . , N ,one has ˘ p i = − α i ( p − p ) − α i ( p − p ) + α i g ,which can be rewritten as ˘ p i = − α i ( p − p ) + α i g . It follows that all the landmarks are located inthe plane that contains p , p and is parallel to thegravity vector.Hence, the rank of M (cid:48) is less than N if landmarks p , . . . , p N are located in the same plane that containstwo of landmarks p , p , p and is parallel to the gravityvector.(c) Consider the case where landmarks p , . . . , p N arealigned with one of the landmarks p , p , p and thecamera position p (cid:48) . Without loss of generality, let p , p , . . . , p N and p (cid:48) be aligned. Then, one can show thatthe first column of the matrix M (cid:48)(cid:48) is linearly dependenton its last N − columns, which implies that the rankof M (cid:48) is less than N .(d) Consider the case where there exists an index < (cid:96) < N such that landmarks p , . . . , p (cid:96) are located in the sameplane that contains two of the landmarks p , p , p (forexample, p and p ) and is parallel to the gravity vector,and landmarks p (cid:96) +1 , . . . , p N are aligned with the thirdlandmark ( i.e. , p ) and the camera position p (cid:48) . It followsthat α i = 0 for all i = 4 , . . . , (cid:96) , p − p i and p i − p (cid:48) arecollinear for all i = (cid:96) + 1 , . . . , N , which implies that therank of M (cid:48) is less than N .If none of these cases hold, one can conclude that matrix O (cid:48) O (cid:48)(cid:48) = I − p I − p I − p I p − p (cid:48) × × × . . . × u I u I u I − u p − p (cid:48) × × . . . × u I u I u I − u × p − p (cid:48) × . . . × I × × × × . . . × g I g I g I × × × × . . . × α g α ( p − p (cid:48) ) α ( p − p (cid:48) ) p − p (cid:48) . . . × ... ... ... ... ... ... ... ... ... . . . ... α N g α N ( p − p (cid:48) ) α N ( p − p (cid:48) ) 0 × . . . p N − p (cid:48) (69)in (22) has full rank. This completes the proof.R EFERENCES[1] M. Wang and A. Tayebi, “Nonlinear observers for stereo-vision-aidedinertial navigation,” in
Proc. 58th IEEE Conference on Decision andControl , 2019, pp. 2516–2521.[2] A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint Kalmanfilter for vision-aided inertial navigation,” in
Proc. IEEE InternationalConference on Robotics and Automation . IEEE, 2007, pp. 3565–3572.[3] M. Li and A. I. Mourikis, “High-precision, consistent EKF-based visual-inertial odometry,”
The International Journal of Robotics Research ,vol. 32, no. 6, pp. 690–711, 2013.[4] R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “ORB-SLAM: aversatile and accurate monocular slam system,”
IEEE transactions onrobotics , vol. 31, no. 5, pp. 1147–1163, 2015.[5] T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocularvisual-inertial state estimator,”
IEEE Transactions on Robotics , vol. 34,no. 4, pp. 1004–1020, 2018.[6] R. Mahony, T. Hamel, and J.-M. Pflimlin, “Nonlinear complementaryfilters on the special orthogonal group,”
IEEE Transactions on automaticcontrol , vol. 53, no. 5, pp. 1203–1218, 2008.[7] S. Bonnabel, P. Martin, and P. Rouchon, “Non-linear symmetry-preserving observers on lie groups,”
IEEE Transactions on AutomaticControl , vol. 54, no. 7, pp. 1709–1713, 2009.[8] M.-D. Hua, G. Ducard, T. Hamel, R. Mahony, and K. Rudin, “Imple-mentation of a nonlinear attitude estimator for aerial robotic vehicles,”
IEEE Transactions on Control Systems Technology , vol. 22, no. 1, pp.201–213, 2013.[9] M.-D. Hua, “Attitude estimation for accelerated vehicles using GPS/INSmeasurements,”
Control Engineering Practice , vol. 18, no. 7, pp. 723–732, 2010.[10] A. Roberts and A. Tayebi, “On the attitude estimation of acceleratingrigid-bodies using GPS and IMU measurements,” in
Proc. 50th IEEEconference on decision and control and European Control conference ,2011, pp. 8088–8093.[11] S. Berkane and A. Tayebi, “Attitude and gyro bias estimation using GPSand IMU measurements,” in
Proc. 56th IEEE Conference on Decisionand Control , 2017, pp. 2402–2407.[12] T. H. Bryne, J. M. Hansen, R. H. Rogne, N. Sokolova, T. I. Fossen, andT. A. Johansen, “Nonlinear observers for integrated ins \ /gnss navigation:implementation aspects,” IEEE Control Systems Magazine , vol. 37, no. 3,pp. 59–86, 2017.[13] A. Barrau and S. Bonnabel, “The invariant extended Kalman filter asa stable observer,”
IEEE Transactions on Automatic Control , vol. 62,no. 4, pp. 1797–1812, 2017.[14] M.-D. Hua and G. Allibert, “Riccati observer design for pose, linearvelocity and gravity direction estimation using landmark position andIMU measurements,” in
Proc. IEEE Conference on Control Technologyand Applications , 2018, pp. 1313–1318.[15] M. Wang and A. Tayebi, “Hybrid nonlinear observers for inertial navi-gation using landmark measurements,”
IEEE Transactions on AutomaticControl , vol. 65, no. 12, pp. 5173–5188, 2020.[16] T. Hamel and C. Samson, “Riccati observers for the nonstationary PnPproblem,”
IEEE Transactions on Automatic Control , vol. 63, no. 3, pp.726–741, 2018.[17] S. Berkane, A. Abdessameud, and A. Tayebi, “Hybrid attitude andgyro-bias observer design on SO(3),”
IEEE Transactions on AutomaticControl , vol. 62, no. 11, pp. 6044–6050, 2017. [18] M. Wang and A. Tayebi, “Hybrid pose and velocity-bias estimation onSE(3) using inertial and landmark measurements,”
IEEE Transactionson Automatic Control , vol. 64, no. 8, pp. 3399–3406, 2019.[19] R. Hartley and A. Zisserman,
Multiple view geometry in computer vision .Cambridge University Press, 2003.[20] S. De Marco, M.-D. Hua, T. Hamel, and C. Samson, “Position, velocity,attitude and accelerometer-bias estimation from IMU and bearing mea-surements,” in
Proc. European Control Conference . IEEE, 2020, pp.1003–1008.[21] F. Ferrante, F. Gouaisbaut, R. G. Sanfelice, and S. Tarbouriech, “Stateestimation of linear systems in the presence of sporadic measurements,”
Automatica , vol. 73, pp. 101–109, 2016.[22] S. Berkane and A. Tayebi, “Attitude estimation with intermittent mea-surements,”
Automatica , vol. 105, pp. 415–421, 2019.[23] M. Wang and A. Tayebi, “Nonlinear state estimation for inertial navi-gation systems with intermittent measurements,”
Preprint submitted toAutomatica , vol. 122, p. 109244, 2020.[24] M. Burri, J. Nikolic, P. Gohl, T. Schneider, J. Rehder, S. Omari, M. W.Achtelik, and R. Siegwart, “The EuRoC micro aerial vehicle datasets,”
The International Journal of Robotics Research , vol. 35, no. 10, pp.1157–1163, 2016.[25] G. Baldwin, R. Mahony, and J. Trumpf, “A nonlinear observer for 6 dofpose estimation from inertial and bearing measurements,” in
Proc. IEEEInternational Conference on Robotics and Automation . IEEE, 2009,pp. 2237–2242.[26] P. Batista, C. Silvestre, and P. Oliveira, “Navigation systems based onmultiple bearing measurements,”
IEEE Transactions on Aerospace andElectronic Systems , vol. 51, no. 4, pp. 2887–2899, 2015.[27] T. Hamel and C. Samson, “Position estimation from direction or rangemeasurements,”
Automatica , vol. 82, pp. 137–144, 2017.[28] S. Berkane, A. Tayebi, and S. de Marco, “A nonlinear navigation ob-server using IMU and generic position information,”
Preprint submittedto Automatica , 2020, arXiv:2006.14056v1.[29] R. S. Bucy, “Global theory of the riccati equation,”
Journal of computerand system sciences , vol. 1, no. 4, pp. 349–361, 1967.[30] ——, “The riccati equation and its bounds,”
Journal of computer andsystem sciences , vol. 6, no. 4, pp. 343–353, 1972.[31] D. E. Koditschek, “Application of a new lyapunov function to globaladaptive attitude tracking,” in
Proc. 27th IEEE Conference on Decisionand Control . IEEE, 1988, pp. 63–68.[32] L. Perko,
Differential equations and dynamical systems , 3rd ed. Textsin Applied Mathematics 7, Springer-Verlag, New York. Inc. 2001, 2013,vol. 7.[33] G. G. Scandaroli, “Visuo-inertial data fusion for pose estimation andself-calibration,” Ph.D. dissertation, Universit´e Nice Sophia Antipolis,2013.[34] V. Lepetit, F. Moreno-Noguer, and P. Fua, “Epnp: An accurate O(n)solution to the PnP problem,”
International journal of computer vision ,vol. 81, no. 2, p. 155, 2009.[35] R. Goebel, R. G. Sanfelice, and A. R. Teel, “Hybrid dynamical systems,”
IEEE control systems magazine , vol. 29, no. 2, pp. 28–93, 2009.[36] ——,
Hybrid Dynamical Systems: modeling, stability, and robustness .Princeton University Press, 2012.[37] J. Deyst and C. Price, “Conditions for asymptotic stability of the discreteminimum-variance linear estimator,”
IEEE Transactions on AutomaticControl , vol. 13, pp. 702–705, 1968.[38] J. Shi and C. Tomasi, “Good features to track,” in