probability, gaussians and estimation

Probability, Gaussians andEstimation

David Johnson

Basic Problem• Approaches so far

– Robot state is a point in state space

• q = ( x, y, vx, vy, heading )• This state based on

measurements– External (GPS, beacons, vision)– Internal (odometry, gyros)

• Measurements not exact• Errors can accumulate• How to “clean” measurements

– filtering• How to combine

measurements?– estimation

Problems to solve

• Localization– Given a map where am I?

• Mapping– Given my position, build a map

• SLAM – Simultaneous Localization and Mapping– Drop down a robot, build a map and location

within in at the same time

Approach

• Treat state variables as probabilities• Combine measurements weighted by

reliability• Use filtering to improve estimate of state

Example - Triangulation• Time of flight from

beacons gives distance– Constrains to a circle– 2 beacons to point

solutions

Noise in Measurements• Uncertainty in

measurement– Can be reported as

plus/minus some value• Creates solution

regions• Even this is a

simplification– Measurements follow

a distribution

Coin Flip

• F=(head, tails)– Discrete distribution

• Probability of a coin flip being head or tails is 0.5 + 0.5 = 1

• But what about continuous distributions?• Probability someone in the room is exactly

2 meters tall is infinitesimal• Talk about probability of intervals instead

Continuous distributions• Probability density

function f(x)• Find probability of a

measurement being within an interval

• What is f(x) for a uniform distribution over range [u,v]?

x

f(x)

Gaussian distributions

• Bell-shaped f(x)• Can assume most measurements with

noise follow a Gaussian distribution• Why

– Central Limit Theorem– Applet

Gaussian Definition

-s s

m

Univariate

m

Multivariate

2

2)(21

2

21)(

:),(~)(

sm

sp

sm

--

=x

exp

Nxp

)()(21

2/12/

1

)2(1

)(

:)(~)(

μxΣμx

Σx

Σμx

--- -

=t

ep

,Νp

dp

The Mean of a Continuous Distribution

Discrete Variance vs Continuous

• Discrete

• Continuous

Gaussians

• Gaussians completely described by mean and variance

• Non-zero mean implies a bias in measurement

• Zero mean can be removed by filtering

),(~),(~ 22

2

smsm

abaNYbaXY

NX

=

Properties of Gaussians

-- 22

21

222

21

21

122

21

22

212222

2111 1,~)()(

),(~),(~

ssm

sssm

sss

smsm NXpXp

NXNX

Filtering

• Gaussian noise = N(0, )• Make repeated measurements• Histogram the samples• Find the peak – that is the mean

– Easy!

• What is the size of the whiteboard in meters (1 decimal place precision)?

2s

Non-static situation

• What happens when state evolves?– Can’t repeat measurements

• Moving average filter

• Introduces lag into system!

Use a state model

• Estimate position from measurements• Measure velocity as well• Evolve position from velocity

– Incorporate evolved state into position measurements

– Need to combine multiple, uncertain measurements

Back to the non-evolving case

• Two different processes measure the same thing

• Want to combine into one better measurement

• Estimation

Estimation

What is meant by estimation?

Data + noise

Data + noise

Data + noise

Estimator Estimation

Hz ŷStochastic process estimate

A Least-Squares Approach

• We want to fuse these measurements to obtain a new estimate for the range

• Using a weighted least-squares approach, the resulting sum of squares error will be

• Minimizing this error with respect to yields

222

111

),0(),0(

vrRNrzvrRNrz

====

=

-=n

iii zrwe

1

2)ˆ(

r̂

r̂

0)ˆ(2)ˆ(ˆˆ 11

2 =-=-

=

==

n

iii

n

iii zrwzrw

rre

A Least-Squares Approach• Rearranging we have

• If we choose the weight to be

we obtain

0ˆ11

=- ==

n

iii

n

ii zwrw

=

== n

ii

n

iii

w

zwr

1

1ˆ

iii Rw 11

2 ==s

221

11

21

2

21

2

2

1

1

11ˆ z

RRRz

RRR

RR

Rz

Rz

r

=

=

• For merging Gaussian distributions, the update rule is

A Least-Squares Approach

22

21

22

212

322

21

22

21

22

21

23

111ss

sssssss

sss =

==

Show for N(0,a) N(0,b)

• This can be rewritten as

or if we think of this as adding a new measurement to our current estimate of the state we would get

• For merging Gaussian distributions, the update rule is

which if we write in our measurement update equation form we get

A Least-Squares Approach

)(ˆ 1221

11 zz

RRRzr -

=

22

21

22

212

322

21

22

21

22

21

23

111ss

sssssss

sss =

==

-

-

-

-

-

= 11111

111 kkk

kk

kkk PKP

RPRPP

)ˆ(ˆˆ 111

111

--

--

-

= kkk

kkk rz

RPPrr )ˆ(ˆˆ 11111

-

- -= kkkkk rzKrr

KalmanGainshow

What happens when you move?

),(~),(~ 22

2

smsm

abaNYbaXY

NX

=

derive

Moving

• As you move– Uncertainty grows– Need to make new measurements– Combine measurements using Kalman gain

The Kalman Filter“an optimal recursive data processing algorithm”

OPTIMAL:- Linear dynamics- Measurements linear w/r to state- Errors in sensors and dynamics

must be zero-mean (un-bias) white Gaussian

RECURSIVE:- Does not require all previous data- Incoming measurements ‘modify’

current estimate

DATA PROCESSING ALGORITHM:The Kalman filter is essentially a technique of estimation given a system model and concurrent measurements

(not a function of frequency)

The Discrete Kalman Filter

Estimate the state of a discrete-time controlled process that is governed by the linear stochastic difference equation:

with a measurement:

The random variables wk and vk represent the process and measurement noise (respectively). They are assumed to be independent (of each other), white, and with normal probability distributions

In practice, the process noise covariance and measurement noise covariancematrices might change with each time step or measurement.

(PDFs)

The Discrete Kalman Filter

First part – model forecast: prediction

“prior” estimate

Process noisecovariance

Statetransition

Stateprediction

Error covarianceprediction

Controlsignal

Prediction is based only the model of the system dynamics.

The Discrete Kalman Filter

Second part – measurement update: correction

“posterior” estimate

statecorrection

“prior” stateprediction

Kalmangain

actualmeasurement

predictedmeasurement

update error covariance matrix (posterior)

The Discrete Kalman Filter

The Kalman gain, K: “Do I trust my model or measurements?”

RHPHHP

K Tk

Tk

k

= -

- variance of the predicted states= ------------------------------------------------------------

variance of the predicted + measured states

measurement sensitivity matrix

measurementnoise covariance

As measurement error covariance, R, approaches zero, the actual measurement, zk is “trusted” more and more. is trusted less and less

But, as the “prior” (predicted) estimate error covariance, P, approaches zero, the actual measurement is trusted less, and predicted measurement, is trusted more and more

Estimate a constant voltage

• Measurements have noise• Update step is

• Measurement step is

Results

Variance

Parameter tuning

More tuning