This section describes a strategy for actively localizing a known number of devices with prior estimates using range-only sensors. In previous work, we presented a similar approach for localizing individual devices [15, 16]. In comparison, here we model the finite range of the sensors.
5.2.1 Estimating Devices’ Locations
Because devices are independent of each other and measurements have a known data asso- ciation, we estimate the position of Ddifferent devices using separate particle filters. Each filter uses a measurement model that accounts for noisy range measurements, as well as the probability of a measurement being received.
Formally, the distribution over device d’s 2D position at time t is approximated as
p(xd | z1:t) = PNi=1wiδ(xd −˜xd) where δ(·) is the Dirac delta function, ˜xdi is the 2D
from time 1 to t [134]. Devices are static, so we omit a time subscript for them, but we do use a zero mean 2D Gaussian with a small fixed covariance for the process model of the filter to avoid particle degeneracy problems.
At time t the team receives a random vector of measurements zt, where ztd,r is the 1-
dimensional range measurement robotr makes to deviced. If the distance from a robot to a device is within the maximum range of the sensor,zmax, we assume that with probability
γ the robot receives a measurement of the true distance perturbed by Gaussian noise. With probability 1−γ, the robot does not detect the device, and gets a measurement of zmax. Defining the true distance as s=kxd−crtk, the measurement model can be expressed as:
p(zd,rt =z|xd) =
γN(z−s, σ2) + (1−γ)N(z−zmax, σ2max) s < zmax
N(z−zmax, σmax2 ) otherwise
(5.1)
whereσ2 is the variance of the sensor andN(z−µ, σ2) is the likelihood ofzwith a Gaussian whose mean isµ with covarianceσ2.
Real world sensors may return an error when they fail to measure the distance to a device instead ofzmax. To compensate for this behavior, a robot can incorporate a “virtual” measurement ofzmaxfor each measurement error. Additionally, when a sensor fails to make a measurement, it will not be perturbed by noise;σ2
maxshould be 0, resulting in Dirac deltas in (5.1) that are centered on zmax. However, this measurement model would be difficult to work with analytically (e.g., the entropy would become −∞). We have also encountered difficulties using small values for σ2
max, as this results in a rapid change in variance of particles that are close to zmax. Consequently, we set σ2max = σ2, which is a reasonable model: when a measurement of zmax is incorporated into the filter, particles that are less than zmax away from the robot will become less likely, while those that are farther away will become more likely.
5.2.2 Calculating Mutual Information
To evaluate mutual information between the expected future location of discovered devices and measurements the team will make, we use the particle representation of the device’s position, p(x|z1:t) and the range-only measurement model (5.1), to calculate the distribu-
tion over expected future measurements,p(zτ). This approach, which we covered in detail in Ch. 4, results in a computationally intractable problem that we address through a series of approximations.
The expression for mutual information between all devices and measurements for a given team trajectory is:
IMI[x;zτ |cτ] = D X d=1 IMI[xd;zdτ] = D X d=1 H[zdτ]−H[zdτ |xd] (5.2) H[zdτ] is the differential entropy of the measurements to deviced, while H[zdτ |xd] is the con- ditional differential entropy of measurements to deviced. We drop the measurements depen- dence on the team’s trajectory,cτ, for brevity. The expression is a sum over the information from devices because devices and their associated measurements are pairwise independent,
p(x,zτ) =Qdp(xd,zdτ) [27].
Calculating the entropy, H[zdτ], is difficult because the distribution over future measure- ments to each device d is a Gaussian mixture model (GMM):
p(zdτ) = PN
i=1wiQtj+=Tt+1
QR
r=1p(zd,rj | xd = ˜xdi) where wi is the weight of the ith parti-
cle inp(xd), ˜xdi is its location,N is the number of particles,R is the number of robots, and
T is the time horizon of the plan (Sect. 5.1.2, Sect. 5.2.1). Unfortunately, p(zd,rj | xd) is also a GMM when the robot is in range of the device (5.1). Consequently,p(zdτ) can be the sum of products of GMMs, resulting in a GMM with a number of components that is ex- ponential in RT. We avoid this computational issue by approximating (5.1) with the most likely component (i.e., the one with maximum weight) when calculating entropy, making the number of components equal toRT. This is reasonable when the probability of detection is high, which is typically the case for range-only sensors. Despite this simplification, p(zdτ) is
still a GMM, whose entropy cannot be evaluated analytically. We approximate it using the 2nd order Taylor-series approximation developed by Huber et al. [59]. This approach has a time complexity ofO(N2RT).
Using the conditional independence assumption, the expression for the conditional en- tropy simplifies H[zdτ |xd] =PN i=1wi Pt+T j=t+1 PR r=1H[z d,r
j |xd= ˜xdi]. Re-applying the max-
imum likelihood estimate for detection, each term is the entropy of a 1-dimensional Gaus- sian, which can be evaluated in constant time [27].