Nonlinear Navigation System Design with Application to ... · Nonlinear Navigation System Design with Application to Autonomous Vehicles J.F. Vasconcelos(Ph.D. Candidate) C. Silvestre

Nonlinear Navigation

System Design

J.F. Vasconcelos

Nonlinear Navigation System Designwith Application to Autonomous Vehicles

J.F. Vasconcelos (Ph.D. Candidate)C. Silvestre (Supervisor) P. Oliveira (Co-supervisor)

Institute for Systems and RoboticsInstituto Superior Técnico

Portugal

January 2010

boot


System Design

J.F. Vasconcelos

Presentation Outline

1. Introduction

� Thesis outline

� Main contributions

2. Kalman Filter Based Navigation Systems

� INS/GPS aided by selective frequency contents

� Embedded UAV model for INS aiding

� Nonlinear complementary Kalman filter

3. Lyapunov Based Navigation System Design

● Attitude and position nonlinear observers

● Combination of Lyapunov and density functions for nonlinear observer stability analysis

● Nonlinear GPS/IMU navigation system

4. Conclusions and Future Work

boot


System Design

J.F. Vasconcelos

IntroductionThesis Outline

• The proposed navigation systems are derived using two methodologies

—Kalman filtering techniques;

—Lyapunov and density functions stability theory.

Kalman

Filtering

EKF/INS

+

Advanced

Vector Aiding

EKF/INS

+

Vehicle Model

Aiding

Complementary

Kalman

Filtering

Nonlinear

Observers

IMU

+

Landmark

Detection

IMU

+

Vector

Observations

IMU

+

GPS

New Stability Results

using Density Functions

boot


System Design

J.F. Vasconcelos

IntroductionMain contributions: Kalman Filtering

�Main contributions in the field of navigation systems based on Kalman filtering

•(A) Advanced aiding techniques for EKF/INS navigation systems

—modelling frequency contents of vector readings,

—integrating efficiently the dynamic of the vehicle.

•(B) Nonlinear complementary Kalman filters

—endowed with stability and performance properties,

—combining stochastic approach and frequency domain design.

Kalman

Filtering

(A)

EKF/INS

+

Advanced

Vector Aiding

(A)

EKF/INS

+

Vehicle Model

Aiding

(B)

Complementary

Kalman

Filtering

boot


System Design

J.F. Vasconcelos

IntroductionMain contributions: Nonlinear Observers

•Main contributions in the field of nonlinear observers

•(A) Nonlinear attitude and position observers for inertial systems

—with diverse aiding sensors.

—endowed with stability properties for worst-case errors.

•(B) New stability analysis tools

—based on the combination of density and Lyapunov functions,

—for almost GAS and almost ISS analysis of nonlinear systems.

—that eventually yield almost ISS of the proposed observers.

Nonlinear

Observers

(A)

IMU

+

Landmark

Detection

(A)

IMU

+

Vector

Observations

(A)

IMU

+

GPS

(B) New Stability Results using Density Functions

boot


System Design

J.F. Vasconcelos


1. Introduction

� Thesis outline








● Combination of Lyapunov and density functions for nonlinear observer stability analysis



boot


System Design

J.F. Vasconcelos

Rate GyroAccelerometer

(Error Correction Routine)

INSEKF

MeasurementResidual

Calculation

GPSDynamic

Vector Readings



INSEKF

MeasurementResidual

Calculation

GPSDynamic

Vector Readings



INSEKF

MeasurementResidual

Calculation

GPSDynamic

Vector Readings

GPS, Magnetometer and GravitySelective Frequency Contents

Position, Velocity andAttitude Computation

Position, Velocity, Attitude andBias Errors Compensation

� High-accuracy, multi-rate INS computes position, velocity and attitude based on the inertial sensor readings.

� Gravity Selective Frequency Contents are introduced in the EKF for error estimation and compensation.

� Direct-feedback error compensation.

Inertial Sensors

Aiding Sensors



INSEKF

