卡尔曼滤波美国北卡大学课件

1、Introduction to Kalman FilterGreg Welch & Gary BishopIntroduction to Kalman FilterGreg Welch & Gary BishopA Simple Example for Single Sensor Data ProcessingBatch Processing Model are n measurements for a variableSuppose the precision of each measurement is the same, why is better than ? A Simple Exa

2、mple for Single Sensor Data ProcessingRecursive Processing Model1st step:2nd step:3rd step:nth step:A Simple Example for Single Sensor Data ProcessingRecursive Processing Model1st step:2nd step:3rd step:nth step:A Simple Example for Single Sensor Data ProcessingRecursive Processing Model1st step:2nd

3、 step:3rd step:nth step:Why Kalman Filter?What if the precision for each measurement is different? Solution: Kalman Filter ! An Intuitive Example for Kalman FilterConsider a quantity (x), for example a length, that is measured twice with the same or with different measurement equipment, for example

4、a mechanical ruler and a laser system. The two measurements are noted and ; they are characterized by Gaussian probability distributions with means and and standard deviations and :An Intuitive Example for Kalman FilterThe two measurements are combined to give an estimate of the length:Because and h

5、ave Gaussian distributions, has also a Gaussian distribution with standard deviation given by ( and are independent)An Intuitive Example for Kalman FilterHence:An Intuitive Example for Kalman FilterTo estimate is equal to minimize the sum of the distances to and , weighted by the respective standard

6、 deviations:An Intuitive Example for Kalman FilterSuppose the two measurements become available sequentially. At time step 1 measurement becomes available. Since this is the only information, the state estimate and its variance are:An Intuitive Example for Kalman FilterThen, at time step 2, measurem

7、ent becomes available, and the estimate is:An Intuitive Example for Kalman FilterAssuming:We have: update gain;: innovationAn Intuitive Example for Kalman FilterPeter S MaybeckStochastic models,estimation,and controlStochastic Process Model for Kalman FilterEdward V. StansfieldStochastic Process Mod

8、el for Kalman FilterEdward V. StansfieldNotations for Kalman FilterEdward V. StansfieldKalman Filter EquationsEdward V. StansfieldLinear Unbiased EstimatorEdward V. StansfieldMinimum Variance EstimatorEdward V. StansfieldMinimum Prediction Error CovarianceEdward V. StansfieldMinimum Estimation Error

9、 CovarianceEdward V. StansfieldKalman Filter Process ModelGreg Welch & Gary BishopKalman Filter Estimator ModelEdward V. StansfieldKalman Filter Process ModelEdward V. StansfieldWhat is Kalman Filter?A Kalman filter is a linear, model based, stochastic, recursive, weighted, least squares estimator.

10、Estimator: the KF estimates the state of a system, or part of it, based on the knowledge (measurement) of the system inputs and outputs. Model based & linear: The KF is based on a system model consisting of a state equation and an output (measurement) equation, which are all linear. Least squares: T

11、he KF then provides the estimate that tries to minimize the inconsistencies with all pieces of information in the least squares sense. In this respect, the KF is an optimal estimator.What is Kalman Filter?A Kalman filter is a linear, model based, stochastic, recursive, weighted, least squares estima

12、tor. Weighted: When minimizing the sum of their least squares, the inconsistencies with the different pieces of information are weighted with a measure of the certainty of the information. Uncertain information is given low weight, whereas highly certain information is given a very high weight. Recu

13、rsive: When all information is available at once, it can be processed in batch. If, however, the information becomes available incrementally, as is the case for an on-line estimator, a recursive formulation of the estimation process is necessary. The KF does nothing more than that. What is Kalman Fi

14、lter?A Kalman filter is a linear, model based, stochastic, recursive, weighted, least squares estimator. Stochastic: The confidence about pieces of information is expressed in terms of probability distributions. The KF works with Gaussian distributions for both measurements and state estimates.A Kal

15、man filter is a stochastic, recursive estimator, which estimates the state of a system based on the knowledge of the system input, the measurement of the system output, and a model of the relation between input and output.An Example: Estimating a Random Constant Attempt to estimate a scalar random c

16、onstant, a voltage for example. Assume that the measurements are corrupted by a 0.1 volt RMS white measurement noise (e.g.our analog to digital converter is not very accurate).State EquationObservation EquationAn Example: Estimating a Random Constant Greg Welch & Gary BishopAn Example: Estimating a

17、Random Constant Greg Welch & Gary BishopAn Example: Estimating a Random Constant Greg Welch & Gary BishopAn Example: Estimating a Random Constant Greg Welch & Gary BishopExample for Multi-sensor Data Fusion : GPS Navigation Using Kalman FilteringGPS: absolute position measurements, but tall building

18、s, bridges, high mountains, and common foliage overhead can block satellite reception.INS: inertial navigation system using sensors such as gyroscopes and accelerometers to maintain relative position information. INS errors tend to accumulate unbounded and result in position estimates that deviate f

