Aero Emech Nonlinearest Mar2011 r02

8/13/2019 Aero Emech Nonlinearest Mar2011 r02

1/35

Aero. Engr. & Engr. Mech., UT Austin31 March 2011

Mark L. Psiaki

Sibley School of Mechanical & Aerospace Engr.,Cornell University

Nonlinear Model-Based EstimationAlgorithms: Tutorial and Recent

Developments


2/35

UT Austin March 11 2 of 35

Acknowledgements Collaborators

Paul Kintner, former Cornell ECE faculty member

Steve Powell, Cornell ECE research engineer

Hee Jung, Eric Klatt, Todd Humphreys, & Shan Mohiuddin,Cornell GPS group Ph.D. alumni

Joanna Hinks, Ryan Dougherty, Ryan Mitch, & Karen Chiang,Cornell GPS group Ph.D. candidates

Jon Schoenberg & Isaac Miller, Cornell Ph.D. candidate/alumnusof Prof. M. Campbells autonomous systems group

Prof. Yaakov Oshman, The Technion, Haifa, Israel, faculty ofAerospace Engineering

Massaki Wada, Saila System Inc. of Tokyo, Japan

Sponsors Boeing Integrated Defense Systems NASA Goddard

NASA OSS

NSF


3/35


Goals: Use sensor data from nonlinear systems to infer internal

states or hidden parameters

Enable navigation, autonomous control, etc. in

challenging environments (e.g., heavy GPS jamming) or

with limited/simplified sensor suites

Strategies: Develop models of system dynamics & sensors that relate

internal states or hidden parameters to sensor outputs

Use nonlinear estimation to invert models & determine

states or parameters that are not directly measured Nonlinear least-squares

Kalman filtering

Bayesian probability analysis


4/35


OutlineI. Related research

II. Example problem: Blind tricyclist w/bearings-onlymeasurements to uncertain target locations

III. Observability/minimum sensor suite

IV. Batch filter estimationMath model of tricyclist problem

Linearized observability analysis

Nonlinear least-squares solution

V. Models w/process noise, batch filter limitations

VI. Nonlinear dynamic estimators: mechanizations & performance Extended Kalman Filter (EKF)

Sigma-points filter/Unscented Kalman Filter (UKF)

Particle filter (PF) Backwards-smoothing EKF (BSEKF)

VII. Introduction of Gaussian sum techniques

VIII. Summary & conclusions


5/35


Related Research Nonlinear least squares batch estimation: Extensive

literature & textbooks,e.g., Gill, Murray, & Wright (1981)

Kalman filter & EKF: Extensive literature & textbooks, e.g.,

Brown & Hwang 1997 or Bar-Shalom, Li & Kirubarajan

(2001)

Sigma-points filter/UKF: Julier, Uhlmann, & Durrant-Whyte

(2000), Wan & van der Merwe (2001), etc.

Particle filter: Gordon, Salmond, & Smith (1993),

Arulampalam et al. tutorial (2002), etc.

Backwards-smoothing EKF: Psiaki (2005)

Gaussian mixture filter: Sorenson & Alspach (1971), van

der Merwe & Wan (2003), Psiaki, Schoenberg, & Miller

(2010), etc.


6/35

A Blind Tricyclist Measuring Relative

Bearing to a Friend on a Merry-Go-Round


Assumptions/constraints: Tricyclist doesnt know initial x-y position or heading, but

can accurately accumulate changes in location & heading

via dead-reckoning

Friend of tricyclist rides a merry-go-round & periodicallycalls to him giving him a relative bearing measurement

Tricyclist knows merry-go-round location & diameter, but

not its initial orientation or its constant rotation rate

Estimation problem: determine initial location &

heading plus merry-go-round initial orientation &

rotation rate


7/35

Example Tricycle Trajectory &

Relative Bearing Measurements See 1stMatlab movie



8/35

Is the System Observable?

Observability is condition of having unique internal

states/parameters that produce a given measurementtime history

Verify observability before designing an estimator

because estimation algorithms do not work for

unobservable systems Linear system observability tested via matrix rank calculations

Nonlinear system observability tested via local linearization rank

calculations & global minimum considerations of associated least-

squares problem

Failed observability test implies need for additional

sensing



9/35

Observability Failure of Tricycle

Problem & a Fix See 2ndMatlab movie for failure/non-

uniqueness

See 3rdMatlab movie for fix via additional

sensing



10/35

Geometry of Tricycle Dynamics &

Measurement Models


m

mm

mX mY XEast,

YNorth,

Y

X

Tricycle

Round-Go-Merrythm

V


11/35


Constant-turn-radius transition from tkto t

k+1=t

k+Dt:

State & control vector definitions

Consistent with standard discrete-time state-vectordynamic model form:

Tricycle Dynamics Model from Kinematics

]

tan

sinccos

tan

