Skip to main content
Open In Colab A localization or mapping filter has to score each LIDAR scan: given a pose sts_t in a map mm, how probable is the scan the sensor returned? The beam model of Probabilistic Robotics (Thrun, Burgard and Fox, Section 6.3) first predicts the range each beam should read. It then allows the measured range to differ from that prediction in four ways: a small measurement error, an object the map does not contain, no return at all, and a random return. You define the four distributions, combine them into the likelihood of a scan, connect their parameters to the datasheet of the TurtleBot3’s LDS-01, test them on a simulated room, and estimate the parameters from range readings.

​
Expected range by ray casting

The prediction comes from the map. Place the sensor at its pose in the map and follow a straight line from it in the direction of beam kk until the line meets the first wall or occupied cell. The length of that line is the expected range z^tk\hat z_t^k, the reading a noise-free sensor would return if the map were complete. Following such a line through the map is called ray casting. In an occupancy grid it steps from cell to cell along the line and stops at the first occupied cell. A line that meets nothing within the sensor’s maximum range zmax⁡z_{\max} gives z^tk=zmax⁡\hat z_t^k = z_{\max}. The line starts at the sensor, not at the robot’s center. With the robot at st=(x,y,θ)s_t = (x, y, \theta) and the sensor mounted at (xs,ys,θs)(x_s, y_s, \theta_s) in the robot frame, the sensor sits at [x+xscos⁡θ−yssin⁡θy+xssin⁡θ+yscos⁡θ]\begin{bmatrix} x + x_s\cos\theta - y_s\sin\theta \\ y + x_s\sin\theta + y_s\cos\theta \end{bmatrix} in the map, and beam kk, at bearing ϕk\phi_k in the sensor frame, points at the angle θ+θs+ϕk\theta + \theta_s + \phi_k. The notation for a scan is:
  • Scan: zt={ztk}k=1K\mathbf z_t = \{ z_t^k \}_{k=1}^K, one range per beam.
  • Expected range: z^tk=r(st,ϕk,m)\hat z_t^k = r(s_t, \phi_k, m), where rr casts the ray of beam kk from the sensor pose above.
  • Range limits of the sensor: zmin⁡z_{\min} and zmax⁡z_{\max}.

​
The four components

Each component is a distribution of the measured range zz of one beam, given its expected range z^\hat z (Thrun, Burgard and Fox, Section 6.3.1). Thrun, Burgard and Fox define them on [0,zmax⁡][0, z_{\max}]. A real sensor cannot report a range below its minimum zmin⁡z_{\min}, so here the continuous components live on [zmin⁡,zmax⁡)[z_{\min}, z_{\max}), and the value zmax⁡z_{\max} itself is kept for a beam that returned nothing. Hit. A return from the surface the map predicts, with Gaussian measurement noise: phit(z∣z^)=ηhit N(z; z^,σhit2)for zmin⁡≤z<zmax⁡,p_{\text{hit}}(z \mid \hat z) = \eta_{\text{hit}}\, \mathcal N(z;\, \hat z, \sigma_{\text{hit}}^2) \quad \text{for } z_{\min} \le z < z_{\max}, where ηhit\eta_{\text{hit}} makes the density integrate to one over that interval. Short. A return from an object between the sensor and the predicted surface, such as a person the map does not contain. Nearer objects block the beam first, so the density falls with range: pshort(z∣z^)=λshort e−λshortze−λshortzmin⁡−e−λshortz^for zmin⁡≤z≤z^.p_{\text{short}}(z \mid \hat z) = \frac{\lambda_{\text{short}}\, e^{-\lambda_{\text{short}} z}}{e^{-\lambda_{\text{short}} z_{\min}} - e^{-\lambda_{\text{short}} \hat z}} \quad \text{for } z_{\min} \le z \le \hat z. Max. A beam that returned nothing reads zmax⁡z_{\max}. This component is a point mass, Pmax⁡(z=zmax⁡)=1P_{\max}(z = z_{\max}) = 1. Thrun, Burgard and Fox write it as the indicator I(z=zmax⁡)I(z = z_{\max}) and note that it has no density (Eq. 6.11). Random. A reading that none of the other components explains, spread evenly over the range: prand(z)=1zmax⁡−zmin⁡for zmin⁡≤z<zmax⁡.p_{\text{rand}}(z) = \frac{1}{z_{\max} - z_{\min}} \quad \text{for } z_{\min} \le z < z_{\max}. Each component is zero outside its interval. The model mixes them with weights αhit,αshort,αmax⁡,αrand≥0\alpha_{\text{hit}}, \alpha_{\text{short}}, \alpha_{\max}, \alpha_{\text{rand}} \ge 0 that sum to one (Thrun, Burgard and Fox write them zhit,…z_{\text{hit}}, \dots; Eq. 6.13). Because one component is a point mass, the mixture has two parts. A reading below zmax⁡z_{\max} has the density p(z∣z^)=αhit phit(z∣z^)+αshort pshort(z∣z^)+αrand prand(z),zmin⁡≤z<zmax⁡,p(z \mid \hat z) = \alpha_{\text{hit}}\, p_{\text{hit}}(z \mid \hat z) + \alpha_{\text{short}}\, p_{\text{short}}(z \mid \hat z) + \alpha_{\text{rand}}\, p_{\text{rand}}(z), \qquad z_{\min} \le z < z_{\max}, which integrates to 1−αmax⁡1 - \alpha_{\max}, and a reading of exactly zmax⁡z_{\max} has probability αmax⁡\alpha_{\max}. The model assumes that each reading comes from a single ray. That suits a LIDAR’s narrow beam better than a sonar, whose echo can come from anywhere in a cone of about 15° (SensComp).

