Another day, another type of test to track down where these strange readings are from.  This time, Phoebe sits on a flat platform which sits on a set of sponge balls on the floor.

I then roll her round in circles on the floor, which should produce sine waves of acceleration from the X and Y sensors 90° out of sync.  And ignoring the noise (I’d turned down dlpf to 40Hz, that’s what’s shown:

Circular acceleration

Circular acceleration

Scale seems plausible at up to 0.4g acceleration. qax has a slight negative offset suggesting she’s leaning slightly – she’s picking up -0.025g or as a first order estimate of the angle -0.025 radians or -1.5° – again plausible.

So the deadzone theory is dead!  But there is something interesting here: above is the complete acceleration including gravity; once gravity is subtracted, then this is what you get:

net acceleration

net acceleration

And this is where it gets interesting – the -0.025g offset has gone, which is good, but so has the bulk of the sine wave shape leaving only spikes – compare the scales of the two graphs.

The measurement for gravity in the earth reference frame is fixed throughout the flight – it’s measured as part of the pre-flight checks.  But critically, the measured acceleration is used to calculate the angles used to redistribute fixed gravity to the new quadcopter reference frame – and I think it’s these angles and their reorientation of gravity between reference frames that’s also removing the sine waves. And sure enough, the angles graph below shows Phoebe pitching and rolling by ±20°, whereas I know full well she is not because she’s sitting on a flat platform that can’t pitch and roll.

Pitch and Roll

Pitch and Roll

So what I need is a way to calculate these rotation matrix angles ignoring the net acceleration; again we’re back to complementary or Kalman filters and using the gyro angles.  Except there’s another problem: the gyro angles are all in the quadcopter reference frame; what’s needed is a way to convert the gyro angles to the various intermediate Euler angles of a rotation matrix which lie in three different reference frame.  I vaguely remember something about this in a paper I read one – time to go on a hunt again.

An itch that needs scratching.

It’s bothered me for a long time that the two sources of angles fed into the complementary filter do not have the same meaning.  Since adding the rotation matrix code this has been increasingly evident.  Finally with the graphs from yesterday, and the consistent forward drift, understanding this has become a top priority.

The nub of the problem is the difference in the context of the angles:

Euler angles are cumulative: they represent the rotation of gravity between the inertial reference frame (earth axes aligned with north, west and up) and the quadcopter X, Y, and Z axes respectively.  To perform the rotation each angle is applied in sequence.

Integrated Gyro angles are independent – they are all in the quad frame, and represent rotation rate around each of the quads axes all within the quad reference frame.  Gravity doesn’t enter the equation.

So currently the complementary filters is trying to merge two different types of angles and can only produce garbage as a result.

I may to convert gyro angles into Euler angles, and for a change I found a site which explained these well, and in clear language and equations that don’t obfuscate the facts behind fancy symbols.

Euler vs Gyro Angles

There’s a few other links from that page that explain other parts of the quad code really clearly too.  Well worth reading.

Or I may need to use Kalman to smooth out the Euler angles, and dispose of the complementary filter altogether.

Or there may be a plan C that I’ve not had time to think of yet.

But I’m stuck on a bus for 2 hours tomorrow, so I’ll have plenty of time to muse and snooze!

Another one bites the dust:

another flaw fixed. After some more PhD / Masters paper reading yesterday, I realized the angles being calculated should not all be in comparison to the the initial inertial reference frame; instead as the rotation matrix is applied, each angle is calculated against the reference frame the previous rotation had achieved.

So my pitch angles were wrong, which lead to bad gravity calculation prior to take-off and that’s never going to be a good thing!  With the code change, the values read of the sloping takeoff platform are as follows where qfrgv_* are gravity read in the quad reference frame, and efrgv* are those reorientated back to the earth frame.  The X and Y values are perfect, but Z is out – that doesn’t surprise me as the Z-axis accelerometer is twice as inaccurate as the X and Y.  Never the less, the ‘drift’ it shows is 7cms-2 which isn’t bad, though it is making it hard for phoebe getting as far off the ground as far as she should be.

pitch 8.722535, roll -0.448118
qfrgv_x: 0.152762 qfrgv_y: -0.007787 qfrgv_z: = 0.995656
efrgv_x: 0.000000 efrgv: 0.000000 efrgv_z: = 1.007337

There’s still something wrong though as this plot of the flight shows:

Mucky lawn flight path

Mucky lawn flight path

She thinks she’s moving backwards according to the sensors and her compensation for that actually has her moving forwards in the real world.

The accurate measure of gravity values suggests that calibration isn’t the problem.  My next suspicion is the way the earth frame velocity PID targets are converted to quad frame.  Fingers crossed.

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.

Latest code and new documentation on GitHub

Have a look at PiStuffing/Quadcopter to see all the changes made relating to

  • reference frames
  • timing accuracy
  • PID selection
  • performance and averaging

I’ve also added a very early alpha draft of the PDF guide about quadcopters in general and Phoebe in particular.

Oh, and I forgot to mention that with the code changes for reference frames, timing, PIDs and averaging, she’s flying much better now – less drift, and much more stable.  Not quite worthy of a video yet, but soon I hope.

Moron reference frames

Best go read here and here if you haven’t already.

Software Layout

Software Layout

There are two reference frames with Phoebe:

  • the Earth-frame is defined by gravity and two other orthogonal axes which together define horizontal
  • the quadcopter-frame defined by the sensor axes assuming the sensors are aligned well with the quad’s arms.

Because I don’t (can’t) compensate for yaw, the Earth-frame horizontal axes point in the same direction as the quadcopter-frame sensor X and Y axes as viewed from above.

The key to the following train-of-though is that the inputs on the left of the diagram are in the Earth-frame (horizontal and vertical velocities), and the output’s on the right-hand side are in the quadcopter-frame because that’s where the motors and propellers reside.

There is a rotation matrix for converting from the Earth-frame to the quadcopter-frame as described in the Beard PDF linked above1.

OK, lets work through the details of moving through the PIDs from LHS inputs to RHS outputs, starting with the simpler example of the vertical velocity PID at the bottom right of the diagram.

The target is an Earth-frame vertical velocity.  The input is the quadcopter-frame accelerometer Z-axis sensor, matrixed2 to Earth-frame before integrating to give the current Earth-axis vertical speed.  However, the output needs to be in the quadcopter-frame to be correct for driving the PWM error compensation.  This cannot be done on the output side directly as the relationship between PWM to the ESC and ESC output is non-linear. To me, that suggests the only option is for the the vertical PID target to be adjusted from the Earth-frame back to the quadcopter-frame to allow for any tilt between the Earth- and quadcopter-frames.  In other words, if Phoebe has an overall small tilt of θ (derived from short term averaged Euler angles), then the vertical velocity PID’s current Earth-frame target, e,  needs to be converted to the quad-frame target, q, thus:

e/q = cos θ
q = e/cos θ

In other words, the vertical velocity PID target (configured at 0 for hover for example or 0.3 as 30cm per second climb) is the Earth-frame target speed increased by 1/cosθ to ensure the correct Earth-frame lift is provided.

Having sorted the vertical velocity, let’s move on to the horizontal, where things are less clear to me.

The leftmost PID (top left) is Earth-frame horizontal velocity input and target with an Earth-frame output representing corrective Earth-frame acceleration.  That output is converted to a desired pitch / yaw target for the absolute angle PID.  But I think there’s a bug there: the input for the absolute angle PID is a combination of Euler angles (long term) and integrated gyro (short term) passed through a complementary filter.  The Euler angles are correct for rotating between the two frames, but I’m concerned but not convinced that the integrated gyro is not, the reason being that the gyro sensor is in the quad-frame; should the outputs be rotated to Earth-frame before integrating2?

The output of this ‘middle’ PID is the rotation rate target. This lies in the quadcopter-frame aligned with the gyro axes. The input comes direct from the gyro, also in the quad-frame. The output needs to be in the quadcopter-frame. So I think all’s well, but I’m not 100% sure.

So net is I need a quadcopter- to Earth-frame matrix (the inverse of Beard), and I need to consider whether I need to add a cos θ tilt angle to the vertical velocity PID earth-frame target.

Answers on a postcard please as to whether I’ve gone wrong here.!


  1. I have an Earth to Quadcopter rotation matrix from Beard.  However, I misread it and am using it as a quadcopter- to Earth-frame matrix which can’t be a “good thing”TM.
  2. I don’t have a quadcopter- to Earth-frame matrix – Beard only shows an Earth- to quadcopter-frame matrix.  I should have spotted that.  I need to invert Beard to produce the matrix I need.  More messy math(s), yuck!