Aero. Engr. & Engr. Mech., UT Austin 31 March 2011 Mark L. Psiaki Sibley School of Mechanical & Aerospace Engr., Cornell University Nonlinear Model-Based

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

Mark L. PsiakiSibley School of Mechanical & Aerospace Engr., Cornell University

Nonlinear Model-Based Estimation Algorithms: Tutorial and Recent Developments

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

of Prof. M. Campbell’s autonomous systems group Prof. Yaakov Oshman, The Technion, Haifa, Israel, faculty of

Aerospace Engineering Massaki Wada, Saila System Inc. of Tokyo, Japan

Sponsors Boeing Integrated Defense Systems NASA Goddard NASA OSS NSF


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


OutlineI. Related researchII. Example problem: Blind tricyclist w/bearings-only

measurements to uncertain target locationsIII. Observability/minimum sensor suiteIV. Batch filter estimation

Math model of tricyclist problem Linearized observability analysis Nonlinear least-squares solution

V. Models w/process noise, batch filter limitationsVI. 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 techniquesVIII. Summary & conclusions


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.

A Blind Tricyclist Measuring Relative Bearing to a Friend on a Merry-Go-Round


Assumptions/constraints: Tricyclist doesn’t 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 & periodically calls 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

Example Tricycle Trajectory & Relative Bearing Measurements See 1st Matlab movie


Is the System Observable? Observability is condition of having unique internal

states/parameters that produce a given measurement time 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


Observability Failure of Tricycle Problem & a Fix See 2nd Matlab movie for failure/non-

uniqueness See 3rd Matlab movie for fix via additional

sensing


Geometry of Tricycle Dynamics & Measurement Models


mm

m

mX mY XEast,

YNorth,

Y

X

Tricycle

Round-Go-Merry thm

V


Constant-turn-radius transition from tk to tk+1 = tk +t:

State & control vector definitions

Consistent with standard discrete-time state-vector dynamic model form:

Tricycle Dynamics Model from Kinematics

]tan

sinccostan

cinc[sin }{}{1w

kkk

w

kkkkkk b

ΔtV

b

ΔtVΔtVXX

]tan

sincsintan

cinccos[ }{}{1w

kkk

w

kkkkkk b

ΔtV

b

ΔtVΔtVYY

w

kkkk b

ΔtV tan1

Δtmkmkmk 1

mkmk 12,1for m

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

),(1 kkkk uxfx


Trigonometry of bearing measurement to mth merry-go-round rider

Sample-dependent measurement vector definition:

Consistent with standard discrete-time state-vector measurement model form:

Bearing Measurement Model

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

shoutsrider neither if[]

shout ridersboth if

shouts 2rider only ifshouts 1rider only if

2

1

2

1

k

k

k

k

k

z

kkkk )(xhz

)}sinsin( krkmkmm bY Y


Over-determined system of equations:

Definitions of vectors & model function:

Nonlinear Batch Filter Model

bigbigbig )( 0xhz

N

big

z

zz

z2

1

N

big

2

1

]}),},,{([{

]}),,([{]},[{

)(

123321

100012

0001

0

NNNNNNN

big

uuufffh

uuxffhuxfh

xh


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:minimize to

)]([)]([)( 01T

021

0 xhzxhzx bigbigbigbigbig RJ

}))({( T00000 xxxx optoptxx EP

11T120

2][][

0

bigbigbig HRH

J

opt

xx

Example Batch Filter Results


-20 -10 0 10 20 30 40 50 60

-30

-20

-10

0

10

20

30

East Position (m)

Nor

th P

ositi

on (

m)

TruthBatch Estimate


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 the

side-slip direction.

Dynamic Models with Process Noise

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

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)

Nor

th P

ositi

on (

m)

No Process NoiseProcess Noise Present

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)

Nor

th P

ositi

on (

m)

TruthBatch Estimate


Dynamic Filtering based on Bayesian Conditional Probability Density

subject to xi for i = 0, …, k-1 determined as functions of xk & v0, …, vk-1 via inversion of the equations:

1

0

1T21

k

iiii QJ vv

)](-[)](-[ 1111

1T

111

iiiiiii R xhzxhz

)ˆ-()ˆ-( 0010

T002

1 xxxx xxP

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

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


Uses Taylor series approximations of fk(xk,uk,vk) & hk(xk) Taylor expansions about approximate xk expectation values &

about vk = 0 Normally only first-order, i.e., linear, expansions used, but

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

nonlinearities

EKF Approximation

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)

Nor

th P

ositi

on (

m)

TruthEKF Estimate

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)

Nor

th P

ositi

on (

m)

TruthEKF Estimate


Evaluate fk(xk,uk,vk) & hk(xk) at specially chosen “sigma” points & compute statistics of results “Sigma” points & weights yield pseudo-random approximate Monte-Carlo

calculations Can be tuned to match statistical effects of more Taylor series terms than

EKF 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 averaging over 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

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)

Nor

th P

ositi

on (

m)

TruthUKF A EstimateUKF B Estimate

UKF Performance, Large Initial Uncertainty


-20 0 20 40 60 80 100

-60

-40

-20

0

20

40

East Position (m)

Nor

th P

ositi

on (

m)

TruthUKF A EstimateUKF B Estimate


Approximate the conditional probability distribution using Monte-Carlo techniques 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 fk(xk,uk,vk) & hk(xk) 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

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)

Nor

th P

ositi

on (

m)

TruthParticle Filter Estimate

PF Performance, Large Initial Uncertainty


-20 0 20 40 60 80

-50

-40

-30

-20

-10

0

10

20

30

East Position (m)

Nor

th P

ositi

on (

m)

TruthParticle Filter Estimate


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

be very near optimal Standard numerical trajectory optimization-type techniques can be

used to form estimates Performs explicit re-estimation of a number of past process noise

vectors & explicitly considers a number of past measurements in addition to the current one, re-linearizing many fi(xi,ui,vi) & hi(xi) for values of i <= k as part of a non-linear smoothing calculation

Drawbacks Computationally intensive, though highly parallelizable MAP not good for multi-modal distributions Tuning parameters adjust span & solution accuracy of re-smoothing

problems

Backwards-Smoothing EKF Approximation


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

x 1

Sample Count, k

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

BSEKF Performance, Moderate Initial Uncertainty


-20 -10 0 10 20 30 40 50 60

-30

-20

-10

0

10

20

30

East Position (m)

Nor

th P

ositi

on (

m)

TruthBSEKF A EstimateBSEKF B Estimate

BSEKF Performance, Large Initial Uncertainty


-40 -20 0 20 40 60

-50

-40

-30

-20

-10

0

10

20

30

East Position (m)

Nor

th P

ositi

on (

m)

TruthBSEKF A EstimateBSEKF B Estimate

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

p x(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

p f(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

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

p x(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

p f(f)

100-element re-sampled Gaussianapproximation of original p

x(x)

probability density function 100 Narrow weighted Gaussiancomponents of re-sampled mixture

EKF/100-narrow-element Gaussianmixture approximation of

propagated pf(f) probability

density function

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


Documents

Aero. Engr. & Engr. Mech., UT Austin 31 March 2011 Mark L. Psiaki Sibley School of Mechanical & Aerospace Engr., Cornell University Nonlinear Model-Based