Fast Robots @Cornell, Spring 2023

Lab 7: Kalman Filter

Objective

The objective of Lab 7 is to implement a Kalman Filter, which will help you execute the behavior you did in Lab 6 faster. The goal now is to use the Kalman Filter to supplement your slowly sampled ToF values, such that you can speed towards the wall as fast as possible, then either stop 1foot from the wall (if you chose Task A, position control) or turn within 2ft of the wall (if you chose Task B, orientation control). Note that part of the lab 8 grade is based on the speed of your solution.

Parts Required

We will have a few setups in and just outside of the labs with crash-pillows mounted along the wall to limit damages. If you practice at home, be sure to do the same!

Lab Procedure

1. Estimate drag and momentum

To build the state space model for your system, you will need to estimate the drag and momentum terms for your A and B matrices. Here, we will do this using a step response. Drive the car towards a wall while logging motor input values and ToF sensor output.

Choose your step-size to be of similar size to the PWM value you used in Lab 6 (to keep the dynamics similar). If you never specified a PWM value directly, but rather computed one using your PID controller, pick something on the order of the maximum PWM value that was produced.
Make sure your step time is long enough to reach steady state (you likely have to use active breaking of the car to avoid crashing into the wall).
Show graphs for the TOF sensor output, the (computed) speed, and the motor input. Please ensure that the x-axis is in seconds.
Measure both the steady state speed and the 90% rise time.

2. Initialize KF

Compute the A and B matrix given the terms you found above, and discretize your matrices. Be sure to note the sampling time in your write-up.
```
Ad = np.eye(n) + Delta_T * A  //n is the dimension of your state space 
Bd = Delta_t * B
```
Identify your C matrix. Recall that C is a m x n matrix, where n are the dimensions in your state space, and m are the number of states you actually measure.
- This could look like C=np.array([[-1,0]]), because you measure the negative distance from the wall (state 0).
Initialize your state vector, x, e.g. like this: x = np.array([[-TOF[0]],[0]])
For the Kalman Filter to work well, you will need to specify your process noise and sensor noise covariance matrices.
- Try to reason about ballpark numbers for the variance of each state variable and sensor input.
- Recall that their relative values determine how much you trust your model versus your sensor measurements. If the values are set too small, the Kalman Filter will not work, if the values are too big, it will barely respond.
- Recall that the covariance matrices take the approximate following form, depending on the dimension of your system state space and the sensor inputs.
```
sig_u=np.array([[sigma_1**2,0],[0,sigma_2**2]]) //We assume uncorrelated noise, and therefore a diagonal matrix works.
sig_z=np.array([[sigma_3**2]])
```

3. Implement and test your Kalman Filter in Jupyter

To sanity check your parameters, implement your Kalman Filter in Jupyter first. You can do this using the function in the code below (for ease, variable names follow the convention from the lecture slides).
- Import timing, ToF, and PWM data from a straight run towards the wall (you should have this data handy from lab 6).
- You may need to format your data first. For the Kalman Filter to work, you’ll need all input arrays to be of equal length. That means that you might have to interpolate data if for example you have fewer ToF measurements than you have motor input updates. Numpy’s linspace and interp commands can help you accomplish this.
- Loop through all of the data, while calling the Kalman Filter.
- Remember to scale your input from 1 to the actual value of your step size (u/step_size).
- Plot the Kalman Filter output to demonstrate how well your Kalman Filter estimated the system state.
- If your Kalman Filter is off, try adjusting the covariance matrices. Discuss how/why you adjust them.

def kf(mu,sigma,u,y):
    
    mu_p = A.dot(mu) + B.dot(u) 
    sigma_p = A.dot(sigma.dot(A.transpose())) + Sigma_u
    
    sigma_m = C.dot(sigma_p.dot(C.transpose())) + Sigma_z
    kkf_gain = sigma_p.dot(C.transpose().dot(np.linalg.inv(sigma_m)))

    y_m = y-C.dot(mu_p)
    mu = mu_p + kkf_gain.dot(y_m)    
    sigma=(np.eye(2)-kkf_gain.dot(C)).dot(sigma_p)

    return mu,sigma

Note that due to the snow day, we have decided to make edits to the lab from this point forward. If you are low on time, consider doing task 4.A; if you are all caught up, consider doing 4.B instead for up to 4 bonus points.

4.A Extrapolation

Now that you understand how the Kalman Filter works, you could implement this on your robot and use it to speed up sampling of the estimated distance-to-the-wall (Task 4.B). However, getting the Kalman Filter to work in practice takes time. If you are low on time, do extrapolation instead.

Write a function to extrapolate based on recent ToF sensor values, such that you can drive your robot quickly towards the wall and initiate your stunt at just the right time. While your solution in this lab does not have to be fast, part of your lab 8 grade will be based on the speed of your solution relative to your class mates. Be sure to demonstrate that your solution works by uploading videos and by plotting corresponding raw and estimated data in the same graph.

4.B Implement the Kalman Filter on the Robot (Optional, for up to 4 bonus points!)

(Original task)

If you have time, integrate the Kalman Filter into your Lab 6 PID solution on the Artemis. Before trying to increase the speed of your controller, use your debugging script to verify that your Kalman Filter works as expected. Be sure to demonstrate that your solution works by uploading videos and by plotting corresponding raw and estimated data in the same graph.

The following code snippets gives helpful hints on how to do matrix operations on the robot:

#include <BasicLinearAlgebra.h>    //Use this library to work with matrices:
using namespace BLA;               //This allows you to declare a matrix

Matrix<2,1> state = {0,0};         //Declares and initializes a 2x1 matrix 
Matrix<1> u;                       //Basically a float that plays nice with the matrix operators
Matrix<2,2> A = {1, 1,
                 0, 1};            //Declares and initializes a 2x2 matrix
state(1,0) = 1;                    //Writes only location 1 in the 2x1 matrix.
Sigma_p = Ad*Sigma*~Ad + Sigma_u;  //Example of how to compute Sigma_p (~Ad equals Ad transposed) 

5 Speed things up (optional)

If you want to get a head start on Lab 8, try speeding up your robot using your new KF (or extrapolator) to increase the execution time of your control loop.

Additional tasks for 5000-level students

You’re off the hook in this lab.

Write-up

To demonstrate that you’ve successfully completed the lab, please upload a brief lab report (<800 words), with code snippets (not included in the word count), photos, and/or videos documenting that everything worked and what you did to make it happen.