Nonlinear Filtering. With Polynomial Chaos. Raktim Bhattacharya. Aerospace Engineering, Texas A&M University uq.tamu.edu

Transcription

1 Nonlinear Filtering With Polynomial Chaos Raktim Bhattacharya Aerospace Engineering, Texas A&M University uq.tamu.edu

2 Nonlinear Filtering with PC Problem Setup. Dynamics: ẋ = f(x, ) Sensor Model: ỹ = h(x) + ν where ν is measurement noise with E [ν] = 0, and E [ νν T ] = R. Measurements available at times t k, t k+1, Parametric uncertainty := ( x0 ρ ) Dynamics transformed to ẋ pc = F pc (x pc ) Initial Condition at time t k x pci (t k ) = x0 ϕ i ( )p(k, )d D Workshop on Uncertainty Analysis & Estimation (ACC 2014) 2 /19

3 Filtering with Higher Order Moment Updates 1. Propagation tk=1 x pc (t k+1 ) = x pc (t k ) + F pc (x pc (τ))dτ t k 2. Get Prior Moments from x pc (t k+1 ) recall x pc := vec (X) and X := [x 0 x 1 x N ]. M 1 i M 2 ij M 3 ijk = p = X i0 = X ip X jq ϕ p ϕ q p q X ip X jq X kr ϕ p ϕ q ϕ r and so on... Inner products are with respect to p(t k, ) q r Workshop on Uncertainty Analysis & Estimation (ACC 2014) 3 /19

4 Filtering with Higher Order Moment Updates 3. Update. Incorporate measurements ỹ := ỹ(t k+1 ) and prior moments to get posterior estimates Consider prior state estimate to be ˆx := E [x] = M 1 Let v := ỹ ŷ = h(x) + ν h(ˆx ) Use linear gain K to update moments as K = P xv (P vv ) 1, P xv ij M 1+ M 2+ M 3+ = M 1 + Kv = M 2 + KP vv K T = E [ x i v T j ], P vv ij = E [ v i v T ] j = M 3 + 3K 2 P xvv 3KP xxv K 3 P vvv Workshop on Uncertainty Analysis & Estimation (ACC 2014) 4 /19

5 Filtering with Higher Order Moment Updates 4. Estimation of Posterior PDF max p k+1 ( ) log(p k+1 ( )) d, p k+1 ( ) D subject to p k+1 ( ) d = M 1+ D D Q 3 ( )p k+1 ( ) d = M 3+ Approximate with p k+1 ( ) = D p k+1 ( )d = 1. Q 2 ( )p k+1 ( ) d = M 2+ D Q 4 ( )p k+1 ( ) d = M 4+ D M α i N (µ i, Σ i ), i M α i = 1, p k+1 ( ) 0 α i 0. i Workshop on Uncertainty Analysis & Estimation (ACC 2014) 5 /19

6 Filtering with Higher Order Moment Updates Example Classical duffing oscillator Two state system, x = [x 1, x 2 ] T, Dynamics ẋ 1 = x 2, ẋ 2 = x x 2 x 3 1. Uncertainty in x 0 N ([1, 1], diag(1, 1)) Simulation x 0 = [2, 2] T Scalar measurement model ỹ = x T x + ν, E [ν] = 0 and E [ νν T ] = Workshop on Uncertainty Analysis & Estimation (ACC 2014) 6 /19

7 Results. (a) EKF based estimator with 0.2s update. (b) EKF based estimator with 0.3s update. Figure : Performance of EKF estimators. If data rate is high, no need for nonlinear estimators!! Workshop on Uncertainty Analysis & Estimation (ACC 2014) 7 /19

8 EKF works with faster updates Otherwise we need nonlinear non Gaussian algorithms. (a) gpc based estimator with first two moments updated every 0.3s. (b) gpc based estimator with first three moments updated every 0.5s. Figure : Performance of gpc estimators. If data rate is low, we need nonlinear estimators, with higher order updates! Workshop on Uncertainty Analysis & Estimation (ACC 2014) 8 /19

9 Publications. 1. P. Dutta, R. Bhattacharya, Nonlinear Estimation of Hypersonic Flight Using Polynomial Chaos, AIAA GNC, P. Dutta, R. Bhattacharya, Nonlinear Estimation with Polynomial Chaos and Higher Order Moment Updates, IEEE ACC 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 ( ), Workshop on Uncertainty Analysis & Estimation (ACC 2014) 9 /19

10 Nonlinear Filtering With Frobenius-Perron Operator Raktim Bhattacharya Aerospace Engineering, Texas A&M University uq.tamu.edu

