MPU6050, фильтр Махони - почему значения крена, тангажа, рыскания постоянно увеличиваются или уменьшаются

Я использую MPU6050-устройство и получаю данные акселерометра и гироскопа.
Затем я обрабатываю эти данные с помощью фильтра Махони в Python.
Проблема в том, что значения крена, тангажа, рыскания постоянно увеличиваются или уменьшаются.

Возможно, есть ошибки в классе фильтра Махони? Может я неправильно вычисляю углы в функции computeAngles()?
Или я должен установить другие параметры фильтра:

DEFAULT_SAMPLE_FREQ = 61.0  # sample frequency in Hz
twoKpDe = (2.0 * 0.5)       # 2 * proportional gain
twoKiDe = (2.0 * 0.1)       # 2 * integral gain

Вот функция расчета углов:

    def computeAngles(self):
        self.roll = math.atan2(2.0 * (self.q0*self.q1 + self.q2*self.q3), 1 - 2.0 * (self.q1*self.q1 - self.q2*self.q2))
        self.pitch = math.asin(2.0 * (self.q0*self.q2 - self.q1*self.q3))
        self.yaw = math.atan2(2.0 * (self.q1*self.q2 + self.q0*self.q3), 1 - 2.0 * (self.q2*self.q2 + self.q3*self.q3))