Gyro Calibration: Done.

Ok, so it has taken me eight months, but I finally have the SharkFin gyro calibration figured out. This should have been a simple task, but several technical issues combined with summer fun held up this project.

Recall that the SharkFin is a wireless helmet mounted inertial brake light for bicyclist. MEMS (read tiny) instruments measure accelerations and rotations to determine if the brakes are being applied and light the rear facing LEDs. Since the Bar End Brake Light and Amy's Brake Light were mounted directly to the bike, tracking the forward direction (against which braking is measured) was fairly simple. But sensors mounted on the helmet make it much more difficult, since the rider is constantly looking around. We (Anool and I) added a gyro, which measures rotational rate, to help keep track of the forward direction as the rider moves his head. We had some issues with our first analog board, but were able to substitute the SparkFun Razor 9DOF sensor board for the troubled sensors.

The trouble is that the gyro measurements are affected by temperature (and other things) that are not being measured, which impacts the knowledge of the forward direction enough to either shine the brake lights when no brakes are applied or to not apply the brakes under hard braking: two bad situations.

On a bike, light braking is about 1/20 G (where 1G is the amount of acceleration on the surface of the Earth due to gravity). In order for the SharkFin to work, the calibration needs to be accurate down to about 1/100G or about 10 cm / sec^2. This is a sensitive measurement for a dynamic platform. This magnitude of error would be caused by only 1/2 degree error in the pitch estimate.

Luckily, the onboard accelerometer can be used as an external rate measurement reference. It is not very accurate for a single measurement, but can be very accurate if averaged over a long period of time. This averaging allows us to continually track and update the gyro parameters and, hopefully, to keep the pitch measurement within that 1/2 degree tolerance mentioned above.

And there are other forces that need to be reckoned to meet this requirement. Namely, the centrifugal and tangential forces experienced by the sensors as the head rotates. In an extreme scenario, such as at a Metallica concert, these forces can be as large as a full G. Even in normal cases, they are too large to be ignored. The calibrated gyro measurements can be used to estimate and remove these not-braking accelerations.

Initially, I was optimistic that I could find some existing code that could be adapted to this situation, but no dice.

Details
Initial Gyro Cal
Here, we estimate the gain and bias parameters of the gyro offline. In this case, we assume the gyro parameters are static and estimate them using the accelerometer measurements (which is already well calibrated). The model for the gyro is linear. That means that the conversion of the integer counts c from the analog to digital converter (ADC) into an actual pitch rate is given by

omega = a c + b,

where a and b are the unknown parameters we seek. You may be more familiar with the related model:
omega = (c - d) f,
which is the same when f = a and -df = b. But this form has issues with linear estimation because the df term is quatratic. The actual pitch angle is the integral of the pitch rate. Subscripts are used to denote the measurement number: c0 is the first count, pitch0 is the first pitch and so on.

For a small amount of time dt, where small means that omega is not changing significantly over this time period, we can approximate the change in pitch as

pitch1 - pitch0 = (ac0 + b) dt, or

pitch1 = pitch0 + (ac0 + b) dt.

Continuing on,
pitch2 = pitch1 + (ac1 + b) dt or,
pitch2 = pitch0 + (ac0 + b) dt + (ac1 + b) dt or,
pitch2 = pitch0 + a dt(c0 + c1) + b 2 dt.


