Building “The Matrix”

Sadly not the Keanu Reeves version – I think we’d need an spherical cluster of Raspberry Pi’s much larger than the Sun to build an earth Matrix, and once you’ve got that many, you may as well let gravity take its course and let it collapse into a real solar system akin to Deep Thought 2.

Anyway, the matrix in question is the one that can convert from the quadcopter axes accelerometer outputs to the earth coordinate system. Currently it looks a bit like this:

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

The matrix is only converting data from Phoebe’s Z axis into data for the Earth’s X, Y and Z axes.

The empty 2 columns add Phoebe’s X and Y accelerometers to the equation.  Filling in the rest of the Matrix is a case of drawing a lot of triangles.

The triangle for the X accelerometer maps from Xq to Xe and Ze suggesting

Ze = Xqsin(pitch)
Xe = Xqcos(pitch)

Adding these couple of entries to the Matrix:

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

Doing the same for the Y axes with roll angles yields

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

Currently, there’s a high probability this is wrong, as although it looks right to me and my aged math(s), it doesn’t ‘feel’ right – my brain needs a night’s sleep to mull it over. And for the moment, that’s fine – the code is running pretty well on just that third column of the matrix, and tomorrow is looking like a good day for testing again, so once I’ve retested the hover to build confidence and hopefully shoot a video, I might try some horizontal movement looking for errors that the additional columns in the matrix would fix.

9 thoughts on “Building “The Matrix”

  1. Your last matrix has two zeros in it – these mean that Xe doesn’t depend on Yq, and Ye doesn’t depend on Xq. The means that if Phoebe yaws, you won’t pick this up from the accelerometer data. Of course, you have a gyro to handle that – and random yawing from external factors is quite unlikely.

    (Alternatively, it means you’re happy for X-axis and Y-axis of Earth to always point in the same direction as Phoebe’s… which won’t be good if you want to send commands like “go 15m west”)

    As a quick sanity check, when pitch=roll=0, then you end up with:
    |Xe| = |1, 0, 0| |Xq|
    |Ye| = |0, 1, 0| |Yq|
    |Ze| = |0, 0, 1*1| |Zq|
    … which is what you want. Further, some waving of my hands suggests that the first row and first column must have no roll terms, and the second row and second column must have no pitch terms. (… because roll doesn’t affect Xe or Xq, etc.)

    If you want to add in yaw, then you end up with:

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

    (I’m assuming pitch = rotation in x.z plane; yaw = rotation in x.y plane; roll = rotation in y.z plane).

      • Comment formatting drives me nuts – WordPress is really good for formatting posts, but doesn’t open up that formatting to comments – but your Matrix changes were very clear regardless, thanks.

    • Thanks Tom,

      That’s hugely helpful, both in confirming my Matrix is fine as far as it goes, and what the 2 blank holes do (and the other changes required for yaw).

      Yaw is a bit tricky – it shouldn’t happen except for external factors due to the orientation of the propellers – diagonally opposite props rotate in the same direction. I do use the Zq gyro to measure this and do have code to compensate, although it’s not great due to the prop layout which makes correction hard. I’m fine for the moment that the quad’s X and Y axes move wrt to the earth as a result.

      If I ever want to add commands like ‘move 15 degrees west’ then a compass (for static orientation) or GPS (for direction of movement) can be added. Movement in an arc could fix the direction of travel without requiring yaw – just some forward velocity and some orthogonal centripetal force – neither of which are hard (although centripetal force – a fixed angular tilt – isn’t coded yet as an input to the PIDs as it’ll be easier / safer to test by remote control rather than autonomously).

      For the moment though, I’ll leave yaw control in it’s simply form of keeping it at 0; soon, I’ll need to add a remote control, so the user can fix problems in flight.

      And in future, when I want to add autonomous route tracking, I know how to update my matrix to include the yaw accurately.

      Hugely appreciated,


      • I’ve had a few minutes to read at least the first few pages of the paper; it has a much harder problem to solve which is stability of an autonomous plane. thrust is horizontal, vertical acceleration comes from pitch plus horizontal flight speed and angle and the wings of course, pitching comes form ailerons, yaw is imposed by the props, and must be countermanded by the ailerons; the whole thing has so many more subtle interactions than a quad with just 4 vertical props, so although it makes a fascinating read, I suspect I can stop letting it scare the heebie jeebies out of me!

    • Not so sure about the demystify, more like help my head implode, but at least if I’m suffering from insomnia, I now have a cure.

      I’m going to have to do an awful lot of rereading to even scratch the surface of what this is covering.

      Thanks for pointing it out though, I’m sure it’ll make sense to me eventually..


      • I was a very junior member of the group that developed the gentlenav system 5 years ago, that you refer to above. The matrix approach was such a major breakthrough in control that other groups ported the matrix code into their systems. Having flown both model helicopters and planes, in my opinion there many similarities between the two if you want to eventually do more that the simplest, slow flights. There may be good reasons why the matrix cannot be ported to the Pi, but there is lots of info about it on and . The PIC processors used offered matix calculations in the hardware.
        Best wishes for your Pi project.

Leave a Reply

Your email address will not be published. Required fields are marked *