Lab 8: Drift Stunt

Lab Procedure Implementation

To perform grid localization using the Bayes filter as outlined in the lab manual, the following steps were undertaken within the provided Jupyter notebook (lab_10_notebook.ipynb):

Opened the Jupyter Notebook: The first step involved navigating to the notebooks directory within the simulation base code and opening the lab_10_notebook.ipynb file. This notebook contained the skeleton code, necessary helper functions, and instructions for implementing the Bayes filter.

Followed Notebook Instructions: The subsequent steps involved meticulously following the instructions provided within the notebook. This included implementing the core functions of the Bayes filter: the prediction step and the update step, along with any necessary helper functions like the motion model and sensor model.

Implemented the Motion Model (odom_motion_model):

As described in the "Odometry Motion Model in the Prediction Step" section of the lab manual, the odom_motion_model function was implemented to calculate the probability of transitioning from a previous robot pose to a current pose given the odometry control input ($u = \begin{pmatrix} \Delta rot_1 & \Delta trans & \Delta rot_2 \end{pmatrix}$). This involved:

Using the compute_control function to determine the actual control input ($actual\_u$) based on the previous and current odometry readings.
For each possible transition between grid cells (from a previous state to a current state), calculating the predicted control input ($u_{predicted}$) using compute_control.
Utilizing the provided gaussian function to calculate the probability of the actual control input given the predicted control input for rotation 1, translation, and rotation 2, considering the respective noise parameters (loc.odom_rot_sigma and loc.odom_trans_sigma).
Multiplying these individual probabilities to obtain the overall transition probability $p(x_t | x_{t-1}, u_t)$.

The implemented (example) code snippet for the motion model is shown below:

def odom_motion_model(cur_pose, prev_pose, u):
    u1= compute_control(cur_pose, prev_pose)
    prob_rot1 = loc.gaussian(u[0], u1[0], loc.odom_rot_sigma)
    prob_trans = loc.gaussian(u[1], u1[1], loc.odom_trans_sigma)
    prob_rot2 = loc.gaussian(u[2], u1[2], loc.odom_rot_sigma)
    prob = prob_rot1 * prob_trans * prob_rot2
    return prob

Implemented the Prediction Step (prediction_step):

The prediction_step function was implemented to update the belief state (loc.bel_bar) based on the previous belief (loc.bel) and the odometry motion model. This involved iterating through all possible previous grid cells and all possible current grid cells. For each pair, the probability of transitioning from the previous to the current cell (calculated using odom_motion_model with the actual control input) was multiplied by the probability of being in the previous cell. This product was then added to the probability of being in the current cell in the loc.bel_bar.

To optimize computation time, grid cells with a very low probability (below 0.0001) were skipped in the inner loops.

The implemented (example) code snippet for the prediction step is shown below:

def prediction_step(cur_odom, prev_odom):
    u = compute_control(cur_odom, prev_odom)
    for x1 in range(mapper.MAX_CELLS_X):
        for y1 in range(mapper.MAX_CELLS_Y):
            for a1 in range(mapper.MAX_CELLS_A):
                bel = loc.bel[x1][y1][a1]
                if bel >= 0.0001:
                    for x2 in range(mapper.MAX_CELLS_X):
                        for y2 in range(mapper.MAX_CELLS_Y):
                            for a2 in range(mapper.MAX_CELLS_A):
                                prob = odom_motion_model(mapper.from_map(x2, y2, a2), mapper.from_map(x1, y1, a1), u)
                                loc.bel_bar[x2][y2][a2] += (prob * bel)

Implemented the Sensor Model (sensor_model):

The sensor_model function was implemented to calculate the likelihood of the sensor measurements ($z_t$) given a particular robot state ($x_t$). As described, this involved assuming independence of the 18 individual laser range measurements. For each measurement, the gaussian function was used to calculate the likelihood of the observed range (loc.obs_range_data[i]) given the expected range from the map for the current grid cell (obtained using mapper.get_views(x, y, a)) and the sensor noise parameter (loc.sensor_sigma). The function returned an array of these individual likelihoods.

The implemented (example) code snippet for the sensor model is shown below:

def sensor_model(obs):
    prob_array = np.zeros(mapper.OBS_PER_CELL)
    for i in range(mapper.OBS_PER_CELL):
        prob_array[i] = loc.gaussian(loc.obs_range_data[i], obs[i], loc.sensor_sigma)
    return prob_array

Implemented the Update Step (update_step):

The update_step function was implemented to update the belief state (loc.bel) based on the prior belief (loc.bel_bar) and the sensor measurements. This involved iterating through all grid cells. For each cell, the likelihood of the sensor measurements given that state (calculated by taking the product of the probabilities returned by sensor_model for the corresponding map readings) was multiplied by the prior belief for that cell. Finally, the belief grid (loc.bel) was normalized to ensure that the sum of probabilities across all cells equals 1, addressing potential arithmetic underflow issues.

The implemented (example) code snippet for the update step is shown below:

def update_step():
    for x in range(mapper.MAX_CELLS_X):
        for y in range(mapper.MAX_CELLS_Y):
            for a in range(mapper.MAX_CELLS_A):
                prob_z = np.prod(sensor_model(mapper.get_views(x, y, a)))
                loc.bel[x][y][a] = prob_z * loc.bel_bar[x][y][a]
    loc.bel = loc.bel / np.sum(loc.bel)

Executed the Localization Loop: The notebook provided a loop to simulate the robot's movement and sensor readings over a trajectory. Within this loop, for each time step:

The prediction_step function was called to update the belief based on the robot's movement (odometry).
Simulated observation data was obtained using loc.get_observation_data().
The update_step function was called to incorporate the sensor measurements into the belief.
The most probable state was tracked and potentially visualized.

Normalization: As highlighted in the "Arithmetic Underflow" tip, the belief grid (loc.bel) was normalized after each update step using loc.bel = loc.bel / np.sum(loc.bel) to prevent probability values from becoming infinitesimally small and to maintain a valid probability distribution.

Angle Normalization: The mapper.normalize_angle() function was used within the compute_control function to ensure that calculated angles (rotations) were within the range of [-180, +180) degrees, as specified in the lab manual.

Computational Efficiency: Efforts were made to improve computational efficiency by skipping grid cells with very low probabilities in the prediction step, as suggested in the "Computation Time" tips. While the provided code snippets illustrate the core logic, further optimizations using NumPy operations might have been explored for larger-scale simulations.

By following these steps and implementing the described functions, the grid localization using the Bayes filter was performed for the sample trajectory within the simulation environment. Shown below is two simulation runs with their respective plots:

The green line is the line representing the actual path the robot is taking, the blue line is the belief at each cell the robot travels too and the red line is the odemtery of the robot as it moves through the sim.

These are the prediction stats at each cell the robot traveled:

These are the prediction stats at each cell the robot traveled:

Lab 10: Grid Localization using Bayes Filter

Lab Procedure Implementation