How do I account for gravity using a wiimote's accelerometer?

Question

For a project my team and I have been trying to track a wiimote in a 3D space using the built in accelerometer and the WiiMotion Plus gyroscope.

We’ve been able to track the rotation and position using an ODE (Found at http://www.alglib.net/,) but we’ve run into a problem with removing the gravity component from the accelerometer.

We looked at Accelerometer gravity components which had the formula (implemented in 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;
    }

But it doesn’t work at all. As a reference, the acceleration is straight from the accelerometer, and the rotation is measured in radians after it has been worked through the ODE.

Also, We are having problems with understanding how this formula works. Due to the fact that our tracking is taking into account all dimensions, why is Yaw not taken into account?

Thanks in advance for any advice or help that is offered.

EDIT:

After discussing it with my teammates and boss, we've come to find that this formula would actually work if we were using X, Y, and Z correctly. We've come to another stump though.

The problem that we're having is that the Wiimote library that we're using returns relative rotational values based on the gyroscope movement. In otherwords, if the buttons are facing up, rotating the wiimote left and right is yaw and if the buttons are facing toward you, yaw is the same when it SHOULD be the rotation of the entire wiimote.

We've found that Euler angles may be our answer, but we're unsure how to use them appropriately. If there is any input on this new development or any other suggestions please give them.

Answer 1

I'd bet that your accelerometer was not calibrated in zero gravity, so removing the effect of gravity will be difficult, at the very least.

Answer 2

First I'd suggest not using individual components to store the rotation (gimbal lock), a matrix would work better. calibrate by holding it still and measuring (it will be 1g downward). then for each rotation, multiple the rotation matrix by it. then you can tell which way is up and subtract a matrix of 1g down from the vector representing the acceleration. I know that doesn't make a lot of sense but I'm in a bit of a rush, add comments if you have questions.