Phoebe’s metaphysical weight loss.

Ever since things took a turning point a few months ago, and every change I made had a positive impact on Phoebe’s flights, I’ve been very aware that the code was shrinking.

Her largest point was when I added the rotation matrix, and that definitely was the turning point.  Some stats on the number of lines of code, comments and spaces

Beard   2420 code   425 comments  337 blank lines
Now     1931 code   314 comments  279 blank lines

No significant conclusions to be drawn, other than code’s much simpler when you really understand what you’re trying to code!  KISS principle.

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.

What’s tilt for?

Tilt is the angle between the Phoebe-frame Z-axis, and Earth-frame ‘vertical’.  It drops out of Euler calculations.  It seems the ideal way to work out the conversion from the earth-frame vertical velocity target to the quad frame vertical velocity target thus:

qvz = evz / cos(tilt)

i.e. if the quad is tilted then the power applied to all four motors is increased to maintain the correct earth axis vertical speed.  Seems obvious doesn’t it?

Yet up to now, I’ve been using the rotation matrix to take Earth-frame vertical velocity target to the quad-frame.  Here’s what that looks like:

qvz = evx * (c_ra * s_pa * c_ya + s_ra * s_ya) + evy * (c_ra * s_pa * s_ya - s_ra * c_ya) + evz * c_pa * c_ra

where

• c_pa = cos(pitch angle)
• s_pa = sin(pitch angle)
• c_ra = cos(roll angle)
• s_ra = sin(roll angle)
• c_ya = cos(yaw angle)
• s_ya = sin(yaw angle)

I’m struggling to believe these two very different equations produce the same results.  So I ran a test.  Without the motors running I picked Phoebe up and rotated her around her X and Y axes producing a broad range of tilt:

Euler vs. Beard

• green  is the vertical velocity target in the Earth-frame
• yellow is using the Euler tilt angle to transform the earth target to quad-frame target
• blue uses beard to do the same.

The flight plan is rise for 3s, hover for 5s and descend for 4.5s – the rest of the time is spent softening the transitions between these targets.

Anyway, back to the plot.  While the target is 0, the tilt has no effect; that happens only for non-zero targets as you can see on the graph.  I’m sure you’ve also noticed the Euler tilt and Beard matrix results don’t match.  Similar in shape but inverted.

Euler looks right to me; as Phoebe tilts, Euler ups the quad-target to ensure the earth-target remains as set.  Beard seems to be doing the exact opposite.  I need to understand this discrepancy as I’m relying on Beard for horizontal velocity too where Euler can’t help.

Any thoughts?  Probably an axis error in my implementation of Beard I guess?

I tried a test flight using Euler tilt angles rather than Beard’s matrix for conversion of the vertical velocity PID target in Phoene’s frame.  Phoebe climbed higher, but seemed less stable.  Then went back to Beard, and yet another near perfect flight.  So Beard wins even though Euler’s tilt seems more logical to me!

I ψ with my little eye…

something beginning with Y.  See Beard for why I’ve referred to psi.

Sometimes I amaze myself with how stupid I can be.  I’ve been ignoring yaw for a long time because:

• a quad shouldn’t yaw due to its prop rotations
• if it does, there’s nowt can be done to stop it.

But from another long chat on the Raspberry Pi forum, the completely obvious point finally became obvious to me: although yaw can’t be prevented, it still needs to be accounted for in drift correction.  The fix for forwards drift plus yaw is almost certainly not reverse acceleration, and in the extreme case, with say 180° yaw, it may be forward acceleration needed probably along with some left or right also.

I’ve wasted months on drift control which couldn’t possibly work without including yaw.

For the moment, I’ll just cross my fingers and use the integrated gyro z axis, but I now definitely need a compass for longer flights.

Moron reference frames

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.!

Footnotes:

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!

Reference Frames and Rotation Matrices

As background, here’s my current matrix which is flawed

|eax| = | cos(pitch), sin(roll), -sin(pitch)         | |qax|
|eay| = | sin(pitch), cos(roll), -sin(roll)          | |qay|
|eaz| = | sin(pitch), sin(roll), cos(pitch).cos(roll)| |qaz|

To make sense of what’s below, please go and read this PDF, particularly section 1.2 describing the frames of reference of a quadcopter.  As a quick summary…

• Inertial frame  (ƒi) – this doesn’t move – it’s axes (in Phoebe’s case) are North, West and Down (gravity). This then converts to the…
• Vehicle Frame (ƒv) – the axes remain the same as above (North, West and Down, so the quad could be skew against this axis, but centred on the quad center of gravity. This then converts via yaw angle psi (ψ) to the…
• Vehicle 1 Frame (ƒv1) – this aligns the frame with the quad’s fore-aft pitch axis. This then converts via pitch angle theta (θ) to the…
• Vehicle 2 Frame (ƒv2) – this aligns the frame with the quad’s left-right roll axis. This then converts via roll angle phi (φ)  to the…
• Body Frame (ƒb)- the frame whose axis align with the quad’s sensors.

I don’t have a compass (magnetometer) sensor, so will not be using the Inertial Frame as my starting point.

In addition, yaw measurement should really use the compass as the other option of integrating the gyro Z axis is only accurate short term. I might try it though when I’m next stuck for something to do.

So I’ll be starting from Vehicle Frame 1 but may, once this is working, add yaw and start from Vehicle Frame.

Here’s the matrix pretty much copied straight from Beard.

| 1    0     0    | | cos θ  0 -sin θ | | cos ψ  sin ψ  0 |
| 0  cos φ  sin φ | |   0    1    0   | |-sin ψ  cos ψ  0 |
| 0 -sin φ  cos φ | | sin θ  0  cos θ | |   0      0    1 |

If we then throw away yaw (ψ = 0), and collapse we get

| 1    0     0    | | cos θ  0 -sin θ | | 1  0  0 |
| 0  cos φ  sin φ | |   0    1    0   | | 0  1  0 |
| 0 -sin φ  cos φ | | sin θ  0  cos θ | | 0  0  1 |

Multiply these three together gives:

|    cos θ          0        -sin θ     |
| sin φ * sin θ   cos φ   sin φ * cos θ |
| cos φ * sin θ  -sin φ   cos φ * cos θ |

There’s a passing resemblance to my flawed matrix up at the top which gives me a level of confidence I hadn’t completely cocked it up, and that the updated matrix won’t have a disastrous effect on the flight, but should make them much more accurate flights.

I’ve updated the code and will test fly it tomorrow.