Here’s the current code used for calculating pitch, role, and yaw angles, which in turn are used to extract gravity from acceleration, and integrate the results to get velocities:

```
#-------------------------------------------------------------------------------------------
# Get angles in radians for Euler and quad frame: rotate the accelerometer readings to earth
# frame, pass them through the butterworth filter, rotate the new gravity back to the quad
# frame, and get the revised angles.
#-------------------------------------------------------------------------------------------
eax, eay, eaz = RotateQ2E(qax, qay, qaz, pa, ra, ya)
egx = bfx.filter(eax)
egy = bfy.filter(eay)
egz = bfz.filter(eaz)
qgx, qgy, qgz = RotateE2Q(egx, egy, egz, pa, ra, ya)
uap, uar = GetRotationAngles(qgx, qgy, qgz)
#-------------------------------------------------------------------------------------------
# Convert the gyro quad-frame rotation rates into the Euler frames rotation rates using the
# revised angles from the Butterworth filter
#-------------------------------------------------------------------------------------------
urp, urr, ury = Body2EulerRates(qry, qrx, qrz, uap, uar)
#-------------------------------------------------------------------------------------------
# Merge rotation frames angles with a complementary filter and fill in the blanks
#-------------------------------------------------------------------------------------------
tau_fraction = tau / (tau + i_time)
pa = tau_fraction * (pa + urp * i_time) + (1 - tau_fraction) * uap
ra = tau_fraction * (ra + urr * i_time) + (1 - tau_fraction) * uar
ya += i_qrz
```

The flaw in this is that it is using the previous set of angles to rotate the latest set of quad-frame accelerometer readings into the earth frame, as shown in red.

The revised code looks like this:

#------------------------------------------------------------------------------------------- # Update the previous pitch, roll and yaw angles with the latest gyro output #------------------------------------------------------------------------------------------- urp, urr, ury = Body2EulerRates(qry, qrx, qrz, pa, ra) pa += urp * i_time ra += urr * i_time ya += ury * i_time #------------------------------------------------------------------------------------------- # Based upon the revised angles, rotate the latest accelerometer readings to earth frame. # Next, run the earth frame acceleration through the Butterworth LPF to extract gravity. # Next, rotate and revise gravity back to the quad frame. #------------------------------------------------------------------------------------------- eax, eay, eaz = RotateQ2E(qax, qay, qaz, pa, ra, ya) egx = bfx.filter(eax) egy = bfy.filter(eay) egz = bfz.filter(eaz) qgx, qgy, qgz = RotateE2Q(egx, egy, egz, pa, ra, ya)

In this case, the angles are updated first, based upon converting the gyro anglular rates into Euler anglular rates, and integrating these onto the existing Euler angles. Only then with the updated angles are the accelerometer readings reoriented to the earth-frame in order to extract gravity.

Although this definitely fixes one problem with the gravity extraction (and hence potential drift), I do have concerns about the revised version: primarily, you’ll notice that the pitch and role angles are never now defined by accelerometer readings, only increments to gyro readings. As a result, you’ll notice the complementary filter has vanished, as the gyros are not prone to noise. However, this may actually be a double edged sword: while there is now a reliance on zero drift in gyro accuracy, the fact the angles now don’t rely on the accelerometer might mean the results are less prone to accelerometer drift. Without testing, it’s not possible to say which side wins.

Worst case, I could re-add the complementary filter, but that introduces it’s own problems with lag. Not sure yet when I have the change to take Phoebe out to test this. The predicted weather for the coming week isn’t great.