17 Nonlinear Observer for Attitude, Position, and Velocityfolk.ntnu.no/torarnj/K26956_C017.pdf ·...

291

17 Nonlinear Observer for Attitude, Position, and VelocityTheory and Experiments

Håvard Fjær Grip, Thor I. Fossen, Tor A. Johansen, and Ali Saberi

17.1 INTRODUCTION

Strapdown inertial navigation is a method of estimating position, velocity, and attitude (PVA) by integrating accelerometer and rate gyro measurements in a dead-reckoning fashion. Inertial navi-gation systems (INSs) can be highly accurate in the short term, but errors accumulate over time, leading to poor long-term accuracy. For this reason, INSs are often aided by absolute reference information, obtained regularly or intermittently from other sources, to help correct the INS esti-mates and determine sources of error, such as sensor bias (see, e.g., [4,6]).

CONTENTS

17.1 Introduction ........................................................................................................................ 29117.1.1 Nonlinear Observers for GNSS/INS Integration ..................................................29217.1.2 Contributions of this Chapter ................................................................................29217.1.3 Notation and Preliminaries ................................................................................... 293

17.2 Problem Formulation .......................................................................................................... 29317.2.1 Sensor Configuration ............................................................................................29417.2.2 Assumptions ..........................................................................................................294

17.3 Observer .............................................................................................................................. 29517.3.1 Attitude and Gyro Bias Estimation ....................................................................... 29517.3.2 Integration Observer .............................................................................................29617.3.3 Accelerometer Bias Estimation .............................................................................29717.3.4 Alternative Representation of Velocity and Attitude ............................................ 298

17.4 Experimental Setup ............................................................................................................29917.5 Implementation and Tuning ................................................................................................300

17.5.1 Choice of Vectors for Attitude and Gyro Bias Estimation....................................30017.5.2 Tuning ...................................................................................................................30017.5.3 Deteriorating GPS Quality .................................................................................... 301

17.6 Experimental Results .......................................................................................................... 30117.6.1 Discussion .............................................................................................................307

17.7 Concluding Remarks ..........................................................................................................308Acknowledgments ..........................................................................................................................308Appendix A ....................................................................................................................................308Appendix B ....................................................................................................................................308Bibliography .................................................................................................................................. 312

K26956_C017.indd 291 04/27/16 1:46:26 AM

292 Multisensor Attitude Estimation

Aided INS has traditionally been limited to high-cost applications, due in part to the need for accurate inertial sensors capable of coasting through long periods of time without reference infor-mation. With the advent of microelectromechanical system technology, cheap inertial sensors are appearing in an increasingly wide range of products, such as cars, mobile phones, and game con-trollers. Although the accuracy of these sensors is limited, they often appear in combination with GNSS receivers, such as GPS, which provide frequent reference information in the form of position fixes and, in many cases, Doppler-based velocity. This development holds the promise of making GNSS-aided inertial navigation widely available at low cost.

GNSS/INS integration has been studied for decades and is typically accomplished using an extended Kalman filter (EKF) (see, e.g., [7,23,24]). The EKF may not be the best choice for low-cost applications, however, due to its relatively large computational footprint. An alternative approach is to construct lower-order observers based on nonlinear stability theory.

17.1.1 NoNliNear observers for GNss/iNs iNteGratioN

A challenging part of the GNSS/INS integration task is the estimation of attitude. Nonlinear attitude estimation has received significant attention as a stand-alone problem [2,3,8,20,21,26,28], resulting in a number of different designs that rely on either an attitude measurement, or on a set of vector measurements in the body-fixed frame that can be compared with reference vectors in the naviga-tion frame. An extensive survey of attitude estimation methods is given by Crassidis et al. [5].

Vik and Fossen [28] expanded the attitude observer of Salcudean [25] to include gyro bias esti-mation, and used this observer as part of a complete GNSS/INS integration observer. Their approach assumed the availability of an independent attitude measurement, allowing the attitude observer to be implemented as a stand-alone module. In most cases, however, an attitude measurement is not available, and the attitude estimation must be combined with position and velocity estimation in a more complicated way.

Hua [17] used the vector-based attitude observer of Hamel and Mahony [14] and Mahony et al. [20] as the basis for an attitude and velocity observer that depends only on inertial measurements, GNSS velocity measurements, and magnetometer measurements. This observer exploits the fact that the vehicle’s acceleration vector in the navigation frame is implicitly available in the derivative of the GNSS velocity, and it can therefore be compared to the accelerometer measurement to help determine the attitude.

In a similar vein, GNSS/INS integration was used as an application example in the authors’ study of observer design for interconnected nonlinear and linear systems [12,13]. In this case, the attitude estimate was allowed to develop in 9 degrees of freedom (DOF), similar to the work of Batista et al. [2,3], resulting in a global exponential stability result. This approach has recently been developed into a more complete integration scheme including estimation of gyro bias [9,11].

17.1.2 CoNtributioNs of this Chapter

This chapter presents an observer for GNSS-aided inertial navigation that extends the work of Grip et al. [9] in several respects:

1. The attitude estimate is represented by a unit quaternion rather than a 9-DOF matrix. 2. The design is based on a full set of navigation equations that take the rotation and cur-

vature of the Earth, and the gravity vector’s dependence on the vehicle’s position, into account.

3. The estimation of accelerometer bias is considered in addition to gyro bias.

These improvements require new theoretical developments, which are presented in the first part of the chapter. Quaternion-based attitude estimation is achieved by drawing on the results of Hamel

AQ 1

K26956_C017.indd 292 04/27/16 1:46:26 AM

293Nonlinear Observer for Attitude, Position, and Velocity

and Mahony [14] and Mahony et al. [20], as well as the extended analysis of these results by the authors [8]. The overriding philosophy of the integration algorithm is based on the theory of Grip et al. [13] for interconnected linear and nonlinear systems; however, the use of full navigation equa-tions without simplifying assumptions alters the structure of the system in such a way that the theory can no longer be applied directly. The stability analysis is therefore adjusted by drawing on elements of high-gain observer theory.

In addition to the new theoretical results, this chapter focuses on practical implementation aspects and experimental validation. In order to evaluate the observer, a light fixed-wing aircraft has been equipped with two sets of sensors, one of lower quality that provides data to the observer, and one of higher quality that is used to generate a reference.

A conference version of this chapter was presented at the 2013 American Control Conference [10].

17.1.3 NotatioN aNd prelimiNaries

For a vector or matrix X, X′ denotes its transpose. The operator ∥⋅∥ denotes the Euclidean norm for vectors and the induced Euclidean norm for matrices. For vectors x y, ∈3, S(x) denotes the skew-symmetric matrix such that S(x)y = x × y. For a symmetric positive-definite matrix P, λmin(P), and λmax(P) denote the minimum and maximum eigenvalue, respectively. The expression [z1; …; zn] denotes the vector obtained by stacking the vectors z1, …, zn. All dynamical systems are assumed to be initialized at time t = 0. For notational simplicity, function arguments are omitted whenever possible.

A quaternion q = [sq;rq] consists of a real part sq ∈ and a vector part rq ∈3. The quaternion product between q and p is given by q ⊗ p = [sqsp − rq′rp; sqrp + sprq + rq × rp]. The conjugate of q, defined as [sq;−rq], is denoted by q*. For a vector x∈3, xdenotes the quaternion with real part zero and vector part x (i.e., x x= [ ; ]0 ).

Unless clear from the context, a superscript index is used to indicate the coordinate system in which a given vector is decomposed; thus, xa and xb refer to the same vector decomposed in the coor-dinate frames indexed by a and b, respectively. The rotation between these coordinate frames can be represented by a unit quaternion q q qa

bab

ab= [ ; ]s r , such that x q x qb

ab a

ab= ⊗ ⊗ *. The corresponding

