Kalman Filters. Derivation of Kalman Filter equations

Transcription

1 Kalman Filters Derivation of Kalman Filter equations Mar Fiala CRV 10 Tutorial Day May 29/2010

2 Kalman Filters Predict x ˆ + = F xˆ B u P F + 1 = F P 1 1 T Q Update S H + K ~ y P = H P 1 T T 1 = P H 1 S = z H x 1 = xˆ ~ y ˆ x ˆ + K 1 R = ( I K H ) P 1

3 Normal Distribution - Gaussian Gaussian models normal distribution 1-D parameters: mean, sigma 2-D parameters: mean vector, covariance matrix

4 1-D Gaussians The Kalman filter is based on manipulating gaussian approximations of probability density functions (PDF s). Useful property of gaussian functions is that multiplying two gaussian functions yields a third gaussian. Hardware designers select one or the other based on situation, usually favor separate enables for timing reasons

5 Multiplying two 1-D Gaussians

6 Deriving multiplying two 1-D Gaussians

7 Deriving multiplying three 1-D Gaussians

8 Multiplying 1-D Gaussians Multiplying 2 gaussians Multiplying 3 gaussians

9 Show 1d_gaussian_gui.exe

10 N-D Gaussians Multi-dimensional gaussian can be created by putting gaussians on different orthogonal axes = multiplying with different variables 2-D example: One gaussian in X-axis, one in Y-axis (Image from Wiipedia)

11 2-D gaussian aligned along X, Y axes Use rotation matrix R to align along arbitrary axes Matrix C is not diagonal as is Σ

12 Justification of N-D gaussians Covariance matrix of an 2-D data set SVD A=UDV t D = diagonal SVD of A=S t S : U=V A=UDU t Covariance matrix of an N-D data set We need 1/σ 2 form Therefore -a covariance matrix can be rotated to axes where Σ is a diagonal matrix -a covariance matrix can be represented as an N-D shape of orthogonal gaussians PDF= =ellipse in 2-D, =ellipsoid in 3-D

13 Linear Functions and PDF s What happens to a PDF when passing through a linear function (matrix operation)? Input variable X has mean U x and covariance Cov x Linear function Y = A X Output variable Y has mean U y and covariance Cov y U y = A U x Cov y = ACov x A t

14 Expanded form useful for multiplication

15 Multiplying N-D Gaussians Each gaussian PDF has a mean (centroid) vector U and a covariance C Inputs Output Multiplying 2 gaussians

16 Multiplying N-D Gaussians Each gaussian PDF has a mean (centroid) vector U and a covariance C Inputs Output Inputs Rename covariance P = C Output P r = (P 1 + P 2 ) U r = P r (P 1 U 1 + P 2 U 2 )

17 Multiplying Gaussians Multiplying 1-D gaussians Multiplying N-D gaussians Inputs Output P r = (P 1 + P 2 ) U r = P r (P 1 U 1 + P 2 U 2 )

18 Kalman Filters Predict x ˆ + = F xˆ B u P F + 1 = F P 1 1 T Q Update S H + K ~ y P = H P 1 T T 1 = P H 1 S = z H x 1 = xˆ ~ y ˆ x ˆ + K 1 R = ( I K H ) P 1

19 Kalman Filters Predict x ˆ + = F xˆ B u Linear functions and PDF s U y = A U x Cov y = ACov x A t Linear functions and PDF s U y = A U x Cov y = ACov x A t P F + Q 1 Update = F P 1 1 = H P 1 T T S H + K ~ y P T 1 = P H 1 S = z H x 1 = xˆ ~ y ˆ x ˆ + K 1 R = ( I K H ) P 1

20 Baye s Rule and Kalman Filters Baye s Rule Prob(A B) ~= Prob(A)Prob(B A) Output variable Y has mean U y and covariance Cov y Kalman Filter Find Prob(X) given measurements Z X=state variables, Z=measurements Want Prob(X Z) Prob(X Z) ~= Prob(X)Prob(Z X) (update eqns) X has normal distribution PDF given by mean = and Covar = P What is probability of observed Z given X? P(Z X) has mean = Z and Covar = S S H + = H P 1 T R Multiply two PDF s = Kalman Filter

21 Kalman Filter = Multiply two N-D Gaussians Multiplying N-D gaussians Inputs Output P r = (P 1 + P 2 ) U r = P r (P 1 U 1 + P 2 U 2 ) Prob(X Z) ~= Prob(X)Prob(Z X) (update eqns) PDF 1 =X has normal distribution PDF given by mean = and Covar = P PDF 2 =Z has normal distribution PDF given by mean = Z and Covar = HPH t + R PDF r = PDF 1 PDF 2 = next iteration X,P P r = P next = [P 1 + (HPH t +R)] 1 Multiply two PDF s = Kalman Filter U r =X next = [P 1 + (HPH t +R)] 1 [P 1 X^ + (HPH t +R) 1 Z]

22 Kalman Filter = Multiply two N-D Gaussians P r = P next = [P 1 + (HPH t +R)] 1 Multiply two PDF s = Kalman Filter U r =X next = [P 1 + (HPH t +R)] 1 [P 1 X^ + (HPH t +R) 1 Z] After some algebra and use of inversion lemma K ~ y P T 1 = P H 1 S = z H x 1 = xˆ ~ y = ( I K H ) P 1 ˆ x ˆ + K 1

23 Slide courtesy Adam Rachmielowsi (Univ. Alberta) EKF: Extended Kalman filter Allow non-linear functions (F, H) Apply functions to state Apply jacobian to covariances x = f ( x 1, u, w ) z = h( x, v ) Linearizing functions around current estimate Mar Fiala CRV 10 Tutorial Day May 29/2010