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.