​
The likelihood of a scan

The likelihood of beam kk is the mixture evaluated at its own reading and expected range, p(ztk∣st,m)=p(ztk∣z^tk),z^tk=r(st,ϕk,m).p(z_t^k \mid s_t, m) = p\big(z_t^k \mid \hat z_t^k\big), \qquad \hat z_t^k = r(s_t, \phi_k, m). The model treats the beams as conditionally independent given the pose and the map, so the likelihood of a scan is the product over its beams (Table 6.1). A computer adds logarithms instead, since a product of hundreds of small numbers underflows: log⁡p(zt∣st,m)=∑k=1Klog⁡p(ztk∣z^tk).\log p(\mathbf z_t \mid s_t, m) = \sum_{k=1}^K \log p\big(z_t^k \mid \hat z_t^k\big). The beams are not really independent. Neighboring beams hit the same surface, and an error in the pose or the map moves many of their predictions together. The product counts that shared evidence many times over, which makes the likelihood overconfident. Implementations therefore use only some of the beams, or scale down the summed log likelihood.

​
Model parameters from the datasheet

The datasheet supplies the range limits, the angular sampling and the scan rate. A calibration of the mounting supplies the sensor pose, and recorded scans supply the mixture weights and the noise parameters. The table maps each entry to its symbol, with the values of the TurtleBot3’s ROBOTIS LDS-01. A ROS driver reports the range limits with each scan, as range_min and range_max of the LaserScan message (sensor_msgs/LaserScan). REP 117 asks drivers to report a beam with no return as +∞+\infty, an invalid reading as NaN and a reading too close to measure as −∞-\infty. Before scoring a scan, map +∞+\infty to zmax⁡z_{\max}, so that the max component scores it, and drop the NaN and −∞-\infty readings. A beam with no return carries information: no surface within zmax⁡z_{\max} reflected the pulse along it. The datasheet gives two error figures. Precision usually describes how much repeated readings of the same surface scatter. Accuracy also includes a systematic offset. phitp_{\text{hit}} is centered on the true range, so it models the scatter, and an offset is better removed by calibration. Neither figure is a standard deviation, since the datasheet quotes a bound and does not say how many standard deviations it covers. The code below reads the ±3.5% as two standard deviations, an illustrative choice that gives σhit≈0.0175 z^\sigma_{\text{hit}} \approx 0.0175\, \hat z, or 3.5 cm at 2 m. That standard deviation grows with the expected range. Thrun’s phitp_{\text{hit}} has one σhit\sigma_{\text{hit}} for all ranges, so a LIDAR specified in percent calls for σhit(z^)\sigma_{\text{hit}}(\hat z) in its place. The fit at the end of this section estimates it from readings instead. The code takes the datasheet values, derives KK, the bearings ϕk\phi_k, the range limits and σhit(z^)\sigma_{\text{hit}}(\hat z), and evaluates the mixture for expected ranges of 1 m and 3 m. The mixture weights are illustrative.
Two densities of an LDS-01 reading on a log scale: narrow and wider hit peaks at 1 and 3 meters above a decaying short-return part and a flat random part, with an arrow at the maximum range of 3.5 meters for the point mass of no-return readings. Density of one LDS-01 reading below zmax⁡z_{\max} for expected ranges of 1 m and 3 m, on a log scale. The curves carry 96% of the probability. The other 4%, αmax⁡\alpha_{\max}, sits at zmax⁡z_{\max}, where the arrow stands for it. At 3 m the hit peak is three times wider than at 1 m, because the datasheet gives the precision as a percentage of the range. With these weights, the short-return part is larger than the random part below the peak, and beyond the peak only the random part remains. Four small plots for an expected range of 2 meters: a narrow Gaussian peak at 2 meters, an exponential decaying from the minimum range to 2 meters, a flat uniform density up to 3.5 meters, and an arrow for the unit point mass at 3.5 meters. The four components for one LDS-01 beam with an expected range z^\hat z of 2 m, before mixing. Pmax⁡P_{\max} is a point mass at zmax⁡z_{\max}, not a density. Only phitp_{\text{hit}} and pshortp_{\text{short}} depend on z^\hat z. The scan rate matters for a moving robot because a scan is not taken at one instant. The LDS-01 turns at 300 ± 10 rpm, so one scan takes about 0.2 s, and a robot driving at 0.2 m/s moves 4 cm during it. That is already more than σhit\sigma_{\text{hit}} at 1 m, 1.8 cm, and turning in place moves distant endpoints farther still. The model here treats a scan as taken from one pose sts_t. A robot that drives fast, and every car, corrects each beam for the motion during the sweep, using the time at which the beam was measured. No datasheet gives the mixture weights α\alpha or the rate λshort\lambda_{\text{short}}. They depend on the scene: how many people walk through it, how much glass and dark furniture it holds, the weather. A darker surface returns a weaker echo and can be lost at a shorter range. The Hokuyo UST-10LX, for example, guarantees detection to 10 m on white card but only to 4 m on a 10% reflective surface (spec sheet). How much that raises αmax⁡\alpha_{\max} for the LDS-01 in a given room has to be measured there.

