PROPAGATION OF PDF BY SOLVING THE KOLMOGOROV EQUATION

Transcription

1 PROPAGATION OF PDF BY SOLVING THE KOLMOGOROV EQUATION Puneet Singla Department of Mechanical & Aerospace Engineering University at Buffalo, Buffalo, NY-1460 Workshop: New Advances in Uncertainty Analysis & Estimation June 3rd, 014 Acknowledgement: K. Vishwajeet & N. Adurthi

2 KOLMOGOROV EQUATION Discrete System: x k+1 = Φ x k,t k,t k+1 ) + w k px k+1 ) = px k+1 x k )px k )dx k px k+1 x k ) = p wk x k+1 Φ x k,t k,t k+1 )) Continuous System: dx t = f x t,t)dt + Gx t,t)dβ t n px,t) p f i = t + 1 n n x i ) p[gqg] i j x i x j i=1 } {{ } Drift Term i=1 j=1 } {{ } Diffusion Term This is the differential form of the CKE which is known as the Fokker-Planck-Kolmogorov Equation FPKE) or Kolmogorov s Forward Equation. PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND / 36

3 SOLVING KOLMOGOROV EQUATION LINEAR SYSTEM LINEAR SYSTEM: x k+1 = F k x k + ν k Assumptions: x 0 N x 0 : ˆx 0,P 0 ), ν k N ν k : 0,Q k ) Time Evolution: px k+1 ) = N x k+1 : F k x k,q k ). N x k : ˆx k,p k ) dx k px k+1 ) = = 1 [ πqk exp 1 1 πqk. 1 πpk. exp [ 1 Simplifying the exponent 1 xk+1 F k x k ) T Q k ) T ) ] xk+1 F k x k Q k xk+1 F k x k 1 [. πpk exp 1 ) T ) ] xk ˆx k P k xk ˆx k dx k ) T ) xk+1 F k x k Q k xk+1 F k x k xk+1 F k x k ) 1 1 ) T ) ] xk ˆx k P k xk ˆx k dx k ) T ) xk ˆx k P k xk ˆx k PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 3 / 36

4 SOLVING KOLMOGOROV EQUATION LINEAR SYSTEM Simplifying the exponent 1 = 1 xk+1 F k x k ) T Q k x T k [ P k + Fk T }{{ Q } A ) 1 xk+1 F k x k ] k F k x k [ˆx T k P k + x T ] k+1 Q k F k }{{} c T ) T ) xk ˆx k P k xk ˆx k T x k + x ) k+1 Q k x k+1 + ˆx T k P k ˆx k 1 px k+1 ) = πqk. 1 [ πpk. exp 1 xt k+1 Q k x k+1 1 ] ˆxT k P k ˆx k exp [ 1 xt k Ax k + c T ] x k dxk Using the identity exp [ 1 xt Ax + c T x ] dx = πa exp [ 1 c T A T c ] 1 πa px k+1 ) = πqk. πpk [. exp 1 xt k+1 Q k x k+1 1 ˆxT k P k ˆx k + 1 ] ct A T c PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 4 / 36

5 SOLVING KOLMOGOROV EQUATION LINEAR SYSTEM Simplifying the exponent further gives 1 xt k+1 Q k x k+1 1 ˆxT k P k ˆx k + 1 ct A T c 1 xt k+1 Q k x k+1 1 ˆxT k P 1 [ xt k+1 Q k Q k F k P k + F T [ˆx T k P k k ˆx k x T ][ k+1 Q k F k P k + Fk T ] T Q T k F k [ˆx k Pk ) ] F k Q T k F k k Q k x k+1 + ˆx T k P k P k + F T ) F k Q T k F k k Q k }{{}}{{} C 1 [ ˆxT k Pk Pk P k + Fk T ) ] P Q k F k k ˆx k }{{} D 1 xt k+1 C x k+1 + b T x k+1 1 ˆxT k D ˆx k + x T ] T k+1 Q k F k b T x k+1 1 xk+1 C b ) T C xk+1 C b ) + 1 bt C b 1 ˆxT k D ˆx k PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 5 / 36

