# [Self driving vehicle] Histogram Filtering
Hello! today I am going to discuss about (Monte carlo) probabilistic localization with Histogram Filtering, one of the most straightforward localization methods with probability. The content of this post refers and gives credit to a very useful clss on Udacity: Artificial Intelligence for Robotics. This post is my first article also, so please point out if something is wrong or any suggestion!

Localization is the process to identify the location or yourself, or state of a system by some observation and some priot knowledge. Localization, breifly, includes only two steps that repeat over and over again. This includes "sensing" sensing and "moving" step. We will look at it trough a toy example and then formulate the undestanding on mathematical equations.

The first problem is set as follows:
A robot is in 1D world with five posible state. Each state is represented as $x_i$ and it is looped, which means that if we take one step from the right from $x_5$, we will land on $x_1$.
Also, each state has its own color. We set them as [green, red, red, green, green].

Okay, now we are ready to talk about those two steps mentioned before. Firstly let's take a look at sensing.

## Sensing

In sensing, we initially have our own belief as a probability on which state the robot is (But we have no clue in the first place! So, our "Prior probability distribution" are distributed equally.)

Next step is we take a measurement. We found out that it is red. This information can be used to calculate our new belief on which state the robot is (surely it might be in the second or the third state if the sensor is 100% accurate)

Upto here, we can write a simple code as follows:

# prior probability
p=[0.2, 0.2, 0.2, 0.2, 0.2]

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

# our measurement
measurements = ['red']

# the measurement is 100% accurate
pHit = 1
pMiss = 0

# we can update or belief distribution by this function
def sense(p, Z):
    q=[]
    for i in range(len(p)):
        hit = (Z == world[i])
        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
        
    # for normalization
    s = sum(q)
    for i in range(len(q)):
        q[i] = q[i] / s
    return q


p = sense(p,measurements[0])
print(p)

[0.0, 0.5, 0.5, 0.0, 0.0]

But what if our measurement tool are not 100% accurate? We can just add some probability of this by some weights. As it is hinted in the previous program, we can set pHit and pMiss as some kind of confident weights.

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

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

# our measurement
measurements = ['red']

# the measurement is 100% accurate
pHit = 0.6
pMiss = 0.2

# we can update or belief distribution by this function
def sense(p, Z):
    q=[]
    for i in range(len(p)):
        hit = (Z == world[i])
        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
        
    # for normalization
    s = sum(q)
    for i in range(len(q)):
        q[i] = q[i] / s
    return q


p = sense(p,measurements[0])
print(p)

[0.1111111111111111, 0.3333333333333332, 0.3333333333333332, 0.1111111111111111, 0.1111111111111111]

we can see that there are some probability that the robot will be in the state with green color even the measurement is red.

One more next step is when we do the measurement two times. We can just simply repeat the sense function again. In the second time, our "Prosterior probability" from the first calculation will be the "prior probability" of the second calculation. Since the robot does not move at all in this case, we can just use the probability as it is. We will see the case that the robot moves later in this article.

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

# two measurement
measurements = ['red', 'green']
pHit = 0.6
pMiss = 0.2

def sense(p, Z):
    q=[]
    for i in range(len(p)):
        hit = (Z == world[i])
        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
    s = sum(q)
    for i in range(len(q)):
        q[i] = q[i] / s
    return q

# repeat the step
for i in range(len(measurements)):
    p = sense(p,measurements[i])
print(p)

[0.20000000000000004, 0.19999999999999996, 0.19999999999999996, 0.20000000000000004, 0.20000000000000004]

You can see that the probability distribution returns like the initial one because we can measure both red and green from the same state.

## robot motion
Next, we gonna look at the motion of robot. Suppose the robot moves to the right one step, we can imagine that our belief probability will just shift to the right. That's easy. However, in the real motion, there are uncertainty the robot will move to the state it should move to. So, here we introduce another conditional probability of the the next state given the current state

Assume that the robot will move to the next two state to the right but there might fall into one step nearer or one step farther with probability 0.1 each

$$
p(x_{i+2}|x_i) = 0.8 \\
p(x_{i+1}|x_i) = 0.1 \\
p(x_{i+3}|x_i) = 0.1 \\
$$

For clearer image, we can write it as the following program.

p=[0, 1, 0, 0, 0]
pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1

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

print (move(p, 1))

[0.0, 0.1, 0.8, 0.1, 0.0]

## Let's combine them!

now it's time to combine them! The "sense" part is where we gain information and "move" part is where we lose information. Let's see the code first.

p=[0.2, 0.2, 0.2, 0.2, 0.2]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
motions = [1,1]
pHit = 0.6
pMiss = 0.2
pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1

def sense(p, Z):
    q=[]
    for i in range(len(p)):
        hit = (Z == world[i])
        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
    s = sum(q)
    for i in range(len(q)):
        q[i] = q[i] / s
    return q

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

p = sense(p,measurements[0])
print('posterior prob dis after sense 1:',p)   
p = move(p, motions[0])
print('posterior prob dis after move 1:', p)   
p = sense(p,measurements[1])
print('posterior prob dis after sense 2:', p)   
p = move(p, motions[1])
print('posterior prob dis after move 2:', p)         

posterior prob dis after sense 1: [0.1111111111111111, 0.3333333333333332, 0.3333333333333332, 0.1111111111111111, 0.1111111111111111]
posterior prob dis after move 1: [0.11111111111111112, 0.13333333333333333, 0.311111111111111, 0.311111111111111, 0.1333333333333333]
posterior prob dis after sense 2: [0.1578947368421053, 0.06315789473684212, 0.1473684210526316, 0.44210526315789467, 0.1894736842105263]
posterior prob dis after move 2: [0.21157894736842103, 0.1515789473684211, 0.08105263157894739, 0.16842105263157897, 0.3873684210526316]

