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!

“Hmm, that’s interesting but…”

“but what?” (sorry, too much Matrix recently – there is a contextual link though)

Here’s the plot from the same test run, but this time with accelerometer data aligned with Phoebe’s axes – virtually raw from the sensors rather than converted to earth coordinates.

Phoebe axis accelerometer

Phoebe axis accelerometer

If you compare it to the plot from the previous post, there are similarities in the X and Z axes: Phoebe climbs to roughly 1m and drifts in the X direction by a meter showing signs of drift correction in action; but from Phoebe’s Y axis point of view, she drifted nearly 5 meters towards the wall she ended up hitting, and yet there’s no sign of that in the Earth coordinate plot from earlier.

Given the difference between these readings is just the coordinate conversion matrix (see, told you so), it seems I definitely have a bug there in the Y axis conversion!

Phoebe, where on earth do you think you’re going?

The YouTube motion control video got me thinking: in their setup, their sponge ball tracking systems gives them centimeter accuracy of the speed, position and orientation of the quad(s).

Phoebe uses the accelerometer with coordinates reorientated to the earth’s axes using the various angle calculations to do the same for speed and position.  She currently doesn’t track yaw ( => orientation) – I just try to minimize it using the z-axis gyro – I’ll need a compass to track it accurately.

And despite all my best attempts, she drifts and I’m really struggling to track down why, primarily due to a lack of a plan.  I’d just assumed the drift was due to bad PID tuning, or poor angles reorientating the quad accelerometer axis to earth coordinates; my approach has just been to tweak PIDs and other config parameters in search of the holy grail combination, but so far to no avail.

So in today’s tests, I’ve converted her sensor plots into a map of where Phoebe thinks she’d been compared to where I saw her go.


Phoebe's Flight Map

Phoebe’s Flight Map

And actually, despite having to double integrate acceleration to distance, it’s a reasonable match of the flight.

She moves forward by roughly 0.9 meters, and moves to the right (negative is reverse / right direction) by about 0.5 meter.


  1. it’s clear she’s trying to correct left / right drift for a bit before she just goes off on one
  2. it’s also clear she doesn’t make a similar effort to correct the forward drift
  3. the left / right scale is out by a factor of 5 – she eventually drifted right by about 2.5 meters

The first problem suggests that once the complementary filters starts to favour the accelerometer, then drift protection starts to fail.

The second problem suggests I may have my forward / backward drift compensation the wrong way round, so she accelerates forward in compensation for forward acceleration – though I’m not convinced here.

The cause of the third problem I can’t explain, but the difference in resolution in the accelerometer axes makes it hard for consistent calculations for drift – not good

For me, I’m delighted, as by mapping her path, I’ve found lots of useful facts that previously I’ve just been speculating on.

Further thoughts a few hours later…

The consistent forward acceleration (rather than drift compensation) is due to a bug I fixed a while back that seems to have crept back in  – my bad.

The short term left / right drift compensation seems to just need PID tuning – I think oscillations like this are usually due to an excessive P, so I need to reduce that, and consider adding some I (for “return to where you a started”) and D (for “get a shift on now!”.

The longer term left / right drift’s (lack of) compensation is most interesting: the equidistant dots suggest strongly Phoebe believes she is moving to the right at constant speed and is horizontal; that matches what I saw. Bearing in mind this is sensor data, it’s used as PID feedback, so the Y axis horizontal speed PID should do something about this, yet it doesn’t appear to.  For the moment, I have no idea how this can come about so it’s frustrating – interesting too, but primarily frustrating.

I have no idea where to start about the factor of five scaling difference between the axes; I’ll just have to keep my eye on it during Monday’s testing.  Currently, the only solution is a dirty hack to multiply the left-right earth coordinate acceleration by 5 before integrating.

Finally, I’m still delighted that producing this map works, and provides a new kind of insight into flight problems.

P.S. Anyone else think this looks like an eagle soaring?

Phoebe, stop hovering around and get a move on!

Assuming the next few test flights continue to show the stability of the other day, then it’s time to start including horizontal movement – an enormous elephant in the corner of the room I’ve been avoiding because I’ve convinced myself it’s hard.

At the moment, the code only considers horizontal movement due to the quad propellers; if it tilts from horizontal then the quad z-axis lift provided by the props will then be split between horizontal and vertical like this in the earth X, Y, and Z plane:

eax = faz * sin(pitch)
eay = faz * sin(roll)
eaz = faz * cos(pitch) * cos(roll)

This code is already in use to ensure climb at the right rate even if there is a minor tilt. It can also be used to add horizontal movement.

What is doesn’t, and can’t cover is drift. When Phoebe is hovering perfectly horizontal, but there’s a tail wind blowing her forwards, her Z-axis accelerometer cannot detect this horizontal movement as it’s pointing vertically; it’s down to the X & Y axis accelerometers – also fine as long as there’s no tilt.

But when you have tilt, then vertical acceleration is measured by all three axes, and this is the point I get lost in matrix + vector linear algebra.

The sensors together read values [X, Y, Z] in Phoebe’s coordinate system. Then a 3×3 matrix full of sin() and cos() and pitch angles and roll angles convert [X, Y, Z] (Phoebe’s point of view) to [X’, Y’, Z’] (the rest of the world’s horizontal + vertical coordinate system).

Any help on this would be hugely appreciated!