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

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

0 голосов
/ 19 февраля 2020

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

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

    DEFAULT_SAMPLE_FREQ = 61.0  # sample frequency in Hz
    twoKpDe = (2.0 * 0.5)   # 2 * proportional gain
    twoKiDe = (2.0 * 0.9)   # 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))
        self.anglesComputed = 1
...