19、rom the actual position.Example for Multi-sensor Data Fusion : GPS Navigation Using Kalman FilteringObjectives: fuse the GPS position measurements and inertial navigation measurements to provide a best estimate of position at any given time.Solution: Kalman filteringDavid M MayhewExample for Multi-s

20、ensor Data Fusion : GPS Navigation Using Kalman FilteringDavid M MayhewExample: a mobile robot with ultrasonic sensorA robot moves on a straight line with orientation and with velocityExample: a mobile robot with ultrasonic sensorExample: a mobile robot with ultrasonic sensorThe Extended Kalman Filt

21、erLets say that we are going to use a Kalman filter to localize an Aibo based upon taking range measurement(s) to beaconsKalman Filter Localization ExampleKalman Filter RevisitedLets say your Aibo takes 3 measurements of the distance to a beacon as Z = 2000, 1900, 2100TWhat would be your estimate of

22、 the beacon distance?Well, a good estimate might be the mean of the 3 sensor values:Kalman Filter RevisitedNow lets say your Aibo takes 3 measurements of the distance to a beacon using another groups shoddy code and you get Z = 2000, 900, 3100TWe could again use the mean as the range estimate and ob

23、tainWould you have as much confidence in this estimate as the first?Kalman Filter RevisitedNow lets say your Aibo takes 3 measurements of the distance to a beacon using another groups shoddy code and you get Z = 2000, 900, 3100TWe could again use the mean as the range estimate and obtainWould you ha

24、ve as much confidence in this estimate as the first?Kalman Filter RevisitedThe main idea behind the Kalman filter is that you do not just have an estimate for a parameter x but also have some estimate for the uncertainty in your value for xThis is represented by the variance/covariance of the estima

25、te PxThere are many advantages to this, as it allows you a means for estimating the confidence in your robots ability to execute a task (e.g. navigating through a tight doorway)In the case of the KF, it also provides a nice mechanism for optimally combining data over timeThis optimality condition as

26、sumes we have linear models, and the error characteristics of our sensors can be modeled as zero-mean, Gaussian noiseKalman Filter Localization ExampleLets say that we are going to use a Kalman filter to localize our Aibo based upon taking range measurement(s) to beaconsWe could write our state upda

27、te equation as This looks great as its nice and linear. Now lets look at our measurement equations taking range to a beacon at (xb, yb)Houston, We have a problemKalman Filter Localization ExampleFor many applications, the time update and measurement equations are NOT linearAs a consequence, the KF i

28、s not applicableHowever, the KF is such a nice algorithm that maybe if we linearize around the non-linearities, we can still get good performance in practiceThis line of thought lead to the development of the Extended Kalman Filter (EKF)By relaxing the linear assumptions, the use of the KF is extend

29、ed dramaticallyLife Rule: There is no such thing as a free lunchWe can no longer use the word “optimal” with the EKFThe Extended Kalman Filter (EKF)The Extended Kalman (EKF) is a sub-optimal extension of the original KF algorithmThe EKF allows for estimation of non-linear processes or measurement re

30、lationshipsThis is accomplished by linearizing the current mean and covariance estimates (similar to a first order Taylor series approximation)Suppose our process and measurement equations are the non-linear functionsKalman Filter Extended Kalman Filter EKF Time Update PhaseFor the state update equa

31、tion, we do not know the noise values at each time step. So, we approximate the state and without themEKF Time Update PhaseHowever when we propagate the covariance ahead in time, the underlying function needs to be linear in order to properly combine the Gaussian uncertainty in our state x our covar

32、iance matrix P-k+1 with our process uncertainty QQ: How do you think we could do this?A: Linearization.Again, our wonderful friend the Taylor series comes to the rescue Transforming UncertaintyLets say we know the uncertainty of a variable x, and we want to compute the uncertainty of y=f(x)We know t

33、hat where is the distribution mean and is zero mean noiseWe can then use the Jacobian to linearly approximate yThe mean of the distribution would then be ThereforeTransforming UncertaintyThe covariance of the transformed distribution would then be Thus, to transform uncertainty across a non-linear t

34、ransformation, we perform a similarity transform with the JacobianNote that because of the symbols on the previous page, normal distributions are NOT preservedThe optimality/robustness of the KF allows the EKF to work well in practiceEKF Time Update PhaseSo the covariance is projected ahead aswhere

35、A is now the Jacobian of f with respect to x and W is the Jacobian of f with respect to wKalman Filter Extended Kalman Filter EKF Robot Implementation ExampleAssume that we have a mobile robot using odometry and range measurements to landmark to estimate its position and orientationAssume that the odometry provides a velocity estimate V and an angular velocity estimate that are both corrupted by gaussian noiseWe can write the state update equation as which is obvious