MeasurementResidual

Calculation

GPSDynamic

Vector Readings

INS/GPS aided by selective frequency contentsEKF/INS navigation system architecture

boot


System Design

J.F. Vasconcelos

• Pendular measurements are distorted by accelerations:—Coriolis terms can be compensated by the INS estimates,—linear accelerations can be modeled as bandpass signals

for terrestrial and oceanic vehicles.

• The linear acceleration component is characterized in the frequency domain using the EKF state model.

INS/GPS aided by selective frequency contentsGravity Frequency Contents: Pendular Measurement

Eg

nLA

Transfer Function

Bg

aLABaSF

RT

sαh

(s+αl)(s+αh)

ω ×Bv

αl αh

BaSF =

Bg+ aLA +ω ×

Bv

boot


System Design

J.F. Vasconcelos

The pendular measurement is a function of the EKF states.

INS/GPS aided by selective frequency contentsGravity Frequency Contents: Accuracy Enhancements

0 2 4 6 8 10-20

-10

0

10

Time (s)

δ b

ω x

(o /s

)

z=zGPS

z=[zGPS

zm

]

z=[zGPS

zm

zg]

0 4 8 12 16 20-10

0

10

20

30

Time (s)

Rol

l err

or (

o)

z=zGPS

z=[zGPS

zm

]

z=[zGPS

zm

zg]

The integration of the pendularmeasurement in the EKF enhances bias compensation and roll estimation.

0 5 10 15 20-0.2

-0.1

0

0.1

0.2

Time (s)

δ b

ax (

m/s

2 )

z=zGPS

z=[zGPS

zm

]

z=[zGPS

zm

zg]

δzg = B g − gr = − aLA + (ω)×R

′δv + δba +

(

Bv)

×δbω

+[

R′(

Eg)

×+ (ω)

×

(

Bv)

×R

′

]

δλ − na −(

Bv)

×nω

δzg

boot


System Design

J.F. Vasconcelos

INS


Inertial Sensors

Aiding Sensors

(Bias Update)EKF

MeasurementResidual

Calculation

Vehicle ModelThrusters Input


Vehicle Model??

Embedded UAV model for INS aidingVehicle Model Aiding Techniques

•The external vehicle dynamics (VD) aiding is implemented using a standalone simulator.

•The INS and VD error derivation, estimation, and compensation techniques are similar.

boot


System Design

J.F. Vasconcelos

INS


Inertial Sensors

Aiding Sensors

(Bias Update)EKF

MeasurementResidual

Calculation

Vehicle ModelThrusters Input

Embedded UAV model for INS aidingVehicle Model Aiding Techniques

•The embedded vehicle dynamics (VD) are integrated directly in the EKF architecture.

•The vehicle model equations are computed using the inertial estimates.

•The computational load of VD aiding is reduced and the implementation flexibility is added, while retaining the accuracy enhancements of VD aiding.

Bv ω

boot


System Design

J.F. Vasconcelos

Vario X-treme R/C helicopter

The embedded VD aiding technique was validated for a nonlinear dynamic model of the Vario X-Treme R/C Helicopter.

The helicopter dynamics are described using a six degree of freedom rigid body model that includes the effects of the main rotor, Bell-Hiller stabilizing bar, tail rotor, fuselage, horizontal tail plane, and vertical fin.

Embedded UAV model for INS aidingVario X-Treme helicopter model

boot


System Design

J.F. Vasconcelos

• Bias and velocity estimationresults are enhanced.

• The application of the embeddedVD aiding to a complex andnonlinear Vario X-Tremehelicopter model validates theproposed technique.

Embedded UAV model for INS aidingSimulation Results

0 10 20 30 400

0.1

0.2

0.3

0.4

0.5

0.6

0.7

time(s)

| δv y| (

m/s

)

GPS AidingVario X-Treme Aiding

0 10 20 30 400

0.02

0.04

0.06

0.08

0.1

time(s)

| δb a

x| (m

/s2 )


