Lab 7:

Estimate Drag and Momentum

The first step of this lab was to estimtae drag and momentum of the system. I was initially unsure of how to proceed with doing this given that we could not easily measure the drag of the robot. After speaking with Dr. Jaramillo, I realized that this is done because we need a way to esimate the system dynamics of the system for the state space despite not being able to measure/quantify certain values. Thus, creating “virtual” versions of these values that use “pseudo” estimations of these parameters, the state space models can be made using relevant parameters.

To accomplish this, I picked a step response of 100% of the maximum u. I then used a for loop for a long enough state space (played around with it to make sure the car didn’t run into the wall). Here are graphs with the data for time steps (note the slight variations in the data are from when there was a slight obstruction that came in the way)

I took the part of the data that had more accurate steady state values and got the following results (the data is negative because I placed the TOF sensor on the opposite side, but the results should still be the same just inverted; i.e. I did it so that in between two obstructions, I measured TOF data going away from one wall instead of toward the other).

Graphs shifted to first time being 0 seconds and units being corrected:

To compute the velocity, I simply computed the slope between corresponding TOF sensor readings (v = d/t) and plotted those values. The motor input stays 100% the entire time (because there was no PID control here). Note: I referenced other students outputs to check if my values seemed reasonable when I was doing runs!

The steady state speed is around 1500 mm/s, and the rise time is around 0.4 seconds with a rise time velocity of about 2000 m/s.

So d is: uss/v = 1 / 2000 = 0.005

And m is: -dt/ln(0.1) = -0.0050.4/ln(0.1) = 0.00086858896

I saved the relevant data to a csv file for future use!

Initialize KF

I next computed the A and B matrices using the d and m values calculated above:

I then discretized the state space with dimension of the state space being 2 (second-order system) and the sampling time being 0.1 seconds.

I next initialized the covariances. sig_z defines noise in the measurement (i.e. we estimate this to be around 20 mm). sig_u defines noise in the process (depending on the sampling rate). Because we assume uncorrelated noise for the Kalman filter, the off diagonal terms are 0 sig_u.

For the position this is: sqrt(100 / 0.1) = 31 mm. For the velocity this is: sqrt(100 / 0.1) = 31 mm/s.

After implementing the Kalman Filter Function, I received this graph of the data:

Initially, my Kalman Filter was entirely off because the covariance matrices were too skewed towards the model as opposed to the sensor mesaurement. To fix this, I increased the initial covariance of the model which would lead to distrust in the model and more weightage of the sensor reading. The largest effect came from altering the covariance of the position model. When slightly increasing it, I noticed that there was a more accurate following of the sensor data:

However, if the covariance of the sensor data was increased, there would be less probability that it was accurate and therefore more trust on the model itself. This results in a Kalman Filter that falls slightly behind the sensor readings:

Thus, I was able to tune my covariances to get a fairly accurate Kalman Filter as seen above!