rotation matrix R q( ) ( )ab SO∈ 3 such that x R q xb

ab a= ( ) is given as R q I S r S rq q q( ) ( ) ( )a

bab

ab

ab= + +2 2 2s .

The rate of rotation of the coordinate system indexed by b with respect to a, decomposed in c, is denoted by ωab

c .Four particular coordinate frames are used throughout the chapter, namely, the earth-cen-

tered inertial (ECI), earth-centered earth-fixed (ECEF), north-east-down (NED), and body-fixed (BODY) coordinate frames, with corresponding indices i, e, n, and b. The ECI frame is considered to be a perfectly inertial frame. For details about these coordinate frames, the reader is referred to Fossen [6].

17.2 PROBLEM FORMULATION

The goal is to estimate position, velocity, and attitude (PVA) by integrating measurements from a GNSS receiver and an IMU including a magnetometer. Because inertial sensors are typically subject to bias, the gyro bias bg

b is also estimated. Consideration of accelerometer bias is postponed until Section 17.3.3.

There are several different options for how to represent PVA. Here, the position and velocity are represented as vectors pe and ve in the ECEF coordinate system.* This representation is convenient because pe and ve are quantities typically output by a GNSS receiver. The attitude will be represented

* As is common, the velocity ve is here taken to represent the rate of change of the position pe (i.e., v pe e:= ).

K26956_C017.indd 293 04/27/16 1:46:27 AM


as a unit quaternion qbe representing the rotation from BODY to ECEF. These representations give

rise to the following dynamic equations (see, e.g., [4]):

p ve e= , (17.1)

v S v f g peiee e e e e= − + +2 ( ) ( ),ω (17.2)

q q qbe

be

ibb

iee

be= ⊗ − ⊗1

212

ω ω , (17.3)

bgb = 0, (17.4)

where:f e denotes the specific force acting on the vehicleg e (pe) denotes the plumb bob gravity vector, which is a function of the vehicle’s position

The known vector ωiee represents the Earth’s rate of rotation around the ECEF z-axis.

17.2.1 seNsor CoNfiGuratioN

The sensor configuration consists of a GNSS receiver and an IMU containing 3-DOF accelerom-eters, rate gyros, and magnetometers. This sensor configuration provides the following information:

1. A measurement p pGNSSe e= of the position.

2. A full or partial measurement v C vvGNSSe e= of the velocity, where the measurement matrix

Cv may be an empty matrix, the identity matrix, or some other matrix of appropriate dimensions.

3. A biased measurement ω ωib IMUb

ibb

gb

, = + b of the vehicle’s angular velocity relative to the ECI frame.

4. A measurement f fIMUb b= of the specific force acting on the vehicle (the additional estima-

tion of accelerometer bias is considered in Section 17.3.3). 5. A measurement m mIMU

b b= of the Earth’s magnetic field.

The Earth’s magnetic field me decomposed in ECEF is considered a known quantity.

17.2.2 assumptioNs

In order to ensure uniform observability, the following assumption is made about the normalized vectors fb:= fb /∥fb ∥ and mb:= mb /∥mb ∥.

Assumption 17.1

There exists a constant cobs > 0 such that, for all t ≥ 0, ∥fb × mb∥ ≥ cobs.

Although Assumption 17.1 is needed in the analysis, the observer equations are well-defined even when it does not hold, and brief violations are of little practical consequence. In most applications, Assumption 17.1 is reasonable when operating away from polar regions.

Knowledge of constants Mf > 0 and mf > 0 such that mf ≤ ∥fb ∥ ≤ Mf (note that ∥fb ∥ = ∥ge (pe)∥ when the vehicle is unaccelerated), and a bound Mbg > 0 such that bg

b ≤ Mbg, is also assumed. Furthermore, it is assumed that the time derivative f b of the specific force, and the angular velocity ωib

b , are uniformly bounded. Finally, the gravity ge (pe) is assumed to be given by a Lipschitz con-tinuous function of the position pe.

K26956_C017.indd 294 04/27/16 1:46:29 AM


17.3 OBSERVER

This section presents the observer algorithm and develops theoretical stability results that are proven in Appendix B. Section 17.3.1 focuses on a stand-alone attitude and gyro bias observer. In Section 17.3.2, it is shown how this observer can be integrated with position and velocity estimation using only available measurements, and semiglobal stability is proven. In Section 17.3.3, it is shown how accelerometer bias estimation can also be included, albeit at the cost of weakening the theoreti-cal stability results.

17.3.1 attitude aNd Gyro bias estimatioN

As a step toward the complete design, the subproblem of estimating the attitude qbe and gyro bias

bgb is considered first. Throughout this section it will be assumed, hypothetically, that the vec-

tor f e, which represents the specific force decomposed in ECEF, is available in addition to the measurements already mentioned. In this case, two unit length vectors m m mb

IMUb

IMUb= / and

f f fbIMUb

IMUb= / can be constructed in body coordinates and two corresponding reference vec-

tors me:= me / me and f e:= f e/ f e can be constructed in ECEF. Based on this type of vector information, Hamel and Mahony [14] and Mahony et al. [20] have presented an elegant solution that guarantees almost-global asymptotic stability for the case of constant reference vectors. The relevant stability analysis was extended by the authors to prove semiglobal stability in the case of time-varying reference vectors, by introducing a parameter projection in the bias estimate [8]. Here, the observer is modified to account for the rotation of the Earth, resulting in the following observer equations:

q q b qbe

be

ib IMUb

gb

iee

be= ⊗ − + − ⊗1

212

( ) ,,ω σ ω (17.5)

b Proj bgb

gb

= −( , ),kIσ (17.6)

where:kI > 0 is a gainσ is an injection termProj(⋅,⋅) denotes a parameter projection that restricts the parameter estimate b g

b to the compact set defined by b g

b

≤ M bg

Mbg is a positive constant chosen slightly larger than Mbg

The parameter projection is defined by Equations 17.24 and 17.25 in Appendix A with Λ = kI, M = Mbg, and M = M bg.

The injection term is defined as

σ : ( )’ ( ) ,’= × + ×k k1 2m R q m f R q fbbe e b

be e

where k1 and k2 are gains satisfying k1 ≥ kP and k2 ≥ kP for some kP > 0.To state a stability result for this observer, define the estimation errors q q q: *= ⊗b

ebe and

b b bg gb

gb

:= − , and let s and r denote the real part and the vector part of q, respectively. Note that r = 0 (equivalently | |s =1) corresponds to zero attitude error, whereas s = 0 corresponds to the maximum attitude error of 180° around some axis. It will be shown that a combined error vari-able

χ : [ ; ]= r bg converges exponentially to zero from a set of initial conditions that can be made arbitrarily large.

Let ( ) ε ε= >q s represent a set of attitude errors bounded away from 180° by a mar-gin determined by ε ∈ ( , / )0 12 . Then the following semiglobal exponential stability result can be stated.

K26956_C017.indd 295 04/27/16 1:46:30 AM


Lemma 17.1

For each ε∈( , / )0 12 , there exists a kP* > 0 such that, if kP > kP

*, then for all initial conditions such that q( ) ( )0 ∈ ε and b g

b( )0 ≤ M bg, χ χλ( ) ( )t K t≤ −e 0 , for some constants K > 0 and λ > 0.

17.3.2 iNteGratioN observer

In this section, the attitude and gyro bias observer of the previous section is used as part of a GNSS/INS integration observer based only on available measurements. It can no longer be assumed that fe is an available quantity, which means that fe cannot be used as a reference vector. To deal with this problem, an estimate f

e of fe is constructed, and the reference vector fe is replaced by

f f f

e e e: /max , = δ , where 0 < δ < mf. The new reference vector is not of unit length when f

e is

small, but it is well-defined even when fe is zero.

The attitude and gyro bias observer is now given by

