Miniproject 2 -- Probabilistic Graphical Models

The questions below are due on Monday November 13, 2023; 11:59:00 PM.
 

This miniproject's Colab notebook can be found here: Colab Notebook (Click Me!)

In this mini project, continuing the "search and rescue" domain from the first miniproject, in this assignment, we will consider the problem of inferring the location of the robot as it moves around. Specifically, you will:

• Implement a particle filter for the localization problem.
• Implement a particle filter for the simultaneous localization and mapping (SLAM) problem.
• Implement a Rao-blackwellized particle filter for the simultaneous localization and mapping (SLAM) problem.
• Run simulations to test the performance of your algorithms.

1) Warming up with sampling§

Let us first check to make sure we know how to sample from different distributions.

1.1) Warmup 1§

Let's make sure we know how to sample from a set of values based on their weights.

Please write a function that:

• Takes in a list of samples and associated weights.
• Re-samples from the list of samples with replacement according the likelihood of each sample given by the list of weights.

Hint: You should use the function numpy.random.choice.

For reference, our solution is 2 line(s) of code.

1.2) Warmup 2§

We are going to be sampling from Gaussian distributions in this miniproject. First, let us check that we know how to sample from a multivariate Gaussian. Please write a function that takes in a mean vector, a covariance matrix and a integer number of samples and returns a list of the requested number of samples drawn from a Gaussian with the given mean and covariance.

Hint: You should use the function numpy.random.multivariate_normal.

For reference, our solution is 2 line(s) of code.

1.3) Experiment 1: Sampling from Multivariate Gaussian §

Please generate a 2D scatter plot of 1000 samples drawn from a 2D Gaussian with mean = [0, 0] and covariance = np.diag([0.05, 0.01]), using the function you just coded: warmup_2. Consider using the helper function provided: plot_samples.

Let's get some intuition about geometry. Please generate a second 2D scatter plot of 1000 samples drawn from a 2D Gaussian distribution. You should pick the mean and covariance so that the figure shows an ellipse with a 45-degree angle with the x-y axes. Specifically, try to modify the mean and covariance to rotate figure 1a by 45-degree counter-clockwise.

1.4) Experiment 2: Motion Noise §

We have provided a simulator class for you, that simulates the noisy motion and sensing of the robot. Let’s begin by ignoring any observations and just trusting that the robot goes where we have commanded. We’ll look at the relationship between commands and actual trajectories.

We have provided a Simulator class for you. It simulates the noisy motion and sensing of the robot. Let's begin by exploring what happens if we don't do any inference, and assume we always know where the robot is.

We have provided a method Simulator.run that simulates the motion of the robot given a sequence of commands. For now, you may ignore the infer parameter. If you call this method, you will get a SimulationResult. Here, SimulationResult.true_poses holds a list of poses representing the ''true'' trajectory of the robot. You can ignore the rest of the fields in SimulationResult for now.

Please call run twice, passing in drive_in_a_square_commands() as the commands.

• For the first time, run on a simulator with motion_noise_covariance=zeros((2, 2))
• For the second time, run on a simulator with motion_noise_covariance=np.diag([1e-3, np.deg2rad(3)**2]). The drive_in_a_square_commands () generates a sequence of commands that attempt to drive the robot in a square motion. However, you should see a significant difference between the trajectories without noise, which should be a perfect square, versus with noise.

Hint: You may use the helper function plot_trajectories to generate the trajectory plot.

If we knew the noise on each step, the noise wouldn't be a problem. We could just correct for it as we steer the robot, but the problem is we don't the noise --- it's just the random perturbations added by nature. Therefore, we have to use the sensors to know where the robot is and correct for the "drift" in the robot motion. That is why we perform inference.

Now we've convinced ourselves of the need for inference, let us implement the two functions we need for sampling-based inference.

2) Particle Filter Localization §

Now, let’s assume that there are some landmarks (e.g. radio beacons or visible mountains in the distance) with known absolute locations. The robot can observe measurements of its relative position to these landmarks. This problem becomes an HMM, in which the hidden state is the robot’s position and orientation (called a "pose" for short), the noisy steering we saw above is the transition model, and the landmark measurements are the observations. We are interested in performing filtering to obtain a distribution over the robot’s pose at each time step. We could consider using a Kalman filter, but as we will see, the transition dynamics are substantially non-linear and a KF will not perform well.
Instead, we will use a particle filter.

