Mini-Course 07 Kalman Particle Filters. Henrique Massard da Fonseca Cesar Cunha Pacheco Wellington Bettencurte Julio Dutra

Size: px

Start display at page:

Download "Mini-Course 07 Kalman Particle Filters. Henrique Massard da Fonseca Cesar Cunha Pacheco Wellington Bettencurte Julio Dutra"

Sandra Mathews
5 years ago
Views:

1 Mini-Course 07 Kalman Particle Filters Henrique Massard da Fonseca Cesar Cunha Pacheco Wellington Bettencurte Julio Dutra

2 Agenda State Estimation Problems & Kalman Filter Henrique Massard Steady State Kalman Filter, Extended & Encented Kalman Filter Cesar Cunha Pacheco Particle Filter Wellington Bettencurte and Julio Dutra

3 State Estimation Problem Introduction State estimation problems, also designated as nonstationary inverse problems; Available measured data is used together with prior nowledge about the physical phenomena and the measuring devices, in order to sequentially produce estimates of the desired dynamic variables; This is accomplished in such a manner that the error is minimized statistically.

4 State Estimation Problem Position of an aircraft Time integration of velocity components since departure Models aren t perfect Measured with an GPS and altimeter Measurement devices aren t perfect

5 State Estimation Problem Measurement error Model error

6 State Estimation Problem Consider the model for the evolution of the vector x x f ( x, w ) 1 1 Measurements available and are related to x as Where: y h ( x, v ) =1,2,... denotes time instant R n x x = state variables to be estimated y R n y = measurements w R n w = state noise vector v R n v = measurement noise

7 State Estimation Problem The state estimation problem aims at obtaining information about x based on the state evolution model and on the measurements observation model. given by the

8 Bayesian Framewor The solution of the inverse problem within the Bayesian framewor is recast in the form of statistical inference from the posterior probability density, which is the model for the conditional probability distribution of the unnown parameters given the measurements. The measurement model incorporating the related uncertainties is called the lielihood, that is, the conditional probability of the measurements given the unnown parameters. The model for the unnowns that reflects all the uncertainty of the parameters without the information conveyed by the measurements, is called the prior model.

9 Bayesian Framewor The formal mechanism to combine the new information (measurements) with the previously available information (prior) is nown as the Bayes theorem: posterior ( x) ( y x) ( x) ( x y) ( y) where posterior (x) is the posterior probability density, (x) is the prior density, (y x) is the lielihood function and (y) is the marginal probability density of the measurements, which plays the role of a normalizing constant.

10 State Estimation Problem State Evolution Model: Observation Model: x = f (x -1, w -1 ) y h ( x, v ) The evolution-observation model is based on the following assumptions: The sequence x for =1,2., is a Marovian process, that is, π(x x 0,x 1,x 2, x -1 )= π(x x -1 ) The sequence y for =1,2,, is a Marovian process with respect to the history of x, that is, π(y x 0,x 1,x 2, x )= π(y x ) The sequence x depends on the past observations only through its own history, that is, π(x x -1,y 1:-1 )= π(x x -1 )

11 State Estimation Problem State Evolution Model: x = f (x -1,w -1 ) Observation Model: y h ( x, v ) time domain: used to classify the method of solution in the domain y 1:f ={y, =1,,f} The prediction problem, concerned with the determination of π(x y 1:-1 ); The filtering problem, concerned with the determination of π(x y 1: ); The fixed-lag smoothing problem, concerned with the determination of π(x y 1:+p ), where p 1 is the fixed lag; The whole-domain smoothing problem, concerned with the determination of π(x y 1:f )

