Sensor Model

When we drive autonomous car we can divide path to grid.

We want to find probability to find object given sensor accuracy .

so lets say we have simple grid of 5 cells

our Road

So now lets say we know probability to be in one cell is 1/5 = 0.2

and we have probability to detect green if we on green Phit=0.6

and to detect green on red Pmiss=0.2 so how we can compute the total probability per cell ?

we need to multiply each cell probability to select by the probability to detect the green.

The next step is to Normalize the probabilities so the sum will be 1.

import numpy as np

def Sense(WorldColor,pHit,pMiss,ZSensor,prob):

    n = WorldColor.shape[0]
    #probPerSquare =1/n# probability per one cell

    probOut = np.zeros(n)
    #1 green
    if(ZSensor==1):
        for ind in range(0,n):
            if(WorldColor[ind]==1):
                probOut[ind] = prob[ind]*pHit
            else:
                probOut[ind]= prob[ind]*pMiss

    return probOut


def Normalize(probOut):
    sumProb = np.sum(probOut)
    probOut = probOut/sumProb
    return probOut

ZSensor=1
WorldColor = np.array([0, 0, 1, 1, 0])
prob = np.array([0.2, 0.2, 0.2, 0.2, 0.2])
pHit =  0.6
pMiss = 0.2

probOut = Sense(WorldColor, pHit, pMiss, ZSensor, prob)
print(probOut)

probOut = Normalize(probOut)
print(probOut)

Example

[0.04 0.04 0.12 0.12 0.04]
[0.11111111 0.11111111 0.33333333 0.33333333 0.11111111]

so as we see the final probability 1/9 1/9 1/3 1/3 1/9 (all sum to 1)

to detect green!