2.1) Localization: Update by Motion Model§

Let us now start by building a motion model for a simple ground robot that moves in straight lines, and performs point turns.

A common motion model for such a robot assumes that the robot has a pose given by [x_t, y_t, \theta_t] at time t. The robot takes a command [\Delta p_t, \Delta \theta_t] which modifies the robot's pose according to the following:

where [\omega_t, \nu_t] is Gaussian error injected at each time step, distributed as \omega_t \sim \mathcal{N}(0, \sigma^2_p), and \nu_t \sim \mathcal{N}(0, \sigma^2_\theta). The units of \Delta p_t is in metres and \Delta \theta_t in radians.

Let us assume that the robot's pose at each time step is a random variable, represented as a set of samples.

Please implement a function that takes in a set of sampled poses drawn from prior pose random variable \mathbf{X}_t and a control [\Delta p_t, \Delta \theta_t] and returns a set of samples drawn from the posterior X_{t+1}.

Note In this project, we will use a helper decorator implementation_for to implement class methods outside of the class. The decorator @implementation_for(Localization) simply inserts the declared function as a method of the Localization class. Using this decorator, you copy only the implemented method into catsoop's solution box and avoid repeating other parts of the class definition.

Hint: Use np.random.multivariate_normal to generate the noise.

For reference, our solution is 10 line(s) of code.

Experiment 3

Now that you have implemented motion_model, let's play with the noise parameters. Let's first use the motion parameters of motion_noise_covariance=np.eye(2):

• Initialize your particle filter using the default parameters (but pass in the above motion noise parameter and use 1000 particles), and step through the motion model twice.
• Use a Command of \Delta_p = 1m and \Delta_\theta = 0 for both times.
• Please generate plots of the [x, y] component of the particles after the first and second motions (two separate plots).

This is a pretty noisy motion! You should see the particles much more spread out than they were at the start. Why is the distribution long and skinny after one motion and then really spread out after the second motion?

Experiment 4

Now let's try reducing the noise parameters. Please perform the experiment 3 again, but set motion_noise_covariance=np.diag([1e-2, np.deg2rad(.5)**2]). Please generate two plots same as in experiment 3. The particle set should be much more compact than before and track the motion you commanded.

Experiment 5

For the two experiments above, we're taking pretty big motions, commanding a meter at a time. A mobile robot would generally send commands and get measurements at a much higher frequency. For example, the robot sends commands at 10 Hz and travels at 1 m/s. Please perform an experiment that moves the robot the same 2m forward but in .1 m increments. Please upload a plot of the particles' [x, y] component after 2 m.

In this question, please use the same reduced noise parameters: motion_noise_covariance=np.diag([1e-2, np.deg2rad(.5)**2]) as in the previous experiment.

Experiment 6

Even though all we did is changed how often we sent a command to the robot, the sample set is much more spread out. You might think that the uncertainty in the robot's position is independent of things like control frequency, but this plot should show you that they aren't. When we adjust the control frequency, we also have to adjust the model parameters.

Let's set motion_noise_covariance=np.diag([1e-3, np.deg2rad(.5)**2]). In your submitted pdf, plaese include two plots of the [x, y] component of the 1000 particles at the first two seconds. The particle set should be much more compact again, and track the motion you commanded.

Experiment 7

What happens when our motion model has worse rotation noise than translation noise? Let's set motion_noise_covariance=np.diag([1e-3, np.deg2rad(30)**2]). Please generate plots of the [x, y] component of the samples after 1m, 2m and 3m of motion made in 1m increments. You should see the samples are tightly contained, but are in a curved shape. This shape is a result of the non-linear relationship between orientation and position, and is one of the reasons we often use sampling for localization, rather than a parametric form such as a Gaussian.

Experiment 8

We still have the relationship between the noise parameters and the control frequency. Let's motion_noise_covariance=np.diag([1e-3, np.deg2rad(3)**2]). Please generate a plot of the sample distribution after 3m of motion, but after 30 motions of .1m.

2.2) Localization: Computing the Importance Weights§

