经典kalman滤波PPT

1、2021/3/101 Introduction to Kalman Filters Michael Williams 5 June 2003 2021/3/102 Overview The Problem Why do we need Kalman Filters? What is a Kalman Filter? Conceptual Overview The Theory of Kalman Filter Simple Example 2021/3/103 The Problem System state cannot be measured directly Need to estima

2、te “optimally” from measurements 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 2021/3/104 What is a Kalman Filter? Recursive data processing alg

3、orithm Generates optimal estimate 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 m

4、easurements and reprocess all data each time step 2021/3/105 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 2021/3/106 Conceptual Overview Lost on the 1-di

5、mensional line Position y(t) Assume Gaussian distributed measurements y 2021/3/107 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 (

6、t1) = 2z1 Boat in same position at time t2 - Predicted position is z1 2021/3/108 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

7、 get (t2) Closer to more trusted measurement linear interpolation? prediction -(t2) measurement z(t2) 2021/3/109 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 previ

8、ous two variances measurement z(t2) corrected optimal estimate (t2) prediction -(t2) 2021/3/1010 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

9、 = Variance of prediction * (1 Kalman Gain) 2021/3/1011 0102030405060708090100 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 m

10、odel) (t2) Nave Prediction -(t3) 2021/3/1012 0102030405060708090100 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) 2021/

11、3/1013 0102030405060708090100 0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 Conceptual 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) 2021/3/1014 Conceptual Overview Lessons learnt from c

12、onceptual overview: Initial conditions (k-1 and k-1) Prediction (-k , -k) Use 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 mergin

13、g only two Gaussians Optimal estimate with smaller variance 2021/3/1015 Theoretical 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 previo

14、us time-steps k = -k + K(zk - H -k ) Corrected: k has additional information the measurement at time k K = P-kHT(HP-kHT + R)-1 -k = Ayk-1 + Buk P-k = APk-1AT + Q Pk = (I - KH)P-k 2021/3/1016 Blending Factor If we are sure about measurements: Measurement error covariance (R) decreases to zero K decre

15、ases and weights residual more heavily than prediction If we are sure about prediction Prediction error covariance P-k decreases to zero K increases and weights prediction more heavily than residual 2021/3/1017 Theoretical Basis -k = Ayk-1 + Buk P-k = APk-1AT + Q Prediction (Time Update) (1) Project

16、 the state ahead (2) Project the error covariance ahead Correction (Measurement Update) (1) 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 2021/3/1018 Quick Example Constant Model Measuring D

17、evices Estimator Measurement Error Sources System State External Controls Observed Measurements Optimal Estimate of System State System Error Sources System Black Box 2021/3/1019 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

18、 - K)P-k 2021/3/1020 Quick Example Constant Model 0102030405060708090100 -0.7 -0.6 -0.5 -0.4 -0.3 -0.2 -0.1 0 2021/3/1021 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 2021/3/1022 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 2021/3/1023 References Kalman, R. E. 1960. “A New Approach to Linear Filtering and Pred