advertisement

Lecture 10: Observers and Kalman Filters CS 344R: Robotics Benjamin Kuipers Stochastic Models of an Uncertain World xÝ F(x,u) xÝ F(x,u,1) y G(x) y G(x,2 ) • Actions are uncertain. • Observations are uncertain. • i ~ N(0,i) are random variables Observers xÝ F(x,u,1 ) y G(x,2 ) • The state x is unobservable. • The sense vector y provides noisy information about x. ˆ Obs(y) is a process that • An observer x uses sensory history to estimate x. • Then a control law can be written u Hi (xˆ ) Kalman Filter: Optimal Observer 1 u x y 2 xˆ Estimates and Uncertainty • Conditional probability density function Gaussian (Normal) Distribution • Completely described by N(,) – Mean – Standard deviation , variance 2 1 2 (x )2 / 2 2 e The Central Limit Theorem • The sum of many random variables – with the same mean, but – with arbitrary conditional density functions, converges to a Gaussian density function. • If a model omits many small unmodeled effects, then the resulting error should converge to a Gaussian density function. Estimating a Value • Suppose there is a constant value x. – Distance to wall; angle to wall; etc. • At time t1, observe value z1 with variance • The optimal estimate is xˆ (t1) z1 with 2 variance 1 2 1 A Second Observation • At time t2, observe value z2 with variance 2 2 Merged Evidence Update Mean and Variance • Weighted average of estimates. xˆ (t 2) Az1 Bz2 A B 1 • The weights come from the variances. – Smaller variance = more certainty 2 2 ˆx (t 2 ) 2 2 2 z1 2 1 2 z2 1 2 1 2 1 1 1 2 2 2 (t 2 ) 1 2 From Weighted Average to Predictor-Corrector • Weighted average: xˆ (t2) Az1 Bz2 (1 K)z1 Kz2 • Predictor-corrector: xˆ (t 2) z1 K(z2 z1) xˆ (t1) K(z2 xˆ (t1)) • This version can be applied “recursively”. Predictor-Corrector • Update best estimate given new data xˆ (t 2) xˆ(t1) K(t 2)(z2 xˆ (t1)) • Update variance: 12 K(t 2 ) 2 2 1 2 (t 2 ) (t1 ) K(t2 ) (t1 ) 2 2 2 (1 K(t2 )) (t1 ) 2 Static to Dynamic • Now suppose x changes according to xÝ F(x,u,) u (N (0, )) Dynamic Prediction 2 ˆ • At t2 we know x (t 2 ) (t 2 ) • At t3 after the change, before an observation. 3 xˆ (t ) xˆ (t2 ) u[t3 t 2 ] 3 (t ) (t2 ) [t3 t2 ] 2 2 2 • Next, we correct this prediction with the observation at time t3. Dynamic Correction • At time t3 we observe z3 with variance • Combine prediction with observation. 3 3 xˆ (t 3 ) xˆ(t ) K(t3 )(z3 xˆ (t )) 3 (t 3 ) (1 K(t 3 )) (t ) 2 2 (t ) K(t 3 ) 2 2 (t3 ) 3 2 3 2 3 Qualitative Properties 3 3 xˆ (t 3 ) xˆ(t ) K(t3 )(z3 xˆ (t )) 2 (t3 ) K(t 3 ) 2 2 (t3 ) 3 • Suppose measurement noise 2 3 is large. – Then K(t3) approaches 0, and the measurement will be mostly ignored. 3 • Suppose prediction noise (t ) is large. 2 – Then K(t3) approaches 1, and the measurement will dominate the estimate. Kalman Filter • Takes a stream of observations, and a dynamical model. • At each step, a weighted average between – prediction from the dynamical model – correction from the observation. • The Kalman gain K(t) is the weighting, 2 2 – based on the variances (t) and 2 • With time, K(t) and (t) tend to stabilize. Simplifications • We have only discussed a one-dimensional system. – Most applications are higher dimensional. • We have assumed the state variable is observable. – In general, sense data give indirect evidence. xÝ F(x,u,1) u 1 z G(x,2) x 2 • We will discuss the more complex case next.