Let us now add in a measurement model. Let us assume that we have a set of landmarks given by grid_of_landmarks() (i.e., the default set of landmarks of a Simulator). The robot is equipped with a sensor that can measure the range and relative bearing [r, b] from the robot's location to each of these landmarks. If the robot's pose is (x, y, \theta) and the landmark position is (l_x, l_y), then range is just the Euclidean distance

, and the bearing is the angle to the landmark in the robot's body frame (hence relative bearing), Each of these measurements is corrupted with noise [q_r, q_b] distributed according to q_r \sim \mathcal{N}(0, \sigma^2_r) and q_b \sim \mathcal{N}(0, \sigma^2_b).

Please take a look at our implementation in simulator.simulate_sensing and understand how the values are simulated.

Now the problem is that for a real robot, even if you know this is the model that generates the measurements, you can't know the noise that nature applied. (That would be omniscience!) What we can do is work out how likely each measurement is. More precisely, given a set of measurements, we can use this model to compute the importance weights of a set of samples of the robot pose. See Lecture 13 Page 13 for details.

In particular, to compute the importance weight of a particle:

• Compute the predicted measurements from the current state of the robot's pose and the landmark locations.
• Compute the errors \mathit{err} between the predicted and true measurements.
• Compute the likelihood of the \mathit{err} as the unnormalized importance weights. The likelihood of the error is the evaluation of the probability density function (PDF) of the two-dimensional Gaussian distribution with covariance sensor_noise_covariance at \mathit{err}.
• Finally, normalize the weights so that they sum to one.

Please implement a function that takes a set of samples and measurements and returns the importance weights of the samples.

There are a few technicalities during this process:

• We highly recommend you normalize all the rotational values (in radians) to the interval [-\pi, \pi] (see function normalize_angles). Notably, the rotational component of the \mathit{err}, which we denote as \mathit{err}_\theta, is a rotational value in radians. Therefore, theoretically, the likelihood of \mathit{err}_\theta should have been the evaluation of the PDF of a wrapped Gaussian distribution. In other words, to evaluate the likelihood of \mathit{err}_\theta, we should have taken the sum of the Gaussian PDFs at the points \mathit{err}_\theta + 2k\pi for all k \in \mathbb{Z}. Nonetheless, since our sensor noise is relatively small, we expect only the single point at k=0 \mathit{err}_\theta \in [-\pi, \pi] to have a relatively large value, and all other points to have close-to-zero values. Therefore, we may use the Gaussian distribution as a good approximation to the true wrapped Gaussian distribution.
• The likelihood for the particles can become really small. For numerical stability, you will want to compute the log-likelihood instead of the likelihood. To that end, you may:
  • Use the utility multivariate_normal_logpdf that we provided you to evaluate the log PDF.
  • Take the sum (instead of multiplications) of the log-likelihoods to compute the unnormalized log-weight of each particle.
  • Normalize the log-weights. You should use scipy.special.logsumexp to evaluate the log-sum of the weights. In short, The logsumexp function calculates the following expression in a more numerically stable way:
    \texttt{logsumexp}(x_1, ... x_n) = \log \left( \sum_{i=1}^{n} \exp(x_i) \right).
    See this blog for a more detailed description of this function.
  • Use numpy.exp to undo the logarithm from the log-weights and return the weights.

For reference, our solution is 29 line(s) of code.

2.3) Localization: Particle Filter Update§

What we want you to do is to write a method for step 4: update that takes in a set of samples, the most recent command, and the most recent measurements, and then:

• Use the motion_model method to update the sample based on the command
• Use compute_weights to compute the sample weights from the measurement,
• Resample a new set of samples,
• Blur the samples by applying noise drawn from a zero-mean Gaussian with covariance equal to np.diag([1e-3, 1e-3, np.deg2rad(1)**2]). (You'll need to recall from lecture why we do this blurring --- if you have machine learning background, it's a form of regularization, but you don't need to know about regularization to understand why this is important.)

For reference, our solution is 16 line(s) of code.

In addition to all the utilities defined at the top of the Colab notebook, the following functions are available in this question environment: compute_weights, motion_model. You may not need to use all of them.

Experiment 9

Let's visualize our particle filter.

Please generate a trajectory plot and an animation of the robot carrying out the localization inference. In particular:

• Use the default Simulator parameters (i.e. simply use Simulator())
• Run localization with num_particles=100.
• Plot (and also animate) the obtained results.

If everything is working, your estimated and ground truth trajectories should match up closely, but not exactly. (You should be able to inspect the estimated and ground truth trajectories manually, and see that they differ by some small errors at each step.)

Here are some handy functions for your experiments:

• run_inference: It takes in a simulator instance and runs inference in a particular mode --- for this question, you should pass in the "localization" mode. It returns a SimulationResult instance.
• plot_simulation_result: It visualizes a SimulationResult.
• render_animation: It creates an animated visualization of a SimulationResult. If you are using Colab, you may:
  • Render the animation on Colab, and download it from the Colab GUI.
  • Save the animation to the Colab's local directory (using the save_file argument), and then download the file to your computer.

3) Particle Filter SLAM §

