Как использовать кватернион Мэгвик, чтобы повернуть вектор 3D ускорения в Android?

Question

Я пытался повернуть трехмерный вектор ускорения с помощью алгоритма Мэджвика без особого успеха.

Я использую реализацию Madgwick Java из здесь , что Примерно так же, как и оригинальная C#, и следуя этой бумаге (страница 6 из pdf), я попытался повернуть вектор ускорения, но мне не повезло.

заявляет, что «единичные кватернионы могут применяться для управления вращениями трехмерных векторов» и действует по формуле

^B v _q = ^B _A q⊗ ^A v _д ⊗ ^B _A д ^*

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

Код умножения кватернионов:

   public float[] quaternionMultiplication(float[] a, float[] b) {
    float[] quaternion = new float[4];
    quaternion[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3]; //W
    quaternion[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2]; //X
    quaternion[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1]; //Y
    quaternion[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0]; //Z
    return quaternion;
}

и код сопряжения кватернионов:

    public float[] quaternionConjugate(float[] a){
    float[] quaternionConjugate = new float[4];
    quaternionConjugate[0] = a[0];
    quaternionConjugate[1] = -a[1];
    quaternionConjugate[2] = -a[2];
    quaternionConjugate[3] = -a[3];

    return quaternionConjugate;
}

Использование данных, предоставленных TYPE_ACCELEROMETER , TYPE_GYROSCOPE и TYPE_MAGNETIC_FIELD из Android Я вызываю метод update класса MadgwickAHRS, который создает кватернион после всей фильтрации, и с помощью .getQuaternion я копирую значения этого значения в локальную переменную [] Q.

Использование массива с плавающей точкой Q Я пытаюсь вычислить вращение, следуя приведенной выше статье. Код для последней части:

        mMadgwickAHRS.update(gyroscopeArray[0], gyroscopeArray[1], gyroscopeArray[2],
                accArray[0], accArray[1], accArray[2],
                geomagneticArray[0], geomagneticArray[1], geomagneticArray[2]);

            float[] Q = new float[4];
            Q[0] = mMadgwickAHRS.getQuaternion()[0]; //W
            Q[1] = mMadgwickAHRS.getQuaternion()[1]; //X
            Q[2] = mMadgwickAHRS.getQuaternion()[2]; //Y
            Q[3] = mMadgwickAHRS.getQuaternion()[3]; //Z

            float[] quaternionMultiplication = quaternionMultiplication(Q,accelerationArray);

            float[] invertedOriginalQuaternion = quaternionConjugate(Q);

            float[] resultVector = quaternionMultiplication(quaternionMultiplication,invertedOriginalQuaternion);

Теперь float[] resultVector должен иметь вектор ускорения по отношению к фрейму мира, однако это не так. Если телефон плоский и я перемещаю его вперед / назад, в стороны и вверх / вниз, все работает нормально (я создаю график из значений XYZ, чтобы проверить результаты), но если я подниму телефон и поверну его в некоторых Кстати, XYZ вектора ускорения по-прежнему дает результаты на основе фрейма устройства, а не фрейма мира.

Вместо этого он должен выполнять своего рода «блокировку» (из-за отсутствия лучшего термина) Ось XYZ и независимо от ориентации телефона, если я перемещу ее вперед / назад Ось Z должна изменить свои значения, если я переместлю ее вверх / вниз, ось Y должна изменить свои значения и если я переместу ее в сторону Ось X.

В качестве последнего замечания я не могу использовать TYPE_ROTATION_VECTOR. Несмотря на то, что это прекрасно работает и с его помощью я смог сделать именно то, что хотел, мне нужно предоставить данные 9-Axis от внешнего датчика, поэтому мне нужно решение, которое позволяет мне предоставлять данные оси вместо использования внутренние (это то, что делает TYPE_ROTATION_VECTOR).