NonlinearFiltering - GitHub Pages · NonlinearFiltering With Frobenius-Perron Operator Raktim...

Nonlinear Filtering2015 ACC Workshop on Uncertainty Analysis & Estimation

Raktim Bhattacharya

Intelligent Systems Research LaboratoryAerospace Engineering, Texas A&M University.

isrlab.github.io

Nonlinear FilteringWith Polynomial Chaos

Raktim BhattacharyaAerospace Engineering, Texas A&M University

isrlab.tamu.edu

Nonlinear Filtering with PC Nonlinear Filtering with Frobenius-Perron

Nonlinear Filtering with PCProblem Setup

Dynamics: x = f(x,∆) Sensor Model: y = h(x) + ν

where ν is measurement noise with

E [ν] = 0, and E[ννT

]= R.

Measurements available at times tk, tk+1, · · ·

Parametric uncertainty

∆ :=

(∆x0

∆ρ

) Dynamics transformed to

xpc = F pc(xpc)

Initial Condition at time tk

xpci(tk) =

∫D∆

∆x0ϕi(∆)p(k,∆)d∆

Workshop on Uncertainty Analysis & Estimation (ACC 2015) 3 / 20


Filtering with Higher Order Moment Updates1. Propagation

xpc(tk+1) = xpc(tk) +

∫ tk=1

tk

F pc(xpc(τ))dτ

2. Get Prior Moments from xpc(tk+1)recall xpc := vec (X) and X := [x0 x1 · · · xN ]

M1−

i = Xi0

M2−

ij =∑p

∑q

XipXjq ⟨ϕpϕq⟩

M3−

ijk =∑p

∑q

∑r

XipXjqXkr ⟨ϕpϕqϕr⟩ and so on ...

Inner products are with respect to p(tk,∆)



Filtering with Higher Order Moment Updates3. Update

Incorporate measurements y := y(tk+1) and prior moments to getposterior estimatesConsider prior state estimate to be x− := E [x] = M1−

Letv := y − y− = h(x) + ν − h(x−)

Use linear gain K to update moments as

K = P xv (P vv)−1

, Pxvij = E

[xiv

Tj

], P vv

ij = E[viv

Tj

]M1+ = M1− +Kv

M2+ = M2− +KP vvKT

M3+ = M3− + 3K2P xvv − 3KP xxv −K3P vvv



Filtering with Higher Order Moment Updates4. Estimation of Posterior PDF

maxpk+1(∆)

−∫D∆

pk+1(∆) log(pk+1(∆)) d∆,

subject to∫D∆

∆pk+1(∆) d∆ = M1+

∫D∆

Q2(∆)pk+1(∆) d∆ = M2+∫D∆

Q3(∆)pk+1(∆) d∆ = M3+

∫D∆

Q4(∆)pk+1(∆) d∆ = M4+

Approximate

pk+1(∆) =

M∑i

αiN (µi,Σi),

with∫D∆

pk+1(∆)d∆ = 1 ⇒M∑i

αi = 1, pk+1(∆) ≥ 0 ⇒ αi ≥ 0.



Filtering with Higher Order Moment UpdatesExample

Classical duffing oscillatorTwo state system, x = [x1, x2]

T ,Dynamics

x1 = x2, x2 = −x1 −1

4x2 − x3

1.

Uncertainty in x0 ∼ N ([1, 1], diag(1, 1))Simulation x0 = [2, 2]T

Scalar measurement model y = xTx+ ν,

E [ν] = 0 and E[ννT

]= 0.006.



Results

(a) EKF based estimator with 0.2supdate.

(b) EKF based estimator with 0.3supdate.

Figure: Performance of EKF estimators.

If data rate is high, no need for nonlinear estimators!!



EKF works with faster updatesOtherwise we need nonlinear non Gaussian algorithms

(a) gPC based estimator with first twomoments updated every 0.3s.

(b) gPC based estimator with firstthree moments updated every 0.5s.

Figure: Performance of gPC estimators.

If data rate is low, we need nonlinear estimators, with higher orderupdates!



Publications1. P. Dutta, R. Bhattacharya, Nonlinear Estimation of Hypersonic Flight Using Polynomial

Chaos, AIAA GNC, 2010.2. P. Dutta, R. Bhattacharya, Nonlinear Estimation with Polynomial Chaos and Higher Order

Moment Updates, IEEE ACC 2010.3. P. Dutta, R. Bhattacharya, Nonlinear Estimation of Hypersonic State Trajectories in

Bayesian Framework with Polynomial Chaos, Journal of Guidance, Control, and Dynamics,vol.33 no.6 (1765-1778), 2011.


Nonlinear FilteringWith Frobenius-Perron Operator

