Aero Emech Nonlinearest Mar2011 r02
-
Upload
juan-carlos-ladino-vega -
Category
Documents
-
view
225 -
download
0
Transcript of 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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
3/35
UT Austin March 11 3 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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
4/35
UT Austin March 11 4 of 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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
5/35
UT Austin March 11 5 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.
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
6/35
A Blind Tricyclist Measuring Relative
Bearing to a Friend on a Merry-Go-Round
UT Austin March 11 6 of 35
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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
7/35
Example Tricycle Trajectory &
Relative Bearing Measurements See 1stMatlab movie
UT Austin March 11 7 of 35
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
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
UT Austin March 11 8 of 35
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
9/35
Observability Failure of Tricycle
Problem & a Fix See 2ndMatlab movie for failure/non-
uniqueness
See 3rdMatlab movie for fix via additional
sensing
UT Austin March 11 9 of 35
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
10/35
Geometry of Tricycle Dynamics &
Measurement Models
UT Austin March 11 10 of 35
m
mm
mX mY XEast,
YNorth,
Y
X
Tricycle
Round-Go-Merrythm
V
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
11/35
UT Austin March 11 11 of 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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
12/35
UT Austin March 11 12 of 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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
13/35
UT Austin March 11 13 of 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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
14/35
UT Austin March 11 14 of 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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
15/35
Example Batch Filter Results
UT Austin March 11 15 of 35
-20 -10 0 10 20 30 40 50 60
-30
-20
-10
0
10
20
30
East Position (m)
NorthPosition(m)
Truth
Batch Estimate
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
16/35
UT Austin March 11 16 of 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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
17/35
Effect of Process Noise on Truth Trajectory
UT Austin March 11 17 of 35
-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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
18/35
Effect of Process Noise on Batch Filter
UT Austin March 11 18 of 35
-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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
19/35
UT Austin March 11 19 of 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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
20/35
UT Austin March 11 20 of 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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
21/35
EKF Performance, Moderate Initial Uncertainty
UT Austin March 11 21 of 35
-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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
22/35
EKF Performance, Large Initial Uncertainty
UT Austin March 11 22 of 35
-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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
23/35
UT Austin March 11 23 of 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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
24/35
UKF Performance, Moderate Initial Uncertainty
UT Austin March 11 24 of 35
-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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
25/35
UKF Performance, Large Initial Uncertainty
UT Austin March 11 25 of 35
-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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
26/35
UT Austin March 11 26 of 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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
27/35
PF Performance, Moderate Initial Uncertainty
UT Austin March 11 27 of 35
-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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
28/35
PF Performance, Large Initial Uncertainty
UT Austin March 11 28 of 35
-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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
29/35
UT Austin March 11 29 of 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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
30/35
UT Austin March 11 30 of 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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
31/35
BSEKF Performance, Moderate Initial Uncertainty
UT Austin March 11 31 of 35
-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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
32/35
BSEKF Performance, Large Initial Uncertainty
UT Austin March 11 32 of 35
-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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
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
-
8/13/2019 Aero Emech Nonlinearest Mar2011 r02
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
UT Austin March 11 35 of 35