.

( ) ,,q q b q

be

be

ib IMUb

gb

iee

be= ⊗ − + − ⊗1

212

ω σ ω (17.7)

b Proj bg

bgb= −( , ),kI σ (17.8)

where

σ

: ( )’ ( )’ .= × + ×k k1 2m R q m f R q fbbe e b

be e

(17.9)

The only difference with the observer (Equations 17.5 and 17.6) is that σ has been replaced by σ .The estimate f

e is generated together with the position estimate p

e and the velocity estimate v e

by the following observer:

p v K p p K v C vpp pv ve e

GNSSe e

GNSSe e= + − + −θ ( ) ( ), (17.10)

v S v f g p

K p p K vvp vv

eiee e e e e

GNSSe e

GNSS

= − + +

+ − +

2

2

( ) ( )

( ) (

ω

θ θ ee e− C vv ), (17.11)

ξ σ

θ θξ ξ

= −

+ − + −

R q S f

K p p K v C vp v v

( ) ( )

( ) (

be

IMUb

GNSSe e

GNSSe e3 2 )),

(17.12)

f R q f

e

be

IMUb= +( ) ,ξ (17.13)

where Kpp, Kpv, Kvp, Kvv, Kξp, and Kξv are gain matrices. These gain matrices are chosen freely to ensure that the matrix A − KC is Hurwitz, where

A0 I 00 0 I0 0 0

CI 0 00 C 0

KK KK KKv

pp pv

vp vv

p

=

=

=

3

33

, ,

ξ KK cξ

.

The parameter θ ≥ 1 will be used as a tuning parameter to guarantee stability.To state the main result, define the estimation errors p p p:= −e e, v v v:= −e e, and f f f:= −e e, and

the combined error vector

x p v f: [ ; ; ]= . The main semiglobal stability result can now be stated as follows.

K26956_C017.indd 296 04/27/16 1:46:33 AM


Theorem 17.1

Let ∈9 be an arbitrary compact set containing the origin, and let ε ∈( , / )0 12 be an arbitrary con-stant. Let kP be chosen to ensure stability according to Lemma 17.1 with respect to some ε ε< . There exists a θ* ≥ 1 such that, if θ ≥ θ*, then for all initial conditions such that ( ( ) ( ) ( ))

p v0 0 0× × ∈ξ , q( ) ( )0 ∈ ε , and b g

b( )0 ≤ M bg,

x x( ) ( ) ( ) ( ) ,t t K t2 2 2 20 0+ ≤ +−χ χλe

for some K > 0 and λ > 0.

17.3.3 aCCelerometer bias estimatioN

In this section, the additional estimation of accelerometer bias is considered. The accelerometer measurement fIMU

b is therefore written as f f bIMUb b

ab= + , where ba

b is a constant bias. The starting point is the accelerometer bias estimation algorithm of Grip et al. [8], which estimates an overpa-rameterized vector w b= [ ; ]p a

b , where p := bab 2

. The underlying principle is to compare the norm of f e with the norm of fIMU

b . These two norms should be identical in the case of zero bias. The same principle has previously been used by various authors to estimate magnetometer bias (see [1]).

The estimator is based on the following persistency-of-excitation (PE) condition on the regressor vector φ : [ ; ]= −1 2fIMU

b .

Assumption 17.2

There exist constants ε > 0 and T > 0 such that, for all t ≥ 0, φ τ φ τ τt

t T+

∫ ′ ≥( ) ( ) d I.

Here, it is also assumed that a bound Mw on the magnitude of w is known. A parameter projection is used to restrict the estimate w b

= [ ; ]p ab to the compact set w ≥ Mw, where Mw is chosen slightly

larger than Mw.In the hypothetical case that fe is available directly, the estimator can then be written as

.

