Introduction to Kálmán Filtering

Transcription

1 Introduction to Kálmán Filtering Jiří Dvořák Institute of Information Theory and Automation of the AS CR, Department of Probability and Mathematical Statistics, MFF UK, Prague Mariánská,

2 Interpolation, filtering & prediction Cover of the book Filtering and Prediction: A Primer by B. Fristedt, N. Jain & N. Krylov. Jir ı Dvor a k (U TIA, MFF UK) Kalman Filtering Maria nska / 15

3 Terminology & notation Discrete time steps! {X 0, X 1, X 2,...}... unobserved signal process, {Y 0, Y 1, Y 2,...}... process of observations, Y [0,n]... observations {Y i, 0 i n}. We want to estimate X t0 based on the observations Y [0,t1 ]. t 0 = t 1... filtering, t 0 > t 1... prediction, t 0 < t 1... interpolation, smoothing. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

4 Filtering problem How to find optimal estimates? Two approaches. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

5 Filtering problem How to find optimal estimates? Two approaches. Probability X i s, Y i s... random variables. Conditional expectation is the answer: E [ ] X n Y [0,n]. Optimal in the mean square sense. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

6 Filtering problem How to find optimal estimates? Two approaches. Probability X i s, Y i s... random variables. Conditional expectation is the answer: E [ ] X n Y [0,n]. Optimal in the mean square sense. Engineering OK, but can we do it efficiently? Can we do it in real time? Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

7 Conditional expectation E [ X n Y [0,n] ] Random variable! Function of Y [0,n]. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

8 Conditional expectation E [ X n Y [0,n] ] Random variable! Function of Y [0,n]. Optimality in the mean square sense: E(X n E [ ] X n Y [0,n] ) 2 = min(x n f (Y [0,n] )) 2, f is a (measurable) function of Y [0,n]. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

9 Conditional expectation E [ X n Y [0,n] ] Random variable! Function of Y [0,n]. Optimality in the mean square sense: E(X n E [ X n Y [0,n] ] ) 2 = min(x n f (Y [0,n] )) 2, f is a (measurable) function of Y [0,n]. If (X n, Y n, Y n 1,... Y 0 ) has a normal distribution, conditional distribution of X n given Y [0,n] is normal, too (determined by its mean value and covariance matrix). Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

10 Conditional expectation E [ X n Y [0,n] ] Random variable! Function of Y [0,n]. Optimality in the mean square sense: E(X n E [ X n Y [0,n] ] ) 2 = min(x n f (Y [0,n] )) 2, f is a (measurable) function of Y [0,n]. If (X n, Y n, Y n 1,... Y 0 ) has a normal distribution, conditional distribution of X n given Y [0,n] is normal, too (determined by its mean value and covariance matrix). In this case E [ X n Y [0,n] ] is LINEAR function of (Y n, Y n 1,... Y 0 ). Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

11 Kalman filter Simple model: linear dynamics of the system (state vector) with random perturbations, observation is a linear function of the state vector, random errors of measurement, normal distribution of errors. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

12 Kalman filter Simple model: linear dynamics of the system (state vector) with random perturbations, observation is a linear function of the state vector, random errors of measurement, normal distribution of errors. Recursive algorithm, estimate of the state vector in time n + 1 computed from estimate in time n, new measurement Y n+1. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

13 Kalman filter Simple model: linear dynamics of the system (state vector) with random perturbations, observation is a linear function of the state vector, random errors of measurement, normal distribution of errors. Recursive algorithm, estimate of the state vector in time n + 1 computed from estimate in time n, new measurement Y n+1. Efficient algorithm, only matrix operations. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

14 Kalman filter II. Developed in early 1960 s, used in trajectory estimation in Appolo program, used in the guidance and navigation systems of cruise missiles (U.S. Navy s Tomahawk missile, NASA space shuttles, ISS, etc.). Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

15 Kalman filter II. Developed in early 1960 s, used in trajectory estimation in Appolo program, used in the guidance and navigation systems of cruise missiles (U.S. Navy s Tomahawk missile, NASA space shuttles, ISS, etc.). Alternates two steps: predict and update, thus enabling prediction, too. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