0 10 20 30 400

0.2

0.4

0.6

0.8

1x 10-3

time(s)

| δb ω

y| (

rad/

s)


boot


System Design

J.F. Vasconcelos

Embedded UAV model for INS aidingSimulation Results

� The embedded VD aiding is computationally more efficient than the classical VD aiding.

GPS Aided External VD Aided Embedded VD Aided

(ω,Bv) aiding Bv aiding

Execution Time (s) 293 543 400 310

� The VD linear velocity aiding yields about the accuracy of the full VD aiding and has a negligible computational cost with respect to GPS aiding.

RMS

Aiding information ||δp||(m) ||δv||(m/s) Yaw error(o) Pitch error(o) Roll error(o)GPS 2.42 3.50 × 10−1 1.26 × 10−2 9.15 × 10−2 7.95 × 10−2

Vario X-Treme Model 3.07 × 10−1 2.84 × 10−2 1.27 × 10−2 2.10 × 10−2 1.31 × 10−2

Vario X-Treme Model (zv only) 5.91 × 10−1 3.20 × 10−2 1.26 × 10−2 2.17 × 10−2 1.40 × 10−2

boot


System Design

J.F. Vasconcelos

� The navigation system is cascade architecture of an attitude anda position complementary filters.

� The proposed system exploits the sensor information over complementary frequency regions.

� The attitude and position filters are endowed with stability and performance properties.

� Multirate architecture is obtained by a synthesis methodology based on periodic systems.

Euler Angles Estimate

Rate Gyro Bias Estimate

Position Estimate

Body Velocity Estimate

Nonlinear complementary Kalman filterNavigation system architecture

boot


System Design

J.F. Vasconcelos

AccelerometerIntegration

GPS

Position Filter

Rate GyroIntegration

High Frequency

Vector Observations

Low Frequency

Attitude Filter

Nonlinear complementary Kalman filterSensor fusion

� The complementary filters combine sensor information inthe frequency domain.

� The feedback gains can be designed using a stochasticcharacterization of the sensors.

boot


System Design

J.F. Vasconcelos

Nonlinear complementary Kalman filterAttitude complementary filter

� The gains are computed for the LTI system

� The attitude complementary filter is uniformly asymptoticallystable for bounded roll (θ<π).

� The complementary filter is the steady-state Kalman filter for the attitude kinematics, assuming constant roll and pitch.

� Performance degradation for time-varying roll and pitch is small.

K1λ, K2λ[

xλ k+1

xb k+1

]

=

[

I −T I

0 I

] [

xλ k

xb k

]

+

[

−T I 0

0 I

] [

wωr k

wb k

]

,

yx k =[

I 0]

[

xλ k

xb k

]

+ vλ k.

boot


System Design

J.F. Vasconcelos

Nonlinear complementary Kalman filterPosition complementary filter

� The gains are computed for the LTI system

� The position complementary filter is uniformly asymptoticallystable and is identified with the steady-state Kalman filterfor the position kinematics with isotropic accelerometer noise.

[

xp k+1

xv k+1

]

=

[

I T I

0 I

] [

xp k

xv k

]

+

[

I −T 2

0 −T

] [

wp k

wv k

]

,

yx k =[

I 0]

[

xp k

xv k

]

+ vp k.

K1p, K2p

boot


System Design

J.F. Vasconcelos

Nonlinear complementary Kalman filterExperimental results: DELFIMx

boot


System Design

J.F. Vasconcelos

245

250

255

260

Yaw

(o)

-5

0

5

10

Pitc

h (o

)

505 506 507 508 509 510-10

-5

0

5R

oll (

o)

Time (s)

0 200 400 600 800-0.5

0

0.5

1

1.5

2

2.5

3

Time (s)

Bod

y V

eloc

ity (

m/s

)

BvxBvyBv

z

1.2

1.31.4

1.5

1.6

505 510-0.1

0

0.10.2

0.3

-100 0 100 200 300-700

-650

-600

-550

-500

-450

-400

-350

-300

-250

0 s 50 s100 s200 s

300 s350 s 400 s

450 s

500 s550 s

600 s650 s

700 s750 s

800 s

850 s900 s

950 s

986 s

Y (m)

X (

m)

Comp. FilterGPS Unit

TRAJECTORY ESTIMATION BODY VELOCITY ESTIMATION

PITCH AND ROLL ESTIMATIONYAW ESTIMATION

Nonlinear complementary Kalman filterExperimental results: Time domain

boot


System Design

J.F. Vasconcelos AIDING SENSOR INERTIAL SENSOR INTEGRATION FILTER ESTIMATE

(Rate Gyro)

(Accelerometer)

(Pendulum)

(GPS)

PIT

CH

x-ax

isp

osi

tio

n

Nonlinear complementary Kalman filterExperimental results: Frequency domain

θ

px

boot


System Design

J.F. Vasconcelos


1. Introduction

� Thesis outline








● Combination of Lyapunov and density functions for stability analysis



boot


System Design

J.F. Vasconcelos

Lyapunov Based Navigation System DesignProblem formulation: landmark readings

Estimate attitude (R ) and position (p) of a rigid body using landmark observations (q

r) and non-ideal angular

and linear velocity measurements (wrand v

r,respectively).

Rigid Body Kinematics

Objective (Landmark based observer)

R pqr

ωr vr

R = R (ω)×,

Bp = B

v − (ω)×

Bp

Velocity Measurements

ωr = ω + bω , vr =Bv + bv

Landmark Measurements

qr i = R′Lxi −

Bp

boot


System Design

J.F. Vasconcelos

Lyapunov Based Navigation System DesignProblem formulation: vector observations

Estimate attitude ( R) of a rigid body using vector observations (h

r) and non-ideal angular velocity

measurements (wr).


Objective (Vector based attitude observer)

R

ωr

Velocity Measurements

Body Frame Vectors (measured)

hr

Reference Vectors (known)

Hr =[

hr 1 . . . hr n

]

=[

Bh1 . . .

Bhn

]

= R′H.

H =[

Lh1 . . . L

hn

]

,

{L} → local frame

{B} → body frame

ωr = ω + bωR = R (ω)×

boot


System Design

J.F. Vasconcelos

Lyapunov Based Navigation System DesignObserver Design Technique

• Define a Lyapunov function V(x) based on the observation error of the landmarks.

• Derive a feedback law such that V(x)<0.

• Analyze the stability properties of the closed loop system using suitable Lyapunov based tools.

Observer Design Methodology

V (x)

Design Issues

• Topological obstacles to global stability on manifolds, namely SE(3). Relaxation to almost global stability and almost ISS frameworks is adopted.

• The feedback law must be a function of the sensor readings. The true position and orientation are unknown.

• Velocity readings can be distorted by bias and/or noise.

SE(3)

V (x) ≤ 0

boot


System Design

J.F. Vasconcelos

Lyapunov Based Navigation System DesignObserver design: Landmark based observer

• The synthesis Lyapunov Function is a linear combination of landmark readings and bias estimation errors

where

Vb = γϕ∑n−1

i=1 ‖Bui −Bui‖

2 + γp‖Bun − Bun‖

2 + γbω b′

ωbω + γbv b′

vbv

Bun = − 1n

∑n

i=1 qi.Buj=1..(n−1) =

∑n−1i=1 aij(qi+1 − qi),

• The obtained observer kinematics are given by

where

that are a function of the sensor readings (output feedback).

˙R = R (ω)

×, B ˙p = Bv− (ω)

×

Bp,

˙bω = γb(γϕsω − γp

(

Bp)

×sv),

˙bv =

γp

γbsv,

ω = ωr − bω − kωsω,B v = vr − bv +

(

(

ωr − bω

)

×

− kvI

)

sv + kω(

Bp)

×sω,

sω =

n∑

i=1

(R′XDXAXei) × (QDXAXei), sv = B p +1

