Как скомпенсировать отклонение угла при вращении гироскопа?

hint000, большое спасибо. Мне нужно было знать что я иду в правильном направлении

Как скомпенсировать отклонение угла при вращении гироскопа?

Это первый подобный проект, поэтому сравнивать не с чем. Специалистов, к которым можно обратиться нет.

Задача.
Прежде всего нужен немагнитный компас, который инициализируется вначале и держит определенное направление на протяжении некоторого времени практически в любых положениях (наклон в 90 +-5 градусов не рассматривается). Пытался использовать алгоритмы Махони и Маджвика, Калман оказался слишком тяжел в реализации и настройке.
Проблема.
Алгоритмы Маджвика и Махони ведут себя примерно одинаково, а именно, при вертикальном повороте устройства происходит смещение курса, в зависимости от оси поворота.
График зависимости отклонения по оси Z в зависимости от угла наклона

1. Зеленый – поворот вокруг оси Y
2. Желтый – поворот вокруг оси Y + 20 градусов
3. Серый – поворот вокруг оси Y + 40 градусов
…
4. Голубой – поворот вокруг оси X

Используемый вариант кода алгоритма Маджвика.
Сейчас код обновления кватерниона такой:
class TMadgwickFilter …
void __fastcall Update(TVector3_t G, TVector3_t A)
{
A.Norm();
TQuaternion_t S(
-2.0f * (FQ.Y * A.X - FQ.X * A.Y - FQ.W * A.Z),
2.0f * (FQ.Z * A.X + FQ.W * A.Y - FQ.X * A.Z),
-2.0f * (FQ.W * A.X - FQ.Z * A.Y + FQ.Y * A.Z),
2.0f * (FQ.X * A.X + FQ.Y * A.Y + FQ.Z * A.Z));
S.Norm();
S = S * FBeta;

TQuaternion_t QDot(
-FQ.X * G.X - FQ.Y * G.Y - FQ.Z * G.Z,
FQ.W * G.X + FQ.Y * G.Z - FQ.Z * G.Y,
FQ.W * G.Y - FQ.X * G.Z + FQ.Z * G.X,
FQ.W * G.Z + FQ.X * G.Y - FQ.Y * G.X);
QDot = QDot * 0.5f - S;
FQ = FQ + QDot * Fdt;
FQ.Norm();
}
Углы Эйлера извлекаю так:
TVector3_t __fastcall GetEiler()
{
return TVector3_t(
atan2(2. * (W * X + Y * Z), 1. - 2. * (X * X + Y * Y)),
asin(2. * (W * Y - Z * X)),
atan2(2. * (W * Z + X * Y), 1. - 2. * (Y * Y + Z * Z)));
}
Это основные фрагменты кода.
В остальном фильтр устраивает, т.к. в вертикальных положениях данные возвращаются в исходное состояние стабильно при любых поворотах.
Я так понял, что отклонение связано с физикой или с особенностями алгоритма Маджвика. Отсюда и возник вопрос, возможно ли как-то, относительно точно, скомпенсировать изменение курса при наклонах или я хочу невозможного. Возможно есть другие пути решения задачи.
Датчик.
Используемый датчик DMU11.