Can anyone provide me .c code for direction cosine matrix to transform body frame accelerometer measurements into earth frame, and account for gravity.
Tags:
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
235 members
220 members
3123 members
1441 members
1036 members
© 2017 Created by Chris Anderson. Powered by