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

В рамках проекта мы с моей командой пытались отследить Wiimote в трехмерном пространстве с помощью встроенного акселерометра и гироскопа WiiMotion Plus.

Мы нам удалось отследить вращение и положение с помощью ODE (найдено на http://www.alglib.net/ ), но мы столкнулись с проблемой при удалении гравитационной составляющей из акселерометра. .

Мы рассмотрели Гравитационные компоненты акселерометра , для которых использовалась формула (реализованная в C # / XNA)

    private Vector3 RemoveGravityFactor(Vector3 accel)
    {
        float g = -1f;
        float pitchAngle = (Rotation.Z);
        float rollAngle = (Rotation.Y);
        float yawAngle = (Rotation.X);

        float x = (float)(g * Math.Sin(pitchAngle));
        float y = (float)(-g * Math.Cos(pitchAngle) * Math.Sin(rollAngle));
        float z = (float)(-g * Math.Cos(pitchAngle) * Math.Cos(rollAngle));

        Vector3 offset = new Vector3(x, y, z);

        accel = accel - offset;
        return accel;
    }

Но она вообще не работает. Для справки: ускорение напрямую от акселерометра, а вращение измеряется в радианах после прохождения через ODE.

Кроме того, у нас возникли проблемы с пониманием того, как работает эта формула. В связи с тем, что наше отслеживание учитывает все измерения, почему рыскание не принимается во внимание?

Заранее благодарим за любой совет или предложенную помощь.

РЕДАКТИРОВАТЬ:

После обсуждения этого с моими товарищи по команде и босс, мы пришли к выводу, что эта формула действительно работала бы, если бы мы правильно использовали X, Y и Z. Но мы подошли к другому пню.

Проблема в том, что библиотека Wiimote, которую мы re using возвращает значения относительного вращения, основанные на движении гироскопа. Другими словами, если кнопки обращены вверх, поворот wiimote влево и вправо - это рыскание, а если кнопки обращены к вам, рыскание будет таким же, когда ДОЛЖНО быть поворотом всего wiimote.

Мы обнаружили, что Углы Эйлера могут быть нашим ответом, но мы не уверены, как их правильно использовать. Если есть какие-либо отзывы об этой новой разработке или какие-либо другие предложения, пожалуйста, дайте их.