Localization in Robotics (by bayes' rule)

I am not really into blogging but today I would like to write about Localization, the knowledge I gained while doing an online course on programming self driving car with practically implementing it on python. I assume you know what probability, distributions and baye's theorem are and very basics of python like for loop, if else block.  The localization problem is finding yourself where you are given you know the map of the environment you are in.

It consist of two steps:

• sense  
• move

_______________________________________________________________________
SENSE:

Localization works on discrete variable and multi-modal in nature. I know its not at all clear right now but I will try to explain as I learned.

Suppose we have a known 1-D map. It has five blocks. namely [green, red, red, green, green] and we have no idea where we are. We are equipped with sensors and it is capable of sensing the environment parameters(color in this example). So what would be our initial belief about where we are??

We are totally unbiased about where we currently are. So the probability distribution about the belief of our location is uniform and all the blocks are assigned equal probability. This is the prior in baye's theorem. 

world=['green', 'red', 'red', 'green', 'green']

p       =[0.2, 0.2, 0.2, 0.2, 0.2]

We take two variables pHit and pMiss both less than 1 as these are the probabilities. Assign pHit a large value as 0.6 and pMiss a smaller value as 0.2 .  pHit is the probability that sensors reading is correct and vice versa for pMiss.

pHit=0.6
pMiss=0.2

Now suppose our sensors see red color so our belief increases in red region. Now since we have seen red color as our first measurement we have:

Z=['red']


we multiply p[i] with pHit if location is world[i]='red'  or else with pMiss. So lets do it and see what happens.

world=              ['green', 'red',  'red',    'green',  'green']
p       =              [0.2     ,  0.2  ,   0.2,     0.2    ,    0.2   ]
decision|red =   [pMiss, pHit , pHit,   pMiss ,  pMiss]
multiplier =      [0.2    ,  0.6  ,   0.6,     0.2    ,    0.2   ]
result       =       [0.04  ,  0.12,   0.12,   0.04  ,    0.04 ]

The result we got is unnormalized probabilities. We can see that all the probabilities dont sum up to one.

P(X|Z) = P(Z|X)*P(X)*normalizer    // baye's theorem

OR

posterior = likelihood*prior/evidence



P(World|Measurements) = P(Measurement|World)*P(World)*normalizer

Depending on the Measurement 'red' or 'green' we have:

P(Measurement = 'red' | world = 'red') = pHit
P(Measurement = 'green' | world = 'green') = pHit
P(Measurement = 'green' | world = 'red') = pMiss
P(Measurement = 'red' | world = 'green') = pMiss



So if we say that the unnormalized probability is:

P''(World|Measurements) = P(Measurement|World)*P(World) = result


which we got earlier. Now lets calculate the normalizer and the sum of all values of P'' i.e. result.

P'' = sum(result)=sum([0.04  ,  0.12,   0.12,   0.04  ,    0.04 ])= 0.36  = 1/normalizer

to get the actual probabilities lets normalize it.

P(World|Measurements) = P(Measurement|World)*P(World)*normalizer
                                        = [0.04  ,  0.12,   0.12,   0.04  ,    0.04 ]/0.36
                                        = [1/9    , 1/3   ,   1/3  ,   1/9   ,     1/9  ]

So lets formally see the code.

#sense function

p=[0.2, 0.2, 0.2, 0.2, 0.2]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red']

pHit = 0.6
pMiss = 0.2

def sense(p, Z):
    q=p
    for i in range(len(p)):
         if world[i] == Z:
            q[i]=q[i]*pHit
        else:
            q[i]=q[i]*pMiss
    s = sum(q)
    for i in range(len(q)):
        q[i] = q[i] / s
    return q
#end of sense

now the only thing left is multi-modal. Suppose the world is:::   

world=['green', 'red',  'green', 'green', 'red' ]. Now if Z = 'red' then we have 2 peaks far apart and these multiple peak constitute of 2 modes. Later I will also write a post about Kalman filter and we see then that Kalman filters are uni-modal.

_________________________________________________________________________
MOVE:

Now in our 1-D world where world is ['green', 'red',  'red',    'green',  'green']. Suppose we can move forward and backwards. A +1 means 1 step right and a -1 means one step left. Similarly we move for other magnitudes. But our actuators or movements are not ideal. So we may reach one block more or less. If we move one block more in desired direction then it is called overshooting and if we move one block less than desired in desired direction then it is called undershooting. We also assume that the 1-D world is cyclic.

so we give some probabilities to the movements.

pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1

Now let me define move(p, U) function. It takes 2 parameters p and U, p is the distribution of belief and U is no. of steps to the right. If U is negative then we move to the left. Now let us see what happens to the probablities when we move right one step.

[0, 0, 1, 0, 0 ] = move([0, 1, 0, 0, 0],1)  #U +ve move right
[1, 0, 0, 0, 0 ] = move([0, 0, 0, 0, 1],1)  # cyclic world
[1, 0, 0, 0, 0 ] = move([0, 1, 0, 0, 0],-1)  #U -ve move right
[0, 0, 0, 1, 0 ] = move([0, 1, 0, 0, 0],2)  #multiple steps

Now you would have understood the working of the function move(p,U). The implementation is a bit tricky and non-trivial but I will try to explain it. Lets first see the code and then try to understand it.

suppose our moves are ideal.

pExact         = 1.0
pOvershoot  = 0.0
pUndershoot = 0.0

def move(p, U):
    q = []
    for i in range(len(p)):
        s = pExact * p[(i-U) % len(p)]
        q.append(s)
    return q

Whatever move we select. If U is positive , we start U blocks from the end and U blocks get cycled up from the end to the begining. As we are appending the blocks at the end of the list. and then we again start from the begining of p and keep appending to the end of q. Same holds true for negative U. It may take a while to think about it but u may give some time and then you may figure it out. Its like a graph shifting. p(i-U) is U steps shifted right.

Now lets consider non-ideal moves.

#defining function move formally and generally

pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1

def move(p, U):
    q = []
    for i in range(len(p)):
        s = pExact * p[(i-U) % len(p)] + pOvershoot * p[(i-(U+1)) % len(p)]
                                                         +pUndershoot * p[(i-(U-1)) % len(p)]
        q.append(s)
    return q

# end of function
This also looks like convolution. But this code also distributes small amount of probability to neighbouring blocks as moves are not ideal.


__________________________________________________________________________
 LOCALIZATION:

Now  it time to group the peices. Localization is actually unending sequence of sense and move.

We gain information when we sense and we loose information when we move and its a cycle of information gain and lose. We can see this by finding entropy during sense and move.

world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'red']
motions = [1,1]

for i in range(len(measurements)):
    p=sense(p,measurements[i])
    p=move(p,motions[i])
print p

For programming assignment see the link below:

Localization in 2-D space HW assignment