Lab 7

Kalman Filter

Task 1: Estimate drag and momentum

I set the distance sensor to long mode so I can place the robot as far as possible from the wall and receive reliable distance readings from the TOF sensor. My step response value is 125, which is similar to the PWM values used in Lab 5. I placed my robot 3.3 meters from the wall and stopped it when it was 150 mm from the wall. Because my robot wasn’t able to achieve a steady state velocity with the space given, I used an exponential fit to extrapolate the velocity.

The steady state speed is 2.004 m/s, the 90% rise time is 7.097 seconds, and the speed at 90% rise time is 1.803 m/s.

After sending the data back to my laptop, I saved the data in a CSV file.

Task 2: Initialize KF (Python)

The state space equation for the Kalman filter is:

To calculate d, I set u = 1 and x_dot as the steady state speed.

To calculate m, I used the following equation:

So,

There were 101 datapoints in 10 seconds, so the sampling time is 0.09901 seconds (99.01 ms).

Discretize matrices A and B:

I am measuring the distance from the wall as positive. x is the state vector. distance[0] should be positive because I am treating all distances from the wall as positive.

We need to specify the process noise and sensor noise covariance matrices for the Kalman filter to work well. To calculate sigma_1 and sigma_2, we use the sampling time in seconds.

sigma_3 represents the measurement noise. Since we are using long range mode, the measurement noise according to the TOF sensor data sheet is 25 mm.

Task 3: Implement and test your Kalman Filter in Jupyter (Python)

Using the function provided, I applied the Kalman filter to the data collected at the beginning of this lab.

This initial Kalman filter lags behind the actual signal, which means the filter is trusting the model too much. I can tune the filter by increasing the process noise. I increased sigma_1 and sigma_2 to 200.

Another parameter that affects the performance of the Kalman filter is the value of the measurement noise. If the value is too large, the filter will think the measurements are noisy and unreliable and will rely more on the model.

Task 4: Implement the Kalman Filter on the Robot

I integrated the Kalman filter into my Lab 5 PID implementation. I placed the robot 3 feet away from the wall and had it stop 1 foot from the wall. The filter smooths out the noisy distance readings while matching the shape of the raw data. The robot was quick to start and stop at an accurate distance from the wall. This Kalman filter implementation makes the robot much quicker compared to just only using PID in Lab 5. The final values I used for this run is Kp = 0.06, Ki = 0.25, Kd = 0.075, and sigma_1 = sigma_2 = 20. I reduced the process noise so the Kalman filter predicts the distances rather than matching exactly to the raw distance data.

Collaborators

I collaborated with Sam Zhen while working on this lab.