Now, what if we don’t know the landmark positions in advance?
If the robot knew its locations exactly, it could use its observations of the landmarks to build a map of the landmarks' locations in absolute coordinates. But the robot is as lost as it was before --- what can we do? This problem is known as simultaneous localization and mapping (SLAM) --- we need to estimate both the robot’s pose and the map (i.e., the positions of the landmarks).

In this section, we will use the same algorithm from last section, the particle filter, to approach the SLAM problem. Recall each particle in the localization problem only contains a robos pose. In the SLAM case, each particle will now contain a complete "possible world" --- made up of a robot pose and a location of each landmark that is known by to the robot so far.

3.1) SLAM: Update by Motion Model§

Please implement a function that takes in a control command [\Delta p_t, \Delta \theta_t] and updates the particles by drawing from the posterior X_{t+1} using the motion model.

This function behaves the same as Localization.motion_model for the pose components of the particles, and simply keeps the landmarks components (SLAMParticles.landmarks_id_to_idx and SLAMParticles.landmarks_loc) of the particles the same.

Hint

Please take a look at the fields of the SLAMParticles class. Note that landmarks_id_to_idx stores all the landmarks the robot has encountered so far. It is a mapping from a landmark identifier (a string) to an index (an integer). You can use the integer index to index into the landmarks_loc array. In particular, landmarks_loc is an array of shape (num_particles, len(landmarks_id_to_idx), 2). Therefore, to access the coordinate of the landmark with the identifier "Stata" contained in the i-th particle:

# assert isinstance(particles, SLAMParticles)
loc = particles.landmarks_loc[i, particles.landmarks_id_to_idx["Stata"]]

For reference, our solution is 12 line(s) of code.

3.2) SLAM: Tracking New Landmarks§

Since our robot might have limited sensing range (see the parameter Simulator.max_sensing_range), the robot is not aware of all the possible landmarks out there in the world. Therefore, we have to match newly observed landmarks to ones we have seen before, and add landmarks that we are seeing for the first time to our map.
In some situations, the matching is difficult, but for our purposes, we’ll assume that the landmarks are uniquely identifiable, so we know exactly which ones we are observing on each step.

One way to handle missing data due to sensing range is to explicitly model the sensing range in our probabilistic model of the sensors. Howewer, this approach would induce a difficult inference problem.
Instead, we will take the approach of treating the missingness as uninformative. In particular, we will consider sensor data within-range as observed variable, and sensor data out-of-range as unobserved variables. When we see a new landmark for the first time, we will assume that our belief about the location of that landmark has a mean equal to that first observed location and a standard variance (new_landmarks_init_covariance). Note, though, that in practice it might be more common to wait until we have gotten a few observations and use their mean as the mean of the distribution for nitializing our particles.

Please implement a function that takes a set of measurements and updates the particles to include potentially new landmarks that are not yet tracked by the particles.

Below we give a description for this procedure:

• Filter the input measurements to contain only measurements to the new (untracked) landmarks.
• For each new landmark measurement and each particle:
  • Find the predicted location using the robot's pose of that particle and the landmark's measurement.
  • Add a Gaussian noise of covariance self.new_landmarks_init_covariance to this predicted location.
  • This noisy location becomes the landmark's location in the current particle.
• Update self.particles.landmarks_id_to_idx to maintain the invariance that it holds a mapping from each tracked unique landmark indentifier to the index of that landmark in self.particles.landmarks_loc.

For reference, our solution is 31 line(s) of code.

3.3) SLAM: Computing the Importance Weights§