6 SOLVING KOLMOGOROV EQUATION LINEAR SYSTEM px k+1 ) = 1 [ π) n Q k P k A. exp 1 xk+1 C b ) T C xk+1 C b ) + 1 bt C b 1 ] ˆxT k D ˆx k [ C = Q k Q k C = Q k + F k P k F T k F k P k + Fk T ) ] F Q T k F k k Q k b T = ˆx T k P k C b = F k ˆx k P k + Fk T ) F Q T k F k k Q k b T C b = ˆx T k P k P k + Fk T ) F Q T k F k k Q k F ˆx k = ˆx T k FT k Fk P k Fk T + Q Fk k) ˆx k D = Pk Pk P k + Fk T ) P Q k F k k = Fk T Fk P k Fk T + Q Fk k) Q k P k A = Q k P k Pk + Fk T ) Q k F k = Fk P k F T + Q k by Sylvester s determinant theorem) px k+1 ) = 1 π) n F k P k Fk T + Q k [. exp 1 The prior pdf px k+1 ) has mean ˆx k+1 and covariance P k+1 given as ˆx k+1 = F k ˆx k ) T xk+1 F k ˆx k Fk P k Fk T + Q ) ) ] k xk+1 F k ˆx k P k+1 = F k P k Fk T + Q k PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 6 / 36

7 SOLVING KOLMOGOROV EQUATION LINEAR SYSTEM: x k+1 = F k x k + ν k TIME EVOLUTION px k+1 ) = 1 [ π) n P k+1. exp 1 ) T ) ] xk+1 ˆx k+1 P k+1 xk+1 ˆx k+1 The pdf px k+1 ) has mean ˆx k+1 and covariance P k+1 given as Hence, the prior pdf remains Gaussian ˆx k+1 = F k ˆx k P k+1 = F k P k F T + Q k As we know the pdf remains Gaussian, an easier approach is to directly compute the mean and covariance from the linear model equations as: ˆx k+1 = E[x k+1 ] = E [ ] F k x k + E[νk ] = F k E [ ] x k = Fk ˆx k P k+1 = E [ ) ) T ] [ ) ) T ] x k+1 F k ˆx k xk+1 F k ˆx k = E Fk x k + ν k F k ˆx k Fk x k + ν k F k ˆx k = E [ F k x k ˆx k )x k ˆx k ) T Fk T + ν kνk T ] = Fk P k F T + Q k PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 7 / 36

8 SOLVING KOLMOGOROV EQUATION LINEAR SYSTEM: dxt) = Ft)xt)dt +dβt) TIME EVOLUTION pxt)) = 1 [ π) n Pt). exp 1 ) T xt) ˆxt) Pt) xt) ˆxt) )] The pdf pxt) has mean ˆxt) and covariance Pt) given as: ˆxt) = Ft)ˆxt) Ṗt) = Ft)Pt) + Pt)F T t) + Qt) Hence, the prior pdf remains Gaussian PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 8 / 36

9 EVOLUTION OF PDF FOKKER PLANCK KOLMOGOROV EQUATION Consider the following n dimensional dynamical system dxt) = f xt),t)dt + Gxt),t)dβt) E[dβ t)β T t)] = Qt)dt pxt 0 )) = N x µ 0, Σ 0 ) Problem statement: Find pdf, px,t) x,t > t 0. Rate of change of px,t) is given by Fokker Planck Kolmogorov Equation FPKE): px,t) t = L F P px,t)) [ n p f i ) = + 1 n GQG T ) ] i j i=1 x i p i, j=1 x i x j FIGURE: evolution of pdf PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 9 / 36

