Объединить показания гироскопа и акселерометра независимо от вращения?

Итак, мой Arduino подключен к 6-осевой гироскоп + акселерометр MPY-6050 GY-521. У меня есть это отправка последовательных данных в программу на моем компьютере, все работает отлично. Я задам этот вопрос на C ++, хотя моя программа написана на DBPro.

В настоящее время я использую дополнительный фильтр для комбинирования гироскопических скоростей углового вращения с вектором силы тяжести акселерометра для стабильных, но чувствительных вращений. Вот мой текущий код:

angleX=0.98*(angleX+(gyroX/16.4)*timeMultiplier)+0.02*atanfull(accelY,accelZ)
angleY=0.98*(angleY+(gyroY/16.4)*timeMultiplier)+0.02*atanfull(accelZ,accelX)
angleZ=0.98*(angleZ+(gyroZ/16.4)*timeMultiplier)+0.02*atanfull(accelX,accelZ)

angleX/Y/Z а также timeMultiplier являются поплавками. timeMultiplier в основном (millis()-lastMillis)/1000.0, atanfull в основном atan2 но вывод в градусах (0-360), а не в радианах (я думаю, что это единственная разница).

Проблема в том, что когда доска лежит на столе ровно, акселерометр, очевидно, не может измерить повороты вокруг оси Y (вверх), но гироскоп может. Когда я поворачиваю доску вокруг Y, трехмерное представление куба, которое я рендую в течение секунды, затем возвращается к 0. Это потому, что когда доска находится в вертикальном положении (ось Y направлена ​​вверх), atanfull приводит к 0.

То, что я искал в интернете (но на самом деле не знаю, что искать), — это способ уменьшить влияние данных акселерометра, чем меньше они используются. Так как ось X, скажем, вращается так, что она перпендикулярна земле (и становится бесполезной для измерений), гироскопу разрешается все больше и больше контроля, пока ось X не станет полностью перпендикулярной, гироскоп не заметит акселерометр для эта ось и полностью опирается на гироскоп.

TL; DRЯ пытаюсь найти способ объединить показания гироскопа и акселерометра независимо от вращения. То есть, во всех реализациях дополнительных фильтров gyro + acl я могу находить, что управление тоном / поворотом отличается от рыскания, и я не хочу этого делать. Я хочу иметь возможность поворачивать доску в любом направлении и при этом получать хорошие показания.

Я действительно понятия не имею, что я должен искать, чтобы узнать, как это сделать, и был бы признателен за любую помощь 🙂