Correcting for gravity in minimu-9-ahrs

Hi all, I’m using the following code as the basis for my AHRS:

github.com/pololu/minimu-9-ahrs-arduino

Is there a simple modification that would get me gravity-corrected acceleration values? It seems like I should create a gravity vector when the device is turned on, then multiply the it by the current DCM matrix and subtract it from the acceleration vector. That doesn’t seem to work, though.

I’m struggling with gravity adjustment using the software at github.com/pololu/minimu-9-ahrs-arduino.

When the callibration function runs, I set the gravity vector to {0, 0, GRAVITY}. Then I tried using the approach from this post; specifically, based on the corrected code in comment #9.

forum.arduino.cc/index.php?topic=267280.0

The rotated vector always has the correct amplitude, 1*G, and the z-component of the adjusted gravity vector seems just fine, but the x- and y- components don’t make much sense. I’m wondering if the rotation matrix in this code is built on different conventions than the code mentioned in the Arduino forum post? Any thoughts?

Here are my relevant code snippets:

void Vector_by_Matrix(float a[3], float b[3][3], float v[3]) { float op[3]; for(int i=0; i<3; i++) { for(int j=0; j<3; j++) { op[j]=a[j]*b[i][j]; } v[i]=op[0]+op[1]+op[2]; } }

~

float rG[3]; Vector_by_Matrix(G,DCM_Matrix,rG); trueAcc[0] = accel_x - rG[0]; trueAcc[1] = accel_y - rG[1]; trueAcc[2] = accel_z - rG[2];

Hello.

Can you post all your code and a log of the output that it generates, and what you expect to get?

-Jon

Okay, I actually managed to figure this out myself. It turns out the DCM matrix was transposed from what I was expecting it to be, so by vector by matrix function should have had this line instead:

//op[j]=a[j]*b[i][j];
op[j]=a[j]*b[j][i];

Rotating the gravity vector now works correctly so long as the board starts in the orientation expected by the code; i.e. with the z-axis pointed down. I haven’t yet figured out how to make the program detect the initial orientation automatically.

I am glad you figured that part out. You might consider taking a look at this forum post by Kevin, which indicates which lines in the AHRS code to comment to prevent the MinIMU-9 from assuming it starts in a level orientation.

-Jon