10 EVOLUTION OF PDF FOKKER PLANCK KOLMOGOROV EQUATION INHERENT DIFFICULTIES IN NUMERICALLY SOLVING THE FPKE Positivity and normality constraint of pd:f px,t) 0, x,t and, R n px,t)dx = 1 No unique domain for enforcing boundary conditions: Domain of FPKE solution is, ) n. Numerical methods require finite domain. Number of spatial coordinates. FPKE contains the partial derivatives w.r.t. all the states. For a dimensional rigid body motion, we need discretization in 6 dimensions. Solution of FPKE for stationary pdf has been derived for a restricted class of dynamical systems: A.T.Fuller 1, Stratonovich 1 Analysis of nonlinear stochastic systems by means of Fokker-Planck equation Topics in the theory of random noise PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 10 / 36

11 EVOLUTION OF PDF FOKKER PLANCK KOLMOGOROV EQUATION INHERENT DIFFICULTIES IN NUMERICALLY SOLVING THE FPKE Positivity and normality constraint of pd:f px,t) 0, x,t and, R n px,t)dx = 1 No unique domain for enforcing boundary conditions: Domain of FPKE solution is, ) n. Numerical methods require finite domain. Number of spatial coordinates. FPKE contains the partial derivatives w.r.t. all the states. For a dimensional rigid body motion, we need discretization in 6 dimensions. Solution of FPKE for stationary pdf has been derived for a restricted class of dynamical systems: A.T.Fuller 1, Stratonovich How to find FPKE solution for non-stationary pdf 1 Analysis of nonlinear stochastic systems by means of Fokker-Planck equation Topics in the theory of random noise PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 10 / 36

12 SOLUTION OF FPKE STATIONARY PDF PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 11 / 36

13 SOLUTION OF FPKE STATIONARY PDF Let there be an n dimensional system described by F = ma. If F is conservative, then there exists a potential V, such that dx i dt Let us define new coordinates as follows: F = V = v i ; m dv i dt = i V Configuration Variables Conjugate Momenta q i = x i p i = m i ẋ i = m i v i If we define H q, p) as followed, H q, p) = Hamiltonian n p H q, p) = i +V q) i=1 m i then, following equations hold dq i dt = H p i d p i dt = H q i PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 1 / 36

14 SOLUTION OF FPKE STATIONARY PDF Let there be an n dimensional system described by F = ma. If F is conservative, then there exists a potential V, such that dx i dt Let us define new coordinates as follows: Configuration Variables F = V = v i ; m dv i dt q i = x i = i V Now, let us calculate dh dt. dh dt n H dq i = + H d p i i=1 q i dt p i dt n H H = H H i=1 q i p i p i q i = 0 Conjugate Momenta p i = m i ẋ i = m i v i If we define H q, p) as followed, H q, p) = Hamiltonian then, following equations hold n p H q, p) = i +V q) i=1 m i H q, p) is conserved. dq i dt = H p i d p i dt = H q i PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 1 / 36

15 SOLUTION OF FPKE STATIONARY PDF For an n dimensional dynamical system ẋ = fx), n = N, where, first N components correspond to positionx) and the last N to velocityẋ). Let y i = ẋ i. i.e. Using the Hamiltonian, dy i dt dx i dt = f i ; i = 1,,N = f i+n ; i = 1,,N Using FPKE and substituting for f i using Hamiltonian, dx i dt dy i dt = dq i dt = d p i dt = H p i = H q i px,t) t [ n p f i ) = + 1 n gqg T ) ] i j i=1 x i p i, j=1 x i x j N = i=1 x i N = i=1 q i p dx ) i + p dy ) i dt y i dt + 1 n i, j=1 p H ) p H )) p i p i q i + 1 n i, j=1 [ gqg T ) i j p ] x i x j [ gqg T ) i j p ] x i x j PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 13 / 36

