Objective

Implement the Bayes filter in Simulation


To start this lab, I looked over the thorough notes Vivek had left on last week's attempt at psuedo code. I realized a few fundamental misunderstandings I had, as well as some more subtle things I had missed.
To start with, I wrote the compute_control function. According to the feedback from last week, I was very close, and just needed to normalize the angle. I also got rid of some intermediate functions that would cause the sim to run even more slowly.
Although this was the simplest function, it actually ended up causing me the most pain. I made a very silly mistake of converting from radians to degrees incorrectly at first, which was causing all of my probabilities to go to zero and my entire implementation to not work. Vivek helped me out and suggested I use the helper functions in Python, which ended up fixing my code. 

      def compute_control(cur_pose, prev_pose):
    """ Given the current and previous odometry poses, this function extracts
    the control information based on the odometry motion model.

    Args:
        cur_pose  ([Pose]): Current Pose
        prev_pose ([Pose]): Previous Pose 

    Returns:
        [delta_rot_1]: Rotation 1  (degrees)
        [delta_trans]: Translation (meters)
        [delta_rot_2]: Rotation 2  (degrees)
    """
    #getting vars from input
    distX     = cur_pose[0] - prev_pose[0];
    distY     = cur_pose[1] - prev_pose[1];

    
    #calculating results based on lecture 11
    delta_rot_1 = loc.mapper.normalize_angle((math.degrees(math.atan2(distY, distX))) - prev_pose[2]); 
    
    return delta_rot_1, np.sqrt((distX*distX)+(distY*distY)), loc.mapper.normalize_angle((cur_pose[2]-prev_pose[2]-delta_rot_1))

Next, I worked on the odom_motion_model function. This was actually already fine as I wrote it initially, and so i left it alone. I simplified it to one line, again removing intermediate stored values to speed up the code. 
    return loc.gaussian(computed_control[0], u[0], loc.odom_trans_sigma)*loc.gaussian(computed_control[1], u[1], loc.odom_rot_sigma)*loc.gaussian(computed_control[2], u[2], loc.odom_rot_sigma)
Next, I moved to the prediction_step function. Here, my feedback was that I had not used enough for loops. Eventually, I figured out this meant that I was supposed to be looping through every previous case during every current case, which resulted in many many traversals of the nodes. When I first ran this, it took forever. I waited twenty minutes and it hadn't even gone through one node's worth of calculations. In order to fix this, I looked into the implementation hints on the course website. It suggested disgarding probabilities lower than .0001. After adding that in, I would not have to traverse all current nodes for the previous node. Further, I hadn't included the from_map utility that would allow the odom_motion_model to correctly calculate the given nodes. 
        computed_control = compute_control(cur_odom, prev_odom)
    
    for xpast in range(mapper.MAX_CELLS_X):
        for ypast in range(mapper.MAX_CELLS_Y):
            for apast in range(mapper.MAX_CELLS_A):
                

                if (loc.bel[(xpast, ypast, apast)] > .00001): 
                  
                    for xcur in range(mapper.MAX_CELLS_X):
                        for ycur in range(mapper.MAX_CELLS_Y):
                            for acur in range(mapper.MAX_CELLS_A):
                                
                                prob_state = odom_motion_model(mapper.from_map(xcur, ycur, acur), mapper.from_map(xpast, ypast, apast), computed_control)
  
                                loc.bel_bar[(xcur, ycur, acur)] += loc.bel[(xpast, ypast, apast)]*prob_state

Penultimately, I worked on the sensor_model function. This I did not quite understand while doing the psuedocode: I added a "u" which was supposed to represent a mean, because I didn't understand how to use the Gaussian for this single measurement. Then, I realized I needed to use the obs_view function to obtain the views for a passed position, which I could then look at the different for the observation passed, and compare to a mean of zero, which would be the ideal delta, meaning the measurements were the same. 
    def sensor_model(obs, pos):
    """ This is the equivalent of p(z|x).


    Args:
        obs ([ndarray]): A 1D array consisting of the measurements made in rotation loop
        pos            : x, y, a tuple

    Returns:
        [ndarray]: Returns a 1D array of size 18 (=loc.OBS_PER_CELL) with the likelihood of each individual measurements
    """
    prob_array = [0]*mapper.OBS_PER_CELL;
    element = mapper.obs_views[(pos[0], pos[1], pos[2])]
    
    for i in range(mapper.OBS_PER_CELL):
        prob_array[i] = loc.gaussian(obs[i]-element[i], 0, loc.sensor_sigma);


    return prob_array
The final function gave me the most trouble. I was very confused about how to implement it. Using the implementation keys was very helpful, as I learned I needed to multiply all the probabilities of every of the 18 views. Then, I could update each bel function. I also learned the importance of normalizing the bels by using np.sum(loc.bel) so that the probabilities wouldn't dip below zero due to underflow issues. I've had a lot of overflow issues in the past with my programming experience, but this was the first time i needed to consider underflow. 
def update_step():
    """ Update step of the Bayes Filter.
    Update the probabilities in loc.bel based on loc.bel_bar and the sensor model.
    """
    for xpos in range(mapper.MAX_CELLS_X):
        for ypos in range(mapper.MAX_CELLS_Y):
            for apos in range(mapper.MAX_CELLS_A):
                
        
                cur_pose         = np.array([xpos, ypos, apos])
                prob_sense       = 1;
                sense_model      = sensor_model(loc.obs_range_data, cur_pose)
                
                for k in range(mapper.OBS_PER_CELL): 
                    prob_sense *= sense_model[k]

                loc.bel[(xpos, ypos, apos)] = prob_sense*loc.bel_bar[(xpos, ypos, apos)];
    loc.bel = loc.bel/np.sum(loc.bel)
At first, my code was not remotely working, and well reflected how I was feeling about it

After some help from Vivek in debugging, I found that pesky degrees issue mentioned early, and incredibly my filter was working.