Lab 7: Kalman Filter
Lab Objective
The goal for this lab is to incorporate a Kalman Filter to improve the performance and speed of execution of the task from Lab 5. The Kalman Filter will be used to supplement the slowly sampled ToF values.
Estimate Drag and Momentum
The Kalman Filter operates by first predicting the robot’s position using a state-space model and then refining this forecast with actual sensor data. See the function below:
To identify the A, B, and C matrices necessary for the Kalman Filter, I created a state-space model specific to our robot. This model uses the distance between the robot and the wall (position) and the robot’s velocity. See the details below:
To estimate drag (d) and momentum (m) I used the following two formulas:
For the step response, the car was driven at a constant speed towards a wall to find steady-state velocity, d, and m with a PWM value of 200. The 90% rise time, representing the time it takes for the car to reach 90% of steady-state velocity, was also determined. This approach generated the following data:
The 90% step time required finding the time at which the car reached 2200 mm/s * 0.9 = 1980 mm/s. The 90% rise time is around 1s. So we have:
And my matrices then become the following:
Note: the C matrix was initialized as C = [-1, 0], same as in lecture, because we are measuring negative distance from the wall.
Initialize Kalman Filter
To discretize the matrices, it was necessary to select a specific sampling rate for predicting filter values. I used a sampling rate of 20ms. The formula is shown below:
So my matrices are now the following:
Next, I had to estimate the noise variables, sigma_u and sigma_z:
Implement and Test the Kalman Filter in Jupyter
Having calculated these values, I was able to implement the Kalman Filter using the provided function. I added my matrices into the given code. This implementation and the result are shown below:
The Kalman Filter effectively follows the pattern of the data and accurately predicts the subsequent ToF measurement. A sampling rate of 100ms was used, aligning with the sensor’s sampling frequency.