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.