Библиотека Troyka для фильтра Маджвика

[wunderwaffe]

Таки запустил ее - работает вполне прилично для 6-ти степеней свободы, только у них ошибки в переводе кватерниона в углы Эйлера. Может с направлениями осей датчиков перепутано? Нет, к меня же ИХ плата со всеми датчиками на ней. Заменил только следующее -

Вместо

float Madgwick::getYawRad() {
    return atan2(2 * _q1 * _q2 - 2 * _q0 * _q3,
                 2 * _q0 * _q0 + 2 * _q1 * _q1 - 1);
}

float Madgwick::getPitchRad() {
    return atan2(2 * _q2 * _q3 - 2 * _q0 * _q1,
                 2 * _q0 * _q0 + 2 * _q3 * _q3 - 1);
}

float Madgwick::getRollRad() {
    return -1
           * atan2(2.0f * (_q0 * _q2 - _q1 * _q3),
                   1.0f - 2.0f * (_q2 * _q2 + _q1 * _q1));
}

надо

float Madgwick::getYawRad() {
    return atan2(2 * _q1 * _q2 - 2 * _q0 * _q3,
                 2 * _q0 * _q0 + 2 * _q1 * _q1 - 1);
}

float Madgwick::getPitchRad() {
    return asin(-2 * _q1 * _q3 + 2 * _q2 * _q0);
}

float Madgwick::getRollRad() {
    return -1
           * atan2(2.0f * (_q2 * _q3 + _q1 * _q0),
                   _q0*_q0-_q1*_q1-_q2*_q2+_q3*_q3);
}

Ну и еще у них старая ошибка с магнитометром.