Download - 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
,J.B. Hayet Probabilistic robotics, February 2014 2 / 60
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 .
,J.B. Hayet Probabilistic robotics, February 2014 3 / 60
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.
,J.B. Hayet Probabilistic robotics, February 2014 4 / 60
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.
,J.B. Hayet Probabilistic robotics, February 2014 5 / 60
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. . .
,J.B. Hayet Probabilistic robotics, February 2014 6 / 60
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. . .
,J.B. Hayet Probabilistic robotics, February 2014 7 / 60
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. . .
,J.B. Hayet Probabilistic robotics, February 2014 8 / 60
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.
,J.B. Hayet Probabilistic robotics, February 2014 9 / 60
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.
,J.B. Hayet Probabilistic robotics, February 2014 10 / 60
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).
,J.B. Hayet Probabilistic robotics, February 2014 11 / 60
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.
,J.B. Hayet Probabilistic robotics, February 2014 12 / 60
Beam model for range finders
Outline
1 Beam model for range finders
2 Likelihood field model
3 Other observation models
,J.B. Hayet Probabilistic robotics, February 2014 13 / 60
Beam model for range finders
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 .
,J.B. Hayet Probabilistic robotics, February 2014 14 / 60
Beam model for range finders
Expected measurement: volumetric map
zk∗t
r = zmax
,J.B. Hayet Probabilistic robotics, February 2014 15 / 60
Beam model for range finders
Expected measurement: feature map
r = zmax
zk∗t
,J.B. Hayet Probabilistic robotics, February 2014 16 / 60
Beam model for range finders
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.
,J.B. Hayet Probabilistic robotics, February 2014 17 / 60
Beam model for range finders
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?)
,J.B. Hayet Probabilistic robotics, February 2014 18 / 60
Beam model for range finders
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).
,J.B. Hayet Probabilistic robotics, February 2014 19 / 60
Beam model for range finders
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.
,J.B. Hayet Probabilistic robotics, February 2014 20 / 60
Beam model for range finders
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)
,J.B. Hayet Probabilistic robotics, February 2014 21 / 60
Beam model for range finders
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)
,J.B. Hayet Probabilistic robotics, February 2014 22 / 60
Beam model for range finders
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 for range finders
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.
,J.B. Hayet Probabilistic robotics, February 2014 24 / 60
Beam model for range finders
Beam model: calibration
Consider a collection of data (Ri , zi) for expected values z∗i ,around some given value, and perturbed by noise:
[From Probabilistic Robotics, MIT Press]
,J.B. Hayet Probabilistic robotics, February 2014 25 / 60
Beam model for range finders
Beam model: calibration
Θ 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.
,J.B. Hayet Probabilistic robotics, February 2014 26 / 60
Beam model for range finders
Beam model: calibration
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 ,Θ).
,J.B. Hayet Probabilistic robotics, February 2014 27 / 60
Beam model for range finders
Beam model: calibration
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 |
,J.B. Hayet Probabilistic robotics, February 2014 28 / 60
Beam model for range finders
Beam model: calibration
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.
,J.B. Hayet Probabilistic robotics, February 2014 29 / 60
Beam model for range finders
Beam model: calibration
Derivating w.r.t. σr , in order to maximize the log:
∂ log p(Zr |R,M,Θ)∂σr
= − |Zr |σr
+ 1σ3r
∑zi∈Zr
(z∗i − zi)2
= − |Zr |σ3r
(σ2r − 1
|Zr |∑zi∈Zr
(z∗i − zi)2).
Optimal σr is:
σr =
√1
|Zr |∑zi∈Zr
(z∗i − zi)2.
,J.B. Hayet Probabilistic robotics, February 2014 30 / 60
Beam model for range finders
Beam model: calibration
Same process for the measurements in Zu:
P(Zu|Rt ,M ,Θ) ≈ η∏zi∈Zu
λuexp(−λuzi),
and by taking the log
logP(Zu|Rt ,M ,Θ) ≈ cst. + |Zu| log λu − λu∑zi∈Zu
zi ,
which is maximized for
λu =|Zu|∑
zi∈Zu
zi.
,J.B. Hayet Probabilistic robotics, February 2014 31 / 60
Beam model for range finders
Beam model: calibration
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 ,Θ).
,J.B. Hayet Probabilistic robotics, February 2014 32 / 60
Beam model for range finders
Beam model: calibration
We deduce
log p(Z |R ,M ,Θ) =∑
zi∈Zrlog pr (zi |Ri ,M ,Θ)+∑
zi∈Zulog pu(zi |Ri ,M ,Θ)+∑
zi∈Zflog pf (zi |Ri ,M ,Θ)+∑
zi∈Zolog po(zi |Ri ,M ,Θ)
=∑
zi∈Z I [ci = r |zi ,Θ] log pr (zi |Ri ,M ,Θ)+∑zi∈Z I [ci = u|zi ,Θ] log pu(zi |Ri ,M ,Θ)+∑zi∈Z I [ci = f |zi ,Θ] log pf (zi |Ri ,M ,Θ)+∑zi∈Z I [ci = o|zi ,Θ] log po(zi |Ri ,M ,Θ).
,J.B. Hayet Probabilistic robotics, February 2014 33 / 60
Beam model for range finders
Beam model: calibration
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,Θ)
.
,J.B. Hayet Probabilistic robotics, February 2014 34 / 60
Beam model for range finders
Beam model: calibration
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
.
,J.B. Hayet Probabilistic robotics, February 2014 35 / 60
Beam model for range finders
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).
,J.B. Hayet Probabilistic robotics, February 2014 36 / 60
Beam model for range finders
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 !
,J.B. Hayet Probabilistic robotics, February 2014 37 / 60
Likelihood field model
Outline
1 Beam model for range finders
2 Likelihood field model
3 Other observation models
,J.B. Hayet Probabilistic robotics, February 2014 38 / 60
Likelihood field model
Likelihood field model
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
).
,J.B. Hayet Probabilistic robotics, February 2014 39 / 60
Likelihood field model
Likelihood field model
yk,sxk,s θt
yt
xt
θk,s
,J.B. Hayet Probabilistic robotics, February 2014 40 / 60
Likelihood field model
Likelihood field model
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 ].
,J.B. Hayet Probabilistic robotics, February 2014 41 / 60
Likelihood field model
Likelihood field model
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.
,J.B. Hayet Probabilistic robotics, February 2014 42 / 60
Likelihood field model
Likelihood field model
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 π.
,J.B. Hayet Probabilistic robotics, February 2014 43 / 60
Likelihood field model
Likelihood field model
[From Probabilistic Robotics, MIT Press]
Underlying structure: likelihood field associated to pr .,
J.B. Hayet Probabilistic robotics, February 2014 44 / 60
Likelihood field model
Likelihood field model
[From Probabilistic Robotics, MIT Press]
The distribution pr is the intersection of the field with the sensor ray.
,J.B. Hayet Probabilistic robotics, February 2014 45 / 60
Likelihood field model
Likelihood field model
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.
,J.B. Hayet Probabilistic robotics, February 2014 46 / 60
Likelihood field model
Likelihood field model
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 .
,J.B. Hayet Probabilistic robotics, February 2014 47 / 60
Likelihood field model
Likelihood field model
[From Probabilistic Robotics, MIT Press]
,J.B. Hayet Probabilistic robotics, February 2014 48 / 60
Other observation models
Outline
1 Beam model for range finders
2 Likelihood field model
3 Other observation models
,J.B. Hayet Probabilistic robotics, February 2014 49 / 60
Other observation models
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 .
,J.B. Hayet Probabilistic robotics, February 2014 50 / 60
Other observation models
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)).
,J.B. Hayet Probabilistic robotics, February 2014 51 / 60
Other observation models
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.
,J.B. Hayet Probabilistic robotics, February 2014 52 / 60
Other observation models
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.
,J.B. Hayet Probabilistic robotics, February 2014 53 / 60
Other observation models
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
,J.B. Hayet Probabilistic robotics, February 2014 54 / 60
Other observation models
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.
,J.B. Hayet Probabilistic robotics, February 2014 55 / 60
Other observation models
Landmarks
sit
yt
xt
dit
φit
,J.B. Hayet Probabilistic robotics, February 2014 56 / 60
Other observation models
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. . .
,J.B. Hayet Probabilistic robotics, February 2014 57 / 60
Other observation models
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.
,J.B. Hayet Probabilistic robotics, February 2014 58 / 60
Other observation models
Landmarks: distance-only
Sometimes, only distance is available. . .
d2d1
,J.B. Hayet Probabilistic robotics, February 2014 59 / 60
Other observation models
Landmarks: angle-only
m2m1
,J.B. Hayet Probabilistic robotics, February 2014 60 / 60