bayes rule -> practical application with Sharp IR sensor

Question

I have a robot using IR and Sonar sensors for measuring distances. I build an occupancy map/grid. So far I use a simple integer based system to "calculate" the probability of a cell being occupied. Something like +1 if sensor hit and -1 for all calls between 0 and sensor reading-1. If the number in the array for one cell if above the threshold the cell is counted as occupied and via versa for unoccupied. All between in uncertain. (a bit more complex but based on this idea)

I now wonder if it is worth to use a Bayes theorem based solution for this (first code snip below). As most people do it this way the answer is most likely yes :-).

What do p1 and p2 mean in this specific example - let's say for an IR distance sensor? I understand the examples when the theorem is explained. But somehow I can't translate them to the IR sensor situation. (my mind got a bit stuck here) I have no clue what and how to estimate the values I should put in there and how to apply them to my array/map.

Would be nice if someone could enlighten me :-) If somehow possible with some pseudo code.

Below also the class for my current map handling.

Thanks Robert

Bayes functions -- but how to apply?

def pos(p0, p1, p2):
    return (p0 * p1)/(p0 * p1 + (1-p0) * (1-p2))

def neg(p0, p1, p2):
    return (p0 * (1-p1))/(p0 * (1-p1) + (1-p0) * p2)

My current Map class:

templateData = {
    'MapWidth' : 800,
    'MapHeight': 600,
    'StartPosX' : 500,
    'StartPosY' : 300,
    'StartTheta' : 0,
    'Resolution' : 5,
    'mapThresholdFree' : 126,
    'mapThresholdOcc' : 130,
    'EmptyValue' : 128,
    'mapMaxOcc' : 255,
    'mapMaxFree' : 0,
    'ServoPos' : 0,
    'CurrentPosX' : 0,
    'CurrentPosY' : 0,
    'CurrentTheta' : 0
}

templateData["MapHeight"] = templateData["MapHeight"] / templateData["Resolution"]
templateData["MapWidth"] = templateData["MapWidth"] / templateData["Resolution"]
templateData["StartPosX"] = templateData["StartPosX"] / templateData["Resolution"]
templateData["StartPosY"] = templateData["StartPosY"] / templateData["Resolution"]

#map
map=robotmap.NewRobotMap(templateData["MapWidth"],templateData["MapHeight"], templateData["Resolution"], templateData["StartPosX"],templateData["StartPosY"], templateData["StartTheta"], templateData["ServoPos"],templateData["mapMaxOcc"],templateData["mapMaxFree"],templateData["EmptyValue"])
map.clear()

class NewRobotMap(object): 
    def __init__(self, sizeX, sizeY, Resolution, RobotPosX, RobotPosY, RobotTheta, ServoPos, mapMaxOcc, mapMaxFree, EmptyValue):
        self.sizeX = sizeX 
        self.sizeY = sizeY 
        self.RobotPosX = int(RobotPosX)
        self.RobotPosY = int(RobotPosY)
        self.mapResolution = int(Resolution)
        self.StartPosX = int(RobotPosX)
        self.StartPosY = int(RobotPosY)
        self.RobotTheta = float(RobotTheta)
        self.EmptyValue = EmptyValue
        self.ServoPos = ServoPos
        self.mapMaxOcc = mapMaxOcc
        self.mapMaxFree = mapMaxFree
    def clear(self):
        self.RobotMap = [[self.EmptyValue for i in xrange(self.sizeY)] for j in xrange(self.sizeX)]
    def updateMap(self ,x ,y , Val):
        oldval = self.RobotMap[x][y]
        self.RobotMap[x][y]=self.RobotMap[x][y] + Val
        if self.RobotMap[x][y] > self.mapMaxOcc:
            self.RobotMap[x][y] = self.mapMaxOcc
        elif self.RobotMap[x][y] < self.mapMaxFree:
            self.RobotMap[x][y] = self.mapMaxFree            
        return oldval, self.RobotMap[x][y]
    def updateRobot(self,theta,x,y):
        self.RobotTheta = float(theta)
        self.RobotPosX = int(round(self.StartPosX + float(int(x)/self.mapResolution), 0))
        self.RobotPosY = int(round(self.StartPosY - float(int(y)/self.mapResolution),0))
    def getRobotPos(self):
        return self.RobotPosX, self.RobotPosY
    def display(self):
        s = [[str(e) for e in row] for row in self.RobotMap]
        lens = [len(max(col, key=len)) for col in zip(*s)]
        fmt = '\t'.join('{{:{}}}'.format(x) for x in lens)
        table = [fmt.format(*row) for row in s]
        print '\n'.join(table)
    def updateServoPos(self, newServoPos):
        self.ServoPos = newServoPos

Answer 1

In pos:

• p0 is the prior probability that an object is there. This would probably require knowing in advance the density of objects in the map.
• p1 is the probability of a sensor hit given that an object is there (IOW a true positive)
• p2 is the probability of no sensor hit given that an object is not there (a true negative)

Now 1 - p2 is therefore the probability of a sensor hit given that an object is not there (a false positive). Additionally, 1 - p0 is the probability of no object being there. If we plug these values into Bayes' Rule, we get:

Pr(Object|Hit) = Pr(Hit|Object)Pr(Object) / ( Pr(Hit|Object)Pr(Object) + Pr(No Hit|Object)Pr(No Object)

= p1 * p0 / ( p1 * p0 + (1 - p2) * (1 - p0) )

Which is the pos() function you gave.

Note: In the event that you don't know the density of objects in the map beforehand, you can use p0 = 0.5, in which case the equation simplifies to:

Pr(Hit|Object) / ( Pr(Hit|Object) + Pr(No Hit|Object) )
= p1 / (p1 + (1-p2))