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.