Converting angular velocities to orientation Wii Motionplus

I'm working on the Wii Motionplus and I've extracted the raw values using WiimoteLib Library. However, when I normalize it, I get random values that don't tally with what is actually happening.

This is h开发者_Go百科ow I'm normalizing:

1. Calibrate the Motionplus (i.e. Find the raw value that corresponds to zero; I do this by holding it stationary for a point of time)

2. For every subsequent raw value read, I subtract the zero value from it to get the "relative" raw value.

3. Then, I scale this value using http://wiibrew.org/wiki/Wiimote/Extension_Controllers (checking for yaw_fast, pitch_fast etc.), where the numerical values are computed using the measure (raw value of 8192 corresponds to 595 deg/s)

4. I sum up all these values over time (discrete integration) to get the angle of the wiimote wrt initial orientation.

However, when I calculate this and plot it out on a graph, a step change in one of the axes is NOT being reflected in the graph. I tried using a digital compass with it to compare, but while the compass reflects the values correctly, the wii values are completely different (even the pattern is not the same)

Can anyone tell me where I'm going wrong with the normalization?

Thanks!

---

The numbers that are being sent out are rotations about the x y and z axis respective to itself. In order to relate this to x,y,z coordinates you will need to use a rotation matrix, and since the rotation readings are not a fixed axis but depend on what orientation you are at you need to use a Euler Matrix to relate this to a fixed x,y,z coordinate

In other words you are receiving roll, yaw and pitch velocities and you need to use a Euler Matrix to relate this to cartessian coordinates. Once you know your initial roll, pitch and yaw you can simply add your next reading of roll, pitch and yaw to that initial times the time interval that that reading applies to.

ROLL is Rotation about the y-axis

PITCH is Rotation about the x-axis

YAW is Rotation about the z-axis