Lab 10:

Compute Control

The first part of this lab asked us to compute_control or given the odometry, poses, extract a change in rotation, a translation, and another change in rotation using the relationship in translation and orientation of the previous and current pose.

The following code was used to do this:

Odometry Motion Model

Next, it was necessary to calculate p(x’ | x, u). This indicates the probability that the robot is in a certain state given the previous state and a control input. Essentially, a Gaussian distribution was used to define what the probability of the robot being in the new position was.

Prediction Step

The next was the prediction step of the model. The prediction step in the Bayes filter takes the probability that the robot is in a certain state (bel(x_t-1), multiplying that by a transition probability (or the probability that the robot has moved to a certain new state) to find bel_bar(x_t). By calculating these for each of the grid spaces and normalized, the predict step has provided a way for us to determine a belief of being in a state given the previous control input and previous state. To speeden up the Bayes Filter, we ignore states that have a probability of less than 0.0001 (thank you to Rafi’s lab for the tip!). This requires less calculations as it is already improbable that the robot is in that state and speedens up the Bayes Filter.

The code to do this is as follows:

Sensor Model

The next step was to find p(z

x). i.e. if we got a certain measurement, what is the probability of receiving that measurement given we are in a certain state.

The code to do this is as follows:

Update Step:

The update step then updates the bel based on the bel_bar and sensor model. I.e. now that there is a sensor model, and given a prediction of what state the robot is in, calculate the new probability of where the robot is (using probabilities of the previous state as well as a measurement of the current state). Then, this is normalized. This is the final estimation of where the robot is.

The code to do this is as follows:

Simulation Results:

After running the Bayes filter, these were the results of the simulation:

The simulator likely works well with a lot of objects in the range of the sensors (to detect and update probabilities) and poorly when there is open space where the probabilites remain largely uniform.

Sim Results:

Initial:

Bayes: