Lab 7: Kalman Filtering

To build the A and B matrices that describe the RC car as a linear system, I first needed to estimate the drag and momentum coefficients of the car by restructuring the equation of motion of the car. The equation of motion is given by m*dv/dt = u - d*v where m is the mass of the car, u is the motor input, d is the drag coefficient and v is the velocity. In a steady state scenario, the velocity of the car is constant and dv/dt = 0. This allows us to rearrange the equation to solve for the drag coefficient as d = u/v. Then, we can use the rise time to 90% of the steady state velocity to solve for the momentum coefficient using the equation m = −dt_0.9 / ln(1 − 0.9).

To collect the velocity data for my car, I used the TOF sensor data as the car moved towards the wall at a constant motor input. The maximum PWM value reached in lab 5 ranged from 150 PWM to 250 PWM. I started testing the step response with a PWM of 160, which I then lowered to 120 PWM to make sure that I was reaching the steady state velocity in the 3 m distance that I had available. The two code snippets below show the helped function I used to collect data and the bluetooth commands for data transmission.

The computed velocity data was very noisy. The TOF sensor update rate resulted in several data points of the same TOF measurements which would yield a velocity of zero for that time step. To clean up the data, I calculated the velocities for each distance step rather than individual TOF reading. Hence, duplicate TOF values were removed to avoid 0 velocity data points.

From the velocity graph analysis, I calculate the steady state velocity to be -2370.4309920870646 mm/s, the 90% velocity to be -2133.3878928783583 mm/s and the time to reach 90% of the steady state velocity to be 1.6552681704260717 seconds. Using these values, I calculated the coefficients to be d = 2.37 and m = 0.303267142088.

$$ \begin{aligned} d &= \frac{1\,\text{N}}{2.37\,\text{m/s}} = 0.422 \\ m &= 0.3033 \\ \\ \frac{d}{m} &= \frac{0.422}{0.3033} = 1.3915 \\ -\frac{d}{m} &= -1.3915 \\ \frac{1}{m} &= \frac{1}{0.3033} = 3.2974 \end{aligned} $$

The A matrix, descrbing the dynamics of the system and the B matrix, describing the effect of the motor control input on the system were calculated using the coefficients derived in the previous section.

$$ A = \begin{bmatrix} 0 & 1 \\ 0 & -\dfrac{d}{m} \end{bmatrix} = \begin{bmatrix} 0 & 1 \\ 0 & -\dfrac{0.422}{0.303267142088} \end{bmatrix} \approx \begin{bmatrix} 0 & 1 \\ 0 & -1.3915 \end{bmatrix} \qquad B = \begin{bmatrix} 0 \\ \dfrac{1}{m} \end{bmatrix} = \begin{bmatrix} 0 \\ \dfrac{1}{0.303267142088} \end{bmatrix} \approx \begin{bmatrix} 0 \\ 3.2974 \end{bmatrix} $$

The C matrix will be 2x1 because we have two state variables - position and velocity. The TOF measurement only directly relates to the position variable therefore the C matrix will be in the form C = [ -1, 0] . The top entry is -1 because we are measuring negative distances to the wall and the bottom entry is 0 because the TOF does not provide any direct information on the RC velocity.

From the earlier velocity graphs it was clear that the TOF sensors are significantly more trust worthy than the velocity data. To get an initial estimate for the the variance of TOF data I reused the data collected in lab 3 that captured samples of TOF readings at specific distances. I used two of my data sets one for 26 cm distance and one for 130 cm distance which yielded variances of 1.0057 and 23.053. From lab 3 I observed the accuracy of the sensor readings decreased significantly with distance, therefore I went with the higher variance value as a starting point and rounded it to 25. For the process noise, I used the equations provided in class and a TOF sampling rate of 50 ms. My calculations yielded a process noise covariance of 44 which I increased to 70 for initial testing to give more weight to the measurements.

I used the code provided in the lab manual in combination with my data from a 3m lab 5 run to simulate the Kalman filter in Python. The plot above shows the true position of the robot and the Kalman filter's estimate of the position. // add more

I used the Arduino basic linear algebra library to initialize and work with the matrices in the kalman filter. The A, B, C, Q and R matrices were initialized as desribed in earlier sections. The state vector was initialized to be a zero vector because the velocity of the car should be zero at the strat and the initial position would be soon updated to the first TOF measurement.

The code below shows the implementation of the Kalman filter on the Artemis. The kalman function takes in the current state estimate, the control input u, and the new mesurement supplied by the TOF sensors. The function preforms a prediction step which estimates the new location of the robot using the the motor inputs and then an update step which incorporates the new measurement. The function then returns the updated state estimate and the updated error covariance matrix.

To integrate the Kalman filter into the PID controller, I replaced the measurement estimated derived from the TOF sensors with the estimate from the Kalman filter. The error is then calculated as the difference between the target angle and the Kalman filter's postional estimate. The rest of the PID control code remains unchanged. In the first run of the Kalman filter, the initial mean value for the position is updated to to the first TOF measurement. The integration snippet is shown below.

My initial testing showed that the simulation results did not translate well to the real robot. The graph below shows the comparison of the first run position estimates of the Kalman filter and the true position of the robot. Following this, I decreased my process noise covariance value to 40 and my measurement noise to 23.

The second run ( graphed below) of testing showed significant improvement in the Kalman filter's estimates but displayed a large amount of disaprity between the true position and the Kalman filter's estimate at the start of the run. I realized that I was not updating the initial position mu to the first TOF measurement which I thought was causing the large initial error.

Even after fizing the position mu initialization the Kalman filter's intitial position estimates are significantly lower than the true position. However, the filter converges to the true position quickly, so the PID is able to perform as intented. The disparity in the initial estimates likely is still caused by the initial zero velocity/position assumption which takes some time to be corrected.