lecture 8: measurement models

Lecture 8: Measurement models

Dr. J.B. Hayet

CENTRO DE INVESTIGACION EN MATEMATICAS

February 2014

,J.B. Hayet Probabilistic robotics, February 2014 1 / 60

Outline

1 Beam model for range finders

2 Likelihood field model

3 Other observation models


Measurement models

In the same way as motion models, we want to define

p(Zt |Xt)

for all cases of estimation problems (SLAM, localization. . . ). It isobviously very specific to the sensor in use. In general itimplies considerations on the physical process of the measurementformation:

acoustics (sonars),

projective geometry,

. . .

As in the case of motion models, we consider

Xt = (RTt ,M

T )T .


Sensors

Contact sensors: bumpers.

Internal sensors:

accelerometers (springs-masses systems),gyroscopes,compass, inclinometers (earth magnetic field, gravity).

Proximity sensors:

sonar (time of flight),radar (phase and frequence),laser range-finders,infrared.

Visual sensors: cameras.

Satellite-based sensors: GPS.


Sensors

Typically, sonars use the time of flight principle to estimatedistances: a Transducer emits a small sound signal (Ping). The wavetravels, is reflected at some point and comes back to the Transducer(Echo). By knowing the sound velocity and by measuring the time offlight (i.e. the time between the Ping and the Echo) one deduces thedistance between the obstacle and the Transducer.


Sensors

[From Probabilistic Robotics, MIT Press]

Often, the structure of the errors is typical of the sensor. In thecase of sonars, it is very frequent to get signals at “maximal range”(no echo), partly explained by reflections in the environment. . .


Sensors

For range sensors (as sonars)

over-estimated range: specular reflections leaving the sensorcone;

under-estimated range: dynamical processes, as objects,persons moving in front of the robot, or cross-talk (i.e. EMinterferences between several transducers);

other causes: sensor failures. . .


Sensors

Laser:

A characteristic that is common with the sonar is that it emitsa signal and uses its echo to estimatedistances-to-impact;

the signal cone is much narrower;

they are in general quite precise;

but expensive. . .


Maps

A map is a collection of N primitives that form a representation ofthe environment, where the primitives are described by their positionand by some descriptors.

M = (m1,m2, . . .mn)T .

Two kinds of maps: volumetric, dense maps or sparse mapsformed by characteristic points.


Maps

Dense maps: Typically, these are grids. They allow to have an

exhaustive (but approximated) representation of theenvironment; they are indexed by positions, and each positioncomes with some information.

Sparse maps: Much lighter to handle, they do not give usinformation “between” characteristic points.


Measurement model

Consider the case of proximity sensors. Each measurement isgenerally made of a whole scan of range data, each data beinga distance from the sensor to the supposedly closestobject in the direction of the emission

Zt = [z0t , . . . , z

Kt ].

In a first approximation, they can be considered as independent

p(Zt |Rt ,M) =K∏

k=1

p(zkt |Rt ,M).


Measurement model

The ideal model

zkt = hk(Rt ,M),

can be extremely complicated, since it should involve allphysical considerations that explain the effective values observed atthe sensor level. The probabilistic model is designed to approximateroughly all these phenomena,

p(zkt |Rt ,M),

where both errors on observation data (noise) and errors onmodeling are handled.


Beam model for range finders

Outline






Measurement model

Given Rt and M (i.e. Xt) one can evaluate, for a sensor k , thetheoretical value zkt ,

zk∗t .

It can be generally easily evaluated by some simple geometricalconsiderations. But the measured data are different from zk∗t .



Expected measurement: volumetric map

zk∗t

r = zmax



Expected measurement: feature map

r = zmax

zk∗t



Beam model

The following model (beam model) holds for each individual sensor,and allows to distinguish among different error sources

errors from local noise on measurements,

errors because of objects not present in the map(example: a person that comes in front of the sensor),

errors because of non-detections (e.g., reflections out of thesensor cone),

errors that are not explained.



Beam model

1. “Local” noise on measurements, modeled by a Gaussian

pr (zkt |Rt ,M) ∝