16 SOLUTION OF FPKE STATIONARY PDF px,t) t px,t) t N = i=1 q i N = i=1 p H ) p H )) p i p i q i p q i H p i p p i H + 1 n i, j=1 [ gqg T ) i j p ] x i x j [ + p H p ) gqg H + 1 n T ) ] i j q i p i q i q i p i p i, j=1 x i x j Let px,t) = ph x 1,,x N,y 1,,y N )) = ph q 1,,q N, p 1,, p N )). Using the fact that for a stationary pdf px,t) t = 0. [ N p H H 0 = p ) gqg p H + 1 n T ) ] i j i=1 H q i p i H p i q i p i, j=1 x i x j If Q = 0, any function of H q 1,,q N, p 1,, p N ) will serve for ph q 1,,q N, p 1,, p N )) provided that it satisfies normality and boundary condition. [ gqg n 1 T ) ] i j Otherwise ph q 1,,q N, p 1,, p N )) is found by solving p = 0 i, j=1 x i x j PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 14 / 36

17 SOLUTION OF FPKE:STATIONARY PDF EXAMPLE:DUFFING OSCILLATOR Let us consider the following example of a duffing oscillator. ẍ + ηẋ + αx + βx 3 = Q; where, α = ;β = 3;η = 10. In state state form, it can be written as: ẋ 1 = x ; ẋ = Q ηx αx 1 βx 3 1 We define the following Hamiltonian kind function: Similar to integrating it. Remember P.E.V ) = F.dx H = x + αx 1 + βx4 1 4 Then, the equation of motion can be written as: dx 1 dt dx dt = H x ; = H x 1 η H x + Q and, H = K.E. + P.E. PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 15 / 36

18 SOLUTION OF FPKE:STATIONARY PDF EXAMPLE:DUFFING OSCILLATOR Further calculations: dx 1 dt = H x ; dx dt Writing FPKE for this system, we get, = H x 1 η H x + Q p H ) + p H ) +η p H ) + 1 x 1 x x x 1 x x Q p x }{{} = 0 = 0 As calculated earlier) η p H ) + 1 x x Q p x = 0 η p H + 1 x Q p = C x Using p±,t) = 0, constant) η p H x + 1 Q p x = 0 Further simplifications: η p H + 1 x Q p H = 0 H x η p + 1 Q p H = 0 p = p 0 exp ηh Q = p 0 exp η Q ) x )) + αx 1 + βx4 1 4 PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 16 / 36

19 SOLUTION OF FPKE:STATIONARY PDF EXAMPLE:DUFFING OSCILLATOR p = p 0 exp ηh Q = p 0 exp η Q Using pt,x)dx = 1, we get, p 0 = ) x )) + αx 1 + βx pdf 1 x x x x 1 a) pdf b) contour FIGURE: pdf and contours using analytical expression PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 17 / 36

20 SOLUTION OF FPKE:STATIONARY PDF EXAMPLE:VANDERPOL OSCILLATOR Finally, we consider the following example of a damping oscillator. ẍ + βẋ + x + α x + ẋ ) ẋ = gν; ν N 0,Q) where, β = 0.5;α = 0.15 and, Q = π 1 ;g = 1. The stationary pdf can be written as: px,ẋ) exp η g βx + ẋ ) + α x + ẋ ) )) X X 4 4 X X 1 FIGURE: pdf using analytical expression FIGURE: contours using analytical expression PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 18 / 36

21 SOLUTION OF FPKE ADAPTIVE GAUSSIAN MIXTURE MODEL With sufficient number of Gaussian components, any pdf can be approximated as closely as desired. ˆpx,t) wt i p gi i=1 p gi N ) x µ t, i Σt, i wt i = 1 wt i 0 i Gaussian kernel is an ideal choice to approximate solution of the FPKE. p gi ± ) = 0 Question is how to find unknown parameters of this Gaussian Sum Mixture? PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 19 / 36

