Sensor calibration

[Cossacs]

I found following docs with almost clean calibration algorithm:

1) http://motionsense.com/services/pdf/Calibration%20Report%20Example.pdf

2) http://www.parallax.com/portals/0/downloads/docs/prod/sens/27911-GyroscopeAppNote1.pdf - pages 31-33

I think this is not end-user procedure because some equipment required, but it is necessary for high-perfomance stabilization platform

Let's start with accellerometer calibration from first document. As i understand 14 measurements with 6 values of each measurement with equation (1) used to determine 15 unknown coefficients:

- 3 coefficients of offset

- 3 coefficients of scalefactor

- 9 coefficients of 3x3 misalignment matrix.

I'm not so familiar with matrix algebra, so my question is how to calculate 15 values from measured 14x6 values using "general least squares algorithm"?

[peabody124]

Welcome Cossacs.  Kenn is at a conference right now who would normally jump on this.

For background a few pieces of information.  If I understood it correctly Cossacs found that after 5x360 degree rotations without any accelerometer correction he got about 20 degrees of under-rotation in the main axis.  He also got approximately 10 degrees of rotation in the wrong axis, which we would like to attempt to calibrate out.  In the parallax document it describes using a rotating platform and putting the sensor (flight controller) in a cube and measuring the results in each of the primary angles.  In that way you get G = M * R (G is measured gyros, R is true rotation, M is misalignment matrix and scale) if the bias is already removed and can fairly easily solve for the misalignment matrix.

If Cossacs can set up the tests I think we can fairly easily perform this by just logging the data in GCS at first and processing it offline.  If the results improve his stability we can go from there in terms of making this an integrated calibration procedure.  We still have the code from Jonathan Brandymeyer's implementation of TwoStep which we can bring back if needed, although I would prefer to make something like that run during continuous movement.  I also believe as implemented it was looking for accel/mag misalignment more than the gyros.

[Kenn Sebesta]

As James says, I'm currently at a controls conference so availability is limited in the coming two weeks.

The coefficients of the misalignment matrix are inter-related and with a bit of math become dependent only on three rotation values. In the way that most people are used to thinking about it, the classic roll-pitch-yaw variables would be used. So this reduces the complexity of the system solution.

As to how to do least-squares, the easiest way is to use Matlab or Octave or Scilab. It's really as easy as saying c=y/A, once you format the A matrix properly. This is pretty simple, as the A matrix is the part of the equation "dependent" on the parameter x, c are the coefficients of the equation, and y are the measurements.[1]

You could also use a Gnu implementation of it, http://www.gnu.org/software/gsl/, specifically the function 'gsl_fit_linear'.

[1]: If you had a polynomial y=c_0 + c_1*x + c_2*x^2, then what we want to do is solve for the coefficients c_0, c_1, and c_2. We have y, as it was the measurement, and we have x, as that was the independent variable. Since we have x, we have x^2. So we write an A matrix where each row is composed of the [1 x x^2] associated with each test, and the y-matrix is where each corresponding row value is the one measured for that given input x. All you have to do is place these rows on top of each other and you have a matrix A and y.

[Cossacs]

I conducted a simple experiment. Accel KI = Accel KP = 0 and gyros was biased just before experiment.

 - I turned the СС3d board on 4 turn clock-wise (CW) by hand as accurately as i can

 - After that i turned the СС3d board on 4 turn CCW

https://dl.dropbox.com/u/90610651/openpilot/gyro_misalignment.png

As you can see most of the rotation was around Gyros.x axis. Integral of gyros.x = AttitudeActual.Roll, integral of gyros.y = AttitudeActual.Pitch. We can observe huge 10 deg impact of rotation around gyros.x axis to AttitudeActual.Pitch. As i understand it is misalignment/cross-axis sensivity impact because it become almost zero after i turned board back (pls note, i made it by hand).

So i think it is very important to calculate misalignment matrix in order to increase gyro accuracy.

ST AN3182 appnote http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/APPLICATION_NOTE/CD00268887.pdf describes very well how to calculate misalignment matrix, it can be done easily with matlab for gyros as well as for accels:

w=[M, ones(n,1)]; ACC=inv(w' * w) * w' * Y

i.e. bias offset vector [X_offset, Y_offset, Z_offset] = - inv(ACC(_1..3,1..3))*ACC(_1..3,0)

_{M - n x [RateX, RateY, RateZ] gyro outputs}

_{Y - n x [RateRefX, RateRefY, RateRefZ] gyro reference outputs}

_{ACC(X,Y) - coefficients from w-matrix}

But i would like to calculate H-matrix of linear acceleration gyro sensivity from this document http://motionsense.com/services/pdf/Calibration%20Report%20Example.pdf . Can anybody explain H-matrix calculation from 15 gyros/accels measurements?

[cyr]

Am I thinking right that it must be more cross-axis sensitivity than actual misalignment? And in that case, the matrix wouldn't be just a rotation?
Or do they in fact amount to the same thing in the end?

In any case, there certainly seems to be a need to compensate for it...

[peabody124]

The code pasted above to solved for the matrix doesn't constrain to solving for only rotation matricies so it shouldn't have a problem with more general sources of cross-sensitivity.

Solving for the H matrix is more challenging, but if you use gravity to help you it should be doable at home.  Imagine if you took your turn table and mounted it on its side.  Then as you rotate at a fixed rate (say 60 dps) as it rotates the force of gravity will rotate between the X/Y plane.  If you see a periodic change in the gyro output you would know that it comes a accel-gyro cross sensitivity.  You would then repeat this with the turn table flat placing the board horizontally and upside down (constant force in each direction).  So from all this data (all at 60 dps in yaw) you have applied force in each direction.  Repeat this now for rotation in each axis (this will take a while I'm afraid).  Ideally you'd run this at multiple rates too to make sure the results are robust.

Now you have a bunch of measurements in all 3 axes from accel and gyro.  Put that in an N x 3 matrix where I'd just use all the data from the logs (while the test was going no and there was no real acceleration).  Make another matrix that is the true rate for all of those tests that is N x 3.

error = (rate - gyro) = H * accel

H = error \ accel % in matlab

if you get the data I'll be happy to help with the analysis too because an important part of this will be verifying the results are robust and that you aren't overfitting noise.

[cyr]

Actually, isn't the H matrix compensating for gyro bias error caused by acceleration and not scale error?  So the simple solution would be to just measure at zero rotation in different orientations (and constant stable temperature)?

[peabody124]

Doh.  Yeah if the model they propose in that diagram is reasonable (additive bias) then you're right.  In the paper they talked about getting it from rotating measurements so went from that. This would be easy to add and measure in the six point calibration.  I created an issue https://github.com/PhoenixPilot/PhoenixPilot/issues/100 and will try and check that tonight.

In the long run before making this active I would prefer to see the data I described above though.  It would be useful to verify that the cross-coupling between acceleration and gyros were well modeled as an additive change.

[peabody124]

Ok.  So I tried this, and at least in the first pass it doesn't do very much good (and what it does is likely overfitting).

This is the measured error (board was stationary) versus the best prediction of it.  I took pretty long measurements in each position so I don't think that was the limitation.  I should perform the 6 point calibration on the accels before doing this to minimize their error but the amount this changes things would make it negligible.

The original average error (measured as the variance of all the measurements after bias correcting) was 0.01 deg/s and after calibration was 0.0088.

% Put in the values output to the debug panel

gyro =[ -0.981886 ,  2.29815 ,  1.39624 ; ...

   -0.952496 ,  2.2322 ,  1.3745 ; ...

   -0.874834 ,  2.1702 ,  1.3744 ; ...

   -0.803864 ,  2.11648 ,  1.29214 ; ...

   -0.796333 ,  2.00924 ,  1.24306 ; ...

   -0.797362 ,  1.88595 ,  1.21317];

% This should really be the accels _after_ applying the

% six point calibration.  Not done yet.

accel = [ -2.42437 ,  -3.43474 ,  -9.03572 ; ...

   -1.06826 ,  8.457 ,  -4.2369 ; ...

   2.39934 ,  2.09681 ,  9.3421 ; ...

   -0.046631 ,  -9.39648 ,  -3.70327 ; ...

   9.1307 ,  -2.36368 ,  -3.64294 ; ...

   -8.73273 ,  -1.45274 ,  -3.93295];

accel_bias = [0.220775   -0.311232   -0.0199542];

accel = bsxfun(@minus, accel, accel_bias);

% Remove any measurable bias

error = bsxfun(@minus, gyro, mean(gyro,1));

% Use least squares to model the (bias corrected) error from the accels

H = accel \ error

% Show what this model _predicts_ the error to be

ep = accel * H;

% Plot our best prediction of the error

plot(error, ep, '.', 'MarkerSize',17)

xlabel('Measured error');

ylabel('Predicted error');

[cyr]

That doesn't look very promising. Interestingly, you seem to have much greater variation in the gyros than I saw yesterday when I tried it. I didn't really collect any data though, just eyeballed GCS traces. I might have overlooked something.

[cyr]

The gyro values look remarkably well "sorted", are you sure they aren't drifting over time because of some other effect which is distorting the results?

[peabody124]

That's a good observation.  I had the board in a box (I needed it to be really still since each position took 30 seconds to acquire enough data) so that could have led to a temperature shift.  I'll repeat it and log temperature to include as a regressor.

[Kenn Sebesta]

Cossacs--

Excuse me for asking this if you have already answered, but was this with the six-point calibration? Or with the old calibration style? I'm surprised that you get so much axis misalignment.

Although the new calibration shouldn't affect this if you return the board to its initial orientation. That should still work no matter what so I'm tempted to think that you might just be looking at gyroscopes that have subtly different rate scale calibration values.

[peabody124]

Making progress on temperature calibration.  On the firmware side, I'm thinking it might make sense to average the temperature over one second blocks to recompute the offset.

[Kenn Sebesta]

That sounds like a good strategy, but even 1 second might be more than necessary. I'm betting that the temperature slew rate is on the order of seconds per degree. We can only be doing better than we currently are, not taking into account temperature dependency at all. I'm not really sure of any advantage to slowing it down, as I suspect that it's not that expensive to calculate the polynomial. If we wanted to be at least three times faster than the system's rise time (which is a pretty decent threshold for "fast enough"), then I would suggest moving to 250-500ms periods.

[cyr]

The approach I was thinking of was to iterate through all the temperature compensated factors (which could be gyro bias x/y/z, accel bias x/y/z, gyro gain x/y/z etc. etc.) and recalculate one of them on each sensor read. Keeps the CPU load low and constant, and the code quite simple.

[Kenn Sebesta]

That could definitely work. The advantage is that you avoid hammering the CPU, you even avoid spiking the CPU. The disadvantage is that the algorithm would become opaque, as it would be difficult to know what coefficients were used at any given timestep. 

[peabody124]

So one of the main reasons for the long filter on temperature is I want to avoid a situation where noise in the temperature measurements could lead to high frequency noise in the compensated gyro values.  However the idea of staggering those calculations is a good one.  The other cheap trick is unrolling 

    bias = alpha0 + alpha1 * t + alpha2 * pow(t,2) + alpha3 * pow(t,3)

into

   bias = alpha0 + alpha1 * t;

   t2 = t * t;

   bias += alpha2 * t2;

   t3 = t2 * t2;

   bias += alpha3 * t3;

I've ported the sensor changes to copter control already (although haven't checked the CPU load).  Right now I just have to finish the UI changes since the UI was written for the Freedom calibration page.

   

[peabody124]

If anyone wants to test temperature compensation

https://dl.dropbox.com/u/6645063/AboveGroundLabs-20121228-1a6ccfd7.dmg

https://dl.dropbox.com/u/6645063/AboveGroundLabs-20121228-1a6ccfd7-install.exe

Ignore the fact the attitude page will show settings for the EKF.  Personally I'm not convinced it does enough to justify the added complexity so feedback would be appreciated.  My worst axis had 0.05 (deg/s) / (deg C).

P.s. don't try this on CC yet ... just CC3D.

[peabody124]

BTW one thing that is a little tricky for this and relates to https://github.com/PhoenixPilot/PhoenixPilot/pull/133 - depending on the board only a subset of the UAVOs that the Attitude page can connect to might be important to monitor.  For example:

- Freedom - noise calibration is relevant (watch RevoCalibration)

- FlyingF3 - no baro (but still watch RevoCalibration)

- CC3D - don't watch RevoCalibration or INSSettings

- CopterControl - dont perform temp comp since it doesn't have a gyro temperature  and no RevoCalibration or INSSettings

[cyr]

I tried to compile 1a6ccfd75f677a5d... to try out (on linux), but failed:

/home/cyrano/OP/PhoenixPilot/ground/gcs/src/plugins/config/configattitudewidget.cpp:28:34: fatal error: ConfigAttitudeWidget.h: No such file or directory

A for the usefulness, that depends on where you fly and what you want to do I think. Around here in winter I could easily go from +20C inside and start flying in -20C ambient. It soon adds up...

[peabody124]

Ah yes, case sensitive file systems ...

https://dl.dropbox.com/u/6645063/abovegroundlabs_0.1.0-20121228-a4abbb6b_amd64.deb

here you go.  Yeah I can imagine in 40 deg swings that will translate to 2 deg/s which be noticeable.  If anyone confirms that it does make a tangible improvement I'll keep polishing it off.  There are a lot of things I'd want to change before this can go into next.  (e.g. add temp min/max fields to prevent it trying to extrapolate outside the calibrated range), make the reworked attitude gadget work better between cc/freedom/quanton (comments appreciated)

[Kenn Sebesta]

There are two classes of users who will greatly appreciate this:

1) Users with boards that have analog gyros. This might come up with manufacturers seeking to use cheaper MEMS, as the Invensense is a little pricey; or it might come up with people using extremely high-precision gyros on custom boards.

2) Users who need the absolute best accuracy possible. Even if it is only a 0.5deg/s correction, that adds up over time. Holonomic navigation without a magnetometer might even be feasible at these accuracies.

[peabody124]

As is it expects the temperature to be bundled into the GyrosData object so if someone updates the CC code to copy CPU temp agency into that it might work.

I'm mostly waiting to hear any reports it makes a tangible difference in leveling.

[peabody124]

Bump.  Is anyone interested in testing this?  If not I'll probably start backporting some of the other changes to the UI since there was some other useful stuff I mixed in.