{N(zkt ; zk∗t , σ2

r ) if 0 ≤ zkt ≤ zmax

0 otherwise

pr(zkt |Rt,M)

zzmaxzk∗t

One parameter: σr . The normalization factor is not the same asusual. . . (why?)



Beam model

2. Unexpected objectsIts distribution should have values that decrease with the range(if they are far away, all the space between the echo and the sensorhas to be free). By construction, it cannot outputmeasurements zkt > zk∗t (such outputs hide the expected one).



Beam model

2. Unexpected objects

pu(zkt |Rt ,M) ∝{λuexp(−λuzkt ) if 0 ≤ zkt ≤ zk∗t

0 otherwise

zmaxz

zk∗t

pu(zkt |Rt,M)

Exponential distribution with one parameter: λu. The normalizationis computed by integrating pu.



Beam model

3. Undetected objects (reflections. . . ). Typically, they cause rangevalues at saturation zmax ,

pf (zkt |Rt ,M) ∝{

1 if zmax − ε ≤ zkt ≤ zmax

0 otherwise

zmaxz

zk∗t

pf (zkt |Rt,M)



Beam model

4. Unexplained errors, modeled in an uniform distribution

po(zkt |Rt ,M) ∝{

1 if 0 ≤ zkt ≤ zmax

0 otherwise

zmaxz

zk∗t

po(zkt |Rt,M)



Beam model

The result is a weighted sum of the four distributions

p(zkt |Rt ,M) =∑

c∈{r ,u,f ,o}

βcpc(zkt |Rt ,M),

where βc define weights summing to 1.

p(zkt |Rt,M)

zzk∗t zmax ,

J.B. Hayet Probabilistic robotics, February 2014 23 / 60


Beam model: calibration

This model is quite generic, for sensors that measure depth; nowthere are parameters to calibrate:

the σr of the normal,

the λu of the exponential,

the βc .

Let us formΘ = (σr , λu, βr , βu, βf , βo)T .

For this calibration, typically, one considers a series of measurementscorresponding to a given expected value, and one tries to identifythe parameters.




Consider a collection of data (Ri , zi) for expected values z∗i ,around some given value, and perturbed by noise:





Θ can be adjusted “by hand”, if one is OK with a rough model(and has some time to spend. . . ).

The classical manner to calibrate the model is to maximizethe likelihood of the observations Z = {zi} that wegot, given the robot positions R = {Ri} and the map M :

Θ = maxΘ

p(Z |R ,M ,Θ),

i.e. Θ is determined as ML estimator.




Suppose that we know, for each i , the true valueci ∈ {r , u, f , o} (i.e. the cause of the noise that caused theobservation i).

The group of observations Z can be divided into Zc , forc ∈ {r , u, f , o}.The estimation of the parameters βc is done by maximizing thelikelihood of the observations given the knowledge of ci :

p(Z |R ,M ,Θ) = p(Zr |R ,M ,Θ)p(Zu|R ,M ,Θ)p(Zf |R ,M ,Θ)p(Zo |R ,M ,Θ).




One can re-write

p(zkt |Rt ,M) =∑

c∈{r ,u,f ,o}

βcpc(zkt |Rt ,M),

as

p(zkt |Rt ,M) =∑

c∈{r ,u,f ,o}

p(c)p(zkt |Rt ,M , c).

From this expression, and by identification, one gets

βc = p(c) =|Zc |∑i |Zi |




Now, let’s focus on the Zr only,

p(Zr |R ,M ,Θ) ∝∏zi∈Zr

pr (zi |Ri ,M ,Θ) ≈ η∏zi∈Zr

1√2πσ2

r

e− 1

2

(z∗i −zi )2

σ2r .

It is approximated, as the normalization is missing (depends on σr !).But it is OK while z is far from 0 or zmax . Hence,

log p(Zr |R ,M ,Θ) = cst. +∑zi∈Zr

−12

log(2πσ2r )− 1

2

(z∗i −zi )2

σ2r

= cst. − |Zi | log σr − 12σ2

r

∑zi∈Zr

(z∗i − zi)2.




Now, in practice we do NOT have the ci , hence we cannot dothe previous thing.

EM scheme: compute the expected values of ci , thenuse them to maximize the parameters Θ, and iterate thiscycle up to convergence.

Note that

p(Z |R ,M ,Θ) =∏

zi∈Zrpr (zi |Ri ,M ,Θ)∏

zi∈Zupu(zi |Ri ,M ,Θ)∏

zi∈Zfpf (zi |Ri ,M ,Θ)∏

zi∈Zopo(zi |Ri ,M ,Θ).




Hence:

E{ci}[log p(Z |R ,M ,Θ)] =∑

zi∈Z p(ci = r |zi ,Θ) log pr (zi |Ri ,M ,Θ)+∑zi∈Z p(ci = u|zi ,Θ) log pu(zi |Ri ,M ,Θ)+∑zi∈Z p(ci = f |zi ,Θ) log pf (zi |Ri ,M ,Θ)+∑zi∈Z p(ci = o|zi ,Θ) log po(zi |Ri ,M ,Θ).

In the first step (Expectation), consider parameters Θ as fixed,and evaluate p(ci = c |zi) por Bayes,

p(ci = c |zi ,Θ) = ηβcpc(zi |Ri ,M ,Θ),

with η = 1∑c βcpc (zi |Ri ,M,Θ)

.




In the second step (Maximization), maximize the likelihood onparameters Θ as we saw it before, supposing known ci , except that,instead, we have p(ci = c).

βc =

∑zi∈Z p(ci = c)

|Z |.

σr =

√1∑

zi∈Z p(ci = r)

∑zi∈Z

p(ci = r)(z∗i − zi)2).

λu =

∑zi∈Z p(ci = u)∑

zi∈Zp(ci = u)zi

.



Beam model

Caution.

Given the number of data per second, and the number ofoperations required per ray (raycasting . . . ), one may selectonly a smaller number of measurements.

It may be interesting to relax the distribution p(zkt |Rt ,M) ifone suspects that the model does not cope with all theobserved effects. For example pα(zkt |Rt ,M), with 0 < α < 1.

Raycasting operations being costly, it may be useful (inlocalization) to pre-compute the expected observationsand keep them in LUTs (i.e. grids).



Beam model

Limits

the model is somewhat too precise, as the distributions mayhave brutal variations in function of Rt : e.g., changes in θtmay lead to discontinuities,

this is not good in techniques based on sampling as (1) formaintaining a faithful representation of the distribution, peakylikelihood may lead to losing distribution modes and (2)because if you use optimization techniques at some point, thenthis distribution will be not smooth enough (local maxima),

the ray-casting is costly !


Likelihood field model

Outline







Project the scan points in the global frame; this would needthe location (xk,s , y k,s , θk,s) of the sensor in a local frame linkedto the robot.

(xtyt

)+

(cos θt − sin θtsin θt cos θt

)(xk,s

y k,s

)+zk

(cos(θt + θk,s)sin(θt + θk,s),

)these are point in the plane (i.e. the workspace)(

xzk,syzk,s

).




yk,sxk,s θt

yt

xt

θk,s




Consider that these zkt should correspond to obstacles or tonon-detection, which could be caused by:

1 a process generated by the closest obstacle on themap, with noise distributed in a normal distribution (σr ), andfunction of the distance to this obstacle,

2 a process corresponding to non-detections (values saturated atzmax), modeled by a narrow distribution with a peak at zmax ,

3 a process corresponding to unexplained measurements, with auniform distribution on [0, zmax ].




Again:

p(zkt |Rt ,M) = βrpr (zkt |Rt ,M) + βf pf (zkt |Rt ,M) + βopo(zkt |Rt ,M),

with po and pf as in the beam model, and

pr(zkt |Rt ,M) ∝ 1√

2πσ2r

e− 1

2d2

σ2r ,

where d is the distance from the position corresponding to theimpact in the global frame (xzk,s , yzk,s ) to the closest obstacle.




In practice,

start with π = 1,

for all measurements of the sensors zkt :1 if the measurement is zmax , do not consider it, otherwise,2 compute the map point corresponding to zkt ,3 compute the distance d from this point to the closest obstacle,

4 compute pr = 1√2πσ2

r

e− 1

2d2

σ2r ,

5 compute p = βrpr + βozmax ,

6 π = πp,

returns π.





Underlying structure: likelihood field associated to pr .,

J.B. Hayet Probabilistic robotics, February 2014 44 / 60




The distribution pr is the intersection of the field with the sensor ray.




It does not make sense to consider zmax values: we mapmeasures in the workspace, and this is not possible with zmax

(not a physical point).

Parameters can be estimated through EM.

The distributions are much smoother than in the beam model(the likelihood field we are using is smooth by essence).

Closest obstacle search may be heavy, this may needpre-computations.




Problems

do not allow to model dynamic objects that cause smallerrange values,

do not take into account the fact that a ray could not traverseobstacles (shadows)

how to handle partially explored maps?

In this latter case, consider also values of “unknown” occupancyvalues, and assume observations on them constant, to some value

1zmax .


Other observation models

Outline






Scan matching

Very common technique for localization or map building: scanmatching.

Consists in maintaining locally a map of the environment. Themap is build by accumulating scans from the range finder.

The observation, in that case, is made by this local map Mloc .



Scan matching

To compare the observation with the “expected local map”, theobserved local map is projected in the global frame (rotation,translation) and one may compute the normalized centeredcorrelation:

ρM,Mloc ,Rt =

∑x ,y

(mx ,y − m)(mx ,y ,loc(Rt)− m)√∑x ,y

(mx ,y − m)2∑x ,y

(mx ,y ,loc(Rt)− m)2,

where mx ,y ,loc(Rt) maps a cell from the local map to cell x , y in theglobal map, for all N cells and where

m =1

2N

∑x ,y

(mx ,y + mx ,y ,loc(Rt)).



Scan matching

Last, one can define the following model:

p(Mloc |Rt ,M) = max(ρM,Mloc ,Rt , 0),

since the values of ρ are in [−1, 1].

The result is not necessarily smooth. . . One can smooth Mbeforehand by a Gaussian kernel and compute correlation valueswith this smoothed map.

The advantage of such a model is that not only the extremitiesof the sensor ray are considered, but also the points along thewhole ray.



Landmarks

One can use only a small part of the sensor outputs to localize therobot, which may reduce considerably the amount of data toprocess. One choice is to detect a few remarkable, distinct elementsamong the data: features/landmarks.

3D points (with stereo).

Bearings (only angles: mountains at the horizon. . . ).

Segments o corners detected by a laser.



Landmarks

Notion of landmark, particular object located in the globalframe mi = (mi

x ,miy , s

i).

Associated to a vector of descriptors s i .

We are able to identify it.

Expected observation:

Z ∗t = (d i∗t , φ

i∗t , s

i∗t )

where

d i∗ =√

(x −mix)2 + (y −mi

y )2 φi∗ = arctanmi

y − yt

mix − xt

− θt



Landmarks

The map M is made by a set of landmarks

M = (m0, . . .mN).

Suppose that we have solved the problem of dataassociation between the landmark we are observing and oneof the mi in the map.



Landmarks

sit

yt

xt

dit

φit



Landmarks

One option: associate an error distribution to all observed quantitiesand suppose independence

pr (Zt |Rt ,M) = εσd (d∗ − d)εσφ(φ∗ − φ)εσs (s∗ − s).

The parameters of the normal laws, as usual, can be calibrated fromdata. Handling s is not always possible. . .



Landmarks

With several landmarks, one can suppose conditional independencebetween landmarks Zt = {(d i

t , φit , s

it)}:

p(Zt |Rt ,M) =∏

i p(d it , φ

it , s

it |Xt ,M)

=∏

i(εσd (d it − dk(i)∗)εσφ(φi

t − φk(i)∗)εσs (sit − sk(i)∗))

where the ak(i)∗ are estimates of the quantities given theassociations i -k(i) and without noise.

Without data association, the problem is much harder.



Landmarks: distance-only

Sometimes, only distance is available. . .

d2d1



Landmarks: angle-only

m2m1


lecture 8: measurement models

Documents