22 SOLUTION OF FPKE ADAPTIVE GAUSSIAN MIXTURE MODEL Each Gaussian kernel capture the local behavior of non-gaussian density function. µ i t = f µi t,t) Σ i t = FΣt i + ΣtF i T + gqg T, F = f x,t) x x=µ t i Update the weights of Gaussian Sum Mixture such that FPKE error is minimized. 3 4 [ Residual Errore) = ˆp n t ˆp f i ) + 1 n gqg T ) ] i j i=1 x i ˆp i, j=1 x i x j N where, ˆp = wt i p gi i=1 3 Terejanu et. al., "Uncertainty propagation for nonlinear dynamic systems using Gaussian mixture models", JGCD, Terejanu et. al., "Adaptive Gaussian Sum Filter for Nonlinear Bayesian Estimation", TAC, 011 PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 0 / 36

23 SOLUTION OF FPKE ADAPTIVE GAUSSIAN MIXTURE MODEL [ gqg e = ˆp n t ˆp f i ) + 1 n T ) ] ˆp N i j i=1 x i ; ˆp = i, j=1 x i x j wt i p gi i=1 Terejanu et al. 5, for the first time, proposed a method to update these weights even during pure propagation. t = time diff. between two successive weight updates N 1 e = i=1 t ˆp = N t t wt i p gi i=1 N p = T ẇt i p gi + wt i g i i=1 µ t i where,ẇt i = 1 w i t t+ t wt) i ) wt+ t i wi t p gi + wt i p T ] g i µ t i µ t i pgi + wi t Tr[ Σ i Σt i t where, p gi = N xt) µ t i, Σ t i ) ] µ t ttr[ ) i + pgi wi Σ i Σ t i t Need to Replace ˆp = N i=1 wi t p gi in last two terms of e [ n ˆp f i ) + 1 n gqg T ) ] i j i=1 x i ˆp i, j=1 x i x j 5 Terejanu et al, JGCD 008, TAC 011 PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 1 / 36

24 SOLUTION OF FPKE ADAPTIVE GAUSSIAN MIXTURE MODEL Following Terejanu et. al. 6 N wt+ t e = i i=1 t p T [ p gi + wt i gi µ i µ t i t + Tr pgi Σ i Σ t i t ] p g i t ) } {{ } M DT [ gqg n ˆp f i ) + 1 n T ) ] ˆp i j i=1 x i i, j=1 x i x j N wt l f T ) p gl wt l p gl tr f l=1 x x [ gqg + 1 N wt l tr n T ) ] i j p g l l=1 i, j=1 x i x j = w T t L F P where, N L F P = f T p g l l=1 x p g l tr f x [ gqg + 1 N tr n T ) ] i j p g l l=1 i, j=1 x i x j Using ˆp = N i=1 w i t p gi, in the last two terms, we get, [ gqg n ˆp f i ) + 1 n T ) ] ˆp i j i=1 x i i, j=1 x i x j N = w l n [ ) gqg p gl f i t + 1 N l=1 i=1 x i wt l n T ) ] i j p g l l=1 i, j=1 x i x j 6 Terejanu et al, JGCD 008, TAC 011 1) Finally, Residual Errore) = p g T w t+ t t + w T t M DT L F P ) PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND / 36

25 SOLUTION OF FPKE ADAPTIVE GAUSSIAN MIXTURE MODEL Residual Errore) = p g T w t+ t t + w T t M DT L F P ) min w i t+ t J = 1 e t,x) dx V N subject to: wt+ t i = 1 i=1 w i t+ t 0,i = 1,,...,N PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 3 / 36

26 SOLUTION OF FPKE ADAPTIVE GAUSSIAN MIXTURE MODEL J = 1 V T p w t+ t g + wt T M DT L F P )) dx t Residual Errore) = min w i t+ t p g T w t+ t t + w T t M DT L F P ) J = 1 e t,x) dx V N subject to: wt+ t i = 1 i=1 w i t+ t 0,i = 1,,...,N J = 1 ) p T gp g wt t+ t V t dx w t+ t } {{ } M + ) wt+ t T p g V t M DT L F P ) T dx w t }{{} N + Terms containing w t min w i t+ t J = 1 wt t+ t Mw t+ + w T t+ t Nwt Convex Optimization Problem N subject to: wt+ t i = 1 i=1 wt+ t i 0,i = 1,,...,N PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 3 / 36

27 SOLUTION OF FPKE ADAPTIVE GAUSSIAN MIXTURE MODEL min w i t+ t J = 1 wt t+ t Mw t+ + w T t+ t Nwt N subject to: wt+ t i = 1 i=1 wt+ t i 0,i = 1,,...,N M = V ) T p g p g t dx Integral of product of two Gaussians: N x µ i, Σ i) N x µ j, Σ j) We get closed form!!! M i j = 1 π t Σ i + Σ j) 1 [ exp 1 µ i µ j) T Σ i + Σ j) µ i µ j)] PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 4 / 36

