经典kalman滤波PPT[教育知识]

1、Introduction to Kalman Filters Michael Williams 5 June 2003 1教书育人 Overview The Problem Why do we need Kalman Filters? What is a Kalman Filter? Conceptual Overview The Theory of Kalman Filter Simple Example 2教书育人 The Problem System state cannot be measured directly Need to estimate “optimally” from m

2、easurements Measuring Devices Estimator Measurement Error Sources System State (desired but not known) External Controls Observed Measurements Optimal Estimate of System State System Error Sources System Black Box 3教书育人 What is a Kalman Filter? Recursive data processing algorithm Generates optimal e

3、stimate of desired quantities given the set of measurements Optimal? For linear system and white Gaussian errors, Kalman filter is “best” estimate based on all previous measurements For non-linear system optimality is qualified Recursive? Doesnt need to store all previous measurements and reprocess

4、all data each time step 4教书育人 Conceptual Overview Simple example to motivate the workings of the Kalman Filter Theoretical Justification to come later for now just focus on the concept Important: Prediction and Correction 5教书育人 Conceptual Overview Lost on the 1-dimensional line Position y(t) Assume

5、Gaussian distributed measurements y 6教书育人 Conceptual Overview 0102030405060708090100 0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 Sextant Measurement at t1: Mean = z1 and Variance = z1 Optimal estimate of position is: (t1) = z1 Variance of error in estimate: 2x (t1) = 2z1 Boat in same position at time t

6、2 - Predicted position is z1 7教书育人 0102030405060708090100 0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 Conceptual Overview So we have the prediction -(t2) GPS Measurement at t2: Mean = z2 and Variance = z2 Need to correct the prediction due to measurement to get (t2) Closer to more trusted measurement l

7、inear interpolation? prediction -(t2) measurement z(t2) 8教书育人 0102030405060708090100 0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 Conceptual Overview Corrected mean is the new optimal estimate of position New variance is smaller than either of the previous two variances measurement z(t2) corrected optim

8、al estimate (t2) prediction -(t2) 9教书育人 Conceptual Overview Lessons so far: Make prediction based on previous data - -, - Take measurement zk, z Optimal estimate () = Prediction + (Kalman Gain) * (Measurement - Prediction) Variance of estimate = Variance of prediction * (1 Kalman Gain) 10教书育人 010203

9、0405060708090100 0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 Conceptual Overview At time t3, boat moves with velocity dy/dt=u Nave approach: Shift probability to the right to predict This would work if we knew the velocity exactly (perfect model) (t2) Nave Prediction -(t3) 11教书育人 0102030405060708090100

10、 0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 Conceptual Overview Better to assume imperfect model by adding Gaussian noise dy/dt = u + w Distribution for prediction moves and spreads out (t2) Nave Prediction -(t3) Prediction -(t3) 12教书育人 0102030405060708090100 0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 C

11、onceptual Overview Now we take a measurement at t3 Need to once again correct the prediction Same as before Prediction -(t3) Measurement z(t3) Corrected optimal estimate (t3) 13教书育人 Conceptual Overview Lessons learnt from conceptual overview: Initial conditions (k-1 and k-1) Prediction (-k , -k) Use

12、 initial conditions and model (eg. constant velocity) to make prediction Measurement (zk) Take measurement Correction (k , k) Use measurement to correct prediction by blending prediction and residual always a case of merging only two Gaussians Optimal estimate with smaller variance 14教书育人 Theoretica

13、l Basis Process to be estimated: yk = Ayk-1 + Buk + wk-1 zk = Hyk + vk Process Noise (w) with covariance Q Measurement Noise (v) with covariance R Kalman Filter Predicted: -k is estimate based on measurements at previous time-steps k = -k + K(zk - H -k ) Corrected: k has additional information the m

14、easurement at time k K = P-kHT(HP-kHT + R)-1 -k = Ayk-1 + Buk P-k = APk-1AT + Q Pk = (I - KH)P-k 15教书育人 Blending Factor If we are sure about measurements: Measurement error covariance (R) decreases to zero K decreases and weights residual more heavily than prediction If we are sure about prediction

15、Prediction error covariance P-k decreases to zero K increases and weights prediction more heavily than residual 16教书育人 Theoretical Basis -k = Ayk-1 + Buk P-k = APk-1AT + Q Prediction (Time Update) (1) Project the state ahead (2) Project the error covariance ahead Correction (Measurement Update) (1)

16、Compute the Kalman Gain (2) Update estimate with measurement zk (3) Update Error Covariance k = -k + K(zk - H -k ) K = P-kHT(HP-kHT + R)-1 Pk = (I - KH)P-k 17教书育人 Quick Example Constant Model Measuring Devices Estimator Measurement Error Sources System State External Controls Observed Measurements O

17、ptimal Estimate of System State System Error Sources System Black Box 18教书育人 Quick Example Constant Model Prediction k = -k + K(zk - H -k ) Correction K = P-k(P-k + R)-1 -k = yk-1 P-k = Pk-1 Pk = (I - K)P-k 19教书育人 Quick Example Constant Model 0102030405060708090100 -0.7 -0.6 -0.5 -0.4 -0.3 -0.2 -0.1

18、 0 20教书育人 Quick Example Constant Model 0102030405060708090100 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 Convergence of Error Covariance - Pk 21教书育人 0102030405060708090100 -0.7 -0.6 -0.5 -0.4 -0.3 -0.2 -0.1 0 Quick Example Constant Model Larger value of R the measurement error covariance (indicates poorer quality of measurements) Filter slower to believe measurements slower convergence 22教书育人 References 1.Kalman, R. E. 1960. “A New Approach to Linear Filtering and Prediction Problems”