Part 2: Kalman Filtering COS 323

Size: px

Start display at page:

Download "Part 2: Kalman Filtering COS 323"

Gwen McCarthy
7 years ago
Views:

1 Part 2: Kalman Filtering COS 323

2 On-Line Estimation Have looked at off-line model estimation: all data is available For many applications, want best estimate immediately when each new datapoint arrives Take advantage of noise reduction Predict (extrapolate) based on model Additionally: Take advantage of multiple sensors (in a principled way) Applications: controllers, tracking,

3 Examples E.g. see on youtube: v=1iwvl83cumo v=0gsikwfkfca

4 On-Line Estimation Have looked at off-line model estimation: all data is available For many applications, want best estimate immediately when each new datapoint arrives Take advantage of noise reduction Predict (extrapolate) based on model Applications: controllers, tracking, How to do this efficiently?

5 Kalman Filtering Assume that results of experiment are noisy measurements of system state Use a model of how system evolves Combine system model and observations to deduce true state Prediction / correction framework Rudolf Emil Kalman Acknowledgment: much of the following material is based on the SIGGRAPH 2001 course by Greg Welch and Gary Bishop (UNC)

6 Simple Example Measurement of a single point z 1 Variance σ 2 1 (uncertainty σ 1 ) Best estimate of true position Uncertainty in best estimate

7 Simple Example Second measurement z 2, variance σ 2 2 Best estimate of true position? z 1 z 2

8 Simple Example Second measurement z 2, variance σ 2 2 Best estimate of true position: weighted average Uncertainty in best estimate:

9 Online Weighted Average Combine successive measurements into constantly-improving estimate Uncertainty usually decreases over time Only need to keep current measurement, last estimate of state and uncertainty

10 Terminology In this example, position is state (in general, any vector) State can be assumed to evolve over time according to a system model or process model (in previous example, nothing changes ) Measurements (possibly incomplete, possibly noisy) according to a measurement model Best estimate of state with covariance P

11 Gaussian Review

12 Linear Models For standard Kalman filtering, everything must be linear System model: The matrix Φ k is state transition matrix The vector ξ k represents additive noise, assumed to have mean 0 and covariance Q x k = x dx dt, Φ k = 1 Δt k 0 1

13 Linear Models Measurement model: Matrix H is measurement matrix The vector µ is measurement noise, assumed to have mean 0 and covariance R

14 Position + Velocity Model

15 Prediction/Correction Multiple values around at each iteration: x k ʹ is prediction of new state on the basis of past data (i.e., our a priori estimate) z ʹ k is predicted observation is new observation z k ˆ is new estimate of state ( a posteriori ) x k

16 Prediction/Correction 1: Predict new state x ʹ k = Φ k 1ˆ x k 1 T P k ʹ = Φ k 1 P k 1 Φ k 1 z ʹ k = H k x ʹ k + Q k 1 2: Correct to take new measurements into account

17 Kalman Gain K is weighting of process model vs. measurements, chosen to minimize P k : Compare to what we saw earlier:

18 Example: Estimate Random Constant

19 Example: Estimate Random Constant

20 Example: Estimate Random Constant x ʹ k = Φ k 1ˆ x k 1 becomes x ʹ k = x ˆ k 1 T P k ʹ = Φ k 1 P k 1 Φ k 1 z ʹ k = H k + Q k 1 becomes P k ʹ = P k 1 + Q k 1 x ʹ k + µ k becomes z ʹ k = x ʹ k + µ k K = P k ʹ /( P k ʹ + R) ˆ x k = x ʹ k + K k P k = ( I K ) k P k ʹ ( z k x ʹ ) k

21 Simulation: R selected to be true measurement error variance

22 Pk decreasing with each iteration

23 Simulation: R overestimates measurement error

24 Simulation: R underestimates measurement error

25 Results: Position-Only Model Moving Still [Welch & Bishop]

26 Results: Position-Velocity Model Moving Still [Welch & Bishop]

27 Extension: Multiple Models Simultaneously run many KFs with different system models Estimate probability each KF is correct Final estimate: weighted average

28 Results: Multiple Models [Welch & Bishop]

29 Results: Multiple Models [Welch & Bishop]

30 Other Extensions & Related Concepts Using known system input (e.g. actuators) Using information from both past and future Non-Gaussian noise and particle filtering Hidden Markov Models: discrete state space Read the Welch & Bishop tutorial here: doi= &rep=rep1&type=pdf

Course 8. An Introduction to the Kalman Filter

Course 8. An Introduction to the Kalman Filter Course 8 An Introduction to the Kalman Filter Speakers Greg Welch Gary Bishop Kalman Filters in 2 hours? Hah! No magic. Pretty simple to apply. Tolerant of abuse. Notes are a standalone reference. These