Created
September 30, 2015 18:20
-
-
Save iamsurya/673deb812984d5c7afa6 to your computer and use it in GitHub Desktop.
Calculating Linear Acceleration by subtracting gravity
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* Read quaternions */ | |
float q0 = float.Parse(values[3]); | |
float q1 = float.Parse(values[4]); | |
float q2 = float.Parse(values[5]); | |
float q3 = float.Parse(values[6]); | |
/* Read Gyro data */ | |
Gx = float.Parse(values[7]); | |
Gy = float.Parse(values[8]); | |
Gz = float.Parse(values[9]); | |
/* Read Raw Acceleration */ | |
/* Units are (m/s^2) * 0.1 */ | |
ax = float.Parse(values[10]); | |
ay = float.Parse(values[11]); | |
az = float.Parse(values[12]); | |
/* Create Rotation Matrix, Android Code */ | |
/* Math: See alternative expression here: https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix */ | |
/* http://androidxref.com/4.4.3_r1.1/xref/frameworks/native/services/sensorservice/quat.h#30 */ | |
float sq_q1 = 2 * q1 * q1; | |
float sq_q2 = 2 * q2 * q2; | |
float sq_q3 = 2 * q3 * q3; | |
float q1_q2 = 2 * q1 * q2; | |
float q3_q0 = 2 * q3 * q0; | |
float q1_q3 = 2 * q1 * q3; | |
float q2_q0 = 2 * q2 * q0; | |
float q2_q3 = 2 * q2 * q3; | |
float q1_q0 = 2 * q1 * q0; | |
R[0,0] = 1 - sq_q2 - sq_q3; | |
R[0,1] = q1_q2 - q3_q0; | |
R[0,2] = q1_q3 + q2_q0; | |
R[1,0] = q1_q2 + q3_q0; | |
R[1,1] = 1 - sq_q1 - sq_q3; | |
R[1,2] = q2_q3 - q1_q0; | |
R[2,0] = q1_q3 - q2_q0; | |
R[2,1] = q2_q3 + q1_q0; | |
R[2,2] = 1 - sq_q1 - sq_q2; | |
/* Seperating gravity. remember units are (m/s^2) * 0.1 */ | |
/* http://androidxref.com/4.4.3_r1.1/xref/frameworks/native/services/sensorservice/mat.h#203 */ | |
/* http://androidxref.com/4.4.3_r1.1/xref/frameworks/native/services/sensorservice/GravitySensor.cpp#56 */ | |
gx = R[2, 0] ; | |
gy = R[2, 1] ; | |
gz = R[2, 2] ; | |
/* Removing gravity from Raw Signal */ | |
/* Data Port Facing Outside */ | |
ax = -(ax - gx); | |
ay = -(ay - gy); | |
az = -(az - gz); | |
/* Move data around for correct axes orientation in Shimmer Vs the iPhone*/ | |
// If data port pointing away from hand Swap X and Y | |
gx = ax; | |
ax = ay; | |
ay = gx; |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment