Nonlinear Model-Based Estimation
Algorithms: Tutorial and Recent
Developments
Mark L. Psiaki
Sibley School of Mechanical & Aerospace Engr.,
Cornell University
Aero. Engr. & Engr. Mech., UT Austin
31 March 2011
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
UT Austin March ‘11
2 of 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
UT Austin March ‘11
3 of 35
Outline
I.
II.
III.
IV.
V.
VI.
VII.
VIII.
Related research
Example problem: Blind tricyclist w/bearings-only
measurements to uncertain target locations
Observability/minimum sensor suite
Batch filter estimation
 Math model of tricyclist problem
 Linearized observability analysis
 Nonlinear least-squares solution
Models w/process noise, batch filter limitations
Nonlinear dynamic estimators: mechanizations & performance
 Extended Kalman Filter (EKF)
 Sigma-points filter/Unscented Kalman Filter (UKF)
 Particle filter (PF)
 Backwards-smoothing EKF (BSEKF)
Introduction of Gaussian sum techniques
Summary & conclusions
UT Austin March ‘11
4 of 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.
UT Austin March ‘11
5 of 35
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
UT Austin March ‘11
6 of 35
Example Tricycle Trajectory &
Relative Bearing Measurements

See 1st Matlab movie
UT Austin March ‘11
7 of 35
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 leastsquares problem
Failed observability test implies need for additional
sensing
UT Austin March ‘11
8 of 35
Observability Failure of Tricycle
Problem & a Fix


See 2nd Matlab movie for failure/nonuniqueness
See 3rd Matlab movie for fix via additional
sensing
UT Austin March ‘11
9 of 35
Geometry of Tricycle Dynamics &
Measurement Models

North, Y
V


X
m
m
Xm
Ym
T ricycle

m
Y
East, X
mth Merry- Go - Round
UT Austin March ‘11
10 of 35
Tricycle Dynamics Model from Kinematics

Constant-turn-radius transition from tk to tk+1 = tk +Dt:
V Δt tan k
X k 1  X k  Vk Δt[sin k cinc{ k
} cosk sinc{Vk Δt tan k }]
bw
bw
V Δt tan k
Yk 1  Yk  Vk Δt[ cos k cinc{ k
} sink sinc{Vk Δt tan k }]
bw
bw
 k 1   k 
Vk Δt tan k
bw
mk1  mk  mk Δt

mk 1

 
for m  1,2
mk
State & control vector definitions
xk  [ X k , Yk , k , 1k , 2k , 1k , 2k ]T

uk  [Vk ,  k ]T
Consistent with standard discrete-time state-vector
dynamic model form: xk 1  f k ( xk , uk )
UT Austin March ‘11
11 of 35
Bearing Measurement Model

Trigonometry of bearing measurement to mth merrygo-round rider
 mk  atan2{( X m  m cosmk  X k  br cosk ),...
(Y m  m sin mk  Yk  br sink )}

Sample-dependent measurement vector definition:

if only rider 1 shouts
 1k

if only rider 2 shouts
2k

zk   1k 
 2k  if both riders shout
[]
if neitherrider shouts

Consistent with standard discrete-time state-vector
measurement model form: zk  hk (xk )   k
UT Austin March ‘11
12 of 35
Nonlinear Batch Filter Model


Over-determined system of equations:
zbig  hbig( x0 ) big
Definitions of vectors & model function:
 z1 
z 
zbig   2 
  
 z N 
 1 
 
 big   2 
  
 N 
h1{ f 0 [ x0 , u0 ]}




h2{ f1[ f 0 ( x0 , u0 ), u1 ]}
hbig ( x0 )  




hN { f N 1[ f N  2 ( f N 3{, uN 3}, uN  2 ), uN 1 ]}
UT Austin March ‘11
13 of 35
Batch Filter Observability & Estimation

Linearized local observability analysis:
H big 

hbig
rank(Hbig)  dim(x0 ) ?
x0
Batch filter nonlinear least-squares estimation problem
find: x0
to minimize:
1
J ( x0 )  1 [ zbig  hbig ( x0 )]T R
big[ zbig  hbig ( x0 )]
2

Approximate estimation error covariance
Pxx0  E{( x0optx0)( x0optx0)T }
[
2J
2
x0 x
0opt
T
1
1
]1  [ H big
R
H
]
big big
UT Austin March ‘11
14 of 35
Example Batch Filter Results
Truth
Batch Estimate
30
North Position (m)
20
10
0
-10
-20
-30
-20
-10
0
10
20
30
East Position (m)
UT Austin March ‘11
40
50
60
15 of 35
Dynamic Models with Process Noise

Typical form driven by Gaussian white random
process noise vk:
xk 1  f k ( xk , uk , vk )


E{vk }  0, E{vk v Tj }  Qk  jk
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.
UT Austin March ‘11
16 of 35
Effect of Process Noise on Truth Trajectory
No Process Noise
Process Noise Present
30
North Position (m)
20
10
0
-10
-20
-30
-20
-10
0
10
20
30
East Position (m)
UT Austin March ‘11
40
50
60
17 of 35
Effect of Process Noise on Batch Filter
40
Truth
Batch Estimate
30
North Position (m)
20
10
0
-10
-20
-30
-20
-10
0
10
20
30
East Position (m)
UT Austin March ‘11
40
50
60
18 of 35
Dynamic Filtering based on Bayesian
Conditional Probability Density
p ( xk , v0 ,, vk 1 | z1,, zk )  C exp{ J }
J
1
2
T 1

 vi Qi vi
i 0
k 1

1
 [ zi1 - hi1( xi1)]T R
i 1[ zi1 - hi1( xi1)]
1
 1 ( x0 - xˆ 0 )T Pxx
0 ( x0 - xˆ 0 )
2
subject to xi for i = 0, …, k-1 determined as
functions of xk & v0, …, vk-1 via inversion of the
equations:
xi 1  fi ( xi , ui , vi ) for i  0,..,k1
UT Austin March ‘11
19 of 35
EKF Approximation

Uses Taylor series approximations of fk(xk,uk,vk) & hk(xk)



Gaussian statistics assumed




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
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
UT Austin March ‘11
20 of 35
EKF Performance, Moderate Initial Uncertainty
Truth
EKF Estimate
30
North Position (m)
20
10
0
-10
-20
-30
-30
-20
-10
0
10
20
30
East Position (m)
UT Austin March ‘11
40
50
60
70
21 of 35
EKF Performance, Large Initial Uncertainty
Truth
EKF Estimate
30
20
North Position (m)
10
0
-10
-20
-30
-40
-50
-60
-40
-20
0
20
East Position (m)
UT Austin March ‘11
40
60
80
22 of 35
Sigma-Points UKF Approximation

Evaluate fk(xk,uk,vk) & hk(xk) at specially chosen “sigma” points &
compute statistics of results



Gaussian statistics assumed, as in EKF





“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
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
UT Austin March ‘11
23 of 35
UKF Performance, Moderate Initial Uncertainty
Truth
UKF A Estimate
UKF B Estimate
30
North Position (m)
20
10
0
-10
-20
-30
-20
-10
0
10
20
30
East Position (m)
UT Austin March ‘11
40
50
60
70
24 of 35
UKF Performance, Large Initial Uncertainty
40
Truth
UKF A Estimate
UKF B Estimate
20
North Position (m)
0
-20
-40
-60
-20
0
20
40
East Position (m)
UT Austin March ‘11
60
80
100
25 of 35
Particle Filter Approximation

Approximate the conditional probability distribution using Monte-Carlo
techniques




Advantages




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
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 highdimensional problems
UT Austin March ‘11
26 of 35
PF Performance, Moderate Initial Uncertainty
Truth
Particle Filter Estimate
30
North Position (m)
20
10
0
-10
-20
-30
-30
-20
-10
0
10
20
30
East Position (m)
UT Austin March ‘11
40
50
60
70
27 of 35
PF Performance, Large Initial Uncertainty
Truth
Particle Filter Estimate
30
20
North Position (m)
10
0
-10
-20
-30
-40
-50
-20
0
20
40
East Position (m)
UT Austin March ‘11
60
80
28 of 35
Backwards-Smoothing EKF Approximation

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
UT Austin March ‘11
29 of 35
Implicit Smoothing in a Kalman Filter
3.5
Filter Output
1-Point Smoother
2-Point Smoother
3-Point Smoother
4-Point Smoother
5-Point Smoother
Truth
3
2.5
2
x1
1.5
1
0.5
0
-0.5
-1
-1.5
0
1
2
3
Sample Count, k
UT Austin March ‘11
4
5
30 of 35
BSEKF Performance, Moderate Initial Uncertainty
30
Truth
BSEKF A Estimate
BSEKF B Estimate
North Position (m)
20
10
0
-10
-20
-30
-20
-10
0
10
20
30
East Position (m)
UT Austin March ‘11
40
50
60
31 of 35
BSEKF Performance, Large Initial Uncertainty
Truth
BSEKF A Estimate
BSEKF B Estimate
30
20
North Position (m)
10
0
-10
-20
-30
-40
-50
-40
-20
0
20
East Position (m)
UT Austin March ‘11
40
60
32 of 35
A PF Approximates the Probability Density
Function as a Sum of Dirac Delta Functions
px(x), f(x)
0.6
0.4
Particle filter approximation
of original px(x) using
50 Dirac delta functions
0.2
0
-8
-6
-4
-2
0
x
2
4
6
8
30
using 50 Dirac delta functions
pf(f)
20
Particle filter approximation of
nonlinearly propagated pf(f)
10
0
0.1
0.15
0.2
0.25
0.3
0.35
f
GNC/Aug. ‘10
0.4
0.45
0.5
0.55
0.6
33 of 24
A Gaussian Sum Spreads the Component
Functions & Can Achieve Better Accuracy
100-element re-sampled Gaussian
approximation of original px(x)
probability density function
px(x), f(x)
0.6
100 Narrow weighted Gaussian
components of re-sampled mixture
0.4
0.2
0
-8
-6
-4
-2
0
x
2
4
6
8
30
EKF/100-narrow-element Gaussian
mixture approximation of
propagated pf(f) probability
density function
pf(f)
20
10
0
0.1
0.15
0.2
0.25
0.3
0.35
f
GNC/Aug. ‘10
0.4
0.45
0.5
0.55
0.6
34 of 24
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
UT Austin March ‘11
35 of 35