Lab 7: Kalman Filter
The purpose of this lab was to implement a Kalman filter to estimate the position and velocity of my car while driving. This can help speed up the PID controller rate since the filter can predict a position at each iteration while updating its model each time a new sensor input is available.
Estimating Drag and Momentum
The first step to implement the filter is estimating the drag and momentum of my car. To do this, I needed to drive my car at a constant speed towards a wall while recording ToF values and timestamps. I then sent this data to my computer, to plot. The goal was to first estimate the steady-state speed (terminal velocity). I chose a motor PWM value of 40000, which is around 60% of the maximum speed. Below is a plot of the ToF readings I recorded.
Using the readings and timestamps I was able to calculate the velocity at each point. I noticed that my first velocity plot was very noisy, so I low-passed the values and then got the resulting plot below.
The velocities seemed to level out slightly at the very end at a value of -1.91 m/s. Using this a steady-state speed of 1.91 m/s, I estimated the car’s drag by d = 1 / steady state speed, and momentum by m = (-d * t) / ln(0.1), where t is the 90% rise time for the steady-state speed. This gave me d = 0.523 and m = 0.212.
Initializing The Filter
The Kalman Filter function is described in the image below. The first step predicts the state and the next step updates the model using the actual data. The input mu is the filter state, sigma is the filter’s uncertainty in the state, u is the force/input to the system (in this case the motor PWM value), and y is the actual observation (in this case the ToF data). So at each step the filter returns the estimated state and the state uncertainty.
For this application the state space is 2-dimensional, including the car’s distance from the wall and velocity. The A and B matrices come from the state-space equation below, and use the d and m values I calculated earlier.
I then had to discretize the A and B matrices relative to the sampling time. After checking the ToF timestamps I found that the sampling time was roughly 45 ms, allowing me to create the Ad and Bd matrices from A and B below.
Since the ToF values are positive I decided to measure distance away from the wall as positive, so my C matrix was {1,0}. In the filter steps image above, the sigma-u and sigma-z variables are the process noise and sensor noise covariance matrices, respectively. The relative values determine how much the filter trusts its model vs the actual data. The matrices are constructed in the form below, For sigma_1 and sigma_2 I used values of 10 to indicate that the filter’s model has an uncertainty of around 10 mm, and for sigma_3 I used 20 mm. This means that I believe the ToF sensor has more noise than the filter’s model.
The following function implements the Kalman filter in Python. I added an extra input to the function to indicate whether to just predict the state or predict and update. While testing I found that in the deadband the ToF readings are constant as the car is not moving, but since u is still > 0, the filter believes that the car is still moving since there is a force being applied. So, in the case of just predicting, I set the motor input u to 0 if the input is within my motor’s deadband. The input u is scaled to the step_size I used to determine the steady-state speed, which is 40000. So, if u = 0.2, then the input is 8000 unscaled. Anything less than this is within the deadband.
Implement and Test
To test the filter, I used old data from Lab 5, where I used PID control to drive my car straight at a wall and stop one foot away. I used the ToF readings, motor input values, and timestamps recorded from that lab. I used 100 measurements of each. I used the following code to loop over the data, and at each iteration use the filter to predict the current state given the previous state estimation, the current motor input value, and the current ToF reading. I stored the predicted distance from the wall in an array. For the initial state I used the first ToF reading and a value of 0 for the velocity. I did my calculations using m/s, so each ToF value is converted from mm to m. Also, I scale the motor input by 40000 since that was the step size I used to estimate drag and momentum. Finally, since I treat positive x as the distance away from the wall, all of the motor values needed to be negative since the motor input is driving towards the wall.
Below is a plot of the actual ToF values against the Kalman Filter’s predicted ToF values. The filter follows the ToF curve closely but has some deviations. This is because when initializing the filter I used more noise for the ToF sensor compared to the filter’s model, so the filter is trusting its model slightly more than the ToF reading.
The covariance matrices largely determine the behavior of the filter. For example, if I increase the believed ToF noise (sigma_3) from 20 mm to 100 mm, then the filter ignores the ToF data more and deviates from the ToF curve a lot more:
If I reduce sigma_3 back to 20 mm, but increase sigma_1 and sigma_2 to 100 mm, the filter prediction is almost exactly the same as the ToF data, because the filter believes its own model has a lot more noise and is thus less reliable than ToF data:
The next test I did was to run the filter using only the prediction step at each iteration, never updating the model using the actual ToF data. This is helpful for running PID faster than ToF data is produced on the robot. The following plot is the resulting filter predictions based on the motor inputs and filter model alone. The curve has the correct shape but is considerably off from the ToF readings. The maximum error is around 0.5 m, arond 1.7 ft. ToF data is still needed for accuracy, but the car could still drive forward and stop safely before the wall using no ToF data at all and just the Kalman filter, which is very impressive and much better than linear extrapolation.
If ToF data is provided every 10 iterations (the filter does predict and update every 10 iterations), the accuracy is already much better:
