Madgwick sensor fusion algorithm on iOS
I am trying to run Madgwick's sensor fusion algorithm on iOS. Since the code is open source, I have already included it in my project and call methods with the provided sensor values.
But it looks like the algorithm is expecting the sensor to measure in a different coordinate system. On the right side is the Apple CoreMotion sensor system, on the left is Madgewick. Here is a picture of the different coordinate systems . Both systems follow the right-hand rule. To me it is like rotating 90 degrees around the z-axis. But it didn't work.
I also tried flipping the x and y axes (and inverting the z) as suggested by this stackoverflow question for WP, but that didn't work either. So do you have a hint? It would be ideal if the output of Alogithm Madgwick could be on the same system as the output of CoreMotion (CMAttitudeReferenceFrameXMagneticNorthZVertical).
Also, I'm looking for a good working value for betaDef on an iPhone. betaDef is the proportional gain and is currently set to 0.1f.
Any help in achieving the goal will be appreciated.
I'm not sure how to write this in lens c, but this is how I accomplished coordinate transformations in vanilla c. I also wanted to rotate the orientation so that + y is north. This translation is also reflected below.
This method expects a 4-element quaternion as wxyz and returns the translated quaternion in the same format:
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;
}
Hope it helps!