Can anyone provide me .c code for direction cosine matrix to transform body frame accelerometer measurements into earth frame, and account for gravity.
You need to be a member of diydrones to add comments!
This reply was deleted.
Can anyone provide me .c code for direction cosine matrix to transform body frame accelerometer measurements into earth frame, and account for gravity.
You need to be a member of diydrones to add comments!
Replies
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:
@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
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