n

n∑

i=1

qi,

boot


System Design

J.F. Vasconcelos

Lyapunov Based Navigation System DesignObserver design: Vector based observer

• The synthesis Lyapunov Function is a linear combination of vector readings and bias estimation error

where

• The obtained observer kinematics are given by

where

that are a function of the sensor readings (output feedback).

Buj =∑n

i=1 aijhi.

Vb =∑n−1

i=1 ‖Bui −Bui‖

2 + γbω b′

ωbω

ω = R′HAHA′

HH′

r

(

ωr − bω

)

− kωsω , sω =

n∑

i=1

(R′HAHei)× (HrAHei),

˙R = R (ω)

×,

˙bω = kbωsω,

boot


System Design

J.F. Vasconcelos

Lyapunov Based Navigation System DesignObserver results

Observer Properties

� Almost GAS with exponential convergence

Almost all initial conditions converge to the origin for ideal velocity measurements.

� Dynamic bias estimation with exponential convergence for worst-case initial conditions

Almost all initial conditions converge to the origin for the case of biased velocity readings.

� Output feedback formulationThe derived feedback law is an explicit function of the sensor measurements.

� Sensor setup characterizationNecessary and sufficient landmark/vector geometry for pose estimation is derived.

boot


System Design

J.F. Vasconcelos

Combination of Lyapunov and density functionsAlmost ISS analysis

Proposition (Almost ISS)If the system is locally ISS and weakly almost ISS, then the system is almost ISS, with

Step 1 (Local ISS)Find a region where

, yielding

Analysis MethodObtain almost ISS by combining local ISS with weakly almost ISS.

Step 2 (Weakly almost ISS)Find a density function ρ such that for , which implies

∀u∀a.a.x(t0) lim inft→∞ |x(t)| ≤ γ2(‖u‖∞).

∀u∀a.a.x(t0) lim supt→∞|x(t)| ≤ γ1(‖u‖∞).

∀u∀|x(t0)| < r lim supt→∞|x(t)| ≤ γ1(‖u‖∞).

γ1(‖u‖∞) < |x(t)| < r

V < 0

div(ρf) > 0|x(t)| > γ2(‖u‖∞)

boot


System Design

J.F. Vasconcelos

Combination of Lyapunov and density functionsAlmost GAS analysis

Analysis MethodGiven the largest invariant set M in , a density function ρ can show that is unstable, yielding aGAS of .

The stability of an equilibrium point can be excluded using a density function analysis.

Theorem 1 Suppose there exists a density function that is C1 and integrable in a neighborhood U of . If in U then the global inset of has zero measure.

Proposition Let the M be the largest invariant set in and assume that M is a countable union of isolated points. If there is a density function ρ that satisfies the conditions of Theorem 1 for all , then the origin is almost GAS.

{x : V (x) = 0}

M \ {0} {0}

ρ : Rn → R+

xu ∈ R

n div(ρf ) > 0 xu

Almost GAS of the origin is obtained by combining Theorem 1 with LaSalle's invariance principle.

{x : V (x) = 0}

xu ∈ M \ {0}

boot


System Design

J.F. Vasconcelos

Combination of Lyapunov and density functionsApplication to Nonlinear Observers

• If rate gyro noise is considered, the proposed stability analysis tools show that the nonlinear attitude observer is almost ISS with respect to , that is

0 1 2 3 4 5 6 7 8 9 1010

-7

10-6

10-5

10-4

10-3

10-2

10-1

100

101

Time (s)

||I−

R||2

||I − R (t0)||2 < r(umax)

||I − R (t0)||2 > r(umax)

r(umax)

γ1(umax)

R = I

• Almost GAS of a nonlinear observer with biased velocity readings is obtained, namely almost all the trajectories of the system

converge to the set

θ = − sin(θ) + b, b = − sin(θ)

{(θ, b) = (2πk, 0), k ∈ Z}.

lim supt→∞

‖I −R(t)‖2 ≤ γ1(‖u‖∞).

∀u∀a.a.R(t0)

boot


System Design

J.F. Vasconcelos

Nonlinear GPS/IMU navigation systemProblem formulation

Estimate attitude (R ) and position (p) of a rigid body using pseudorange observations (pij) and non-ideal angular velocity and acceleration measurements (wr and vr,respectively).


ObjectiveR p

ωr

Inertial Measurements

Pseudorange Measurements

ρij

ar

R = R (ω)×, p = v, v = a

ωr = ω + bω + nω ,

ar = RT (a − g) + na

ρij = ‖pj − pS i‖ + bc

(satellite i, receiver j)

boot


System Design

J.F. Vasconcelos

Nonlinear GPS/IMU navigation systemObserver configuration

� The navigation system is described by a cascade of an attitude observer and a position observer.

� Three GPS receivers onboard the vehicle are necessary for attitude estimation. In alternative, vector readings can be adopted.

boot


System Design

J.F. Vasconcelos

Lyapunov Based Navigation System DesignGPS/INS observer results

GPS/INS Observer Properties

� Almost GAS with exponential convergence

Almost all initial conditions converge to the origin for ideal inertial measurements.

� Dynamic bias estimation with exponential convergence for worst-case initial conditions

Almost all initial conditions converge to the origin for the case of biased velocity readings.

� Almost ISS with inertial sensor noiseThe estimation errors converge to a known neighborhood of the origin.

� Output feedback formulationThe derived feedback law is an explicit function of the sensor measurements.

boot


System Design

J.F. Vasconcelos

0 2 4 6 8 10 12 14 16 18 200

0.5

1

1.5

2

2.5

3

3.5

Time (s)

||Ep|| (m)

namax = 0.3m/s2, nωmax = 0rad/s

namax = 3m/s2, n

ωmax = 0rad/s

namax = 0.3m/s2, nωmax = 10π

180rad/s

0 2 4 6 8 10 12 14 16 18 200

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

Time (s)

||I−R||



180rad/s

0 2 4 6 8 10 12 14 16 18 200

0.5

1

1.5

2

2.5

Time (s)

||Ev|| (m/s)


namax = 3m/s2, n

ωmax = 0rad/s


180rad/s

0 2 4 6 8 10 12 14 16 18 200

1

2

3

4

5

6

7

8

9

10

Time (s)


namax = 3m/s2, n

ωmax = 0rad/s


180rad/s

GRAVITY COMPENSATION ERROR VELOCITY ESTIMATION ERROR

ATTITUDE ESTIMATION ERRORPOSITION ESTIMATION ERROR

Lyapunov Based Navigation System DesignGPS/INS simulation results

boot


System Design

J.F. Vasconcelos

Conclusions and Future Work

Kalman

Filtering

EKF/INS

+

Advanced

Vector Aiding

EKF/INS

+

Vehicle Model

Aiding

Complementary

Kalman

Filtering

Nonlinear

Observers

IMU

+

Landmark

Detection

IMU

+

Vector

Observations

IMU

+

GPS

New Stability Results

using Density Functions

Performance driven design

� High-accuracy navigation systems;

� Kalman filter based architectures;

� Integration of advanced aiding techniques;

� Accounts for a variety of sensor non-idealities and disturbances.

Stability driven design

� Almost global stabilization;

� Guarantees robustness to somesensor non-idealities;

� Accounting for extra non-idealities implies system redesign.


System Design

J.F. Vasconcelos

Nonlinear Navigation System Designwith Application to Autonomous Vehicles

J.F. Vasconcelos (Ph.D. Candidate)C. Silvestre (Supervisor) P. Oliveira (Co-supervisor)

Institute for Systems and RoboticsInstituto Superior Técnico

Portugal

January 2010

Documents

Nonlinear Navigation System Design with Application to ... · Nonlinear Navigation System Design with Application to Autonomous Vehicles J.F. Vasconcelos(Ph.D. Candidate) C. Silvestre