Raktim BhattacharyaAerospace Engineering, Texas A&M University

uq.tamu.edu


Frobenius-Perron OperatorGiven dynamics x = F (t,x),

x is augmented state variable – captures state and system parameters(including those from KL expansion)p(t0,x) as the initial state density function.

Evolution of densityp(t,x) := Pt p(t0,x).

Pt is defined by∂p

∂t+∇ · (pF ) = 0

Equationsx = F (t,x)

p = −p∇ · F



AssumptionsMeasurements are available at times t1, · · · , tk−1, tk, tk+1, · · ·xk and yk are state and measurement at tk

Measurement modely = h(x) + ν

E [ν] = 0, E[ννT

]= R

pk(·) := p(tk, ·)p−k (·) is prior at tkp+k (·) is posterior at tkDx is domain of state augmented



A Particle Filter Based Algorithm1. Initialize

Domain Dx is sampled according to p0(x)

x0,i samples of r.v. x0

p0,i := p0(x0,i)

Recursively apply steps 2, · · · , 6 for k = 1, · · ·



A Particle Filter Based Algorithm (contd.)2. Propagate (

xk,i

p−k,i

)=

(xk−1,i

pk−1,i

)+

∫ tk

tk−1

(F

−p∇ · F

)dt

p−k,i because it is prior state PDF



A Particle Filter Based Algorithm (contd.)3. Determine likelihood function p(yk|xk = xk,i)

for each grid point i,using Gaussian measurement noise and sensor model

y = h(x) + ν

|R| is the determinant of the covariance matrix of measurementnoise

l(yk|xk = xk,i) =1√

(2π)m|R|e−0.5(yk−h(xk,i))

TR−1(yk−h(xk,i)),

4. Update: Get Posterior

p+k,i := pk(xk = xk,i|yk) =l(yk|xk = xk,i)p

−k (xk = xk,i)

N∑j=1

l(yk|xk = xk,j)p−k (xk = xk,j)



A Particle Filter Based Algorithm (contd.)5. Get State Estimatea Maximum-Likelihood Estimate: Maximize the probability thatxk,i = xk

xk = mode p+k (xk,i)

b Minimum-Variance Estimate: The estimate is the mean of p+k (xk,i)

xk = arg minx

N∑i=1

||x− xk,i||2 p+k (xk,i) =

N∑i=1

xk,i p+k (xk,i)

c Minimum-Error Estimate: Minimize maximum |x− xk,i|

x = median p+k (xk,i)

All same for Gaussian p(tk,x)



A Particle Filter Based Algorithm (contd.)6. Resample

Detect degeneracy from p+k (xk,i)

p+k (xk,i) < ϵ ⇒ xk,i is degenerate

Use existing methods for resampling from the new distributionp+k (xk,i).

▶ Importance sampling moderate size problems▶ Resampling – simple random, multinomial, stratified, systematic

Qualitatively, since histogram techniques are not used in determiningdensity functions, this method is less sensitive to the issue of degeneracy.



Example3 DOF Vinh’s Equation Models motion of spacecraft during planetary entry

h = V sin(γ)

V = −ρR0

2Bc

V2 −

gR0

v2c

sin(γ)

γ =ρR0

2Bc

CL

CD

V +gR0

v2c

cos(γ)(

V

R0 + h−

1

V

)

R0 – radius of Marsρ – atmospheric densityvc – escape velocityCLCD

– lift over drag

Bc – ballistic coefficienth – heightV – velocityγ – flight path angle

Measurement Model

y =

[q =

1

2ρV

2, Q = kρ

12 V

3.15, γ

]E [ν] = 03×1, E

[νν

T]= 6 × 10

−5I3 scaled

Gaussian initial condition uncertainty

µ0 = [54 km, 2.4 km/s,−9◦]T

Σ = diag[5.4 km, 240 km/s,−0.9◦]



Example (contd.)Compared with generic particle filter and Bootstrap filterAll 3 perform equally well – FP requires much less number ofsamples

▶ Particle Filter: 25000 samples▶ Bootstrap Filter: 20000 samples▶ Frobenius-Perron Filter: 7000 samples

Generic Particle filter Bootstrap filter FP operator based filter

207.96 s 168.06 s 57.42 s

Table: Computational time for each filter

Details1. P. Dutta and R. Bhattacharya, Hypersonic State Estimation Using Frobenius-Perron

Operator, AIAA Journal of Guidance, Control, and Dynamics, Volume 34, Number 2, 2011.


NonlinearFiltering - GitHub Pages · NonlinearFiltering With Frobenius-Perron Operator Raktim...

Documents

Transcript of NonlinearFiltering - GitHub Pages · NonlinearFiltering With Frobenius-Perron Operator Raktim...