( , ( ),w Proj w w = − ′Γφ φy

where y := −f feIMUb2 2

, and where Proj(∙,∙) is defined by Equations 17.24 and 17.25 in Appendix A with Λ = Γ, M = Mw, and M M

= w.To implement the accelerometer bias estimator without direct access to fe, the estimate fe is

again utilized. Specifically, y is replaced with y M

: max , = −f ffe

IMUb2 2 2

. Furthermore, fIMUb in

Equations 17.10 through 17.13 is replaced with f bIMUb

ab− , and fb in Equation 17.9 is replaced with

f f b f b

bIMUb

ab

IMUb

ab: ( )/(max , )= − − δ . Finally, the dynamic equation (17.12) of ξ is modified by

adding the term R q b( ) be

ab to the right-hand side. The motivation for this approach follows from the

design methodology of Grip et al. [13].The complete observer equations are then

p v K p p K v C vpp pv ve e

GNSSe e

GNSSe e= + − + −θ ( ) ( ), (17.14)

v S v f g p

K p p K vvp vv

eiee e e e e

GNSSe e

GNSS

= − + +

+ − +

2

2

( ) ( )

( ) (

ω

θ θ ee e− C vv ), (17.15)

ξ σ

φ φ

= − −

+ [ ] − ′

R q S f b

R q 0 I Proj w w

( ) ( )( )

( ) ( , (

be

IMUb

ab

be

3 Γ y

))

( ) ( ),+ − + −θ θξ ξ3 2K p p K v C vp v vGNSS

e eGNSSe e

(17.16)

K26956_C017.indd 297 04/27/16 1:46:35 AM


.

( ) ,,q q b q

b

ebe

ib IMUb

gb

iee

be = ⊗ − + − ⊗1

212

ω σ ω (17.17)

b Proj bgb

gb= −( , ),kI σ (17.18)

.

( , ( )),w Proj w w

= − ′Γφ φy (17.19)

where

σ

bbe e b

be e= × + ×k k1 2m R q m f R q f( ) ( ) ,′ ′ (17.20)

f R q f b

ebe

IMUb

ab= − +( )( ) ,ξ (17.21)

y M

= −max , ,f ffe

IMUb2 2 2

(17.22)

φ =−

1

2fIMUb (17.23)

and where m m mbIMUb

IMUb= / , me = me∕ ||me||, f f b f b

bIMUb

ab

IMUb

ab= − −( )/max , δ , and

f f f

e e e= / max , δ .A challenge when analyzing stability of the complete observer is that a nonzero accelerometer

bias might push the attitude estimate out of its region of attraction before the bias estimate has had time to converge, thus causing the attitude estimate to become stuck around a 180° error. Grip et al. [8] solved this problem by using a periodic reset of the attitude estimate. Although a similar strategy could be employed here, the problem appears to be more theoretical than practical in nature. A sim-pler, but weaker, stability result will therefore be presented, which requires the known bound on the accelerometer bias to be sufficiently small. Defining the error w w w= − , it can be stated as follows.

Theorem 17.2

Under the conditions of Theorem 17.1 there exists a constant c > 0 such that, if Mw ≤ c and w ( )0 ≤ Mw, then ∥ζ(t)∥ ≤ Ke−λt ∥ζ(0)∥ for some K > 0 and λ > 0, where ζ χ= [ ; ; ] x w .

An outline of the proof of Theorem 17.2 is given in Appendix B.

17.3.4 alterNative represeNtatioN of veloCity aNd attitude

Representing position and velocity in ECEF, and attitude as the rotation between body and ECEF, is convenient because GNSS measurements are typically given in ECEF. However, these representa-tions are not necessarily the best when it comes to using the estimates for control or for displaying them to a user. The estimates can easily be transformed to a more practical format, where position is given as latitude, longitude, and height with respect to the WGS-84 ellipsoid, velocity is represented in NED, and attitude is represented as the rotation between BODY and NED.

To carry out such a transformation, the position estimate p e is first translated into estimates µ, l , and h of latitude, longitude, and height, using the iterative procedure of Hofmann-Wellenhof et al. [15]. Based on the computed latitude and longitude, a quaternion q q q

en = ⊗µ l, where

q q

l

l

l

=

=

+cos( / )

( / )

,

cos(( / ) / )2

0

0

2

2 2

sin

µ

µ π00

2 2

0

sin(( / ) / ),

µ π +

K26956_C017.indd 298 04/27/16 1:46:37 AM


is computed, representing the rotation between the ECEF and NED coordinate systems (see [6]). This quaternion is used to transform v e into an estimate v R q v

nen e= ( ) in NED, and to compute an

attitude estimate q q q bn

en

be= ⊗ , representing the rotation between BODY and NED.

17.4 EXPERIMENTAL SETUP

In order to gather data for experimental testing, a Piper Cherokee 140 light fixed-wing aircraft, shown in Figure 17.1, has been equipped with an Xsens MTI inertial measurement unit and a MediaTek-3329 GPS module with a built-in patch antenna. The IMU is mounted on a bulkhead in the tail, away from the variable magnetic disturbances in the cockpit, and contains moderately accurate accelerometers, gyros, and magnetometers, with an in-run gyro bias stability of 20°/h (1σ). Measurements are provided at a rate of 100 Hz. The GPS module is mounted on top of the instru-ment panel, and reports position, as well as low-quality speed and heading, at a rate of 10 Hz. In the observer implementation, the speed and heading from the GPS module are discarded, and only the position is used. All the sensor readings are filtered using third-order low-pass filters with a cutoff frequency of 5 Hz, and the GPS readings are converted to 100 Hz before being provided to the observer. The magnetometer has been pre-calibrated to account for soft- and hard-iron distur-bances. The magnetometer measurements are also filtered with a fourth-order notch filter to remove the predominant component of a square-wave disturbance with period 1.2 s due to the aircraft’s anti-collision light.

A reference for the observer estimates is provided by an EKF that takes its measurements from a set of higher-quality sensors, specifically, an Analog Devices Inc.’s ADIS16488 IMU, mounted within the cockpit behind the pilot’s seat, and a uBlox LEA-6H GNSS receiver with an active antenna mounted on the instrument panel. The ADIS16488 contains relatively accurate accelerome-ters, gyros, and magnetometers, with an in-run gyro bias stability of 6.25°/h (1σ). Measurements are gathered at a rate of 410 Hz. The uBlox GNSS receiver provides position and high-quality Doppler-based velocity at a rate of 5 Hz. Accelerometer, gyro, and GNSS position and velocity readings are provided to the EKF after filtering with a 5-Hz, third-order low-pass filter and conversion to 100 Hz. Magnetometer readings are provided by the Xsens MTI instead of the ADIS16488, due to variable magnetic disturbances in the cockpit. The EKF estimates PVA, as well as gyro and accelerometer bias. It is implemented as an indirect filter with resetting, but with both prediction and update taking place at 100 Hz. The attitude is represented by a quaternion in the navigation equations, whereas the

FIGURE 17.1 Aircraft with IMUs and GNSS receivers.

K26956_C017.indd 299 04/27/16 1:46:37 AM


attitude error in the EKF is represented using a 3-DOF Gibbs vector. The attitude quaternion is reset in the style of a multiplicative EKF (see, e.g., [22]).

17.5 IMPLEMENTATION AND TUNING

The observer is implemented at 100 Hz using a forward-Euler discretization. Only the position information from the GPS is used in the observer, while the speed and heading information is dis-carded. At each time step the estimates are transformed to the alternative representation discussed in Section 17.3.4. To avoid significant computational overhead, only one iteration of the algorithm of [15] is performed at each time step, and the computed latitude is carried over to the next time step.

The projection from Appendix A is implemented with the simplification c(x):= 1 (see also [19, App. E]). The bounds on bg

b and ∥w∥ are set to Mbg = 0 5. and Mw = 1.1.Because of numerical errors in the integration, the magnitude of the quaternion q b

e tends to drift away from unity over time. To compensate for this effect, a term ( ( ) )1− ′q q q b

ebe

be is added to the right-

hand side of Equation 17.17 before discretization (see [6]).The gravity vector g pe e( ) is modeled using the J2 gravity model (see [16]).The initial value for the observer’s position is taken from the GPS receiver’s initial position, and

the initial velocity is computed by transforming the GPS receiver’s initial speed and heading into ECEF velocity (with vertical velocity set to zero). The initial attitude is calculated using the QUEST algorithm (see [26]), based on the initial magnetometer and accelerometer readings, with the Earth’s magnetic field in ECEF and the gravity vector ge (pe) used as reference vectors. The other dynamic variables are given a zero initial value.

17.5.1 ChoiCe of veCtors for attitude aNd Gyro bias estimatioN

In Section 17.3, the BODY-fixed vectors used in the attitude and gyro bias estimation are taken as mb and f

b, and the corresponding reference vectors are taken as me and f

e. However, there are alter-

native ways to construct these vectors based on the same data. In the implementation, the BODY-fixed vectors are chosen as f b

and f m

b b× , and the reference vectors are chosen as fe, and f m

e e× . In this case, the vectors in each frame are perpendicular, and the practical estimation results are improved. It can be confirmed that the theoretical results still hold in this case.

17.5.2 tuNiNG

A number of gain parameters must be chosen when implementing the observer, specifically, k1, k2, kI, Kpp, Kvp, Kξp, θ, and Γ. The above analysis specifies that k1, k2, and kI should be positive; that Kpp, Kvp, and Kξp should be chosen to make A − KC Hurwitz; and that Γ should be positive definite. Moreover, in order to ensure stability, the lower bound kP on k1 and k2, as well as θ, must be consecu-tively chosen sufficiently high according to Lemma 17.1 and Theorem 17.1.

By inspecting the details of the proofs, it is possible to obtain explicit lower bounds on kP and θ. However, these bounds are likely to be conservative and lead to unnecessarily high gains, which is undesirable because it will generally lead to excessive amplification of measurement noise. For practical purposes, k1, k2, and θ should therefore be considered as tuning parameters, to be chosen along with the other gains to achieve the best performance, while keeping in mind that instability is caused by choosing these gains too low.

The basic set of gains is chosen as k1 = 1, k2 = 0.55, kI = 0.008, Kpp = 0.6I, Kvp = 0.11I, Kξp = 0.006I, θ = 2, and Γ = diag(1,1,10,0.1) ⋅ 10−4. However, during the first 60 s after initializa-tion, the gains k1 = 20, kI = 0.01, and Γ = 0 are used to increase the initial rate of convergence for the attitude and gyro bias and to prevent accelerometer bias estimation before the estimate f

e has

had time to settle.

K26956_C017.indd 300 04/27/16 1:46:38 AM


17.5.3 deterioratiNG Gps Quality

The quality of the position reported by the MediaTek GPS receiver tends to deteriorate in certain sit-uations, particularly during maneuvering flight. Typically, the reported position will drift off from the position reported by the higher-quality uBlox receiver, before recovering through an instanta-neous reset. This behavior causes peaking in the estimates v e and f

e, which can be very damaging

to the attitude estimates. The behavior is typically accompanied by an increase in the HDOP value reported by the receiver, which tends to be in the range 0.8–1.2.

A conservative approach is chosen to combat this problem, by setting θ to zero when the reported HDOP value exceeds 1.4 (so that no GPS information is used). Moreover, k1, kI, and Γ are set to zero, the second BODY-fixed vector is temporarily redefined as mb, and the corresponding reference vector is temporarily redefined as me. When the use of GPS data is restored, the position estimate is reset to the current GPS position, and the gains k1, kI, and Γ are kept at the same level for 60 s as during the first 60 s after initialization.

Although this strategy is helpful in limiting the damage caused by deterioration in the GPS qual-ity, the HDOP value is often a lagging indicator of problems with the GPS quality. For this reason, the variable ξ is also utilized as an early indicator of GPS problems. Theoretically, ξ should be zero in steady-state; if ξ grows large, it is indicative of a discrepancy between the attitude estimate and the data from the GPS. Therefore, if it is detected that ||ξ||2 ≥ 0.05, the gains k1, kI, and Γ are modi-fied in the same way as for a high HDOP value, for up to 60 s.

17.6 EXPERIMENTAL RESULTS

This section presents results from a flight lasting approximately 1 h, consisting of a takeoff, climbs and descents, cruise flight, maneuvering (e.g., stalls and steep turns), and a landing. The traffic pat-tern and landing toward the end of the flight is considered first. Figure 17.2 shows the position in the latitude–longitude plane, as well as the height above the WGS-84 ellipsoid, for both the observer estimates and the reference. The estimates are reasonable, although an offset can be seen between the observer-estimated height and the reference height. This offset is caused by an unexplained

Longitude (deg)−117.115

Latit

ude (

deg)

46.74

46.745

46.75

46.755

46.76

46.765

46.77

−117.145 −117.14 −117.135 −117.13 −117.125 −117.12(a)

FIGURE 17.2 Position during traffic pattern and landing (blue: reference; red: observer estimate). (Continued)

K26956_C017.indd 301 04/27/16 1:46:39 AM


but significant difference (up to approximately 12 m) in the height reported by the two GNSS receivers, for which the observer algorithm has no way to compensate. Figure 17.3 shows the esti-mated velocity in NED together with the reference velocity. Again, the discrepancies between the observer estimates and the reference are most prevalent in the vertical direction. Figure 17.4 shows the observer-estimated attitude converted to Euler angles, together with the reference. The attitude accuracy appears to be quite good, with errors generally smaller than 0.5° in roll, 1° in pitch, and 2° in heading.

Time (s)(a)3340

Nor

th v

eloc

ity (m

/s)

−40

−35

−30

−25

−20

−15

−10

−5

0

5

10

3140 3160 3180 3200 3220 3240 3260 3280 3300 3320

FIGURE 17.3 Velocity during traffic pattern and landing (blue: reference; red: observer estimate). (Continued)

Time (s)3340

Hei

ght (

m)

750

800

850

900

950

1000

1050

3140 3160 3180 3200 3220 3240 3260 3280 3300 3320(b)

FIGURE 17.2 (Continued) Position during traffic pattern and landing (blue: reference; red: observer estimate).

K26956_C017.indd 302 04/27/16 1:46:39 AM


Next, the accuracy of the velocity estimates for the entire flight is considered. Figure 17.5 shows the error in the velocity estimates, calculated by subtracting the observer-estimated velocity from the reference velocity. Although the error is mostly restricted to the region ± 2 m/s, there are also a few peaks. These peaks are associated with situations where the MediaTek GPS quality deterio-rates, as discussed in Section 17.5.3. Figure 17.6 shows the error in the attitude estimate, calculated in the same manner as the velocity error. It is interesting to observe that the peaks seen in the velocity error are much less evident in the attitude error. This is a result of the strategy described in Section 17.5.3, whereby the attitude estimation is based on the gyros and magnetometer alone dur-ing periods of reduced GPS quality. The roll and pitch errors are largely restricted to the region ±1°, whereas the heading has somewhat larger errors that reach almost ±5° in a couple of cases.

(b) Time (s)3340

East

velo

city

(m/s

)

−40

−30

−20

−10

0

10

20

30

3140 3160 3180 3200 3220 3240 3260 3280 3300 3320

(c) Time (s)3340

Dow

n ve

loci

ty (m

/s)

−2

−1

0

1

2

3

4

5

6

3140 3160 3180 3200 3220 3240 3260 3280 3300 3320

FIGURE 17.3 (Continued) Velocity during traffic pattern and landing (blue: reference; red: observer estimate).

K26956_C017.indd 303 04/27/16 1:46:40 AM


Figure 17.7 shows the estimated gyro bias for the entire flight, together with a reference formed by taking an average of the gyro outputs at standstill after the flight. A clear relationship between the estimated bias and the reference can be observed. On the other hand, Figure 17.8 shows the estimated accelerometer bias together with a reference formed by averaging the difference between the Xsens accelerations and the bias-corrected ASID16488 accelerations. Here, the relationship between the estimates and the reference is tenuous, and the settling time appears slow.

Time (s)3340

Roll

(deg

)

−20

−15

−10

−5

0

5

10

15

20

Time (s)

Pitc

h (d

eg)

−2

0

2

4

6

8

10

3140(a)

(b)

3160 3180 3200 3220 3240 3260 3280 3300 3320

33403140 3160 3180 3200 3220 3240 3260 3280 3300 3320

FIGURE 17.4 Attitude during traffic pattern and landing (blue: reference; red: observer estimate). (Continued)

K26956_C017.indd 304 04/27/16 1:46:40 AM


Hea

ding

(deg

)

40

60

80

100

120

140

160

180

200

220

240

(c) Time (s)33403140 3160 3180 3200 3220 3240 3260 3280 3300 3320

FIGURE 17.4 (Continued) Attitude during traffic pattern and landing (blue: reference; red: observer estimate).

Time (s)3500

Velo

city

erro

rs (m

/s)

−15

−10

−5

0

5

10

15

0 500 1000 1500 2000 2500 3000

FIGURE 17.5 Velocity error for entire flight (blue: north; green: east; red: down).

K26956_C017.indd 305 04/27/16 1:46:41 AM


Eule

r ang

le er

rors

(deg

)

−5

−4

−3

−2

−1

0

1

2

3

4

5

Time (s)35000 500 1000 1500 2000 2500 3000

FIGURE 17.6 Attitude error for entire flight (blue: roll; green: pitch; red: heading).

Gyr

o bi

as (d

eg/s

)

0

0.05

0.1

0.15

0.2

0.25

Time (s)35000 500 1000 1500 2000 2500 3000

FIGURE 17.7 Gyro bias estimate (solid) and bias computed at standstill (dashed) (blue: longitudinal axis; green: lateral axis; red: vertical axis).

K26956_C017.indd 306 04/27/16 1:46:41 AM


17.6.1 disCussioN

Some caution must be taken in evaluating the observer based on the experimental results, since the EKF estimates used for comparison do not constitute a perfect reference. The EKF estimates are nevertheless based on a separate set of higher-quality sensors; thus, some remarks about perfor-mance are warranted.

In general, the observer behaves in a manner that is consistent with the underlying theory. The attitude estimates appear to be of high quality, particularly in roll and pitch, with somewhat larger errors in the heading. The larger heading error is likely related to uncompensated magnetic distur-bances, which will tend to affect both the EKF and the observer, but in somewhat different ways. Indeed, the norm of the magnetometer measurements indicates the presence of residual soft-iron disturbances, which might be removable by improved calibration. The attitude estimates exhibit robustness against short-term deterioration in the GPS position, due in large part to the gain switch-ing described in Section 17.5.3. The same can be said for the gyro bias estimates, which are well-behaved and show good consistency with the bias calculated at standstill.

With the current tuning, the estimated position tracks the GPS position closely, except when the GPS position is not used by the observer (see Section 17.5.3). Consequently, the estimate is sensitive to GPS errors and does not offer a substantial improvement over using the GPS position directly. As has already been discussed, the velocity error is mostly restricted to ±2 m/s, but it tends to peak in situations where the GPS quality deteriorates.

As seen in Figure 17.8, the accelerometer bias estimation appears slow and inaccurate, which is potentially problematic in cases where the accelerometer biases are larger. The explanation for this behavior lies in a lack of excitation in the vector ϕ. A fixed-wing aircraft in coordinated flight will experience almost no excitation in the lateral acceleration, and only limited excitation

Acc

eler

omet

er b

ias (

m/s

2 )

−0.05

0

0.05

0.1

Time (s)3500 40000 500 1000 1500 2000 2500 3000

FIGURE 17.8 Accelerometer bias estimate (solid) and reference (blue: longitudinal axis; green: lateral axis; red: vertical axis).

K26956_C017.indd 307 04/27/16 1:46:42 AM


in the longitudinal acceleration. Thus, the poor performance of the accelerometer bias estima-tion is unsurprising; better performance should be expected in applications with higher levels of excitation.

17.7 CONCLUDING REMARKS

In this chapter, a nonlinear observer for estimating PVA, gyro bias, and accelerometer bias has been presented and tested experimentally. The experimental results confirm that the observer behaves as predicted by the theory. Moreover, the performance of the observer is promising, particularly with respect to attitude. A main target for future improvement is the estimation of accelerometer bias, particularly for low-excitation applications. Another topic of interest is estimation using low-rate position updates, in which case a discrete-time design framework might be more appropriate.

ACKNOWLEDGMENTS

The research and writing for this chapter was funded by the Research Council of Norway, and was conducted while Håvard Fjær Grip was under the employment of the Research Council of Norway, in collaboration with Thor I. Fossen and Tor A. Johansen at the Norwegian University of Science and Technology and Ali Saberi at Washington State University. The manuscript was partially pre-pared at the Jet Propulsion Laboratory, California Institute of Technology, under a contract with the National Aeronautics and Space Administration.

APPENDIX A

a.1 parameter projeCtioN

Given a gain Λ, and positive constants M and M such that M M > , the parameter projection Proj(⋅,⋅) used in this chapter is a special case of the projection from Krstić et al. [19, App. E], defined by Proj(x, y) = p(x, y)y, where

pc M

( , )( ) , ,

, ,x y

I xxx

x xx x x y

I=

−′

′′ > ′ >

ΛΛ

2 0 and

otherwise (17.24)

cM

M M( ) min , .x

x x=′ −

−

12

2 2

(17.25)

APPENDIX B

b.1 proofs

Lemma 17.1

The proof is only a slight extension of the authors’ previous work [8, Theorem 1], and only the nec-essary changes are outlined here. It is straightforward to confirm that

s = ′ +12

r R q b( )( ),be

g σ (17.26)

r I S r R q b S r= − − + +12

( ( )) ( )( ) ( ) ,s be

g ieeσ ω (17.27)

b Proj bg gb= − −( , ),kIσ (17.28)

K26956_C017.indd 308 04/27/16 1:46:42 AM


which differs from Grip et al. [8] only in the term S r( ) ωiee . The parameter projection ensures

that bg ≤ M, where M > 0 is from Grip et al. [8]. Considering the function V s s( ) = − =1 2 2r from Grip et al. [8], one has V M k c sP≤ − obs

2 α( ), where α( ) : ( ) s s s= −2 21 . Hence, for suffi-ciently large kP, | | ( ) s V M k cP= ⇒ ≤ − <ε α εobs

2 0, which implies that | |s is strictly increas-ing, so the trajectories cannot escape the region defined by | |s ≥ ε. Considering next the function W t s V s s kI( , , , ) ( ) ( ) / r b r R q b b bg b

eg g g= + ′ + ′2 2 from Grip et al. [8], one has

W f t s sW= − ′( , , , ) ( ) ( )r b S r R q bg iee

be

g2 ω , where f t sW ( , , , )

r bg is the derivative from Grip et al. [8] with ω ω= eb

b . Noting that 2 2 sω ωiee

be

g iee

g′ ≤S r R q b r b( ) ( ) and using the results from Grip et al.

[8], one can therefore write

W

k a M

M

P

≤ −

−

− + + −

′

r

bg iee

2

212

1 2

( ( ))ω ω ε

r

bg

,

where a > 0 is from Grip et al. [8] and Mω ω≥ ebb , and where ⋆ is used to indicate matrix symme-

try. Using the same techniques as in Grip et al. [8], one can now show that for sufficiently large kP, W ≤ −κ χ 2

for some κ > 0, and the result follows from the comparison lemma [18, Lemma 3.4]. ◾

Theorem 17.1

The dynamics of p and v is given by

p v K p K C vpp pv v= − −θ ,

v S v f g p g p K p K C vvp vv v= − + + − − −2 2( ) ( ) ( ) .ω θ θiee e e e e

Noting that R q R q S S R q( ) ( ) ( ) ( ) ( )be

be

ibb

iee

be= −ω ω and R q R q S b S R q( ) ( ) ( ) ( ) ( )b

ebe

ibb

g iee

be= + + −ω σ ω ,

and defining R R q R q R q: ( ) ( ) ( )*= =be

be , the dynamics of f f f R q f R q f= − = − −e e

be b

be b( ) ( ) ξ can be

calculated as

f R q f R q f R q f R q f

R q S

= + − − −

=

( ) ( ) ( ) ( )

( ) (

be b

be b

be b

be b

be

ξ

ωiibb b

iee

be b

be b

be

ibb

gb

) ( ) ( ) ( )

( ) ( )

f S R q f R q f

R q S b f

− +

− + + +

ω

ω σ

SS R q f R q f

R q S f K p K Cp v

( ) ( ) ( )

( ) ( )

ω

σ θ θξ ξ

iee

be b

be b

be b

−

+ − −3 2vv

p v v

v

d K p K C v

= − −θ θξ ξ

3 2 ,

where d I R R q S f f S I R R q f R= − ′ + − − ′ − ′( ) ( )( ( ) ) ( )( ) ( )be

ibb b b

iee

be bω ω RR q S b f( ) ( )b

eg

b . Next, let η1 := p, η θ2 : /= v , η θ3

2: /= f , and η:= [η1; η2; η3]. Then it is easy to confirm that

1

1 2θ

η η ρ η ρ χ = − + +( ) ( , ) ( , ),A KC t t (17.29)

where:ρ η θ ω η θ η1 2

210 1 2 1 0( , ) [ ; ( ) ( ) ( )( ( ) ( )); ]t = − + − −/ /ie

e e e e eS g p g pρ χ θ2

30 0 1( , ) [ ; ;( ) ]t = / d

Using the Lipschitz property of ge, it is easy to see that ρ η θ γ η1 11( , ) ( )t ≤ / for some γ1 > 0 inde-pendent of θ. Furthermore, noting that I R S r S r r− ′ = − ≤

s ( ) ( )2 2 , it is easy to confirm that ρ χ θ γ χ2

321( , ) ( )t ≤ / for some γ2 > 0 independent of θ.

K26956_C017.indd 309 04/27/16 1:46:46 AM


Let P = P′ > 0 be the solution of the Lyapunov equation P(A − KC) + (A − KC)′P = −I and define U = ′( )1/θ η ηP . Then

U t t= − + ′ +

≤ − −

+

η η ρ η ρ χ

γθ

ηγ

θη χ

21 2

1 2 2

3

2

12 2

P

P P

( ( , ) ( , ))

Using this function, the following lemma can be stated.

Lemma 17.2

For any δ > 0 and T > 0, there exists a θ1 1* ≥ such that, if θ θ≥ 1*, then for all initial conditions as

specified in Theorem 17.1, one has that for all t ≥ T, x ≤ δ.

Proof. Because the parameter projection ensures bg ≤ M and because r ≤ 1, one has χ ≤ +M 2 1. Define the level set Ωθ η δ θ λ= ≤ ( )/ minU 2 5 P , and note that η η δ θ δθ∈ ⇒ ≤ ⇒ ≤Ω / 2

x . Outside Ωθ, one has η δ θ λ λ≥ / ( ) / ( )min max

2 P P , which implies

U

M

≤ − −

− − +

12

2

2

21

1 2

2

2

32

P

PP

P

γθ

η

δ λθ λ

γθ

( )

( )min

max η

For all sufficiently large θ, the first term can be made smaller than −1 42

/ η and the second term can be made negative, thus yielding U U≤ − ≤ −1 4 4

2/ / ( )maxη θ λ P . By the comparison lemma, one therefore

has U(t) ≤ U(0)exp (−θt ∕(4λmax(P))). Let L > δ be a bound on x( )0 for any initial condition as specified in Theorem 17.1. Then L is also a bound on ∥η(0)∥, and thus U t L t( ) / ( ) ( / ( ( )))max max≤ −1 42θλ θ λP Pexp outside Ωθ. This implies that η must enter Ωθ before the time

tL

*( )

ln( )( )

( )ln( ) l

max max

min

max

=

= +

4

44

4 2

2

λθ

θ λδ λ

λθ

θ

P PP

Pnn

( )( )

.max

min

λδ λ

P LP

2

2

Noting that ln(θ)∕θ → 0 as θ →∞, it is clear that for all sufficiently large θ ≥ 1, t*≤ T. ◾Next, consider the dynamics of s, given by

s = ′ + = ′ + +12

12

1r R q b r R q b( )( ) ( )( ) ,be

g be

gσ σ µ (17.30)

where µ σ σ1 12= ′ −/ ( )( ) r R qbe has the property µ1 21 2≤ −/ k f r f fb e e

. It can then be confirmed that µ γ1 3≤

r f for an appropriate γ3 > 0 independent of θ. One also has | | / ( ) s ≤ +1 2 bg σ . Since bg ≤ M and σ is made up only of bounded signals, one can conclude that s ≤ Ms for some Ms > 0

independent of θ.With reference to Lemma 17.2, define δ α ε ε α ε γ= + − >k cP obs

232 2 0( ( / ) ( )) / ( ) and T Ms= ε / ( )2 ,

where ε ε ε:= − , and let θ be large enough that for all t ≥ T, x ≤ δ. Then

| ( ) | | ( ) | | ( ) | / ( ) / , s T s s t t M MT

s s≥ − ≥ − = +∫0 2 20

d ε ε ε ε

K26956_C017.indd 310 04/27/16 1:46:48 AM


and for all t ≥ T, µ γ γ δ α ε ε α ε1 3 32 2 2≤ ≤ ≤ + −

r f k cP obs( ( / ) ( )) / . Consider now the derivative of the function V s( ) from the proof of Lemma 17.1 for t ≥ T:

V M k c s s

M k c s

P

P

≤ − +

≤ − − + +( )

obs

obs

21

2

2

2

α µ

α α ε ε α ε

( ) | |

( ) ( / ) ( ) .

With reference to the proof of Lemma 17.1, it follows that for | | / s = +ε ε 2, V M k cP≤ − <obs2 0α ε( ) .

Thus, q cannot escape the set ( / ) ( )ε ε ε+ ⊂2 , and | |s ≥ ε is therefore assumed in the remainder of the analysis.

The dynamics of s differ from Equations 17.26 through 17.28 only by the term μ1, which can also be given the bound µ θ γ η1

23≤ r . The dynamics of r and bg is given by

r I S r R q b S r

b Proj b

= − − + + +

= −

12

2( ( )) ( )( ) ( ) ,

(

s be

g iee

g

σ ω µ

ggb, ) ,− +kIσ µ3

where µ σ σ2 1 2= − −/ ( ( )) ( )( )I S r R q s be and µ σ σ3 = − − −Proj b Proj b( , ) ( , )

gb

gbk kI I , which differs

from Equations 17.26 through 17.28 only by the terms μ2 and μ3. One has that µ γ θ γ η2 42

4≤ ≤f for some γ4 > 0 independent of θ. It can also be shown by the properties of the parameter projection that µ γ θ γ η3 5

25≤ ≤f for some γ5 > 0 independent of θ. Considering again the function W from

the proof of Lemma 17.1, one obtains

W g t s s sW= − + ′ + ′( , , , ) ( ) ( )r b r R q b R q bg be

g be

g2 2 21 1 2µ µ µ

++ ′ + ′2 3 3 skI

r R q b( ) ,be

gµ µ

where g t sW ( , , , )

r bg ≤ −κ χ 2 is the derivative of W from the proof of Lemma 17.1. Using the bounds

on μ1, μ2, and μ3, one therefore has

W

kI

≤ − + +

+ + +

κ χ θ γ η θ γ η

θ γ η θ γ η θ γ

2 23

23

24

25

25

2 2

2 2

r b

b r

g

g bbg η

κ χ γ θ χ η≤ − + 26

2 ,

for an appropriate constant γ6, independent of θ.Consider now the Lyapunov function Y U W= +1 5/θ , which has the property

β η χ β η χ12 2

22 2( ) ( )+ ≤ ≤ + Y for some β1 > 0 and β2 > 0. The derivative satisfies

Y ≤ − −

+

− +

12 2

1

1 2 2

3

5

2 63

P Pγθ

ηγ

θη χ

θκ χ γ

θχ η

K26956_C017.indd 311 04/27/16 1:46:51 AM


Assuming θ is chosen large enough that 1 − 2∥P∥γ1∕θ ≥ 1∕2, one can write

Y ≤ −

−+

−+

η χ

γ γθ

γ γθ

κθ

ηχ

12

2

22

2

2 6

3

2 6

3 5

P

P .

The first-order principal minor of the above matrix is 12 0/ > . The second-order principal minor is κ ∕(2θ5)−(2∥P∥γ2 + γ6)2∕(4θ6), which is positive if θ > (2∥P∥γ2 + γ6)2∕(2κ). Hence, Y ≤ − +β η χ3

2 2( )

for some β3 > 0, and the result of the theorem now follows from application of the comparison lemma [18, Lemma 3.4]. ◾

Theorem 17.2

The proof is based on the proof of Theorem 17.1, and is quite lengthy. Only the main steps are out-lined here.

It can be confirmed that the dynamic equations for p, v, and f are the same as before, except that d I R R q S f f S I R R q f R= − ′ + − − ′ − ′( ) ( )( ( ) ) ( )( ) ( )b

eibb b b

iee

be bω ω RR q S b f R R q S b b( ) ( ) ( ) ( )b

eg

bbe

ibb

g a − ′ + +ω

S R R q b( ) ( )ωiee

be

a ′ , and furthermore, that the dynamics of η satisfies (Equation 17.29) with ρ χ2( , )t

replaced by ρ χ θ230 0 1( , , ) [ ; ; / ]t w d= , which satisfies ρ χ θ γ χ γ2

32 21( , , ) / ( )t w w≤ + ˇ for some

constants γ2 > 0 and γ 2 < 0. It is now straightforward to check that Lemma 17.2 holds without modification.

The dynamic equation for s satisfies (Equation 17.30), except that σ σ

− = × − + − ×′ ′k k2 2f R f f f f R q fbbe e e b b

be e( ) ( ) ( ) ( )q , and hence | |µ γ γ θ γ1 3 3

23≤ + ≤ r f r bˇ a

r r wη γ+ ˇ3 for some constants γ3 > 0 and γ 3>0. One can now proceed to prove that q cannot escape the set ( / )ε ε+ 2 , by noting that for sufficiently small bounds c on Mw , ba is arbitrarily small and hence the V is perturbed by an arbitrarily small disturbance.

In a similar manner, it can be shown that the dynamic equations for r and bg are the same as before, except that μ2 and μ3 satisfy µ γ γ θ γ η γ2 4 4

24 4≤ + ≤ + f b wˇ ˇa

and µ γ γ θ γ η γ3 5 52

5 5≤ + ≤ + f b bˇ ˇa a , for some constants γ4, γ4, γ5, and γ5. Hence, W ≤ − + +κ χ γ θ χ η γ χ6

26ˇ w for some constants γ6 and γ6.

The dynamic equation for w can be written as

w Proj w w= − − ′ +( , ( )) ,Γφ φ µy 4

where µ φ φ φ φ4 = − ′ − − ′Proj w w Proj w w( , ( )) ( , ( ))

Γ Γy y satisfies µ γ θ γ η4 72

7≤ ≤f , for some constant γ7 > 0. Let V tˇ( , )w be the Lyapunov function from the proof of Grip et al. [8, Theorem 3], which satisfies [ / ] [ / ] ( , ( ))∂ ∂ − ∂ ∂ − ′ ≤ −V t V yˇ ˇ ˇ w Proj w w wΓφ φ κ

2 and [ / ]∂ ∂ ≤V w wγ8 , for some

constants κ > 0and γ8 > 0. Then V ˇ⋅≤ − +κ θ γ γ η w w

2 28 7 .

Let W = ΘV+W for some constant Θ > 0. Then W ˇ⋅

≤ − + − +Θ Θκ θ γ γ η κ χ w w2 28 7

2

γ θ χ η γ χ62

6 + ˇ w . The constant Θ can be chosen large enough that the quadratic terms in W⋅ dominate the cross term γ χˇ6 w . One then obtains W

⋅≤ − + + +κ χ γ θ χ η( ) 2 2

92 2 2w w , for

some constants κ > 0 and γ9 > 0. By defining Y U= +1 5/θ W, the proof can now be completed in the same way as the proof of Theorem 17.1. ◾

BIBLIOGRAPHY

1. R. Alonso and M. D. Shuster. Attitude-independent magnetometer bias determination: A survey. J. Astronaut. Sci., 50(4):453–475, 2002.

2. P. Batista, C. Silvestre, and P. J. Oliveira. GES attitude observers—Part I: Multiple general vector obser-vations. In Proceedings of the IFAC World Congress, pp. 2985–2990, Milan, Italy, 2011.

K26956_C017.indd 312 04/27/16 1:46:54 AM


3. P. Batista, C. Silvestre, and P. J. Oliveira. GES attitude observers—Part II: Single vector observations. In Proceedings of the IFAC World Congress, pp. 2991–2996, Milan, Italy, 2011.

4. E. Bekir. Introduction to Modern Navigation. World Scientific, Hackensack, NJ, 2007. 5. J. L. Crassidis, F. L. Markley, and Y. Cheng. Survey of nonlinear attitude estimation methods. J. Guid.

Contr. Dynam., 30(1):12–28, 2007. 6. T. I. Fossen. Handbook of Marine Craft Hydrodynamics and Motion Control. Wiley, Hoboken, NJ,

2011. 7. M. S. Grewal, L. R. Weill, and A. P. Andrews. Global Positioning Systems, Inertial Navigation, and

Integration. Wiley, New York, 2001. 8. H. F. Grip, T. I. Fossen, T. A. Johansen, and A. Saberi. Attitude estimation using biased gyro and vec-

tor measurements with time-varying reference vectors. IEEE Trans. Automat. Contr., 57(5):1332–1338, 2012.

9. H. F. Grip, T. I. Fossen, T. A. Johansen, and A. Saberi. A nonlinear observer for integration of GNSS and IMU measurements with gyro bias estimation. In Proceedings of the American Control Conference, IEEE, Montreal, Canada, 2012.

10. H. F. Grip, T. I. Fossen, T. A. Johansen, and A. Saberi. Nonlinear observer for GNSS-aided inertial navi-gation with quaternion-based attitude estimation. In Proceedings of the American Control Conference, IEEE, Washington, DC, 2013.

11. H. F. Grip, T. I. Fossen, T. A. Johansen, and A. Saberi. Globally exponentially stable attitude and gyro bias estimation with application to GNSS/INS integration. Automatica, 51:158–166, 2015.

12. H. F. Grip, A. Saberi, and T. A. Johansen. Observers for cascaded nonlinear and linear systems. In Proceedings of the IEEE Conference Decision Control, pp. 3331–3337, IEEE, Orlando, FL, 2011.

13. H. F. Grip, A. Saberi, and T. A. Johansen. Observers for interconnected nonlinear and linear systems. Automatica, 48(7):1339–1346, 2012.

14. T. Hamel and R. Mahony. Attitude estimation on SO (3) based on direct inertial measurements. In Proceedings of the IEEE International Conference Robotics Automation, pp. 2170–2175, IEEE, Orlando, FL, 2006.

15. B. Hofmann-Wellenhof, H. Lichtenegger, and J. Collins. Global Positioning System: Theory and Practice. Springer-Verlag, New York, 1994.

16. D. Y. Hsu. Comparison of four gravity models. In Proceedings Position Location and Navigation Symposium, pp. 631–635, IEEE, Atlanta, GA, 1996.

17. M.-D. Hua. Attitude estimation for accelerated vehicles using GPS/INS measurements. Contr. Eng. Pract., 18(7):723–732, 2010.

18. H. K. Khalil. Nonlinear Systems, 3rd edition. Prentice Hall, Upper Saddle River, NJ, 2002. 19. M. Krstić, I. Kanellakopoulos, and P. V. Kokotović. Nonlinear and Adaptive Control Design. Wiley,

New York, 1995. 20. R. Mahony, T. Hamel, and J.-M. Pflimlin. Nonlinear complementary filters on the Special Orthogonal

Group. IEEE Trans. Automat. Contr., 53(5):1203–1218, 2008. 21. R. Mahony, T. Hamel, J. Trumpf, and C. Lageman. Nonlinear observers on SO(3) for complementary

and compatible measurements: A theoretical study. In Proceedings of the IEEE Conference Decision Control/Chinese Control Conference, pp. 6407–6412, IEEE, Shanghai, China, 2009.

22. F. L. Markley. Attitude error representations for Kalman filtering. J. Guid. Contr. Dynam., 26(2):311–317, 2003.

23. P. S. Maybeck. Stochastic Models, Estimation, and Control, Volume 1, volume 141 of Mathematics in Science and Engineering. Academic Press, New York, 1979.

24. R.E. Phillips and G.T. Schmidt. GPS/INS integration. In AGARD Lecture Series: System Implications and Innovative Applications of Satellite Navigation, vol. 207, pp. 9.1–9.18, Canada Communication Group, Hull, Canada, 1996.

25. S. Salcudean. A globally convergent angular velocity observer for rigid body motion. IEEE Trans. Automat. Contr., 36(12):1493–1497, 1991.

26. M. D. Shuster and S. D. Oh. Three-axis attitude determination from vector observations. J. Guid. Contr. Dynam., 4(1):70–77, 1981.

27. J. Thienel and R. M. Sanner. A coupled nonlinear spacecraft attitude controller and observer with an unknown constant gyro bias and gyro noise. IEEE Trans. Automat. Contr., 48(11):2011–2015, 2003.

28. B. Vik and T. I. Fossen. A nonlinear observer for GPS and INS integration. In Proceedings of the IEEE Conference Decision Control, pp. 2956–2961, IEEE, Orlando, FL, 2001.

AQ 2

AQ 3

AQ 4

K26956_C017.indd 313 04/27/16 1:46:54 AM

Author Query Sheet

Chapter No.: 17

Query No. Queries Response

AQ 1 Please confirm the inserted full form “microelectromechanical system” for “mems.”

AQ2 Please check inserted publisher location is OK.

AQ 3 Please check inserted Publisher location is OK.

AQ 4 Please check inserted Publisher location is OK.

K26956_C017.indd 314 04/27/16 1:46:54 AM

17 Nonlinear Observer for Attitude, Position, and Velocityfolk.ntnu.no/torarnj/K26956_C017.pdf ·...

Documents

Transcript of 17 Nonlinear Observer for Attitude, Position, and Velocityfolk.ntnu.no/torarnj/K26956_C017.pdf ·...