Iterative dynamic accelerometer calibration

OK, strike the iterative part of accelerometer calibration for the moment. At the start, the quad-frame error vectors (QFEV) look fit-for-purpose, and the intention was that during the course of a flight they would converge towards increasingly better error vectors.  But rather than converging to the ‘right’ value over time, they are diverging rapidly.  For the moment, I’ll have a go just using the initial values and see what happens.

The divergence does seem rather OTT, suggesting a possible bug in the code, so the hunt is on.

Iterative dynamic accelerometer calibration

Currently, my accelerometer calibration of offset / gains against temperature requires two days of action, reading the sensors at various angles across a broad range of temperatures to put into Excel to produce trend lines allowing calculation of offset and gain at the current temperature for the X, Y and Z accelerometers.  If you think that sounds difficult and tedious, multiply by two and you’re getting there.  It works but I don’t like it for several reasons:

  • off-the-shelf quad manufacturers / DIYers don’t do this
  • it’s tuned to a specific sensor – replace a broken sensor with a new one and the calibration needs to be redone
  • the calibration can only really be done on a sunny day in winter to be able to get termperature ranges from 10°C (Phoebe sleeps in the garage overnight) to 25°C (Phoebe moves indoors, and as her temperature rises, many samples are taken).

So I’ve been considering how to calibrate the accelerometer in flight.  It goes a bit like this: Prior to take-off

  • read the accelerometer many times and take the average while she sits quiet and stable in the ground – these readings are a gravity vector in the quadcopter’s frame (QFGV) and will be flawed due to offset and gain errors.
  • Using the QFGV, calculate her angles wrt earth using Euler – again the angles will be incorrect, but testing shows the error isn’t far from the truth
  • Use the quadcopter to Earth matrix to convert the QFGV to earth coordinates – the Earth frame gravity vector (EFGV) – in the perfect world, it should read 0, 0, 1 – the perfect world earth frame gravity vector (PWEFGV) but doesn’t in reality
  • Subtract the PWEFGV from the EFGV to come up with earth-frame error vector (EFEV)
  • Convert that EFEV back from the Earth frame to the quad frame to produce the quad-frame error vector (QFEV).

Now set Phoebe running:

  • read the raw accelerometer values every loop, integrating them over the time between each read
  • Periodically (roughly 40Hz compared to her spin rate of roughly 440Hz) process the readings by dividing by the elapsed time since last processing to come up with averaged accelerometer readings.
  • Subtract the QFEV from the readings to produce a ‘better’ set of quad-frame averaged accelerometer results
  • Use this better set of accelerometer results to come up with better angles via Euler
  • Use the better angles to convert the original fixed QFGV to a new, more accurate EFGV
  • Again subtract the perfect world PWEFGV from the new EFGV to come up with a new EFEV.
  • Convert the EFEV back to a QFEV
  • Repeat ad infinitum

This should produce an ever improving set of offsets by taking the initial fixed QFGV and constantly refining the quadcopter accelerometer readings with the QFEV.  In doing so, there should be convergence on an accurate value for the QFEV – i.e. the accelerometer is calibrated on-the-fly. Initial testing suggests this might just work, but Phoebe is not getting her @R$£ off the ground fast enough, so tilting to stop drift is causing the props to clip the ground. More testing later with a more energetic take-off rate will hopefully be more conclusive.

P.S.  All the above is based on the assumption that calculation of Euler angles from raw accelerometer readings is more accurate than the raw sensor readings themselves.  Currently I’m working on gut-feel, but I need to spend the time producing a mathematical proof.

P.P.S.  On the other hand, it’s easy to prove in real-world testing – if not true, then Phoebe will progressively go mad instead of settling into a stable state of affairs.

Earth-frame gravity

I’ve been having troubles getting Phoebe to take-off, hover and descend at the right speeds.  It’s always been close but not good enough.  The problem is with calculating the value of earth gravity from a non-horizontal take-off platform; I’ve tried a variety of (pure guess-work) ways which improve matters but are definitely wrong.

I realized the only perfect solution was to have an inverse rotation matrix for the earth- to quadcopter-frame to convert quadcopter accelerometer reading to earth readings prior to take-off, but I’ve struggled to find one, or clear description of how to make one – until yesterday.

The key information came from here:

If your matrix is only used for rotations, its inverse is its transpose:

     [ x1 x2 x3]
R =  [ y1 y2 y3]
     [ z1 z2 z3]

       [ x1 y1 z1]
Inv® = [ x2 y2 z2]
       [ x3 y3 z3]

That gave me my way in. The rotation matrix I got from the Beard paper looks like this:

