MATH: Added functions with sensor model equations (some of them)

src/cs4313_utilities/robot_math.py

@@ -6,6 +6,12 @@ import math
 import random
 import numpy
 
+# Some potentially useful constants
+
+BIGNUM = 1e9
+SMALLNUM = 1e-3
+TINYNUM = 1e-9
+
 # General purpose math-related functions
 
 def saturate(num, bound1, bound2):
@@ -121,6 +127,107 @@ def normalize_pi(angle):
     return angle
 
 
+# Sensor and motion model functions
+
+def likelihoodFieldModel(z, z_max, distances, sigma, \
+                         alpha_hit, alpha_max, alpha_rand):
+    ''' Implements the likelihood field algorithm discussed in class to
+    provide a likelihood value that a given range sensor value corresponds
+    to a particular map
+    @param z: sensor reading (range) being checked
+    @param z_max: sensor's maximum range
+    @param distances: iterable object containing distances from all obstacles
+    @param sigma: standard deviation of the likelihood field Gaussian
+    @param alpha_hit: normalized coefficient for the likelihood field model
+    @param alpha_max: normalized coefficient for the maximum range model
+    @param alpha_rand: normalized coefficient for the random return model
+    @return: PDF value for the likelihood that the return corresponds to the map
+    '''
+    return alpha_hit * likelihoodFieldReturnModel(distances, sigma) + \
+           alpha_rand * rangeSensorRandomModel(z_max) + \
+           alpha_max * rangeSensorMaxRangeModel(z, z_max)
+
+
+def rangeSensorModel(z, z_exp, z_max, sigma, lamda, \
+                     alpha_hit, alpha_unexp, alpha_max, alpha_rand):
+    ''' Implements the range sensor model from the CS4313 lecture material
+    @param z: sensor reading (range) being checked
+    @param z_exp: expected value (i.e., expected range to the obstacle)
+    @param z_max: sensor's maximum range
+    @param sigma: standard deviation of the hit model Gaussian
+    @param lamda: rate parameter of the obstacle model exponential
+    @param alpha_hit: normalized coefficient for the hit model
+    @param alpha_unexp: normalized coefficient for the obstacle model
+    @param alpha_max: normalized coefficient for the maximum range model
+    @param alpha_rand: normalized coefficient for the random return model
+    @return: PDF value for the probability that the return corresponds to the map
+    '''
+    return alpha_hit * rangeSensorHitModel(z, z_exp, sigma) + \
+           alpha_unexp * rangeSensorObstacleModel(z, z_exp, lamda) + \
+           alpha_rand * rangeSensorRandomModel(z_max) + \
+           alpha_max * rangeSensorMaxRangeModel(z, z_max)
+
+
+def likelihoodFieldReturnModel(distances, sigma):
+    ''' Computes the likelihood that a range sensor return is from a
+    mapped obstacle.  The likelihood is computed using a Gaussian.
+    @param distances: iterable object containing distances from all obstacles
+    @param sigma: standard deviation of the likelihood field Gaussian
+    @return: the likelihood that the return was from a mapped obstacle
+    '''
+    result = 0.0
+    for d in distances:
+        result += gaussian_PDF(d, sigma)
+    return result
+
+
+def rangeSensorHitModel(z, z_exp, sigma):
+    ''' Computes the probability that a given sensor reading is from a mapped
+    obstacle using a Gaussian distribution centered on the expected range
+    @param z: sensor reading (range) being checked
+    @param z_exp: expected value (i.e., expected range to the obstacle)
+    @param sigma: standard deviation of the Gaussian
+    @return: the PDF value that the return is from a mapped obstacle
+    '''
+    return gaussian_PDF((z-z_exp), sigma)
+
+
+def rangeSensorObstacleModel(z, z_exp, lamda):
+    ''' Computes the probability of a given sensor reading is from an
+    unmapped obstacle using an exponential distribution
+    @param z: sensor reading (range) being checked
+    @param z_exp: expected value (i.e., expected range to the obstacle)
+    @param lamda: rate parameter of the distribution
+    @return: the PDF value that the return is from an unmapped obstacle
+    '''
+    if z > z_exp:
+        return 0.0  # Can't detect an obstacle beyond the mapped one
+
+    exponential = math.exp(-lamda * z)
+    eta = 1.0 / (1.0 - exponential)
+    return eta * lamda * exponential
+
+
+def rangeSensorRandomModel(z_max):
+    ''' Computes the probability that a return is just a random return
+    using a uniform distribution between 0 and the maximum range
+    @param z_max: sensor's maximum range
+    @return: PDF value that the return is random
+    '''
+    return 1.0 / z_max
+
+
+def rangeSensorMaxRangeModel(z, z_max):
+    ''' Computes the probability that a return is a maximum range return
+    (i.e., not object detected)
+    @param z: sensor reading (range) being checked
+    @param z_max: maximum range of the sensor
+    '''
+    if abs(z - z_max) <= SMALLNUM:
+        return 1.0
+    return 0.0
+
+
 # Vector functions
 
 def scalar_multiply(v1, s):