16 Kalman filter II. Developed in early 1960 s, used in trajectory estimation in Appolo program, used in the guidance and navigation systems of cruise missiles (U.S. Navy s Tomahawk missile, NASA space shuttles, ISS, etc.). Alternates two steps: predict and update, thus enabling prediction, too. Some measurements may be skipped (several prediction steps without update). Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

17 Kalman filter linear dynamical system System dynamics: X n = A n X n 1 + W n X n... state vector, A n... transition matrix, W n... process noise, W n N(0, Q n ). Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

18 Kalman filter linear dynamical system System dynamics: X n = A n X n 1 + W n X n... state vector, A n... transition matrix, W n... process noise, W n N(0, Q n ). Measurement model: Y n = H n X n + V n Y n... vector of measurements, H n... observation model, V n... observation noise, V n N(0, R n ). Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

19 Kalman filter linear dynamical system System dynamics: X n = A n X n 1 + W n X n... state vector, A n... transition matrix, W n... process noise, W n N(0, Q n ). Measurement model: Y n = H n X n + V n Y n... vector of measurements, H n... observation model, V n... observation noise, V n N(0, R n ). {X 0, V 0, V 1, W 1, V 2, W 2,..., V n, W n } mutually independent, X 0 normaly distributed. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

20 Example uniform movement on a line X n = A n X n 1 + W n, Y n = H n X n + V n, Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

21 Example uniform movement on a line X n = A n X n 1 + W n, Y n = H n X n + V n, X n = p n... position, v n... velocity, t... time step. ( pn v n ) ( 1 t, A n = 0 1 Y n = (p n ), H n = ( 1 0 ) Random effects affect both position and velocity! ), Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

22 Kalman filter III. {X 0, V 0, V 1, W 1, V 2, W 2,..., V n, W n } normaly distributed, Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

23 Kalman filter III. {X 0, V 0, V 1, W 1, V 2, W 2,..., V n, W n } normaly distributed, {X n, Y n, Y n 1,..., Y 0 } normaly distributed, Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

24 Kalman filter III. {X 0, V 0, V 1, W 1, V 2, W 2,..., V n, W n } normaly distributed, {X n, Y n, Y n 1,..., Y 0 } normaly distributed, X n given Y [0,n] normaly distributed... X n n, Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

25 Kalman filter III. {X 0, V 0, V 1, W 1, V 2, W 2,..., V n, W n } normaly distributed, {X n, Y n, Y n 1,..., Y 0 } normaly distributed, X n given Y [0,n] normaly distributed... X n n, mean value and covariance matrix determine the distribution. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

26 Kalman filter III. {X 0, V 0, V 1, W 1, V 2, W 2,..., V n, W n } normaly distributed, {X n, Y n, Y n 1,..., Y 0 } normaly distributed, X n given Y [0,n] normaly distributed... X n n, mean value and covariance matrix determine the distribution. Similarly, X n+1 given Y [0,n] is normaly distributed... X n+1 n (prediction). Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

27 Kalman filter prediction & update Prediction step: Predicted state estimate: ˆX n n 1 = A n ˆXn 1 n 1, predicted estimate covariance: P n n 1 = A n P n 1 n 1 A T n + Q n. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

28 Kalman filter prediction & update Prediction step: Predicted state estimate: ˆX n n 1 = A n ˆXn 1 n 1, predicted estimate covariance: P n n 1 = A n P n 1 n 1 A T n + Q n. Update step: Measurement residual: Ỹn = Y n H n ˆXn n 1, residual covariance: S n = H n P n n 1 H T n + R n, optimal Kalman gain: K n = P n n 1 H T n S 1 n, updated state estimate: ˆX n n = ˆX n n 1 + K n Ỹ n, updated estimate covariance: P n n = (I K n H n )P n n 1. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

29 Kalman filter remarks Recursive algorithm, does not need the whole history. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

30 Kalman filter remarks Recursive algorithm, does not need the whole history. Rewriting the estimate: ˆX n n = K n Y n + (I K n H n ) ˆX n n 1. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

31 Kalman filter remarks Recursive algorithm, does not need the whole history. Rewriting the estimate: ˆX n n = K n Y n + (I K n H n ) ˆX n n 1. Formula for P n n is deterministic, does not depend on Y [0,n]. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

