Lab 7: Kalman Filter

Step 1: Estimating Drag and Momentum

Computing Speed via Finite Difference

I wrote a function called U_const on the Artemis that drove the car at a constant 100 PWM toward a wall while logging ToF distance and timestamps. I placed foam against the wall and used active braking to stop the run before impact. 100 PWM was chosen to stay in the same range used in Lab 5. Speed was computed from the ToF readings using a simple finite difference between samples:

dt = np.diff(t_s)
dd = np.diff(dist)
speed = dd / dt
      

I ran a couple of different step responses and used the speed traces to fit an exponential and pull out the steady-state speed and rise time.

Exponential Fit and Steady-State Extraction

I fit the speed data to the exponential model below, where the steady-state speed v_ss is the constant term c as the curve levels off:

From the first-order system model, at steady state v̇ = 0, which lets d be computed directly from v_ss:

With 100 PWM normalized to u = 1, d came out to 0.29837.

Rise Time and Mass Estimation

To find m, I used the 70% rise time from the step response. I identified where the speed hit 70% of v_ss and plugged that time and speed into the step response formula to solve for m:

The full results from the 100 PWM run are shown in the table below.

Step Response Graphs

The graphs below show the ToF distance, computed speed, and motor input over time for the 100 PWM run.

Step 2 & 3: Kalman Filter Initialization and Implementation (Python)

Using the d and m values from Step 1, I built the A and B matrices and discretized them using the average dt over the step response run. I set C = [1, 0] because the ToF measures positive distance directly and that is the only thing being measured. The state vector was initialized from the first ToF reading with zero initial speed. I started with sigma1 = 0.1 to reflect low trust in the model's position estimate, sigma2 = 100 since speed is never directly measured and has high uncertainty, and sigma3 = 50 for the ToF sensor noise. All of this setup and the Kalman Filter function are in the block below.

A = np.array([[0, 1], [0, -0.8032]])
B = np.array([[0], [2.6919]])
dt = 0.0991578947
C = np.array([1, 0]) #because I measure distance from the wall as a positive and that is the only measurement
x = np.array([[tof[0]], [0]]) #this takes from the PID notification handler from lab 6, is positive bc
n = 2
Ad = np.eye(n) + dt * A
Bd = dt * B
sigma1 = 0.1 #tof sensor variance, lower trust in my sensor
sigma2 = 100 #speed
sigma3 = 50 #sensor noise
sigu = np.array([[sigma1**2, 0], [0, sigma2**2]]) #assume no correlation
sigz = np.array([[sigma3**2]])

# implementing KF based on design from lecture
def KF(mu, sigma, u, y):
    mu_p = A.dot(mu) + B.dot(u)
    sigma_p = A.dot(sigma.dot(A.transpose())) + sigu
    sigma_m = C.dot(sigma_p.dot(C.transpose())) + sigz
    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
      

The sigma values determine how much the filter trusts the model versus the sensor. If sigma3 is too small the filter ends up mostly passing raw ToF data through; if sigma1 and sigma2 are too small it over-trusts the model and drifts from the measurements. I iterated on the values by comparing the filter output to the raw data until the estimate tracked well without just copying the noise. Output graphs for 100 PWM and 70 PWM steps are below.

References

For this lab I worked with Coby Lai on some sections, and referenced Trevor Dale's Lab 7 write-up when structuring the step response data collection and analysis approach. I also used AI assistance to check syntax and help structure portions of the Python code.