# Beam Models of Range Finders Ranger finders measure the range to nearby objects. Range may be measured along a beam, which is a good model of the workings of laser range finders, or within a cone, which is the preferable model of ultrasonic sensors. ## Measurement Algorithm Our model incorporates four types of measurement errors, all of which are essential to making this model work. 1. Small measurement noise 2. Errors due to unexpected objects 3. Errors due to failures to detect objects 4. Random unexplained noise The desired model $$p(z\_t \mid x\_t, m)$$is a mixture of four densities, each of which corresponds to a particular type of error. ### 1. Correct range with local measurement noise Let us use $$z\_t^{k\*}$$ to denote the true range of the object measured by $$z\_t^{k}$$. In location-based maps, the true range can be determined using ray casting. In feature-based maps, it is usually obtained by searching for the closest feature within a measurement cone. The measurement noise is usually modeled by a Gaussian with mean $$z\_t^{k\*}$$and standard deviation $$\sigma\_{hit}$$. We will denote the Gaussian by $$p\_{hit}$$. The values measured by range sensor are limited to the interval $$\[0, z\_{max}]$$, where $$z\_{max}$$ denotes the maximum sensor range. The measurement probability within the sensor range is given by the following. $$ p\_{hit}(z\_t^k \mid x\_t, m) = \eta; N(z\_t^k, \text{mean}=z\_t^{k\*}, \text{std} = \sigma\_{hit}^2) $$ For measurements outside of the sensor range, we can simply write, $$ p\_{hit}( z\_t^k \mid x\_t, m) = 0 $$ The true value is calculated from $$x\_t$$ and $$m$$ via ray casting. The function $$N$$ denotes the uni-variate normal distribution. The normalizer is evaluated to the following. $$ \eta^{-1} = \int\_0^{z\_{max}} N(z\_t^k, \text{mean} = z\_t^{k\*}, \text{std} = \sigma^2\_{hit}) ; dz\_t^k $$ The standard deviation is an intrinsic noise parameter of the measurement model. This varies from device to device. ### 2. Unexpected objects Environments of mobile robots are dynamic, where as maps are static. As a result, objects not contained in the map can cause range finders to produce surprisingly short ranges. One way to deal with such objects is to treat them as part of the state vector and estimate their location. Another much simpler approach is to treat them as sensor noise. The object is that closer to the sensor is more likely to measure than the object that is further away. We can describe this situation with an exponential distribution. $$ p\_{short}(z\_t^k \mid x\_t, m) = \eta ; \lambda\_{short}e^{-\lambda\_{short}z\_t^k} $$ Again, if the measurement is out of range, we can simply write, $$ p\_{short}(z\_t^k \mid x, m) = 0 $$ If we take the integral and we will find the normalizer is given by the following. $$ \eta = \frac{1}{1 - e^{-\lambda\_{short} z\_t^{k\*}}} $$ ### 3. Failures Sometimes, obstacles are missed altogether. A typical result of sensor failure is a max-range measurement. The sensor returns its maximum allowable value $$z\_{max}$$ because there is material that absorbs laser light beams. We will model this failure with a point-mass distribution centered at $$z\_{max}$$. $$ p\_{max}(z\_t^k \mid x\_t, m) = 1 ;\text{if}; z = z\_{max} $$ Technically, $$p\_{max}$$ does not possess a probability density function. It seems more like a Dirac-Delta function. #### 4. Random measurements Finally, range finders occasionally produce entirely unexplainable measurements. Keep things simple, we will use a uniform distribution spread over the entire sensor measurement range. $$ p\_{rand}(z\_t^k \mid x\_t, m) = \frac{1}{z\_{max}} $$ ## Beam Model Algorithm These four different distributions are now mixed by a weighted average, defined by the parameters $$z\_{hit}$$, $$z\_{short}$$, $$z\_{max}$$, and $$z\_{rand}$$. $$ z\_{hit} + z\_{short} + z\_{max} + z\_{rand} = 1 $$ $$ p(z\_t^k \mid x\_t, m) = \begin{vmatrix} z\_{hit} \ z\_{short} \ z\_{max} \ z\_{rand} \end{vmatrix}^{-1} \begin{vmatrix} p\_{hit}(z\_t^k \mid x\_t, m) \\ p\_{short}(z\_t^k \mid x\_t, m) \\ p\_{max}(z\_t^k \mid x\_t, m) \\ p\_{rand}(z\_t^k \mid x\_t, m) \end{vmatrix} $$ The resulting density is a linear combination of the four different error density. The algorithm will be implemented as follows. ``` q = 1 for k = 1 to K: compute true_z[t][k] for measurement z[t][k] using ray casting p = z_hit * p_hit + z_short * p_short + z_max * p_max + z_rand * p_rand q = q * p return q ``` ### Adjusting Parameters We have a set of parameters, the four mixing parameters $$ { z\_{hit}, z\_{short}, z\_{max}, z\_{rand} } $$ and the other two parameters. $$ { \sigma\_{hit}^2, \lambda\_{short}} $$ We will refer the whole set as $$\Theta$$. We will learn these parameters from actual data. This is achieved by maximizing the likelihood of a reference data set $$Z = { z\_i }$$ with associated positions $$X = { x\_i }$$ and map $$m$$, where each $$z\_i$$ is an actual measurement, $$x\_i$$ is the pose at which the measurement was taken. The likelihood of data is given by the following. $$ p(Z \mid X, m, \Theta) $$ Our goal is to identify intrinsic parameters that maximize this likelihood. ``` until convergence: for i, z in Z: norm = p_hit(z) + p_short(z) + p_max(z) + p_rand(z) norm = 1/norm calculate true_z e_hit[i] = norm * p_hit(z) e_short[i] = norm * p_short(z) e_max[i] = norm * p_max(z) e_rand[i] = norm * p_rand(z) z_hit = norm(Z)^-1 * sum(e_hit) z_short = norm(Z)^-1 * sum(e_short) z_max = norm(Z)^-1 * sum(e_max) z_rand = norm(Z)^-1 * sum(e_rand) sigma = compute_sigma(e_hit, Z, true_Z) lambda = compute_lambda(e_short, Z) ``` The $$\sigma\_{hit}$$ computation can be described as follows. $$ \sigma\_{hit} = \sqrt{\frac {\sum\_i e\_{i, hit}(z\_i - z\_i^\*)^2} {\sum\_i e\_{i, hit}} } $$ The $$\lambda\_{short}$$ computation can be described as follows. $$ \lambda\_{short} = \frac{\sum\_i e\_{i, short}}{\sum\_i e\_{i, short} z\_i} $$ --- # Agent Instructions: Querying This Documentation If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question. Perform an HTTP GET request on the current page URL with the `ask` query parameter: ``` GET https://calvinfeng.gitbook.io/probabilistic-robotics/basics/robot-perception/01-beam-models-of-range-finders.md?ask= ``` The question should be specific, self-contained, and written in natural language. The response will contain a direct answer to the question and relevant excerpts and sources from the documentation. Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.