1、Sajad Saeedi G. University of new Brunswick SUMMER 2010,An Introduction to the Kalman Filter,CONTENTS,1. Introduction 2. Probability and Random Variables 3. The Kalman Filter 4. Extended Kalman Filter (EKF),Introduction,Controllers are Filters Signals in theory and practice 1960, R.E. Kalman for Apo
2、llo project Optimal and recursive Motivation: human walking Application: aerospace, robotics, defense scinece, telecommunication, power pants, economy, weather, ,CONTENTS,1. Introduction 2. Probability and Random Variables 3. The Kalman Filter 4. Extended Kalman Filter (EKF),Probability and Random V
3、ariables,Probability Sample space p(AB)= p(A)+ p(B) p(AB)= p(A)p(B) Joint probability(independent) p(A|B) = p(AB)/p(B) Bays theorem Random Variables (RV) RV is a function, (X) mapping all points in the sample space to real numbers,Probability and Random Variables,Cont.,Probability and Random Variabl
4、es,Cont.Example: tossing a fair coin 3 times (P(h) = P(t) Sample space = HHH, HHT, HTH, THH, HTT, TTH, THT, TTT X is a RV that gives number of tails P(X=2) = ? HHH, HHT, HTH, THH, HTT, TTH, THT, TTT P(X2) = ? HHH, HHT, HTH, THH, HTT, TTH, THT, TTT,Probability and Random Variables,Cumulative Distribu
5、tion Function (CDF), Distribution FunctionProperties,Probability and Random Variables,Cont.,Probability and Random Variables,Determination of probability from CDFDiscrete, FX (x) changes only in jumps, (coin example) , R=ponit Continuous, (rain example) , R=interval Discrete: PMF (Probability Mass F
6、unction) Continuous: PDF (Probability Density Function),Probability and Random Variables,Probability Mass Function (PMF),Probability and Random Variables,Probability and Random Variables,Mean and VarianceProbability weight averaging,Probability and Random Variables,Variance,Probability and Random Va
7、riables,Normal Distribution (Gaussian)Standard normal distribution,Probability and Random Variables,Example of a Gaussian normal noise,Probability and Random Variables,Galton boardBacteria lifetime,Probability and Random Variables,Random Vector Covariance Matrix Let x = X1, X2, ., Xp be a random vec
8、tor with mean vector = 1, 2, ., p. Variance: The dispersion of each Xi around its mean is measured by its variance (which is its own covariance). Covariance: Cov(Xi, Xj ) of the pair Xi, Xj is a measure of the linear coupling between these two variables.,Probability and Random Variables,Cont.,Probab
9、ility and Random Variables,example,Probability and Random Variables,Cont.,Probability and Random Variables,Random Process A random process is a mathematical model of an empirical process whose model is governed by probability laws State space model, queue model, Fixed t, Random variable Fixed sample
10、, Sample function (realization) Process and chain,Probability and Random Variables,Markov processState space model is a Markov process Autocorrelation: a measure of dependence among RVs of X(t)If the process is stationary (the density is invariant with time), R will depend on time difference,Probabi
11、lity and Random Variables,Cont.,Probability and Random Variables,White noise: having power at all frequencies in the spectrum, and being completely uncorrelated with itself at any time except the present (dirac delta autocorolation)At any sample of the signal at one time it is completely independent
12、(uncorrelated) from a sample at any other time.,Stochastic Estimation,Why white noise? No time correlation easy computaion Does it exist?,Stochastic Estimation,Observer design Blackbox problemObservability Luenburger observer,Stochastic Estimation,Belief,Initial state detects nothing:,Moves and dete
13、cts landmark:,Moves and detects nothing:,Moves and detects landmark:,Stochastic Estimation,Parametric Filters Kalman Filter Extended Kalman Filter Unscented Kalman Filter Information FilterNon Parametric Filters Histogram Filter Particle Filter,CONTENTS,1. Introduction 2. Probability and Random Vari
14、ables 3. The Kalman Filter 4. Extended Kalman Filter (EKF),The Kalman Filter,Example1: driving an old car (50s),The Kalman Filter,Example2: Lost at sea during night with your friend Time = t1,The Kalman Filter,Time = t2,The Kalman Filter,Time = t2,The Kalman Filter,Time = t2,The Kalman Filter,Time =
15、 t2 is overProcess model w is Gaussian with zero mean and,The Kalman Filter,.,The Kalman Filter,More detail,The Kalman Filter,More detail,The Kalman Filter,brief.,The Kalman Filter,MATLAB example, voltage estimation Effect of covariance,Tunning,Q and R parameters Online estimation of R using AI (GA,
16、 NN, ) Offline system identification Constant and time varying R and Q Smoothing,CONTENTS,1. Introduction 2. Probability and Random Variables 3. The Kalman Filter 4. Extended Kalman Filter (EKF) 5. Particle Filter 6. SLAM,EKF,Linear transformationNonlinear transformation,EKF,example,EKF,EKFSuboptima
17、l, Inefficiency because of linearization Fundamental flaw changing normal distribution ad hoc,EKF,EKF,EKF,EKF,EKF,EKF,EKF,Cont.,EKF,EKF,Model:Step1: predictStep2: correct,EKF,EKF,Example:,EKF,Assignment: Consider following state space model: x1(k) = sin(k-1)*x2(k-1) +v x2(k) = x2(k-1) y = x1(k) + w
18、v is noisy input signal, vN(0, 0.5), w is observation noise, wN(0, 0.1) Simulate the system with given parameters and filter the states. - MATLAB figures of states and covariance matrix,EKF_this assignments is optional,Assignment(optional): Ackerman steering car process model (nonlinear):States are
19、(x, y, orientation) observation model (linear):H = 1 0 0;0 1 0; 0 0 1; In x-y plane, start from (0,0), go to (4,0), then (4,4), then (0,0),EKF_this assignments is optional,3 MATLAB figures, each including filtered and un-filterd x, y and orientation Simulation time, for T = 0.025 V= 3; % m/s, forwar
20、d velocity is fixed wheel base= 4; sigmaV= 0.3; % m/s sigmaG= (3.0*pi/180); % radians Q= sigmaV2 0; 0 sigmaG2; sigmaX= 0.1; % m, x sigmaY= 0.1; % m, y sigmaO= 0.005; % radian,orientation,CONTENTS,1. Introduction 2. Probability and Random Variables 3. The Kalman Filter 4. Extended Kalman Filter (EKF)
21、 5. Particle Filter 6. SLAM,Bayesian Estimation,Recursive Bayesian EstimationKnown pdf for q and r GivenFind,Bayesian Estimation,1) Chapman-Kolmogorov eq. (using process model)2) update (using observation model),Bayesian Estimation,Histogram Filter,Histogram Filter,Particle Filter,Suboptimal filters
22、 Sequential Monte Carlo (SMC) Based on the point mass (particle) representation of probability density Basic idea proposed in 1950s, 1) but that time there were no fast machine 2) and degeneracy problem Solution: resampling,Particle Filter,Particle Filter,Monte Carlo IntegrationRiemann sumApproximat
23、ion for Integral and expected value,Particle Filter,Importance Sampling (IS)Importance or proposal density q(x),Particle Filter,Sequential Importance Sampling (SIS) recursive The base for all particle (MC) filters: Bootstrap, condensation, particle, interacting particles, survival of fittest Key ide
24、a: Represent required posterior density with a set of samples with associated weights Compute estimation based on samples,CONTENTS,1. Introduction 2. Probability and Random Variables 3. The Kalman Filter 4. Extended Kalman Filter (EKF) 5. Particle Filter 6. SLAM,SLAM,Simultaneous localization and ma
25、pping Build/update map + keep the track of the robot Complexity under noise and error Different domain Indoor, outdoor, underwater, Solution Baysian rule, but there are problems Loop closing (consistent mapping) Convergence Computation power,SLAM,SLAM: Feature based View based Data Association Solut
26、ions (Feature based): EKF-SLAM Graph-SLAM FAST-SLAM, (Particle Filter SLAM),Data Association,Data Association Mahalanobis distance (1936),Data Association,Data Association (X2 distribution),Data Association,Data Association (X2 distribution),Feature based SLAM,SLAM,Probabilistic formulation,SLAM,Fin
27、dGiven Process modelObservation model,SLAM,Solution,EKF-SLAM,EKF-SLAM,EKF-SLAM,EKF-SLAM,Issues with EKF-SLAM Convergence Map convergence Computational Effort computation grows quadratically with the number of landmarks Data Association The association problem is compounded in environments where land
28、marks are not simple points and indeed look different from different viewpoints. Nonlinearity Convergence and consistency can only be guaranteed in the linear case,Graph-SLAM,Graph-SLAM,FAST-SLAM: Correlation,SLAM: robot path and map are both unknown!,Robot path error correlates errors in the map,FA
29、ST-SLAM,Rao-Blackwellized Filter (particle), FASTSLAM (2002) Based on recursive Mont Carlo sampling EKF-SLAM: Linear, Gaussian FAST-SLAM: Nonlinear, Non-Gaussian pose distribution Just linearzing observation model Monte Carlo Sampling,FAST-SLAM,FAST-SLAM,Key property: The pdf is on trajectory not on single pose Rao-Blackwellized state, the trajectory is represented by weighted samples Problem: Degenerecy,
