The Kalman Filter. An Algorithm for Dealing with Uncertainty. Steven Janke. May Steven Janke (Seminar) The Kalman Filter May / 29

Transcription

1 The Kalman Filter An Algorithm for Dealing with Uncertainty Steven Janke May 2011 Steven Janke (Seminar) The Kalman Filter May / 29

2 Autonomous Robots Steven Janke (Seminar) The Kalman Filter May / 29

3 The Problem Steven Janke (Seminar) The Kalman Filter May / 29

4 One Dimensional Problem Let r t be the position of the robot at time t. The variable r t is a random variable. Steven Janke (Seminar) The Kalman Filter May / 29

5 Distributions and Variances Definition The distribution of a random variable is the list of values it takes along with the probability of those values. The variance of a random variable is a measure of how spread out the distribution is. Example Suppose the robot moves either 8, 10, or 12 cm. in one step with probabilities 0.25, 0.5, and 0.25 respectively. The distribution is centered around 10 cm. (the mean). The variance is Var(r 1 ) = 0.25 (8 10) (10 10) (12 10) 2 = 2 cm 2 Steven Janke (Seminar) The Kalman Filter May / 29

6 Variances for One and Two Steps The value of r 1 is the first step. Var(r 1 ) = = 2. The value of r 2 is the sum of two steps. Var(r 2 ) = = 4 Steven Janke (Seminar) The Kalman Filter May / 29

7 Variance Properties Definition (Variance) Suppose we have a random variable X with mean µ. Then Var(X ) = x (x µ) 2 P[X = x] Note that Var(aX ) = a 2 Var(X ). If X and Y are independent random variables: Var(X + Y ) = Var(X ) + Var(Y ) Var(X Y ) = Var(X ) + Var(Y ) If X and Y are not independent random variables: Cov(X, Y ) = x (x µ X )(y µ Y )P[X = x, Y = y] Var(X + Y ) = Var(X ) + 2Cov(X, Y ) + Var(Y ) Steven Janke (Seminar) The Kalman Filter May / 29

8 Normal Density f (x) = 1 σ 2π (x µ) 2 e 2σ 2 Steven Janke (Seminar) The Kalman Filter May / 29

9 Comparing Variances for the Normal Density Steven Janke (Seminar) The Kalman Filter May / 29

10 Estimators for Robot Position Suppose that in one step, the robot moves on average 10 cm. Noise in the process makes r 1 a little different. Nevertheless, our prediction is ˆr 1 = 10. The robot has an ultrasonic sensor for measuring distance. Again because of noise, the reading is not totally accurate, but we have a second estimate of r 1, namely ŝ 1. Both the process noise and the sensor noise are random variables with mean zero. Their variances may not be equal. Steven Janke (Seminar) The Kalman Filter May / 29

11 The Model and Estimates The Model Process: r t = r t 1 + d + Pn t (d is mean distance of one step) Sensor: s t = r t + Sn t The noise random variables Pn t and Sn t are normal with mean 0. The variances of Pn t and Sn t are σ 2 p and σ 2 s. The Estimates Process: ˆr t = ˆr t 1 + d Sensor: ŝ t = r t + Sn t Error variance: Var(ˆr 1 r 1 ) = σ 2 p and Var(ŝ t r t ) = σ 2 s Steven Janke (Seminar) The Kalman Filter May / 29

12 A Better Estimate ˆr 1 is an estimate of where the robot is after one step (r 1 ). ŝ 1 is also an estimate of r 1. The estimator errors have variances σ 2 p and σ 2 s respectively. Example (Combine the Estimators) One way to combine the information in both estimators is to take a linear combination: ˆr 1 = (1 K)ˆr 1 + Kŝ 1 Steven Janke (Seminar) The Kalman Filter May / 29

13 Finding the Best K Our new estimator is ˆr 1 = (1 K)ˆr 1 + Kŝ 1. A good estimator has small error variance. Var(ˆr 1 r 1) = (1 K) 2 Var(ˆr 1 r 1 ) + K 2 Var(ŝ 1 r 1 ) Substituting gives Var(ˆr 1 r 1) = (1 K) 2 σ 2 p + K 2 σ 2 s. To minimize the variance, take the derivative with respect to K and set to zero: 2(1 K)σ 2 p + 2Kσ 2 s = 0 = K = σ2 p σ 2 p + σ 2 s Definition K is called the Kalman Gain. Steven Janke (Seminar) The Kalman Filter May / 29

14 Smaller Variance With the new K, the error variance of ˆr 1 is: ( Var(ˆr 1 r 1 ) = Var (1 σ2 p σ 2 ) p σp 2 + σs 2 )(ˆr 1 r 1 ) + ( σp 2 + σs 2 )(ŝ 1 r 1 ) (1) ( σ 2 ) s 2(σ ( σ 2 ) p 2(σ 2 = σp 2 + σs 2 p ) + σp 2 + σs 2 s ) (2) = σ2 pσ 2 s σ 2 p + σ 2 s = (1 K)Var(ˆr 1 r 1 ) (3) Note that Var(ˆr 1 r 1) is less than both Var(ˆr 1 r 1 ) and Var(ŝ 1 r 1 ). Note the new estimator is ˆr 1 = ˆr 1 + K(ŝ 1 ˆr 1 ) Steven Janke (Seminar) The Kalman Filter May / 29

15 The Kalman Filter Now we can devise an algorithm: Prediction: Add the next step to our last estimate: ˆr t = ˆr t 1 + d Add in variance of next step: Var(ˆr t r t ) = Var(ˆr t 1 r t) + σ 2 p Update: (After Sensor Reading) Minimize: K = Var(ˆr t r t )/(Var(ˆr t r t ) + σs 2 ) Update the position estimate: ˆr t = ˆr t + K(ŝ t ˆr t ) Update the variance: Var(ˆr t r t ) = (1 K)Var(ˆr t r t ) Steven Janke (Seminar) The Kalman Filter May / 29

16 Calculating Estimates Time r t ˆr t Var(ˆr t ) K ŝ(t) ˆr t Var(ˆr t ) Steven Janke (Seminar) The Kalman Filter May / 29

17 Moving Robot (Step Distance = 1) Steven Janke (Seminar) The Kalman Filter May / 29

18 Another Look at the Good Estimator 1 The distribution of r t is normal with density σ p 2π e (rt ˆrt ) 2 2σp 2 1 The distribution of ŝ t is normal with density σ s 2π e (ŝt rt ) 2 2σs 2 The likelihood function is the product of these two densities. Maximizing this likelihood gives a good estimator. To maximize likelihood, we minimize the negative of the exponent: (r t ˆr t ) 2 2σ 2 p + (ŝ t r t ) 2 2σ 2 s Again use calculus to discover: r t = (1 K)ˆr t + Kŝ t where K = σ2 p σ 2 p + σ 2 s Steven Janke (Seminar) The Kalman Filter May / 29

19 Two-Dimensional Problem Example Suppose the state of the robot [ is] both a position and a velocity. Then the pt robot state is a vector: r t =. The process is now: r t = [ pt v t v t ] = [ ] [ ] pt 1 = Fr t 1 v t 1 The sensor reading now looks like this: s t = [ 1 0 ] [ ] p t = Hr v t t Steven Janke (Seminar) The Kalman Filter May / 29

20 Covariance Matrix With two parts to the robot s state, a variance in one may affect the variance in the other. So we define the covariance between two random variables X and Y. Definition (Covariance) The covariance between random variables X and Y is Cov(X, Y ) = (x µ X )(y µ Y )P[X = x, Y = y]. Definition (Covariance Matrix) The covariance [ matrix for our robot] state is Var(pt ) Cov(p Var(r t ) = t, v t ). Cov(v t, p t ) Var(v t ) Steven Janke (Seminar) The Kalman Filter May / 29

21 2D Kalman Filter The two dimensional Kalman Filter gives the predictions and updates in terms of matrices: Prediction: Add the next step to our last estimate: ˆr t = Fˆr t 1 Add in variance of next step: Var(ˆr t r t ) = FVar(ˆr t 1 r t)f T + Var(ˆr t r t ) Update: Gain: K = Var(ˆr t r t )H T ((HVar(ˆr t r t )H T + Var(ŝ t r t )) 1 Update the position estimate: ˆr t = ˆr t + K(ŝ t Hˆr t ) Update the variance: Var(ˆr t r t ) = (I KH)Var(ˆr t r t ) Steven Janke (Seminar) The Kalman Filter May / 29

22 Two-Dimensional Example Example [ ] 1 1 Process: r t = Fr t 1 where F = 0 1 [ ] Q/3 Q/2 Process Covariance matrix: Q/2 Q Sensor: s t = [ 1 0 ] [ ] p t = Hr t itial State: r 0 = [ p0 v 0 v t ] = [ ] 0 0 Steven Janke (Seminar) The Kalman Filter May / 29

23 2D Graph Results Steven Janke (Seminar) The Kalman Filter May / 29

24 Linear Algebra Interpretation Inner Product: (u, v) = E[(u m u )T (v m v )]. (Orthogonal = uncorrelated) Steven Janke (Seminar) The Kalman Filter May / 29

25 Kalman Filter Summary Summary of Kalman Filter Assumptions: The process model must be a linear model. All errors are normally distributed with mean zero. Algorithm: Prediction Step 1: Use the model to find estimate of robot state. Prediction Step 2: Use the model to find variance of estimate. Update Step 1: Calculate Kalman Gain from sensor reading. Update Step 2: Use Gain to update estimate of robot state. Update Step 3: Use Gain to update variance of estimator. Result: This linear estimator of the robot state is unbiased and has minimum error variance. Steven Janke (Seminar) The Kalman Filter May / 29

26 What can go wrong... A Bad Model Suppose we use our original model r t = r t 1 + d + Pn t. But actually the robot is stationary so the real model is r t = r t 1. Partial solution: Increase the variance of Pn t. Not a Linear Model Suppose our sensor measures distance to the origin in a 2D problem. The the sensor reading gives us x 2 + y 2. Consequently, no matrix H exists giving ŝ t = Hr t. Extended Kalman Filter approximates the sensor curve with a straight line. (Taylor Series). Steven Janke (Seminar) The Kalman Filter May / 29

27 Robot is Stationary Steven Janke (Seminar) The Kalman Filter May / 29

28 Robot is Stationary: Increase Process Variance Steven Janke (Seminar) The Kalman Filter May / 29

29 Other Applications Applications: Tracking the Stock Market. (Time Series) Computing an asteroid s orbit. Weather forecasting. Steven Janke (Seminar) The Kalman Filter May / 29