Алгоритм слияния датчиков Madgwick на iOS

Question

Я пытаюсь запустить Madgwick's sensor fusion algorithm на iOS. Поскольку код является открытым исходным кодом, я уже включил его в свой проект и вызываю методы с предоставленными значениями датчиков.

Но, похоже, что алгоритм ожидает измерения датчика в другой системе координат. На правой стороне находится сенсорная система Apple CoreMotion, слева - Madgewick. Вот изображение different coordinate systems. Обе системы следуют правилу правой руки. Для меня это похоже на поворот на 90 градусов вокруг оси z. Но это не сработало.

Я также попытался перевернуть х и у (и инвертировать г) ось, как было предложено other stackoverflow posts для WP, но это не работает также. Так у вас есть намек? Было бы идеально, если выход Alogithm Madgwick мог быть в той же системе, что и выход CoreMotion (CMAttitudeReferenceFrameXMagneticNorthZVertical).

Кроме того, я ищу хорошее рабочее значение для betaDef на iPhone. betaDef - вид пропорционального усиления и в настоящее время установлен на 0,1f.

Любая помощь по достижению цели будет оценена по достоинству.

Answer 1

Я не уверен, как записать это в объективе c, но вот как я совершил преобразования координат в ваниле c. Я также хотел повернуть ориентацию так, чтобы + y был на север. Этот перевод также отражен ниже.

Этот метод ожидает 4 элемента кватернион в виде WXYZ, и возвращает переведенный кватернион в том же формате:

void madgeq_to_openglq(float *fMadgQ, float *fRetQ) { 
    float fTmpQ[4]; 
    // Rotate around Z-axis, 90 degres: 
    float fXYRotationQ[4] = { sqrt(0.5), 0, 0, -1.0*sqrt(0.5) }; 

    // Inverse the rotation vectors to accomodate handedness-issues: 
    fTmpQ[0] = fMadgQ[0]; 
    fTmpQ[1] = fMadgQ[1] * -1.0f; 
    fTmpQ[2] = fMadgQ[2]; 
    fTmpQ[3] = fMadgQ[3] * -1.0f; 

    // And then store the translated Rotation into ret: 
    quatMult((float *) &fTmpQ, (float *) &fXYRotationQ, fRetQ); 
} 

// Quaternion Multiplication operator. Expects its 4-element arrays in wxyz order 
void quatMult(float *a, float *b, float *ret) { 
    ret[0] = (b[0] * a[0]) - (b[1] * a[1]) - (b[2] * a[2]) - (b[3] * a[3]); 
    ret[1] = (b[0] * a[1]) + (b[1] * a[0]) + (b[2] * a[3]) - (b[3] * a[2]); 
    ret[2] = (b[0] * a[2]) + (b[2] * a[0]) + (b[3] * a[1]) - (b[1] * a[3]); 
    ret[3] = (b[0] * a[3]) + (b[3] * a[0]) + (b[1] * a[2]) - (b[2] * a[1]); 

    return; 
} 

Надежда, что помогает!