| c_pa * c_ya,                                 c_pa * s_ya,                -s_pa    |
| s_ra * s_pa * c_ya - c_ra * s_ya,   s_ra * s_pa * s_ya + c_ra * c_ya,  s_ra * c_pa|
| c_ra * s_pa * c_ya + s_ra * s_ya,   c_ra * s_pa * s_ya - s_ra * c_ya,  c_pa * c_ra|

First step was to remove yaw – prior to take-off, yaw is not present.

| c_pa,            0,     -s_pa    |
| s_ra * s_pa,   c_ra,  s_ra * c_pa|
| c_ra * s_pa,  -s_ra,  c_pa * c_ra|

Next, swap the rows and columns to make the transpose matrix:

|  c_pa, s_ra * s_pa,  c_ra * s_pa |
|   0,       c_ra,        -s_ra    |
| -s_pa, s_ra * c_pa,  c_pa * c_ra |

The final step was to multiply the Beard matrix against the transpose to make sure it produces the identity matrix – I won’t share the details as they are fiddly and boring, but suffice it to say it was right.

That means I can now get an accurate gravity reading prior to takeoff, and this then allows more accurate integrations of (total acceleration – gravity) to get accurate speeds.

Fixing the Matrix

The Murder / Suicide flight is almost certain to be due to a bug in the current Matrix:

|Xe| = |cos(pitch),         0,          sin(pitch)||Xq|
|Ye| = |0,          cos(roll),           sin(roll)||Yq|
|Ze| = |sin(pitch), sin(roll), cos(pitch)cos(roll)||Zq|

I’ve arranged it such that if Phoebe is nose down, or left-side down, the pitch and roll angles are negative respectively.

Forward, leftward and upward movement results in positive accelerometer values.

The diagram below shows the vectors representing Phoebe pitching nose down and moving right to left as a result.  Her sensor vectors are in gold, and their translation to earth axis vectors in black.  Phoebe is pitched nose downwards, so the angle of pitch is negative.

Vector conversion

Vector conversion

Here’s the revised value of the Matrix that ensures the earth vectors match those shown in the picture.

|Xe| = |cos(pitch),         0,         -sin(pitch)||Xq|
|Ye| = |0,          cos(roll),          -sin(roll)||Yq|
|Ze| = |sin(pitch), sin(roll), cos(pitch)cos(roll)||Zq|

Critically, this means that when calculating eax based upon the faz vector, because the pitch angle is negative, so is sin(pitch), yet the eax vector is showing forward (and therefore positive) acceleration.  Hence right hand column of the matrix where it shows -sin(pitch) and -sin(roll).

The same is not necessary when deriving eaz from the fax vector; eaz is shown as negative, and so fax*sin (pitch) already produces the correct negative result.

The case for the y axis is identical.

Hopefully there will a slot in the weather tomorrow for me to test this.

An insight into the bloomin’ obvious

Back down the park this afternoon.  The wind had picked up so other than a very minor tweak, I left this morning’s configuration for DLPF, tau and the absolute angle PID gains alone.

Instead, I tried a few flights with the drift-compensation enabled.  And it was garbage as it had always been, doing all sorts of wandering round completely unrelated to the tail wind that was blowing at maybe 10mph.

And then I realized what was wrong – I was using the accelerometer X and Y axes to calculated any horizontal speed; What I should have done is much like I use the z-axis accelerometer for vertical speed.  Essentially, all the power is upwards from Phoebe’s point of view.  Then it needs to be converted to earth axis by accounting for the angle of tilt.  Then you can sum that up over time to get the speed.  So instead of this bit of code:

#==================================================================================
# Axes: Convert the acceleration in g's to earth coordinates, then integrate to
# convert to speeds in earth's X and Y axes meters per second
#==================================================================================
eax = fax * math.cos(pa)
eay = fay * math.cos(ra)
eaz = faz * math.cos(pa) * math.cos(ra)

it should read

#==================================================================================
# Axes: Convert the acceleration in g's to earth coordinates, then integrate to
# convert to speeds in earth's X and Y axes meters per second
#==================================================================================
eax = faz * math.sin(pa)
eay = faz * math.sin(ra)
eaz = faz * math.cos(pa) * math.cos(ra)

thus sharing the power from the blades across the 3 axes depending on the pitch and roll tilt.

I knew what I was doing was wrong; I knew the correct fix was todo with vectors; I also knew I’d completely forgotten how they work and that even when I did know, they made my head hurt. So instead, I’ve gone for the flash of inspiration approach which is even more satisfying!

I’ve not tested this yet, due to inclement weather coming in, but I do believe testing in the park, and so not having to worry about catastrophic crashes has allowed my mind to wander constructively.

I’ll be back there tomorrow to test this out – forecast: “sunny with 20mph winds”; should be fun!