​
A simulated scan

The LIDAR section describes how readings go wrong. In the beam model, a return near the wall the map predicts is scored by the hit term, a return from a closer object such as a person or a chair by the short term, a beam with no return by the max term, and a reading in the valid range that none of these explains, from interference or multipath for example, by the random term. The code below simulates one scan of a 2D LIDAR with a 7 m range in a room that contains each case, and labels every reading with the case that produced it. It generates the readings from the geometry of the scene, not by sampling the mixture. A wall return gets 3 cm of noise, a return from the person gets none, 3% of the beams are replaced by random readings, and every beam through the glass window or onto the dark bin is forced to return nothing. Real glass can also reflect or pass the beam, and a dark surface only weakens the echo. The forced rule stands in for those cases.
Top view of a simulated 2D scan in a room. Most beams hit the walls at the expected range; beams toward a person end short; beams through a glass window and onto a dark bin are forced to return nothing and are drawn toward 7 meters, cut off at the edge of the plot; a few beams return at random ranges. A simulated 2D scan, one beam every 3°. Hits on the walls (green), short returns from a person the map does not contain (orange), forced no-returns through the glass window and onto the dark bin (gray, drawn toward the 7 m maximum range and cut off at the edge of the plot), and a few random returns (gold). Measured and expected ranges of the simulated scan against beam bearing from 0 to 360 degrees. The expected ranges form a stepped black line; most measured dots sit on it, a few read short, a group reads the maximum range of 7 meters, and a few are scattered. The simulated scan as ranges against bearing. The black line is the expected range z^tk\hat z_t^k from ray casting in the map of walls. Most measured ranges ztkz_t^k sit on it; the person’s beams read short, the beams through the glass and onto the bin read zmax⁡z_{\max}, and a few readings are random.

​
Estimating the parameters