28 SOLUTION OF FPKE ADAPTIVE GAUSSIAN MIXTURE MODEL min w i t+ t J = 1 wt t+ t Mw t+ + w T t+ t Nwt N subject to: wt+ t i = 1 i=1 M = V wt+ t i 0,i = 1,,...,N ) T p g p g t dx Integral of product of two Gaussians: N x µ i, Σ i) N x µ j, Σ j) We get closed form!!! M i j = 1 π t Σ i + Σ j) 1 [ exp 1 µ i µ j) T Σ i + Σ j) µ i µ j)] ) p g N = V t M DT L F P ) T dx N p T gi M DT = i=1 µ t i [ ] µ t i + Tr pgi Σ i Σ t i t p ) g i t N L F P = f T p g l l=1 x p g l tr f x [ gqg + 1 N tr n T ) ] i j p g l l=1 i, j=1 x i x j n i j = 1 t V pg i pg i t pg j + pg i 1 N p gi tr l=1 p T [ ] g j pg µ j µ j j + p gi Tr P j Ṗ j p g j x ft,x) + pg i pg j Tr n i, j=1 [ ] ft,x) [ x gqg T ) ] i j pg l dx x i x j PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 4 / 36

29 ADAPTIVE GAUSSIAN MIXTURE MODEL DISCRETE TIME EQUATION OF SYSTEM:DISCRETE TIME x k+1 = φk,x k ) + η k, η k = N 0,Q k ) pdf at t = 0 is pt 0,x 0 ). Find pdf pt k,x) at any given time t = t k > 0, while taking into consideration the solution of the CKE. CKE: pt k+1,x k+1 ) = Gaussian Mixture Approximation: pt k+1,x k+1 t k,x k )pt k,x k )dx k pt k+1,x k+1 ) = ˆpt k+1,x k+1 ) + Residual errore) N ˆpt k+1,x k+1 ) = w i k+1 N x k+1 µ i k+1,pi k+1 ) i=1 Constraints: N wt=k i = 1; i=1 wi t=k 0, i PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 5 / 36

30 ADAPTIVE GAUSSIAN MIXTURE MODEL DISCRETE TIME Each Gaussian kernel capture the local behavior of non-gaussian density function. µ i k+1 = φt k µ i k ) P i k+1 = FPi k + Pi k FT + Q k, F = φk,x k ) x xk =µ i k k Update the weights of Gaussian Sum Mixture such that following residual error is minimized over the whole domain. 7 8 residual errore) = pt k+1,x k+1 ) ˆpt k+1,x k+1 ). 1 min: pt k+1,x k+1 ) ˆpt k+1,x k+1 ) dx k+1 N subject to: w i k+1 = 1 wi k+1 0 i=1 7 Terejanu et. al., "Uncertainty propagation for nonlinear dynamic systems using Gaussian mixture models", JGCD, Terejanu et. al., "Adaptive Gaussian Sum Filter for Nonlinear Bayesian Estimation", TAC, 011 PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 6 / 36

