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: 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 ArchitecturePercept (raw sensor data)

Action

Sensing algorithms

Models

Planning

State estimationMappingTrackingCalibrationObject recognition…

State Estimation Framework

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

zt

xt+1

Sensor model

Dynamics model

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

X0 X1 X2 X3

z1 z2 z3

Hidden state variables

Observed variables

Predict – observe – predict – observe…

General Bayesian Filtering

Compute P(xt|zt), given zt, prior on P(xt-1) Bayes rule:

P(xt|zt) = P(zt|xt)P(xt)

P(xt) = integral [ P(xt | xt-1) P(xt-1) dxt-1 ]

= 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 + ~ N(,)

Linear Gaussian Transition Model for Moving 1D Point Consider position and velocity xt, vt

Time step h Without noise

xt+1 = xt + h vt

vt+1 = vt

With Gaussian noise of std

P(xt+1|xt) exp(-(xt+1 – (xt + h vt))2/(22

i.e. xt+1 ~ N(xt + h vt, )

Linear Gaussian Transition Model If prior on position is Gaussian, then the

posterior is also Gaussianvh 1

N(,) N(+vh,+1)

Linear Gaussian Observation Model

Position observation zt

Gaussian noise of std 2

zt ~ N(xt,)

Linear Gaussian Observation Model If prior on position is Gaussian, then the

posterior is also Gaussian

(2z+22)/(2+2

2)

2 222/(2+2

2)

Position prior

Posterior probability

Observation probability

Kalman Filter

xt ~ N(x,x)

xt+1 = F xt + g + v

zt+1 = H xt+1 + w

v ~ N(,v), w ~ N(,w)

Dynamics noise

Observation noise

Two Steps

Maintain t, t the gaussian distribution over state at time t

PredictCompute 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(,), y = A x + b Then y ~ N(A+b, AAT)

Consequence If x ~ N(x,x), y ~ N(y,y), z=x+y Then z ~ N(x+y,x+y)

Conditional of gaussian If [x1,x2] ~ N([1 2],[11,12;21,22]) Then on observing x2=z, we have

x1 ~ N(1-1222-1(z-2), 11-1222

-121)

Predict Step

Linear transformation rule: xt+1 ~ N(Ft + g, F t FT

+ v) Let these be ’ and ’

Update step

Given ’ and ’, and new observation z Compute the final distribution t+1 and t+1

using the conditional distribution formulas

Presentation

Deriving the Update Rulext

zt

’a= N ( , )’ B

BT C

xt ~ N(’ , ’)

(1) Unknowns a,B,C

(3) Assumption

(7) Conditioning (1)xt | zt ~ N(’-BC-1(zt-a), ’-BC-1BT)

(2) Assumption

zt | xt ~ N(H xt, W)

C-BT’-1B = W => C = H ’ HT + W

H xt = a-BT’-1(xt-’) => a=H’, BT=H’ (5) Set mean (4)=(3)

(6) Set cov. (4)=(3)

(8,9) Kalman filtert = ’ - ’HTC-1(zt-H’)

(4) Conditioning (1)zt | xt ~ N(a-BT’-1xt, C-BT’-1B)

t = ’ - ’HTC-1H’

Filtering Variants

Extended Kalman Filter (EKF) Unscented Kalman Filter (UKF) Particle Filtering

Extended/Unscented Kalman Filter Dynamics / sensing model is nonlinear

xt+1 = f(xt) + vyt+1 = h(xt+1) + w

Gaussian noise, gaussian state estimate as before

Strategy:Propagate means as usualWhat 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 FilterNon-Gaussian distributionsNonlinear observation/transition function

ReadingsRekleitis (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