Please implement the SLAM.compute_weights function that takes a sequence of measurements and returns the importance weights of the particles.

This function is similar to Localization.compute_weights, except that it takes into account the landmarks location tracked in the particles.

For reference, our solution is 30 line(s) of code.

3.4) SLAM: Particle Filter Update§

Please implement the update step of SLAM.

The procedure is similar to Localization.update, except that it does an additional add_new_landmarks step after forward updating with the motion_model and before compute_weights.

For reference, our solution is 20 line(s) of code.

In addition to all the utilities defined at the top of the Colab notebook, the following functions are available in this question environment: add_new_landmarks, compute_weights, motion_model. You may not need to use all of them.

Experiment 10

Let's visualize our SLAM particle filter.

Please generate an animation of the robot carrying out inference in the default Simulator.

In particular:

• Run SLAM in the default Simulator with num_particles=1000 and max_sensing_range=6.
• Visualize the inference using render_animation.

Experiment 11

Let's further visualize our SLAM particle filter for more settings.

Please generate plots of the robot carrying out inference in the default Simulator.

In particular:

• Run SLAM in the default Simulator, for all combinations of these parameters:
  • num_particles in [100, 1000]
  • max_sensing_range in [6, np.inf]
• Use plot_simulation_result to plot the obtained estimated trajectory and landmarks for each setting.

4) Rao-Blackwellization §

With our particle filter for SLAM, as the number of landmarks grows, we need exponentially more particles to be able to retain enough variability in our estimate of the location of each one.
Ultimately, that make the pure particle-based SLAM method impractical. Now, we’ll explore an alternative approach that combines particle filtering with Kalman filtering, which is called "Rao-Blackwellization". The name is fancy but the idea is not so hard. The idea is:

• We use particles to represent our uncertainty about the robot’s pose because, as we saw in the first part, the pose is highly non-linear and it doesn't seem like a Gaussian would do a good job.
• But, for each robot-pose hypothesis (in the particles) we will represent our uncertainty about the landmark locations "analytically" using the mean and covariance parameters of a 2D Gaussian.

4.1) Einsum Warmup (Optional)§

This question intends to get you familiar with using numpy.einsum. This question is optional and you do not have to use einsum in your implementation. However, using einsum can greatly simplify your implementation.

In particular, the Rao-blackwellized particle filter performs quite a few matrix multiplications. The function numpy.einsum allows one to express batched multiplication of multiple matrices succinctly.

If you are not familar with np.einsum, here are some guides online:

• StackOverflow Post
• Blog: einsum is all you need

For this warmup question, please fill in the indexing expression string for einsum to implement a "sandwich matrix product". A sandwich matrix product of two matrices U and S is M = U \cdot S \cdot U^\top.

For reference, our solution is 2 line(s) of code.

4.2) RBSLAM: Update by Motion Model§

Please implement a function that takes in a control command [\Delta p_t, \Delta \theta_t] and updates the particles by drawing from the posterior X_{t+1} using the motion model.

This function behaves the same as Localization.motion_model for the pose components of the particles, and simply keeps the landmarks components (landmarks_id_to_idx, landmarks_loc and landmarks_cov) of the particles the same.

For reference, our solution is 9 line(s) of code.

4.3) RBSLAM: Tracking New Landmarks§

Since our robot might have limited sensing range (see the parameter Simulator.max_sensing_range), the robot is not aware of all the possible landmarks out there in the world. Therefore, we must track new landmarks that appears in the measurements in our particles.

Please implement add_new_landmarks that takes a sequence of measurements and updates the particles to include potentially new landmarks that are not yet tracked by the particles.

When you encounter a new landmark, instead of generating a set of particles to represent a distribution, you should store that landmark into each particle. For each particle, the new landmark location's distribution

• is centered the observed location of the landmark (relative to the particle's robot pose).
• has a fixed initial covariance of self.new_landmarks_init_covariance.

For reference, our solution is 31 line(s) of code.

4.4) RBSLAM: Extended Kalman Filter Update§

Please implement update_EKF that takes a sequence of measurements and performs an extended Kalman filter update on each landmark of the particles.