31 ADAPTIVE GAUSSIAN MIXTURE MODEL DISCRETE TIME Minimizing the residual error in the least square senseterejanu et. al. 9 ): Expanding the terms we get: 1 min: pt k+1,x k+1 ) ˆpt k+1,x k+1 ) dx k+1 N subject to: w i k+1 = 1 wi k+1 0 i=1 1 pt k+1,x k+1 ) ˆpt k+1,x k+1 ) dx k+1 = 1 pt k+1,x k+1 ) dx k+1 pt k+1,x k+1 ) ˆpt k+1,x k+1 ) dx k+1 }{{} constant + 1 ˆpt k+1,x k+1 ) dx k+1 9 Terejanu et al, JGCD 008, TAC 011 PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 7 / 36

32 ADAPTIVE GAUSSIAN MIXTURE MODEL DISCRETE TIME Ignoring constant, J = 1 ˆpt k+1,x k+1 ) dx k+1 pt k+1,x k+1 ) ˆpt k+1,x k+1 ) dx k+1 Substituting ˆpt k+1,x k+1 ) = N i=1 w i k+1 N x k+1 µ i k+1,pi k+1 ), we get, J = 1 N N w i k+1 N x k+1 µ i k+1,pi k+1 ) dx k+1 w i k+1 N x k+1 µ i k+1,pi k+1 )pt k+1,x k+1 )dx k+1 i=1 i=1 = 1 wt k+1 Mw k+1 w T k+1 y m i j = N x k+1 µ i k+1,pi k+1)n x k+1 µ j k+1,pj k+1 )dx k+1 ) = π P i k+1 + P j k+1 / exp[ 1 µi k+1 µ j k+1 )T P i k+1 + P j k+1 ) µ i k+1 µ j k+1 )] y i = pt k+1,x k+1 )N x k+1 µ i k+1,pi k+1 )dx k+1 PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 8 / 36

33 DISCRETE TIME AGMM DISCRETE TIME y i = pt k+1,x k+1 )N x k+1 µ i k+1,pi k+1 )dx k+1 Using CKE, replace pt k+1,x k+1 ). y i = N x k+1 µ i k+1,pi k+1 )dx k+1 pt k+1,x k+1 t k,x k )pt k,x k )dx k } {{ } CKE For the given system, pt k+1,x k+1 t k,x k ) = N x k+1 φk,x k ),Q k ). y i = = = N x k+1 µ i k+1,pi k+1 )N x k+1 φk,x k ),Q k )pt k,x k )dx k dx k+1 pt k,x k ) N x k+1 µ i k+1,pi k+1 )N x k+1 φk,x k ),Q k )dx k+1 dx k }{{} Integral of product of two Gaussian pt k,x k )N φk,x k ) µ i k+1,pi k+1 + Q k)dx k PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 9 / 36

34 DISCRETE TIME AGMM: MINIMIZING RESIDUAL ERROR CONVEX OPTIMIZATION y i = pt k,x k )N φk,x k ) µ i k+1,pi k+1 + Q k)dx k Replace pt k,x k ) with ˆpt k,x k ) = N i=1 wi k N x k µ i k,pi k ) N y i = w j k N x k µ j k,pj k )N φk,x k ) µ i k+1,pi k+1 + Q k)dx k j=1 N = w j k n i j j=1 n i j = N fk,x k ) µ i k+1,pi k+1 + Q k)n x k µ j k,pj k )dx k min w k+1 J = 1 wt k+1 Mw k+1 w T k+1 Nw k subject to: 1 T w k+1 = 1 w k+1 0 PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 30 / 36

35 FPKE/CKE EXAMPLE: DUFFING OSCILLATOR Duffing Oscillator with a soft spring: ẍ + ηẋ + αx + βx 3 = gt)g t) gt) = 1, Q = 1, η = 10, α =, β = 3. )) Stationary pdf: px, ẋ) exp η α x + β g Q 4 x4 + 1 ẋ a) True pdf b) FPKE Solution c) CKE Solution PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 31 / 36

36 FPKE/CKE EXAMPLE: VANDERPOL OSCILLATOR Vanderpol Oscillator : ẍ + βẋ + x + αx + ẋ )ẋ = gt)g t) gt) = 1, Q = 1/π, α = 0.15, β = 0.5. Stationary pdf: px, ẋ) exp η g βx + ẋ ) + α x + ẋ ) )) d) True pdf e) FPKE Solution f) CKE Solution PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 3 / 36

