280 likes | 456 Views
State Estimation and Kalman Filtering. Sensors and Uncertainty. Real world sensors are noisy and suffer from missing data (e.g., occlusions, GPS blackouts) Use sensor models to estimate ground truth, unobserved variables, make forecasts. How does this relate to motion?. A robot must:
E N D
Sensors and Uncertainty • Real world sensors are noisy and suffer from missing data (e.g., occlusions, GPS blackouts) • Use sensor models to estimate ground truth, unobserved variables, make forecasts
How does this relate to motion? • A robot must: • Know where it is (localization) • Model its environment to avoid collisions (mapping, model building) • Move in order to get a better view of an object (view planning, inspection) • Predict how other agents move (tracking)
Intelligent Robot Architecture Percept (raw sensor data) State estimationMappingTrackingCalibrationObject recognition… Sensing algorithms Models Planning Action
State Estimation Framework Dynamics model • Sensor model(aka observation model) • State => observation • Observations may be partial, noisy • Dynamics model(aka transition model) • State => next state • Transition may be noisy, control-dependent • Probabilistic state estimation • Given observations, estimate probability distribution over state xt xt+1 Sensor model zt
X0 X1 X2 X3 Markov Chain • Given an initial distribution and the dynamics model, can predict how the state evolves (as a probability distribution) over time
X0 X1 X2 X3 Hidden Markov Model • Use observations to get a better idea of where the robot is at time t Hidden state variables Observed variables z1 z2 z3 Predict – observe – predict – observe…
General Bayesian Filtering • Compute P(xt|zt), given zt, prior on P(xt-1) • Bayes rule: • P(xt|zt) = a P(zt|xt)P(xt) • P(xt) = integral [ P(xt | xt-1) P(xt-1) dxt-1 ] • 1/a = integral [ P(zt|xt)P(xt) dyt ] Nasty, hairy integrals
Key Representational Issue • How to represent and perform calculations on probability distributions?
Kalman Filtering • In a nutshell • Efficient filtering in continuous state spaces • Gaussian transition and observation models • Ubiquitous for tracking with noisy sensors, e.g. radar, GPS, cameras
Kalman Filtering • Closed form solution for HMM • Assuming linear Gaussian process • Transition and observation function are linear transformation + multivariate Gaussian noise y = A x + e e ~ N(m,S)
Linear Gaussian Transition Model for Moving 1D Point • Consider position and velocity xt, vt • Time step h • Without noise xt+1 = xt + h vtvt+1 = vt • With Gaussian noise of std s1 P(xt+1|xt) exp(-(xt+1 – (xt + h vt))2/(2s12) i.e. xt+1 ~ N(xt + h vt, s1)
vh s1 Linear Gaussian Transition Model • If prior on position is Gaussian, then the posterior is also Gaussian N(m,s) N(m+vh,s+s1)
Linear Gaussian Observation Model • Position observation zt • Gaussian noise of std s2 zt ~ N(xt,s2)
Observation probability Linear Gaussian Observation Model • If prior on position is Gaussian, then the posterior is also Gaussian Posterior probability Position prior • (s2z+s22m)/(s2+s22) s2s2s22/(s2+s22)
Kalman Filter • xt ~ N(mx,Sx) • xt+1 = F xt + g + v • zt+1 = H xt+1 + w • v ~ N(0,Sv), w ~ N(0,Sw) Dynamics noise Observation noise
Two Steps • Maintain mt, St the gaussian distribution over state at time t • Predict • Compute distribution of xt+1 using dynamics model alone • Update • Compute xt+1|zt+1 with Bayes rule
Multivariate Computations • Linear transformations of gaussians • If x ~ N(m,S), y = A x + b • Then y ~ N(Am+b, ASAT) • Consequence • If x ~ N(mx,Sx), y ~ N(my,Sy), z=x+y • Then z ~ N(mx+my,Sx+Sy) • Conditional of gaussian • If [x1,x2] ~ N([m1 m2],[S11,S12;S21,S22]) • Then on observing x2=z, we havex1 ~ N(m1-S12S22-1(z-m2), S11-S12S22-1S21)
Predict Step • Linear transformation rule: • xt+1 ~ N(Fmt + g, F St FT+ Sv) • Let these be m’ and S’
Update step • Given m’ and S’, and new observation z • Compute the final distribution mt+1 and St+1 using the conditional distribution formulas
xt zt m’ a S’ B BT C = N ( , ) Deriving the Update Rule (1) Unknowns a,B,C xt ~ N(m’ , S’) (2) Assumption zt | xt ~ N(H xt, SW) (3) Assumption zt | xt ~ N(a-BTS’-1xt, C-BTS’-1B) (4) Conditioning (1) H xt = a-BTS’-1(xt-m’) => a=Hm’, BT=HS’ (5) Set mean (4)=(3) C-BTS’-1B = SW => C = H S’ HT + SW (6) Set cov. (4)=(3) xt | zt ~ N(m’-BC-1(zt-a), S’-BC-1BT) (7) Conditioning (1) mt = m’ - S’HTC-1(zt-Hm’) (8,9) Kalman filter St = S’ - S’HTC-1HS’
Filtering Variants • Extended Kalman Filter (EKF) • Unscented Kalman Filter (UKF) • Particle Filtering
Extended/Unscented Kalman Filter • Dynamics / sensing model is nonlinearxt+1 = f(xt) + vyt+1 = h(xt+1) + w • Gaussian noise, gaussian state estimate as before • Strategy: • Propagate means as usual • What about covariances & conditioning?
Extended Kalman Filter • Approach: linearize about current estimateF = f/x(xt)H = h/x(xt+1) • Use Jacobians to propagate covariance matrices and perform conditioning Jacobian = matrix of partial derivatives
Unscented Kalman Filter • Approach: propagate a set of points with the same mean/covariance as the input, reconstruct gaussian
Next Class • Particle Filter • Non-Gaussian distributions • Nonlinear observation/transition function • Readings • Rekleitis (2004) • P.R.M. Chapter 9
Midterm Project Report Schedule (tentative) • Tuesday • Changsi and Yang: Indoor person following • Roland: Person tracking • Jiaan and Yubin: Indoor mapping • You-wei: Autonomous driving • Thursday • Adrija: Dynamic collision checking with point clouds • Damien: Netlogo • Santhosh and Yohanand: Robot chess • Jingru and Yajia: Ball collector with robot arm • Ye: UAV simulation