12 State Estimation Problem (x 1 x 0 ) (x 0 ) Prediction (x 1 ) Filtering Problem By assuming that ( x0 y0) ( x0) is available, the posterior probability density ( x y1: ) is then obtained with Bayesian filters in two steps: prediction and update Update (x 2 x 1 ) Update π(x (x 1 y z 1 ) (x π(x 2 y z 1 ) π(y (z 1 x 1 ) Prediction π(y (z 2 x 2 ) π(x (x 2 y z 1:2 )

13 The Kalman Filter Evolution and observation models are linear. Noises in such models are additive and Gaussian, with nown means and covariances. State Evolution Model: x F x 1 s 1 w 1 Observation Model: y H x v The above set o equations leads to the optimum solution of the state variables; F and H are nown matrices for the linear evolutions of the state x and of the observation y, respectively. Vector s is assumed to be a nown input. Noises w and v have zero means and covariance matrices Q and R, respectively p(w)~n(0,q) and p(v)~n(0,r)

14 The Kalman Filter Derivation of the equations The inverse problem solution can be obtained by Where the posterior pdf can be obtained by the following proportionality relation: Reminding the evolution observation models:

15 The Kalman Filter Derivation of the equations Determine the expectation and the covariance matrix for these distributions in order for the posterior distribution to be fully characterized.

16 The Kalman Filter For the lielihood the expectation is obtained by Non-biased using The covariance matrix is obtained by:

17 The Kalman Filter Derivation of the equations Thus the pdf of the lielihood can be written as

18 The Kalman Filter Derivation of the equations For the prior pdf π(x n y 0:n-1 ), the expectation is obtained by: Equivalent Both using And the covariance matrix is obtained by: cov[ x] E xx T Equivalent

19 The Kalman Filter Derivation of the equations Thus the pdf of the priori can be written as

20 The Kalman Filter Derivation of the equations The pdf of the posterior distribution is obtained combining both prior and lielihood pdf s: The logarithm of the posterior pdf is taen and the derivative is calculated as

21 The Kalman Filter Derivation of the equations Taing x n as x nmap one obtains: I II III IV I III By using the following lemma for the first term in the right hand side ( I), Where One obtains,

22 The Kalman Filter Derivation of the equations A further simplification is obtained with the following lemma: Where IV I III Then

23 The Kalman Filter Derivation of the equations Reminding: Derivative of ln((x y)) with respect to x With the information of the previous lemmas: Results in Where MAP n n n1 n n n n n1 x ˆ x ˆ K y H x ˆ

24 The Kalman Filter Derivation of the equations Covariance matrix derivation K H x x n n n n cov[ x] E xx T By using One obtain

25 The Kalman Filter Prediction: Update: ^ + 1 ^ x F x (4.a) + T 1 P F P F Q (4.b) T 1 K P H H P H R (5.a) T y - ^ + ^ ^ x x K ( z Hx ) (5.b) + P I - K H P Measurement Innovation (5.c)

26 Heat flux q Example I: Lumped System d dt m () t mq() t h For t > 0 T h x z y (0) o where o T () t To T T m For t = 0 h cl L

27 Example I: Lumped System Two illustrative cases are examined: Heat flux q(t)=q o constant and deterministically nown; Heat flux q(t)=q o f(t) with unnown time variation; Plate is made of aluminum ( = 2707 gm -3, c = 896 Jg -1 K -1 ), with thicness L = 0.03 m, q 0 = 8000 W/m 2, T =20 o C, h = 50 Wm -2 K -1 and T 0 = 50 o C. Measurement of the transient temperature of the slab are assumed available. These measurements contain additive, uncorrelated, Gaussian error, with zero mean and a constant standard deviation σ z The errors in the state evolution model are also supposed to be additive, uncorrelated, Gaussian, with zero mean and a constant standard deviation σ θ

28 Example I: Lumped System (i) Heat Flux q(t)=q o constant and deterministically nown The analytical solution for this problem is given by: mt qo mt ( t) oe 1 e h The only state variable in this case is the temperature θ(t )= θ since the applied heat flux q o is constant and deterministically nown, as the other parameter appearing in the formulation. By using a forward finite differences approximation for the time derivative in equation: d mq() t m () t dt h We obtain: m t 1 mq h o 1 t

29 Example I: Lumped System Therefore, the state and observation models given by: x F x s w z H x n are obtained with x s m q h o t Q 2 F 1 mt H 1 R 2 z

30 Example I: Lumped System (ii) Heat Flux q(t)=q 0 f(t) with unnown time variation, The analytical solution for this problem is given by: t mq o mt ' ( t) o e f ( t ') dt ' h t ' 0 In this case, the state variables are given by the temperatures θ(t )= θ and the function that gives the time variation of the applied heat flux, that is f(t )=f. As in the case examined previously, the applied heat flux q o is constant and deterministically nown, as the other parameters appearing in the formulation. By using a forward finite-difference approximation for the time derivative we obtain the equation for the evolution of the state variable θ(t )= θ : o (1 m t) mq t f h 1 1 A random wal model is used for the state variable f(t )=f,, which is given in the form f f 1 1 Where -1 is Gaussian with zero mean and constant standard deviation σ rw

31 Example I: Lumped System (ii) Heat Flux q(t) = q o f(t) with unnown time variation Therefore, the state and observation models given by: x F x s w z H x n F are obtained with mqo 1 mt t h 0 1 x s H 0 0 f 1 0 Q R z 0 2 rw

32 Example II: Experimental result Estimation of position-dependent transient heat source T T T h q( x, y, t) C T T t x x y y e e in 0 < x < a, 0 < y < b, for t > 0 T y T x 0 0 T T 0 at x = 0 and x = a, for t > 0 at y = 0 and y = b, for t > 0 for t = 0, in 0 < x < a and 0 < y < b z e 1 T ( x, y, t) T( x, y, z, t) dz e z 0 h h y Ambient temperature T inf Medium with constant properties C and h h h b e a x

33 Example II: Experimental result Schematics of the experiment

34 Example II: Experimental result Nodal approach input System of measurements Predictive model T t ( x, y, t) 2 ( x, y) T H ( x, y) T T G( x, y, t) T 2T T T 2T T Y t t T T H tg ( x) ( y) n n n n n n n1 i1, j i, j i1, j i, j1 i, j i, j1 n i, j 2 2 i, j ( i, j ) i, j i, j T meas + - ˆ (,, ) meas T x y t J T P OLS T T ˆ 1 P J J J T

35 Example II: Experimental result Nodal approach for KF estimation 1 1 T T = T J P v 1 P P = IP v ( ) ( ) ( ) 0 0 M M L t T T t t L t T T L t T T t J M M M a H G a H G a H G P 1 2 M T T T T

36 Example II: Experimental result Therefore, the state and observation models given by: x F x s w are obtained with x T P z H x n F I J 0 I H [ I 0] Q QT 0 0 Q P

37 Example II: Experimental result Square pulse Sinusoidal pulse

38 Example II: Experimental result - Sine

39 Example II: Experimental result - Square

40 Example II: Experimental result - Sine

41 Example II: Experimental result - Square

42 Example II: Experimental result Square Sin

43 The Kalman Filter References WELCH, G., E BISHOP, G., An Introduction to the Kalman Filter, (2006). Available at SORENSON, H. W., Least-squares estimation: from Gauss to Kalman, IEEE Spectrum (1970), pp MAYBECK, P., 1979, Stochastic models, estimation and control, Academic Press, New Yor.

44 The Kalman Filter Than you!

45 The Kalman Filter Extensions Fact: Kalman filter is restrict to linear and Gaussian problems. One investigation: System matrices are allowed to change with time. What if they don t? Second investigation: What if the hypothesis above do not hold? Can we solve nonlinear and/or non Gaussian problems?

46 The Kalman Filter Extensions What if instead of x F x u w y H x n we had its time-invariant version: x Fx u w y Hx n with the noise given by w N 0, Q n N 0, R

47 The Kalman Filter Extensions In other words F F, H H, Q Q, R R As a practical result, K and P matrices behave asymptotically. After a number of steps: Can we use this to our advantage? K K, P P P

48 Steady-State Kalman Filter Applying this result to Kalman filter equations yields: 1 T T T T P FP F FP H HP H R HP F Q T T 1 K P H HP H R ˆ 1 n n n xˆ I K H Fx K y These equations are referred to as the Steady-State Kalman Filter (SSKF).

49 Steady-State Kalman Filter What are the properties of this method? Equations 1 and 2: offline; Equation 3: online; Thus: P and K are calculated offline; x and y appear in one equation; No online matrix inversion; O(n²), instead of O(n³); 1 P FP F FP H HP H R HP F Q T T T T T T 1 K P H HP H R ˆ 1 n n n xˆ I K H Fx K y

50 Example III Introduction Heat Flux Identification problem: Heating of a flat square plate; 3D Nonlinear Heat Conduction; High magnitude heat flux; Measurements taen at opposite side; (a,b,c)=(120,120,30) mm 3D Nonlinear Transient Inverse Heat Conduction Problem: How to estimate the heat flux at real time?

51 Example III Mathematical Modeling Complete Mathematical Model: Governing Equation (t > 0): T C T T T t c, c c c c Initial Condition (t = 0): T ( x, y, z, t) T, 0 Thermal Properties 3 T C T [J/m ] 6 2 T T T [W/mK]

52 Example III Mathematical Modeling Complete Mathematical Model: Boundary Conditions T in x 0 and x a : c 0 x T in y 0 and y b : c 0 y T in z 0 : c 0 z in z c: T c q x, y, t c T z

53 Example III Mathematical Modeling Complete Mathematical Model: Nonlinear: unable to apply Kalman filter for this case; 3D inverse problem might lead to high computational effort; Proposal: Use a reduced model: Linearize it; Use mean temperature at the z-direction instead of actual temperature; Approximate temperature gradients across the thicness. 1 c z 0 T x, y, t T ( x, y, z, t) dz c

54 Example III Mathematical Modeling Reduced Mathematical Model: Governing Equation (t > 0): T 2 T 2 T * * * 2 2 q x, y, t C t x y c, Initial Condition (t = 0): T ( x, y, t) T, 0 C Thermal Properties C T * * T * * * w/ T 600 K

55 Example III Mathematical Modeling Reduced Mathematical Model: Boundary Conditions T in x 0 and x a : 0 x in y 0 and y b : T y 0 Improved Lumped Method Temperature at z=0: c,,0,,,,, 6 * T x y t T x y t q x y t

56 Example III Inverse Problem Settings State vector: Mean temperature; Heat flux; Values throughout the mesh. Observation vector: Temperature at z= 0; Values throughout the mesh. Noise Covariance Matrices: Assumed uncorrelated. x T q c z Hx H 0 I 6 * Q 2 T I I q R 2 T I

57 Example III Inverse Problem Settings Synthetic Measurements: Solution of forward complete problem; Achieved grid/time-step independence; Exact (Reference) heat flux; Total time: 2.0 s (200 measurements); Inverse Problem: Reduced model; 24 x 24 grid with 0.02 s time step; No inverse crime; exato n y N y, R n y exato n Hx exato n exato n n y y ω y OBS: ω N 0, I

58 Example III Inverse Problem Settings Exact (Reference) Heat Flux: x x x, q t t q x, y, t q x x x, t t 0, otherwise 1,1 1,2, 1 y y y, 0 1,1 1,2 2,1 2,2 2 y y y, 0 2,1 2,2 q 1 1E7 W/m 2 q 2 5E6 W/m 2 t 1 t s 0.6 s x 1,1 30 mm x 2,1 90 mm x 1,2 50 mm x 2,2 100 mm y 1,1 30 mm y 2,1 90 mm y 1,2 50 mm y 2,2 100 mm

59 Example III Inverse Problem Settings Exact (Reference) Heat Flux: x x x, q t t q x, y, t q x x x, t t 0, caso contrário 1,1 1,2, 1 y y y, 0 1,1 1,2 2,1 2,2 2 y y y, 0 2,1 2,2

60 Example III Results Hot Spot #1: Temperature and heat flux estimates

61 Example III Results Hot Spot #1: Temperature estimates and residuals

62 Example III Results Hot Spot #2: Temperature and heat flux estimates

63 Example III Results Hot Spot #2: Temperature and heat flux estimates

64 Example III Results Computational time: Physical time: 2.0 s; Kalman filter: 300 s; SSKF: 0.9 s; Only SSKF allows for real-time estimation; Majority of computational effort done at pre-processing; Recursive estimation: 2 matrix-vector multiplications.

65 The Kalman Filter Extensions What if instead of x F x u w y H x n we had a nonlinear non Gaussian model: x f x, u, w y h x, n How does one perform sequential estimation with such models?

66 The Extended KF Idea: Linearize the model and apply classical KF equations: f ˆ,, ˆ f x f x u w x x w x w 1 1 n xˆ 1 xˆ1 h ˆ, ˆ h y h x n x x n x n n xˆ xˆ

67 The Extended KF Equations Extended KF (EKF): Linearization + Kalman Filter: Linearization of both evolution and observation models. Prediction: Linearization: Prior Mean and Covariance F and L 1 1 x xˆ f 1 xˆ1 T T n 1 1 n xˆ f xˆ, u, 0 and P F P F L Q L f w

68 The Extended KF Equations Update: Linearization H h Calculation of Intermediate Covariance Matrices x xˆ and 1 T T T K P H H P H M R M xˆ xˆ K y h xˆ, 0 and P I K H P M h n xˆ

69 Example IV Free falling of a particle: State variables: Altitude (x 1 ); Velocity (x 2 ); Ballistic Coefficient (x 3 ); Observation variable: Observed distance: 2 2 M x t a 1

70 Example IV Free falling of a particle: Evolution-Observation Model x x w x 1 exp x x g w 2 x w 3 3 x 2 y t M x t a v 1 2

71 Example IV Free falling of a particle: Input data: 0 g M lbs /ft ; ft/s ; 210 ft; 10 ft; a = 10 ft. Noise model: E v 2 i E w t 0, i 1,2,3 Initial State: ft xˆ x P diag

72 Example IV Free falling of a particle: Reference values: Estimation error:

73 Higher order methods EKF properties: Sequential nonlinear estimation algorithm of choice; Non-intrusive; Up for parallelization; Hard to accelerate theoretically (there is no SSEKF ). What if nonlinearities are not sufficiently captured? 1st order approximations might be insufficient; EKF might lead to unreliable estimates.

74 Higher order methods Example: Polar to Rectangular mapping: x r rcos and y r sin Goal: Find y statistics, given Nonlinear mapping; x is uncorrelated; Symmetric pdfs. r 1 x = and y h x 2

75 Higher order methods 1st order expansion around mean of x: Thus E E h x y h x h x x x x h 0 y hx E x x y hx x 1 x

76 Higher order methods 1st component: y1 E rcos E r rcos E cos cos sin sin r 0 r r cos Assuming: x r r r Thus: y r cos y 0 1 1

77 Higher order methods 2nd component: y2 E rsin E r rsin E sin cos cos sin r r r sine cos E cos Assuming: x r r r Thus: y E cos but y 1??? 2 2

78 Higher order methods If we assume uniform distribution: U, m m It follows that sin E m cos 1 The first order approximation is incorrect!! m

79 Higher order methods How to improve this estimate? In other words, we see better ways to propagate through a nonlinear mapping x E x and P cov x y h x in order to obtain y E y and P cov y y

80 Higher order methods Solution: Unscented transform. 1st step (2n samples or sigma points): ( i) ( i) ( i) x x x,with x np n is the size of x vector; i subscript: ith row of matrix. T i 2nd step (Mapping of samples): y ( i) ( i) h x

81 Higher order methods Solution: Unscented transform. 3rd step (Averaging of mappings): P y 1 2 n y 1 2 n and 2 n () i i1 2n i1 y yy T OBS: ( i) ( i) y y y

82 Higher order methods Unscented transform: 3rd order accurate; Nonintrusive; No extra matrices required; Sq. root of P: Cholesy decomp.; Deterministic sampling (2n samples) So, for heavily nonlinear problems: UT-based Kalman filter

83 The Unscented KF Equations Unscented KF (UKF): Unscented transform (UT) + Kalman Filter: 1 UT at prediction stage + 1 UT at update stage. Prediction: Sampling of Sigma Points Prior Mean and Covariance T n and, ( i) ( i) ( i) n1 n1 1 n n1 n i xˆ xˆ P xˆ f xˆ u 1 2n 1 2n ( i) and ( i) ( i), T n n1 n n n n 2n 2n i1 i1 xˆ xˆ P xˆ xˆ Q

84 The Unscented KF Equations Update: Sampling of sigma points T n and ( i ) ( i ) ( i ) n n n n i xˆ xˆ P yˆ h xˆ Calculation of Intermediate Covariance Matrices 1 2n yˆ yˆ() i n n 2n i 1 1 2n 1 2n ˆ ( i) ˆ ( i), T and ˆ ( i) ˆ ( i), T y n n n xy n n 2n 2n i1 i1 P y y R P x y

85 The Unscented KF Equations Update: Posterior Mean and Covariance: K P P 1 n xy y T n n n n n n n n y n xˆ xˆ K y yˆ and P P K P K

86 Example V Free falling of a particle revisited: Evolution-Observation Model x x w x 1 exp x x g w 2 x w 3 3 x 2 y t M x t a v 1 2

87 Example V Free falling of a particle:

88 Example VI Introduction Heat Flux Identification problem revisited: Same model as example III; Estimation of heat flux at z=0; Temperature measurements at z=c; 3D, Nonlinear; Heat flux application from t=0; Use Complete Model for Inverse Problem solution;

89 Example VI Results Hot Spot #2: Temperature and heat flux estimates

90 Example VI Results Hot Spot #2: Temperature estimates and residuals

91 Example VI Results Computational time: Physical time: 2.0 s; Kalman filter: N/A; UKF: s (approx. 32 h); UKF and use of the Complete Model: Inverse Analysis is made possible; Computational time is excessively large.

92 Overall Conclusions Kalman filtering techniques Wide range of applications for sequential estimation; Much more variations available; Classical Kalman Filter: Analytical solution for linear and Gaussian problems; Stable, robust, easy to program; Extended and Unscented Kalman Filter: Reliable algorithms for nonlinear and/or non Gaussian problems; Increasing degrees of accuracy and complexity.

93 Overall Conclusions SIMON D., Optimal State Estimation, John Wiley & Sons, 2006.

KALMAN AND PARTICLE FILTERS

KALMAN AND PARTICLE FILTERS H. R. B. Orlande, M. J. Colaço, G. S. Duliravich, F. L. V. Vianna, W. B. da Silva, H. M. da Fonseca and O. Fudym Department of Mechanical Engineering Escola Politécnica/COPPE