37 FPKE/CKE EXAMPLE:LORENZ SYSTEM Consider the following system: ẋ = α x + y) ẏ = βx y xz ż = γz + xy α = 10 β = 8 γ = 0 3 px,y,z,t 0 ) = N µ 0, Σ 0 ) µ = [ ] Σ = diag1,1,1) g) XY Plane h) YZ Plane i) XZ Plane FIGURE: contours after sec PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 33 / 36

38 ADAPTIVE GAUSSIAN MIXTURE MODEL EXAMPLE:TWO BODY PROBLEM The equations of motion of a satellite with atmospheric drag & J effect is 10 : ẍ + µx µ ) ) r 3 = J x + a Dx, J x =.5J Re 1 r 5 z r r ÿ + µy µ ) ) r 3 = J y + a Dy, J y =.5J Re 1 r 5 z r r z + µz µ ) r 3 = J z + a Dz, J z =.5J Re r r ) x r ) y r ) ) 3 5 z z r r a Dx, a Dy, and a Dz denote the atmospheric drag forces modeled as: a Dx = 1 Bρ ẋ + Ω ey) ẋ + Ω e y) + ẏ Ω e x) + ż) a Dy = 1 Bρ ẏ Ω ex) ẋ + Ω e y) + ẏ Ω e x) + ż) a Dz = 1 Bρż ẋ + Ω e y) + ẏ Ω e x) + ż) T Initial Conditions: pt 0,x,y,z,ẋ,ẏ,ż) = N x; µ }{{} 0,P 0 ), µ 0 = 7 103,0,0,0,.0374,7.4771, }{{}}{{} x T km km/sec P 0 = diag 0.01,0.01,0.01,10 6,10 6,10 6 }{{}}{{} km km /sec ) 3) 10 Kumar et. al., "Nonlinear Uncertainty Propagation for Perturbed Two-Body Orbits", JGCD, 014 PUNEET SINGLA LAIRS.ENG.BUFFALO.EDU) UNCERTAINTY ANALYSIS & ESTIMATION 014 ACC PORTLAND 34 / 36

39 A DAPTIVE G AUSSIAN M IXTURE M ODEL E XAMPLE :T WO B ODY P ROBLEM zin km) Yin km) Xin km) a) pdf contours in XY plane at t = b) pdf contours in XZ plane at t = CKE FPKE 00 CKE xin km) 150 EKF 10 0 zin km) yin km) 0 UKF EKF FPKE MC : UKF xin km) c) pdf contours in XY plane at 3. hr F MC xin km) d) pdf contours in XZ plane at 3. hr P UNEET S INGLA LAIRS. ENG. BUFFALOcorresponding. EDU ) U NCERTAINTY NALYSIS IGURE Contours to the Aprior pdf&ine STIMATION cartesian 014 ACC P ORTLAND coordinates with drag using 35 / 36

40 A DAPTIVE G AUSSIAN M IXTURE M ODEL E XAMPLE :T WO B ODY P ROBLEM CKE MC EKF EKF 700 yin km) 00 UKF 10 MC Points UKF Mean CKE Mean EKF Mean FPKE Mean MC Mean FPKE xin km) UKF MC xin km) a) pdf contours in XY plane at 4 hr b) pdf contours in XZ plane at 4 hr EKF 500 CKE FPKE 60 zin km) yin km) FPKE CKE 90 zin km) 80 MC FPKE UKF CKE EKF UKF MC xin km) 6985 c) pdf contours in XY plane at hr F : xin km) d) pdf contours in XZ plane at hr P UNEET S INGLA LAIRS. ENG. BUFFALOcorresponding. EDU ) U NCERTAINTY NALYSIS IGURE Contours to the Aprior pdf&ine STIMATION cartesian 014 ACC P ORTLAND coordinates with drag using 36 / 36