You get the idea:
pitchk = pitch0 + a dt (sum(ci) + b dt k.

If N measurements are taken, we have a system of N equations but only three unknowns (a, b, and theta0), in other words a very over determined system of equations. (We treat theta0 as an unknown, since the actual first measurement is corrupted and we don't want these errors to impact our estimate of a and b.) A little linear algebra makes this situation more manageable.

Let:
Pitch be the column vector of all of the pitch measurements taken from the accelerometer,
A be the N x 3 matrix with columns cumsum(C * DT), cumsum(DT), ones,
and x be the 3-vector [a, b, theta0],
C is the vector of gyro ADC counts,
DT is the vector of time deltas (dt s) in practice these may not be all equal,
ones is the vector of all ones.
The product C * DT is taken element wise.
Here cumsum is the cumulative sum of a vector. An example makes this clear:
cumsum([1, 2, 3, 3, 5]) = [1, 3, 6, 9, 14].

With this notation we have the much improved
Pitch = Ax.

This is easily solved using least sqares

x = (A^t A)^(-1) A^t Pitch,

where ^t is the matrix transpose of the preceding matrix.

That is it! I collected about 30 seconds worth of data pivoting the unit back and forth in my hand and got a really nice fit. The upper plot shows the pitch measurement for both the accelerometer and the gyro. The lower plot shows the difference between the two measurements. The astute reader might take issue with the large differences up to 10 degrees. This is because the accelerometer also as errors and I can't actually pivot about the gyro as I rotate the unit back and forth with my hands. I will set up a better calibration setup if this turns out to be too bad.

UPDATE 2010-10-22. Nat Dyck, my roommate from college, made me a fancy lego structure to calibrate the gyro. The device allows the SharkFin to spin with the accelerometer at the center of rotation. See road testing.

Doomsday Gyro Cal
... in which the continual gyro calibration is discussed.
So the gyro parameters a, and b need to be updated on a regular basis. We do this by updating our prior estimate with new information about the gyro parameters. In this case, since we cannot control the rider's head motion, we must accept the measurements that are provided. The rider my look down a lot to check the chain which would provide a great gain measurement a. Or the rider could hold perfectly still and provide a great bias measurement. One thing we to not want to do is to mess up a perfectly good prior gain estimate with new data.

We only get insight into the gain measurement when the rider is moving his head. If the rider is holding still we get a crappy gain measurement. Either way we get a gain measurement. It could be a good one, a bad one, or anything in between.

We want to update the prior gain estimate with this new information. Suppose we did something dumb and just averaged our prior gain estimate with the update.
gain_new = (gain_prior + gain_est) / 2.
If gain_est is bad, we just messed up our prior estimate with junk. If gain_est is really really good, we are messing up our gain_est with gain_prior. We really want a weighted average that balances how good we believe gain_est and gain_prior are.
Weighting by the inverse covariance does exactly that.

In other words we need to weight the new estimates by our confidence we have in them for each parameter. A great general purpose weighting scheme is to weight each term in the weighted average by its inverse covariance. When in doubt, this is the goto weighting scheme, optimal in several respects.

Our initial estimate of x comes with a covariance of that estimate. Provided we know the covariance of the input data (of Pitch in this case). Since the Pitch vector comes from independent accelerometer measurements, take

cov[Pitch] = sigma^2 I,

I being the NxN identity matrix, and sigma the standard deviation of the individual pitch measurements. Then the covariance of our estimate is

cov(x) = sigma^2 (A^t A)^-1.

This covariance tells us exactly how much we can trust our estimate. If we held very still during the collection period, the gain variance will be very large. The variance comes down the more rate diversity there is over the collection period.

So if we have two estimates for x , x1 and x2, along with the associated covariances cov1 and cov2, we can combine them into a single improved esitmate optimally as

x_new = (cov1^-1 + cov2^-1)^1 (cov1^-1(x1) + cov2^-1(x2)).
So we can keep a running estimate in this way by updated our prior estimate with the new data.

The covariance of x_new is already computed:
cov(x_new) = (cov1^-1 + cov2^-1)^1.

On thing the covariance above does not account for is the amount x changes between estimates. To account for this change we add a covariance term to represent it. This is sometimes referred to as plant noise, so I use the symbol cov_plant for it.

In the algorithm that follows, a shortcut has been made for simplicity. Since the Omega measurements are derived from the difference of two pitch measurements, they are not independent. The covariance matrix for Omega is actually tri-diagonal. The increased accuracy however does not justify the increased computational complexity, especially for an algorithm that is intended to run on a micro-controller.

Algorithm Doomsday Cal.
Inputs:
x_init -- the initial estimate for x.
cov_init -- covariance of x_init
K -- number of measurement to take between each cal update
sigma -- standard deviation of raw pitch measurement from accelometer
cov_plant -- un-modeled covariance

Outputs:
x -- A running estimate of the gyro parameters a and b.

#initialize

last_theta, last_time = measure theta, time

x = x_init

cov = cov_init

while 1: # loop forever (or until doomsday)

Omega = []

Count = []

for k in [0..K-1]:

measure theta, c, time

Omega.append((theta - last_theta) / (time - last_time))

Count.append(c)

last_theta = theta

last_time = time

A = column(Count, ones(K))

x_new = (A^t A)^(-1) A^t Omega

cov_new = sigma^2 (A^t A)^-1

x = (cov^-1 + cov_new^-1)^-1 (cov^-1(x) + cov_new^-1(x_new))

cov = (cov^-1 + cov_new^-1)^-1 + cov_plant

The gyro gain parameter seems to be very stable. We neither want nor expect this measurement to change much if at all. So in practice, cov_plant and cov_init can be made to prevent large updates to a by making the upper left element of these matrices to be very small implying very accurate prior knowledge of a.

Radial and Tangential Accelerations
These sensors are really good. The idea here is to use the gyro measurements to predict the non-braking accelerations and subtract them from the accerometer measurements before the braking estimate is made. It is really exciting to see this Math in Action, literally. We pose a simple circular model for the head movements and take two derivatives to get the acceleration. Comparing the expected results with actual measurements shows both that the model is right and how accurate these sensors are.
The circular model for head movements is:

p = r[sin(pitch), cos(pitch)],

where r is the radius of rotation, and p is the sensor position in the [Forward, Up] frame or f-u frame for short (no slight intended). One of the difficulties is keeping track of the sensor x-y frame relative to the f-u frame. That is, I guess, the entire problem.

The velocity vector is the time derivitive of the position vector. Using the chain rule from Calculus I (since pitch is also a function of time with dpitch / dt = omega) the velocity from head motion alone is computed as.

v = r[cos(pitch), -sin(pitch)] omega.

Recall that chain rule states that f(u(x))' - f'(u(x)) u'(x). The product rule is

[f(x) g(x)]' = f(x) g'(x) + f'(x) g(x)

Using the chain rule once again with the product rule and letting omegadot be the time rate of change of omega, the acceleration measurement is

a = -p omega^2 + v omegadot / omega.
or
a = R + T

where the radial R and tangential T components of the rotation acceleration are given by

R = -p omega^2 and
T = v omegadot / omega
= r[cos(pitch), -sin(pitch)] omegadot
The SharkFin was mounted to a bicycle wheel (see title picture) to confirm no major errors were made in the circular model calculations. A rubber band prevented the wheel from spinning more than 90 degrees. Over the 30 second run, the wheel was spun vigorously back and forth through its range of motion. In the pitcure below, the actual x-y acceleration measurements are transformed into f-u coordinates and compared to these R + T.

The blue line shows the f-u accelerations in Gs predicted from the gyro measurements. The green line shows the accelerometer measurements rotated into the f-u frame. The acceleration due to gravity, [0, 1] in the f-u frame, has been subtracted out as well. This is not a bad fit considering that the two measurements are completely independent.

The next plot shows the accelerations measured in the forward direction. This is the direction we are most concerned with for braking measurements.

The top plot shows the predicted and measured forward accelerations, the middle plot shows the difference between the two (the raw braking measurement). The lower shows the smoothed braking measurement. Ideally, since the motion is strictly circular with no braking involved, we would hope to see zero braking. Because of the large radius and vigorous motion, the errors creep up to .1 Gs. This should be much smaller for an actual helmet mount during non-head-banging activities. The upward trend is due to the gyro measurement drifting away from the true pitch measurement. This is dealt with by using the accelerometer measurements as an absolute, if noisy, reference.

That's it on gyro calibration. If you made it this far, congratulations. I've spared you the details of all of the mistakes and dead ends I have traversed. I'll save the full braking algorithm for another post.