11 Frobenius-Perron Operator. Given dynamics ẋ = F (t, x), x is augmented state variable captures state and system parameters (including those from KL expansion) p(t 0, x) as the initial state density function. Evolution of density p(t, x) := P t p(t 0, x). P t is defined by p t + (pf ) = 0 Equations ẋ = F (t, x) ṗ = p F Workshop on Uncertainty Analysis & Estimation (ACC 2014) 11 /19

12 Assumptions. Measurements are available at times t 1,, t k 1, t k, t k+1, x k and y k are state and measurement at t k Measurement model y = h(x) + ν E [ν] = 0, E [ νν ] T = R p k ( ) := p(t k, ) p k ( ) is prior at t k p + k ( ) is posterior at t k D x is domain of state augmented Workshop on Uncertainty Analysis & Estimation (ACC 2014) 12 /19

13 A Particle Filter Based Algorithm. 1. Initialize Domain D x is sampled according to p 0 (x) x 0,i samples of r.v. x 0 p 0,i := p 0 (x 0,i ) Recursively apply steps 2,, 6 for k = 1, Workshop on Uncertainty Analysis & Estimation (ACC 2014) 13 /19

14 A Particle Filter Based Algorithm (contd.). 2. Propagate ( xk,i p k,i ) = ( ) tk ( xk 1,i + p k 1,i t k 1 F p F ) dt p k,i because it is prior state PDF Workshop on Uncertainty Analysis & Estimation (ACC 2014) 14 /19

15 A Particle Filter Based Algorithm (contd.) 3. Determine likelihood function p(ỹ k x k = x k,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 measurement noise l(ỹ k x k = x k,i ) = 1 (2π)m R e 0.5(ỹ k h(x k,i)) T R 1 (ỹ k h(x k,i )), 4. Update: Get Posterior p + k,i := p k(x k = x k,i ỹ k ) = l(ỹ k x k = x k,i )p k (x k = x k,i ) N l(ỹ k x k = x k,j )p k (x k = x k,j ) j=1 Workshop on Uncertainty Analysis & Estimation (ACC 2014) 15 /19

16 A Particle Filter Based Algorithm (contd.) 5. Get State Estimate a Maximum-Likelihood Estimate: Maximize the probability that x k,i = ˆx k ˆx k = mode p + k (x k,i). b Minimum-Variance Estimate: The estimate is the mean of p + k (x k,i) ˆx k = arg min x N N x x k,i 2 p + k (x k,i) = x k,i p + k (x k,i) i=1 i=1 c Minimum-Error Estimate: Minimize maximum x x k,i All same for Gaussian p(t k, x) ˆx = median p + k (x k,i) Workshop on Uncertainty Analysis & Estimation (ACC 2014) 16 /19

17 A Particle Filter Based Algorithm (contd.) 6. Resample Detect degeneracy from p + k (x k,i) p + k (x k,i) < ϵ x k,i is degenerate. Use existing methods for resampling from the new distribution p + k (x k,i). Importance sampling moderate size problems Resampling simple random, multinomial, stratified, systematic Qualitatively, since histogram techniques are not used in determining density functions, this method is less sensitive to the issue of degeneracy. Workshop on Uncertainty Analysis & Estimation (ACC 2014) 17 /19

18 Example. 3 DOF Vinh s Equation Models motion of spacecraft during planetary entry ḣ = V sin(γ) V = ρr 0 V 2 gr 0 sin(γ) 2B c vc 2 γ = ρr0 C L V + gr0 cos(γ) 2B c C D vc 2 ( V R 0 + h 1 V ) R 0 radius of Mars ρ atmospheric density v c escape velocity C L C lift over drag D B c ballistic coefficient h height V velocity γ flight path angle Measurement Model [ ỹ = q = 1 ] 2 ρv 2, Q = kρ 1 2 V 3.15, γ [ ] E [ν] = 0 3 1, E νν T = I 3 scaled Gaussian initial condition uncertainty µ 0 = [54 km, 2.4 km/s, 9 ] T Σ = diag[5.4 km, 240 km/s, 0.9 ] Workshop on Uncertainty Analysis & Estimation (ACC 2014) 18 /19

19 Example (contd.). Compared with generic particle filter and Bootstrap filter All 3 perform equally well FP requires much less number of samples Particle Filter: samples Bootstrap Filter: samples Frobenius-Perron Filter: 7000 samples Generic Particle filter Bootstrap filter FP operator based filter s s s Table : Computational time for each filter Details 1. P. Dutta and R. Bhattacharya, Hypersonic State Estimation Using Frobenius-Perron Operator, AIAA Journal of Guidance, Control, and Dynamics, Volume 34, Number 2, Workshop on Uncertainty Analysis & Estimation (ACC 2014) 19 /19