Nonlinear Navigation System Design with Application...

38
Nonlinear Navigation System Design J.F. Vasconcelos Nonlinear Navigation System Design with Application to Autonomous Vehicles J.F. Vasconcelos (Ph.D. Candidate) C. Silvestre (Supervisor) P. Oliveira (Co-supervisor) Institute for Systems and Robotics Instituto Superior Técnico Portugal January 2010

Transcript of Nonlinear Navigation System Design with Application...

Page 1: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

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

Page 2: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

System Design

J.F. Vasconcelos

Presentation outline

1. Introduction

� Thesis outline

� Main contributions

2. Kalman Filter Based Navigation Systems

� 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

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 3: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

System Design

J.F. Vasconcelos

IntroductionProblem statement

• Design criteria

—Stability: the state estimate converges to the real position andattitude.

—Performance: the state estimate is obtained by exploiting the sensor readings with optimality criteria.

Estimate attitude and position of a rigid body with respect to a reference frame, using available sensor data.

Navigation problem

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 4: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 5: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

System Design

J.F. Vasconcelos

IntroductionMain contributions: Kalman filtering

Kalman

Filtering

(A)

EKF/INS

+

Advanced

Vector Aiding

(A)

EKF/INS

+

Vehicle Model

Aiding

(B)

Complementary

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.

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 6: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

System Design

J.F. Vasconcelos

IntroductionMain contributions: Nonlinear observers

Nonlinear

Observers

(A)

IMU

+

Landmark

Detection

(A)

IMU

+

Vector

Observations

(A)

IMU

+

GPS

(B) New Stability Results using Density Functions

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.

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 7: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

System Design

J.F. Vasconcelos

Presentation outline

1. Introduction

� Thesis outline

� Main contributions

2. Kalman Filter Based Navigation Systems

� 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

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 8: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

System Design

J.F. Vasconcelos

Rate GyroAccelerometer

(Error Correction Routine)

INSEKF

MeasurementResidual

Calculation

GPSDynamic

Vector Readings

Rate GyroAccelerometer

(Error Correction Routine)

INSEKF

MeasurementResidual

Calculation

GPSDynamic

Vector Readings

Rate GyroAccelerometer

(Error Correction Routine)

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.

� Aiding sensor information, such as GPS, magnetometer and gravity selective frequency contents are introduced in the EKF for error estimation and compensation.

� Direct-feedback error compensation.

Inertial Sensors

Aiding Sensors

Rate GyroAccelerometer

(Error Correction Routine)

INSEKF

MeasurementResidual

Calculation

GPSDynamic

Vector Readings

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

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 9: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

System Design

J.F. Vasconcelos

INS

(Error Correction Routine)

Inertial Sensors

Aiding Sensors

(Bias Update)EKF

MeasurementResidual

Calculation

Vehicle ModelThrusters Input

(Error Correction Routine)

Vehicle Model??

Embedded UAV model for INS aidingVehicle model aiding techniques: External VD aiding

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

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

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 10: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

System Design

J.F. Vasconcelos

INS

(Error Correction Routine)

Inertial Sensors

Aiding Sensors

(Bias Update)EKF

MeasurementResidual

Calculation

Vehicle ModelThrusters Input

Embedded UAV model for INS aidingVehicle model aiding techniques: Embedded VD aiding

•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 ω

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 11: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 12: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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 )

GPS AidingVario X-Treme Aiding

0 10 20 30 400

0.2

0.4

0.6

0.8

1x 10-3

time(s)

| δb ω

y| (

rad/

s)

GPS AidingVario X-Treme Aiding

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 13: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 14: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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.

Euler Angles Estimate

Rate Gyro Bias Estimate

Position Estimate

Body Velocity Estimate

Nonlinear complementary Kalman filterNavigation system architecture

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 15: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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.

� However, the feedback gains can be designed using a stochastic characterization of the sensors.

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 16: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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 .

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 17: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 18: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

System Design

J.F. Vasconcelos

Nonlinear complementary Kalman filterExperimental results: DELFIMx catamaran

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 19: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 20: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

System Design

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

(Rate Gyro)

(Accelerometer)

(Pendulum)

(GPS)

PIT

CH

X-A

XIS

PO

SIT

ION

Nonlinear complementary Kalman filterExperimental results: Frequency domain

θ

px

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 21: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

System Design

J.F. Vasconcelos

Presentation outline

1. Introduction

� Thesis outline

� Main contributions

2. Kalman Filter Based Navigation Systems

� 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

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 22: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 23: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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).

Rigid Body Kinematics

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 (ω)×

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 24: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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 aiding sensors (landmarks/vector readings).

• 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) ≤ 0Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 25: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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−1

i=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,

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 26: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

System Design

J.F. Vasconcelos

Lyapunov based navigation system design Observer 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ω,

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 27: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

System Design

J.F. Vasconcelos

Lyapunov based navigation system design Properties of the derived observers

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. Directionality of the estimation errors is analyzed.

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 28: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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‖∞)

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 29: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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 ∈ Rn 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}

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 30: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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)Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 31: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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).

Rigid Body Kinematics

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)

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 32: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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.

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 33: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

System Design

J.F. Vasconcelos

Nonlinear GPS/IMU navigation systemProperties of the derived observer

GPS/IMU 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.

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 34: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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||

namax = 0.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.5

1

1.5

2

2.5

Time (s)

||Ev|| (m/s)

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

1

2

3

4

5

6

7

8

9

10

Time (s)

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

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

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

180rad/s

GRAVITY COMPENSATION ERROR VELOCITY ESTIMATION ERROR

ATTITUDE ESTIMATION ERRORPOSITION ESTIMATION ERROR

Nonlinear GPS/IMU navigation systemSimulation results

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 35: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

System Design

J.F. Vasconcelos

Presentation outline

1. Introduction

� Thesis outline

� Main contributions

2. Kalman Filter Based Navigation Systems

� 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

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 36: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

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

� Integrated advanced aiding techniques;

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

Stability driven design

� Guaranteed stability with some sensor non-idealities;

� Accounting for extra non-idealities implies system redesign.

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 37: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

boot

Nonlinear Navigation

System Design

J.F. Vasconcelos

Journal publications

� Kalman Filter Based Navigation Systems

� Lyapunov Based Navigation System Design

� Magnetometer Calibration in Sensor Frame

Control Engineering Practice, Elsevier- “Embedded UAV model and Laser Aiding for inertial Navigation Systems”

Transactions on Control Systems Technology, IEEE

- “A nonlinear position and attitude observer on SE(3) using landmark measurements”

Transactions on Aerospace and Control Systems, IEEE

In preparation

Systems & Control Letters, Elsevier

- “Discrete-time complementary filters for attitude and position: Design, analysis and experimental validation”

- “Combination of Lyapunov and density functions for stability analysis”- “Nonlinear GPS/IMU navigation system”

Introduction

KF Based Nav. SystemsEmbedded VD model aiding

Nonlinear CKF

LyapunovBased Nav. SystemsAttitude and position nonlinear observers

Lyapunov and density functionsstability analysis

Nonlinear GPS/IMU

Conclusions

Page 38: Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application

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