One of the magical properties of Rao-Blackwellization for SLAM is that we can assume the landmarks are all independent of each other, given the pose. Since our particles are already conditioning the landmarks on the filtered pose, we can run a separate estimator for each landmark. This has very nice consequences for computational complexity --- our complexity grows linearly with the number of landmarks (a 2x2 EKF for each landmark), as opposed to quadratically for a single EKF over all landmarks (requiring a 2nx2n EKF).

Suppose our robot's pose is \texttt{Pose}(x, y, \theta). The state that we would like to track by our filter is the landmark location [\hat{l_x}, \hat{l_y}]. Recall that our observation model (without noise) is

This model is non-linear, and the standard Kalman filter does not apply. This is where an extended Kalman filter could be useful.

In our case, an EKF update is the same as a Kalman filter update. The idea of EKF is to approximate \mathit{Obs} by its Jacobian around the current belief of location of the landmark, [\hat{l_x}, \hat{l_y}] , so that we get a linear approximation:

, where r = \sqrt{(\hat{l_x} - x)^2 + (\hat{l_y} - y)^2}. Therefore, the implementation of an EKF for our application is the same as a KF where we use the matrix H above for the observation model for each landmark.

For reference, our solution is 46 line(s) of code.

4.5) RBSLAM: Computing the Importance Weights§

Please implement a function that takes a set of particles and measurements and returns the importance weights of the particles. As in the pure particle filter approach implementation, the weight is just the likelihood of the measurement given the sample, but remember that our sample is now a Gaussian for each landmark (the EKF). So the likelihood of each measurement is going to be given by the product of the Gaussian of the landmark and the sensor model.

For reference, our solution is 56 line(s) of code.

4.6) RBSLAM: Particle Filter Update§

Please implement the update step of RB SLAM.

The procedure is similar to SLAM.update, except it does an additional update_EKF step after add_new_landmarks and before compute_weights.

Further, since by using EKF we approximated the non-linear observation model by a linear model, the approximation can be quite bad when the robot is very close to a landmark. To fix, we will simply ignore any landmark measurement that is within min_sensing_range! Don't panic! If everything is implemented correctly, you will see that even when we ignore information of nearby landmarks, we still get a much better result than using a plain particle filter for SLAM.

For reference, our solution is 27 line(s) of code.

In addition to all the utilities defined at the top of the Colab notebook, the following functions are available in this question environment: add_new_landmarks, compute_weights, motion_model, update_EKF. You may not need to use all of them.

Experiment 12

Let's visualize our Rao-blackwellized particle filter.

Please generate an animation of the robot carrying out inference in the default Simulator.

In particular:

• Run RBSLAM in the default Simulator with num_particles=100 and max_sensing_range=6.
• Visualize the inference using render_animation.

Experiment 13

Let's further visualize our Rao-blackwellized particle filter.

Please generate plots of the robot carrying out inference in the default Simulator.

In particular:

• Repeat the experiments in Experiment 11, but this time with the Rao-blackwellized particle filter.
• Use plot_simulation_result to plot the obtained estimated trajectory and landmarks for each setting. If everything works out, you should see ellipses that visualizes the uncertainty about each landmark's location.
• Compare the plots of this experiment to the plots in experiment 11 side by side. Show that RBSLAM outperforms SLAM in every setting (with the same num_particles and max_sensing_range).

5) Open-ended questions §

In your submitted pdf, please answer the following questions.

1. Draw a factor graph representation of the robots’s belief for the SLAM problem, with four landmarks L_1, L_2, L_3, L_4. You should have variables for the pose of the robot at time t and t+1, and a variable for each of the landmarks. Now, draw the factor graph representing the belief plus a single observation of the landmark L_1 at time t+1 (i.e., add the observation as a node in your graph). Assume that the other landmarks are not observed at time t+1.

2. Suppose that, in addition to the absolute landmark locations (i.e. locations relative to the initial location of the robot), we are also interested in the relative position of two landmarks L_1 and L_2. How would you compute P(\mathit{RelPos}(L_1, L_2)) over two landmarks that you have in your state?

3. Consider a situation in which the robot has substantial uncertainty in its position estimate when it sees two new landmarks for the first time, and adds them to its belief state.
   Is the resulting P(\mathit{RelPos}(L_1, L_2)) a satisfying representation of the robot's belief about the relationship between these two landmarks?
   How might you change your representation of the belief to improve this problem?
   Draw the associated factor graph. What might be the disadvantages of this strategy?