经典kalman滤波PPT

2021/3/101Introduction to Kalman FiltersMichael Williams5 June 20032021/3/102Overview The Problem Why do we need Kalman Filters?What is a Kalman Filter?Conceptual Overview The Theory of Kalman Filter Simple Example2021/3/103The Problem System state cannot be measured directly Need to estimate“optimally”from measurementsMeasuring DevicesEstimatorMeasurementError SourcesSystem State(desired but not known)External ControlsObserved MeasurementsOptimal Estimate of System StateSystemError SourcesSystemBlack Box2021/3/104What is a Kalman Filter?Recursive data processing algorithm 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 measurements and reprocess all data each time step2021/3/105Conceptual 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 Correction2021/3/106Conceptual Overview Lost on the 1-dimensional line Position y(t)Assume Gaussian distributed measurementsy2021/3/107Conceptual Overview010203040506070809010000.020.040.060.080.10.120.140.16Sextant Measurement at t1:Mean=z1 and Variance=z1Optimal estimate of position is:(t1)=z1Variance of error in estimate:2x(t1)=2z1Boat in same position at time t2-Predicted position is z12021/3/108010203040506070809010000.020.040.060.080.10.120.140.16Conceptual OverviewSo we have the prediction-(t2)GPS Measurement at t2:Mean=z2 and Variance=z2Need to correct the prediction due to measurement to get(t2)Closer to more trusted measurement linear interpolation?prediction-(t2)measurement z(t2)2021/3/109010203040506070809010000.020.040.060.080.10.120.140.16Conceptual OverviewCorrected mean is the new optimal estimate of positionNew variance is smaller than either of the previous two variancesmeasurement z(t2)corrected optimal estimate(t2)prediction-(t2)2021/3/1010Conceptual Overview Lessons so far:Make prediction based on previous data-,-Take measurement zk,zOptimal estimate()=Prediction+(Kalman Gain)*(Measurement-Prediction)Variance of estimate=Variance of prediction*(1 Kalman Gain)2021/3/1011010203040506070809010000.020.040.060.080.10.120.140.16Conceptual OverviewAt time t3,boat moves with velocity dy/dt=uNave approach:Shift probability to the right to predictThis would work if we knew the velocity exactly(perfect model)(t2)Nave Prediction-(t3)2021/3/1012010203040506070809010000.020.040.060.080.10.120.140.16Conceptual OverviewBetter to assume imperfect model by adding Gaussian noisedy/dt=u+wDistribution for prediction moves and spreads out(t2)Nave Prediction-(t3)Prediction-(t3)2021/3/1013010203040506070809010000.020.040.060.080.10.120.140.16Conceptual OverviewNow we take a measurement at t3Need to once again correct the predictionSame as beforePrediction-(t3)Measurement z(t3)Corrected optimal estimate(t3)2021/3/1014Conceptual Overview Lessons learnt from conceptual 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 merging only two Gaussians Optimal estimate with smaller variance2021/3/1015Theoretical Basis Process to be estimated:yk=Ayk-1+Buk+wk-1zk=Hyk+vkProcess Noise(w)with covariance QMeasurement Noise(v)with covariance R Kalman FilterPredicted:-k is estimate based on measurements at previous time-stepsk=-k+K(zk-H-k)Corrected:k has additional information the measurement at time kK=P-kHT(HP-kHT+R)-1-k=Ayk-1+BukP-k=APk-1AT+QPk=(I-KH)P-k2021/3/1016Blending 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 Prediction error covariance P-k decreases to zero K increases and weights prediction more heavily than residual2021/3/1017Theoretical Basis-k=Ayk-1+BukP-k=APk-1AT+QPrediction(Time Update)(1)Project the state ahead(2)Project the error covariance aheadCorrection(Measurement Update)(1)Compute the Kalman Gain(2)Update estimate with measurement zk(3)Update Error Covariancek=-k+K(zk-H-k)K=P-kHT(HP-kHT+R)-1Pk=(I-KH)P-k2021/3/1018Quick Example Constant ModelMeasuring DevicesEstimatorMeasurementError SourcesSystem StateExternal ControlsObserved MeasurementsOptimal Estimate of System StateSystemError SourcesSystemBlack Box2021/3/1019Quick Example Constant ModelPredictionk=-k+K(zk-H-k)CorrectionK=P-k(P-k+R)-1-k=yk-1P-k=Pk-1Pk=(I-K)P-k2021/3/1020Quick Example Constant Model0102030405060708090100-0.7-0.6-0.5-0.4-0.3-0.2-0.102021/3/1021Quick Example Constant Model010203040506070809010000.10.20.30.40.50.60.70.80.91Convergence of Error Covariance-Pk2021/3/10220102030405060708090100-0.7-0.6-0.5-0.4-0.3-0.2-0.10Quick Example Constant ModelLarger value of R the measurement error covariance(indicates poorer quality of measurements)Filter slower to believe measurements slower convergence2021/3/1023ReferencesKalman,R.E.1960.“A New Approach to Linear Filtering and Prediction Problems”,Transaction of the ASME-Journal of Basic Engineering,pp.35-45(March 1960).Maybeck,P.S.1979.“Stochastic Models,Estimation,and Control,Volume 1”,Academic Press,Inc.Welch,G and Bishop,G.2001.“An introduction to the Kalman Filter”,http:/www.cs.unc.edu/welch/kalman/