Can anyone provide me .c code for direction cosine matrix to transform body frame accelerometer measurements into earth frame, and account for gravity.

Views: 2543

Reply to This

Replies to This Discussion

basicly convert your imu readings to quaternion  then

gx, gy, gz represents gravity in x, y and z axis respectively

gx = 2 * (q1 * q3 - q0 * q2);
gy = 2 * (q0 * q1 + q2 * q3);
gz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

the result of gx gy and gz will be unit vector

then conver all your accelerometer results to g ex: assume your accelerometer reads z = 256 x = 0 y = 0  when its leveled so accel_z_scaled = 256/256 = 1g,  accel_x_scaled = 0/256 = 0g, accel_y_scaled= 0/256 = 0g  

acc_x_wog = (accel_x_scaled - gx) * 9.8;

acc_y_wog = (accel_y_scaled - gy) * 9.8;
acc_z_wog = (accel_z_scaled - gz) * 9.8;

will give you linear acceleration in each axis even in any attitude

note: wog means: w/o gravity

and sorry this is not in cosine matrix, I only know in quaternion

@Baris can you clarify  
is q0 q1 q2 q3 is w x y z of quat or x y z w. 

my data is like this

Sample acceleration data is
-0.92 0.18 10.15
-2.14 0.83 10.07
-1.05 0.23 10.14 

And sample quat data is (Not normalized)
16363 201 754 -300
16363 201 754 -273
16364 201 753 -234

what do mean by accel_x_scaled? And what should i do in my case

can you clarify  
is q0 q1 q2 q3 is w x y z of quat or x y z w. 

my data is like this

Sample acceleration data is
-0.92 0.18 10.15
-2.14 0.83 10.07
-1.05 0.23 10.14 

And sample quat data is (Not normalized)
16363 201 754 -300
16363 201 754 -273
16364 201 753 -234

what do mean by accel_x_scaled? And what should i do in my case



Baris said:

basicly convert your imu readings to quaternion  then

gx, gy, gz represents gravity in x, y and z axis respectively

gx = 2 * (q1 * q3 - q0 * q2);
gy = 2 * (q0 * q1 + q2 * q3);
gz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

the result of gx gy and gz will be unit vector

then conver all your accelerometer results to g ex: assume your accelerometer reads z = 256 x = 0 y = 0  when its leveled so accel_z_scaled = 256/256 = 1g,  accel_x_scaled = 0/256 = 0g, accel_y_scaled= 0/256 = 0g  

acc_x_wog = (accel_x_scaled - gx) * 9.8;

acc_y_wog = (accel_y_scaled - gy) * 9.8;
acc_z_wog = (accel_z_scaled - gz) * 9.8;

will give you linear acceleration in each axis even in any attitude

note: wog means: w/o gravity

and sorry this is not in cosine matrix, I only know in quaternion

Reply to Discussion

RSS

© 2017   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service