cinc[sin }{}{1 wkk

kw

kkkkkk b

tV

b

tV

tVXX

]tan

sincsintan

cinccos[ }{}{1w

kkk

w

kkkkkk

b

tV

b

tVtVYY

w

kkkk

b

tV

tan1

tmkmkmk 1

mkmk 1

2,1for m

T2121 ],,,,,,[ kkkkkkkk YX x T],[ kkk V u

),(1 kkkk uxfx


12/35


Trigonometry of bearing measurement to mthmerry-go-round rider

Sample-dependent measurement vector definition:

Consistent with standard discrete-time state-vectormeasurement model form:

Bearing Measurement Model

),...coscos{(atan2 krkmkmmmk bX X

shoutsriderneitherif[]

shoutridersbothif

shouts2rideronlyifshouts1rideronlyif

2

1

2

1

k

k

k

k

k

z

kkkk )(xhz

)}sinsin( krkmkmm bY Y


13/35


Over-determined system of equations:

Definitions of vectors & model function:

Nonlinear Batch Filter Model

bigbigbig )(

0xhz

N

big

z

z

z

z

2

1

N

big

2

1

]}),},,{([{

]}),,([{

]},[{

)(

123321

100012

0001

0

NNNNNNN

big

uuufffh

uuxffh

uxfh

xh


14/35


Linearized local observability analysis:

Batch filter nonlinear least-squares estimation problem

Approximate estimation error covariance

Batch Filter Observability & Estimation

0x

h

big

bigH ?)dim()( 0xbigHrank

0:find x

:minimizeto )]([)]([)( 01T

021

0 xhzxhzx bigbigbigbigbig RJ

}))({( T00000 xxxx optoptxx EP

11T1

20

2

][][

0

bigbigbig HRH

J

opt

xx


15/35

Example Batch Filter Results


-20 -10 0 10 20 30 40 50 60

-30

-20

-10

0

10

20

30

East Position (m)

NorthPosition(m)

Truth

Batch Estimate


16/35


Typical form driven by Gaussian white random

process noise vk:

Tricycle problem dead-reckoning errors naturally

modeled as process noise Specific process noise terms

Random errors between true speed V& true steer angle and the measured values used for dead-reckoning

Wheel slip that causes odometry errors or that occurs in theside-slip direction.

Dynamic Models with Process Noise

),,(1 kkkkk vuxfx jkkjkk QEE }{,0}{T

vvv


17/35

Effect of Process Noise on Truth Trajectory


-20 -10 0 10 20 30 40 50 60

-30

-20

-10

0

10

20

30

East Position (m)

NorthPos

ition(m)

No Process Noise

Process Noise Present


18/35

Effect of Process Noise on Batch Filter


-20 -10 0 10 20 30 40 50 60

-30

-20

-10

0

10

20

30

40

East Position (m)

NorthPos

ition(m)

Truth

Batch Estimate


19/35


Dynamic Filtering based on Bayesian

Conditional Probability Density

subject to xifor i= 0, , k-1 determined as

functions of xk& v0, , vk-1via inversion of theequations:

1

0

1T

21k

iiii QJ vv

)](-[)](-[ 1111

1

T

111

iiiiiii R xhzxhz

)-()-( 0010

T002

1xxxx

xxP

}exp{),,|,,,( 110 JCkkk zzvvx p

1,..,0for),,(1 kiiiiii vuxfx


20/35


Uses Taylor series approximations of fk(x

k,u

k,v

k) & h

k(x

k)

Taylor expansions about approximate xk

expectation values &about v

k= 0

Normally only first-order, i.e., linear, expansions used, butsometimes quadratic terms are used

Gaussian statistics assumed Allows complete probability density characterization in terms of

means & covariances

Allows closed-form mean & covariance propagations

Optimal for truly linear, truly Gaussian systems

Drawbacks Requires encoding of analytic derivatives

Loses accuracy or even stability in the presence of severenonlinearities

EKF Approximation


21/35

EKF Performance, Moderate Initial Uncertainty


-30 -20 -10 0 10 20 30 40 50 60 70

-30

-20

-10

0

10

20

30

East Position (m)

NorthPosition(m)

Truth

EKF Estimate


22/35

EKF Performance, Large Initial Uncertainty


-40 -20 0 20 40 60 80-60

-50

-40

-30

-20

-10

0

10

20

30

East Position (m)

NorthPos

ition(m)

Truth

EKF Estimate


23/35


Evaluate fk(x

k,u

k,v

k) & h

k(x

k) at specially chosen sigma points &

compute statistics of results

Sigma points & weights yield pseudo-random approximate Monte-Carlocalculations

Can be tuned to match statistical effects of more Taylor series terms thanEKF approximation

Gaussian statistics assumed, as in EKF

Mean & covariance assumed to fully characterize distribution

Sigma points provide a describing-function-type method for improving mean& covariance propagations, which are performed via weighted averagingover sigma points

No need for analytic derivatives of functions

Also optimal for truly linear, truly Gaussian systems

Drawback Additional Taylor series approximation accuracy may not be sufficient for

severe nonlinearities

Extra parameters to tune

Singularities & discontinuities may hurt UKF more than other filters

Sigma-Points UKF Approximation


24/35

UKF Performance, Moderate Initial Uncertainty


-20 -10 0 10 20 30 40 50 60 70

-30

-20

-10

0

10

20

30

East Position (m)

NorthPos

ition(m)

Truth

UKF A Estimate

UKF B Estimate


25/35

UKF Performance, Large Initial Uncertainty


-20 0 20 40 60 80 100

-60

-40

-20

0

20

40

East Position (m)

NorthPosition(m)

Truth

UKF A Estimate

UKF B Estimate


26/35


Approximate the conditional probability distribution using Monte-Carlotechniques

Keep track of a large number of state samples & corresponding weights Update weights based on relative goodness of their fits to measured data

Re-sample distribution if weights become overly skewed to a few points,using regularization to avoid point degeneracy

Advantages

No need for Gaussian assumption Evaluates f

k(x

k,u

k,v

k) & h

k(x

k) at many points, does not need analytic

derivatives

Theoretically exact in the limit of large numbers of points

Drawbacks

Point degeneracy due to skewed weights not fully compensated by

regularization Too many points required for accuracy/convergence robustness for high-

dimensional problems

Particle Filter Approximation


27/35

PF Performance, Moderate Initial Uncertainty


-30 -20 -10 0 10 20 30 40 50 60 70

-30

-20

-10

0

10

20

30

East Position (m)

NorthPosition(m)

Truth

Particle Filter Estimate


28/35

PF Performance, Large Initial Uncertainty


-20 0 20 40 60 80

-50

-40

-30

-20

-10

0

10

20

30

East Position (m)

NorthPos

ition(m)

Truth

Particle Filter Estimate


29/35


Maximizes probability density instead of trying toapproximate intractable integrals Maximum a posteriori (MAP) estimation can be biased, but also can

be very near optimal

Standard numerical trajectory optimization-type techniques can beused to form estimates

Performs explicit re-estimation of a number of past process noise

vectors & explicitly considers a number of past measurements inaddition to the current one, re-linearizing many f

i(x

i,u

i,v

i) & h

i(x

i) for

values of i


30/35


Implicit Smoothing in a Kalman Filter

0 1 2 3 4 5-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

3

3.5

x1

Sample Count, k

Filter Output1-Point Smoother2-Point Smoother3-Point Smoother4-Point Smoother5-Point SmootherTruth


31/35

BSEKF Performance, Moderate Initial Uncertainty


-20 -10 0 10 20 30 40 50 60

-30

-20

-10

0

10

20

30

East Position (m)

NorthPos

ition(m)

Truth

BSEKF A Estimate

BSEKF B Estimate


32/35

BSEKF Performance, Large Initial Uncertainty


-40 -20 0 20 40 60

-50

-40

-30

-20

-10

0

10

20

30

East Position (m)

NorthPosition(m)

Truth

BSEKF A Estimate

BSEKF B Estimate


33/35

A PF Approximates the Probability Density

Function as a Sum of Dirac Delta Functions

GNC/Aug. 10 33 of 24

-8 -6 -4 -2 0 2 4 6 80

0.2

0.4

0.6

x

px

(x),f(x)

0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.60

10

20

30

f

pf(f)

Particle filter approximation ofnonlinearly propagated p

f(f)

using 50 Dirac delta functions

Particle filter approximationof original p

x(x) using

50 Dirac delta functions


34/35

A Gaussian Sum Spreads the Component

Functions & Can Achieve Better Accuracy

GNC/Aug. 10 34 of 24

-8 -6 -4 -2 0 2 4 6 80

0.2

0.4

0.6

x

px

(x),f(x)

0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.60

10

20

30

f

pf(f)

100-element re-sampled Gaussian

approximation of original px(x)probability density function 100 Narrow weighted Gaussian

components of re-sampled mixture

EKF/100-narrow-element Gaussianmixture approximation of

propagated pf(f) probability

density function


35/35

Summary & Conclusions Developed novel navigation problem to illustrate

challenges & opportunities of nonlinear estimation Reviewed estimation methods that extract/estimate

internal states from sensor data

Presented & evaluated 5 nonlinear estimation

algorithms Examined Batch filter, EKF, UKF, PF, & BSEKF

EKF, PF, & BSEKF have good performance for moderate initial errors

Only BSEKF has good performance for large initial errors

BSEKF has batch-like properties of insensitivity to initial

estimates/guesses due to nonlinear least-squares optimization with

algorithmic convergence guarantees


Aero Emech Nonlinearest Mar2011 r02

Documents

Transcript of Aero Emech Nonlinearest Mar2011 r02