Objective
The goal of this lab is to implement a Kalman Filter, which will interpolate data in between ToF readings faster than the extrapolation in Lab 5.
Lab 7
Lab Tasks
Estimate drag and momentum
From lab 5, the maximum u(t) I used was approximately pwm=140. For this section I set pwm = 80. I used the ToF sensor's short mode; to allow speed to reach steady state while preventing the robot from running into the wall I set pwm lower.
I applied the step input to my robot and measured the steady state speed, 90% rise time, and speed at 90% rise time.
Fitted parameters: a=-36.87422499737086, b=-1.0135561712646444, c=45.53425693676829, y=a*exp(bx)+c
I found that the 90% rise time is ~2.27s, and the speed at this time is 41.8in/s
Initialize KF
I first created my state space model by calculating the damping and mass from the data collected in the previous step. To discretize my matrices, I multiplied them by the sampling period.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| b = 0.0133 | |
| m = 0.0131 | |
| A = np.array([[0,1],[0,-b/m]]) | |
| B = np.array([[0],[1/m]]) | |
| Ad = np.eye(2)+sampling_period*A | |
| Bd = sampling_period*B | |
| C = np.array([[1, 0]]) |
Next, I set my process noise and measurement noise matrices. For process noise, I used the equations from the lecture slides, substituting in a sampling time of .02s since the timing budget of my ToF sensor is 20ms. For measurement noise, I referenced the ToF datasheet, which states that the short-range measurement mode has a ranging error of 20mm. With these parameters, sigma_1 = sigma_2 = 70.3
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #process noise | |
| sig_u=np.array([[(10**2)*freq,0],[0,(10**2)*freq]]) | |
| #measurement noise | |
| sigma_3 = 20/25.4 #convert mm to in | |
| sig_z=np.array([[sigma_3**2]]) |
Implement KF in Python
To implement the KF in Python, I modified the function provided in lecture to be able to test different process noise and measurement noise matrices.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| def kf(mu,sigma,u,y,sig_u,sig_z): | |
| """ | |
| mu: system state | |
| sigma: process noise | |
| u: control input | |
| y: measurement | |
| """ | |
| mu_p = Ad.dot(mu) + Bd.dot(u) | |
| sigma_p = Ad.dot(sigma.dot(Ad.transpose())) + sig_u | |
| # print(sigma_p) | |
| sigma_m = C.dot(sigma_p.dot(C.transpose())) + sig_z | |
| # print(sigma_m) | |
| 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) | |
| # print(mu, sigma) | |
| return mu,sigma |
Next, I tested the KF on a dataset I collected in Lab 5. For this dataset, I was sampling at 50Hz because I set the ToF timing budget to 20ms. Eventually, I want to collect distance and orientation data at roughly the same rate. I set a "desired frequency" of 180Hz, since that is the IMU sampling rate with DMP enabled (see Lab 6). I wrote a for-loop that would simulate sampling at 180Hz to test if my KF could successfully interpolate distances in between the 50Hz ToF measurements.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| fast_freq = 180/1000 #mHz | |
| fast_period = 1/fast_freq | |
| time_elapsed = runtime[-1]-runtime[0] | |
| N = int(time_elapsed*desired_freq) | |
| currtime = 0 | |
| fast_time = [currtime+i/fast_freq for i in range(N)] | |
| x = np.array([[tof[0]],[0]]) | |
| sig = np.array([[sigma_3,0],[0,0.1]]) | |
| filtered = [] | |
| filtered.append(tof[0]) | |
| i = 0 | |
| for counter in range(len(fast_time)-1): | |
| if runtime[i+1]>fast_time[counter]: | |
| x,sig = kf(x,sig,-pwm[i],filtered[counter], sig_u, sig_z) | |
| filtered.append(x[0][0]) | |
| else: | |
| i+=1 | |
| x,sig = kf(x,sig,-pwm[i],tof[i], sig_u, sig_z) | |
| filtered.append(x[0][0]) |
To my surprise, the KF interpolated in between measurements surprisingly well.
I then tested different process noise and measurement noise matrices to see how they would affect the KF.
From the plots, we can see that increasing process noise causes the KF to rely more on measurements, whereas increasing measurement noise causes the KF to rely more on the model.
Implement KF in Arduino
[UPDATE 4/12] Unfortunately my Kalman filter doesn't work yet. Below is the latest code snippet:
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| void kf() { | |
| // Serial.println("in kf()"); | |
| // Serial.print("old mu: "); | |
| // Serial.println(mu); | |
| // Serial.print("mu(0,0): "); | |
| // Serial.println(mu(0,0)); | |
| // Serial.print("mu(1,0): "); | |
| // Serial.println(mu(1,0)); | |
| BLA::Matrix<2, 1> mu_p = { Ad(0,0) * mu(0,0) + Ad(0,1) * mu(1,0) + Bd(0,0) * control, | |
| Ad(1,0) * mu(0,0) + Ad(1,1) * mu(1,0) + Bd(1,0) * control}; | |
| Serial.print("predicted state: "); | |
| Serial.println(mu_p); | |
| BLA::Matrix<2, 2> sigma_p = { Ad * sigma * ~Ad + sig_u }; | |
| BLA::Matrix<1> sigma_m = { C * sigma_p * ~C + sig_z }; | |
| BLA::Matrix<2,1> kkf_gain = sigma_p * ~C * Inverse(sigma_m); | |
| // Serial.print("kkf_gain: "); | |
| Serial.println(kkf_gain); | |
| BLA::Matrix<1> y_m = measured_dist - C * mu_p; | |
| // Serial.print("y_m: "); | |
| // Serial.println(y_m); | |
| mu = mu_p + kkf_gain * y_m; //update mu | |
| // Serial.print("new mu: "); | |
| // Serial.println(mu); | |
| sigma = (I2 - kkf_gain * C) * sigma_p; | |
| } |
I also neglected to take screenshots of my python plots. From print statements and my plots, I found that I am definitely calculating velocity wrong (I was getting very large negative numbers on the order of 10^3). At some point yesterday, my KF was able could follow sensor measurements quite well, but then would grossly undershoot the interpolated value. After making some edits to my velocity function and changing a few lines in my KF function, I got rid of the weird interpolated values but my sampling rate reduced to 30Hz, which is even worse than the raw ToF sampling rate. To stay on track I'll probably need to neglect my KF for the next week but I hope to fix it soon.