## formal definition

Let's write down some equations here. Let $X=\{x_1,...,x_5\}$ is a random variable for the state of the robot, $Z =\{green, red\} $ is a ramdom variable for measurement value.

The "sense" part is just a Bayes rule:

$$
p(X|Z) = \frac{p(Z|X) p(X)}{p(Z)}
$$

where

$p(X)$ is the prior probability

$p(Z|X)$ is the measurement probability of the state (likelihood)

$p(X|Z)$ is the posterior probability

$p(Z)$ is the total probability of observing the evidence $ = \sum p(Z|X) p(X) over every x_i$

The "move" part is total probability of the state

$$
p(X) = \sum_{X'} p(X')p(X|X')
$$

where $X$ is the next time step state, $X'$ is the current time step state. To understand it easier, we can denote it as

$$
P(X^t = x_i) = \sum_j p(X^{t-1} = x_j)p(X^t = x_i|X^{t-1} = k_j)
$$

## We are done!!
Yes! that's it. We finish the localization in 1D. We can also extend this concept to higher dimensional state too. The following code extends to 2D grid state of similar robot (but in this 2D case, the motion probability is simplified a bit for simplicity) Thank you so much for read until the end!

# from Udacity course
# The function localize takes the following arguments:
#
# colors:
#        2D list, each entry either 'R' (for red cell) or 'G' (for green cell)
#
# measurements:
#        list of measurements taken by the robot, each entry either 'R' or 'G'
#
# motions:
#        list of actions taken by the robot, each entry of the form [dy,dx],
#        where dx refers to the change in the x-direction (positive meaning
#        movement to the right) and dy refers to the change in the y-direction
#        (positive meaning movement downward)
#        NOTE: the *first* coordinate is change in y; the *second* coordinate is
#              change in x
#
# sensor_right:
#        float between 0 and 1, giving the probability that any given
#        measurement is correct; the probability that the measurement is
#        incorrect is 1-sensor_right
#
# p_move:
#        float between 0 and 1, giving the probability that any given movement
#        command takes place; the probability that the movement command fails
#        (and the robot remains still) is 1-p_move; the robot will NOT overshoot
#        its destination in this exercise
#
# The function should RETURN (not just show or print) a 2D list (of the same
# dimensions as colors) that gives the probabilities that the robot occupies
# each cell in the world.
#
# Compute the probabilities by assuming the robot initially has a uniform
# probability of being in any cell.
#
# Also assume that at each step, the robot:
# 1) first makes a movement,
# 2) then takes a measurement.
#
# Motion:
#  [0,0] - stay
#  [0,1] - right
#  [0,-1] - left
#  [1,0] - down
#  [-1,0] - up

import numpy as np

def sense(p,colors,measurement,sensor_right):
    q = np.zeros_like(p)
    for i in range(len(p)): # row
        for j in range(len(p[0])): # colomn
            hit = (measurement == colors[i][j])
            q[i][j] = p[i][j]*(hit*sensor_right + (1-hit)*(1-sensor_right))
            
    sum_q = np.sum(q)
    for i in range(len(p)): # row
        for j in range(len(p[0])): # colomn  
            p[i][j] = q[i][j] / sum_q
     
    return p

def move(p,motion,p_move):
    q = np.zeros_like(p)
    motion_row = motion[0]
    motion_colomn = motion[1]
    p_row = len(p)
    p_colomn = len(p[0])
    
    for i in range(len(p)): # row
        for j in range(len(p[1])): # colomn
            s = p_move * p[(i-motion_row)%p_row][(j-motion_colomn)%p_colomn]
            q[i][j] = s + (1-p_move) * p[i%p_row][j%p_colomn]
    return q

def localize(colors,measurements,motions,sensor_right,p_move):
    # initializes p to a uniform distribution over a grid of the same dimensions as colors
    pinit = 1.0 / float(len(colors)) / float(len(colors[0]))
    p = [[pinit for row in range(len(colors[0]))] for col in range(len(colors))] #prior
    
    for i in range(len(measurements)):
        p = move(p,motions[i],p_move)
        p = sense(p,colors,measurements[i],sensor_right)
        
    return p

def show(p):
    rows = ['[' + ','.join(map(lambda x: '{0:.5f}'.format(x),r)) + ']' for r in p]
    print ('[' + ',\n '.join(rows) + ']')
    
#############################################################
# For the following test case, your output should be 
# [[0.01105, 0.02464, 0.06799, 0.04472, 0.02465],
#  [0.00715, 0.01017, 0.08696, 0.07988, 0.00935],
#  [0.00739, 0.00894, 0.11272, 0.35350, 0.04065],
#  [0.00910, 0.00715, 0.01434, 0.04313, 0.03642]]
# (within a tolerance of +/- 0.001 for each entry)

colors = [['R','G','G','R','R'],
          ['R','R','G','R','R'],
          ['R','R','G','G','R'],
          ['R','R','R','R','R']]
measurements = ['G','G','G','G','G']
motions = [[0,0],[0,1],[1,0],[1,0],[0,1]]
p = localize(colors,measurements,motions,sensor_right = 0.7, p_move = 0.8)
show(p) # displays your answer

[[0.01106,0.02464,0.06800,0.04472,0.02465],
[0.00715,0.01017,0.08697,0.07988,0.00935],
[0.00740,0.00894,0.11273,0.35351,0.04066],
[0.00911,0.00715,0.01435,0.04313,0.03643]]