32 Kalman filter remarks Recursive algorithm, does not need the whole history. Rewriting the estimate: ˆX n n = K n Y n + (I K n H n ) ˆX n n 1. Formula for P n n is deterministic, does not depend on Y [0,n]. Noise covariances Q n and R n must be known or estimated. Usually assumed to be constant. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

33 Kalman filter remarks Recursive algorithm, does not need the whole history. Rewriting the estimate: ˆX n n = K n Y n + (I K n H n ) ˆX n n 1. Formula for P n n is deterministic, does not depend on Y [0,n]. Noise covariances Q n and R n must be known or estimated. Usually assumed to be constant. K n minimizes Tr(P n n ). Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

34 Extended Kalman Filter Instead of X n = A n X n 1 + W n, Y n = H n X n + V n we use X n = a(x n 1 ) + W n, Y n = h(x n ) + V n. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

35 Extended Kalman Filter Instead of X n = A n X n 1 + W n, Y n = H n X n + V n we use X n = a(x n 1 ) + W n, Y n = h(x n ) + V n. In prediction and update steps we use linearization of a and h: Instead of A n use Ā n = a x ˆXn 1 n 1, instead of H n use H n = h x ˆXn n 1. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

36 Extended Kalman Filter Instead of X n = A n X n 1 + W n, Y n = H n X n + V n we use X n = a(x n 1 ) + W n, Y n = h(x n ) + V n. In prediction and update steps we use linearization of a and h: Instead of A n use Ā n = a x ˆXn 1 n 1, instead of H n use H n = h x ˆXn n 1. Requires sufficiently high sampling rate. In general not optimal estimator. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

37 Applications Multiple particle tracking (Michal Šorel, Tomáš Zámečník): Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

38 Applications Multiple particle tracking (Michal Šorel, Tomáš Zámečník): data sequence of images of a system including many particles (e.g. living cells), Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

39 Applications Multiple particle tracking (Michal Šorel, Tomáš Zámečník): data sequence of images of a system including many particles (e.g. living cells), task connect existing trajectories to particles in new image. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

40 Applications Multiple particle tracking (Michal Šorel, Tomáš Zámečník): data sequence of images of a system including many particles (e.g. living cells), task connect existing trajectories to particles in new image. For each trajectory individual Kalman filter is maintained, Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

41 Applications Multiple particle tracking (Michal Šorel, Tomáš Zámečník): data sequence of images of a system including many particles (e.g. living cells), task connect existing trajectories to particles in new image. For each trajectory individual Kalman filter is maintained, for each trajectory a prediction is made where the corresponding particle should be in the next (new) image, Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

42 Applications Multiple particle tracking (Michal Šorel, Tomáš Zámečník): data sequence of images of a system including many particles (e.g. living cells), task connect existing trajectories to particles in new image. For each trajectory individual Kalman filter is maintained, for each trajectory a prediction is made where the corresponding particle should be in the next (new) image, for each PAIR trajectory-particle compute a cost, based on the conditional distribution, Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

43 Applications Multiple particle tracking (Michal Šorel, Tomáš Zámečník): data sequence of images of a system including many particles (e.g. living cells), task connect existing trajectories to particles in new image. For each trajectory individual Kalman filter is maintained, for each trajectory a prediction is made where the corresponding particle should be in the next (new) image, for each PAIR trajectory-particle compute a cost, based on the conditional distribution, find a mapping of existing trajectories to particles with minimum cost, Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

44 Applications Multiple particle tracking (Michal Šorel, Tomáš Zámečník): data sequence of images of a system including many particles (e.g. living cells), task connect existing trajectories to particles in new image. For each trajectory individual Kalman filter is maintained, for each trajectory a prediction is made where the corresponding particle should be in the next (new) image, for each PAIR trajectory-particle compute a cost, based on the conditional distribution, find a mapping of existing trajectories to particles with minimum cost, start a new trajectory for each unconnected particle, etc. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

45 Applications II. Pose estimation evening session, with practical demonstration. Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15

46 Applications II. Pose estimation evening session, with practical demonstration. Thank you for your attention! Jiří Dvořák (ÚTIA, MFF UK) Kalman Filtering Mariánská / 15