The weights α\alpha, the rate λshort\lambda_{\text{short}} and the width of phitp_{\text{hit}} are estimated from readings znz_n whose expected ranges z^n\hat z_n are known, D={(zn,z^n)}n=1N\mathcal D = \{(z_n, \hat z_n)\}_{n=1}^N. You get such data by recording scans at known poses in a room with a known map and ray casting each beam. Each z^n\hat z_n is the same quantity as z^tk\hat z_t^k, with one index running over all the beams of all the training scans.
In the terms of supervised learning, z^n\hat z_n plays the role of the input xx and the reading znz_n the role of the target yy, since the model is a conditional distribution p(z∣z^)p(z \mid \hat z). Unlike a regression, nothing here learns to predict zz from z^\hat z: the hit term is already centered on z^\hat z. The fit learns only the shape of the error around that prediction, namely its width, the rate of early returns, and how often a beam returns nothing or a random range. In localization the roles change. The scan is observed, the filter computes z^\hat z for each candidate pose by ray casting, and p(z∣z^)p(z \mid \hat z) scores how well that pose explains the scan.
If you knew which component produced each reading, every parameter would have a closed-form estimate. You do not, so expectation maximization (EM) alternates two steps, as in the algorithm learn_intrinsic_parameters (Thrun, Burgard and Fox, Section 6.3.2, Table 6.2). The E-step computes, for every reading, the probability that each component produced it, its responsibility. A reading of exactly zmax⁡z_{\max} can only come from the max component, so its responsibility rn,max⁡r_{n,\max} is 1. For a reading below zmax⁡z_{\max}, rn,i=αi pi(zn∣z^n)αhit phit(zn∣z^n)+αshort pshort(zn∣z^n)+αrand prand(zn),i∈{hit,short,rand}.r_{n,i} = \frac{\alpha_i\, p_i(z_n \mid \hat z_n)}{\alpha_{\text{hit}}\, p_{\text{hit}}(z_n \mid \hat z_n) + \alpha_{\text{short}}\, p_{\text{short}}(z_n \mid \hat z_n) + \alpha_{\text{rand}}\, p_{\text{rand}}(z_n)}, \qquad i \in \{\text{hit}, \text{short}, \text{rand}\}. Table 6.2 as printed leaves the weights αi\alpha_i out of this normalization. With them, each step is an EM step and can only increase the likelihood. The M-step treats the responsibilities as soft labels and sets each parameter to its weighted maximum-likelihood value. The weights are the average responsibilities, αi=1N∑nrn,i\alpha_i = \frac{1}{N} \sum_n r_{n,i}. Table 6.2 sets the width from the weighted squared residuals. Here σhit=c z^\sigma_{\text{hit}} = c\,\hat z grows with range, so the same update applies to the relative residuals: c2=∑nrn,hit((zn−z^n)/z^n)2∑nrn,hit.c^2 = \frac{\sum_n r_{n,\text{hit}} \left( (z_n - \hat z_n) / \hat z_n \right)^2}{\sum_n r_{n,\text{hit}}}. This ignores the cut at zmin⁡z_{\min} and zmax⁡z_{\max} in ηhit\eta_{\text{hit}}, which is harmless when σhit\sigma_{\text{hit}} is a few centimeters. Table 6.2 sets λshort=∑nrn,short/∑nrn,short zn\lambda_{\text{short}} = \sum_n r_{n,\text{short}} / \sum_n r_{n,\text{short}}\, z_n, which is the maximum-likelihood rate of an exponential with no upper cut. Here the cut at z^\hat z matters, because a rate of 1/m spreads short readings over the whole interval. The code therefore maximizes the weighted log likelihood of the truncated exponential numerically. The code below draws 20,000 readings from the model itself, with the parameters of the code above as the truth, at expected ranges between 0.5 m and 3.4 m. Because the true parameters are known, you can check that EM recovers them. EM starts far from them, with equal weights, c=0.05c = 0.05 and λshort=3\lambda_{\text{short}} = 3 per meter.
Left: the log likelihood rising over the EM iterations and leveling off. Right: a histogram of 5,000 new readings at an expected range of 2 meters on a log scale, with the fitted density following the hit peak, the short-return slope and the flat random floor. Left: the log likelihood of the 20,000 readings at each EM iteration. Right: 5,000 new readings at an expected range of 2 m against the density of the fitted model; the readings at zmax⁡z_{\max} are left out of the histogram. EM recovers the four weights to within 0.002 and the relative width cc to the printed digits, starting far from all of them, and the log likelihood rises at every iteration. The rate λshort\lambda_{\text{short}} comes out at 0.93 per meter against the true 1. It is the hardest parameter to pin down: only 8% of the readings are short, and a short reading near the sensor looks much like a random one. On the same responsibilities, the update of Table 6.2, which ignores the cut at z^\hat z, gives 1.35 per meter, 35% too high. With real readings, the residuals also contain errors of the pose and the map, and the fit absorbs them into σhit\sigma_{\text{hit}} and the weights. Record the training scans at accurately known poses in a room whose map you trust, so that the estimates describe the sensor and the scene rather than those errors.

​
References

Found an error on this page? Report an issue.