Skip to content

Instantly share code, notes, and snippets.

@iamsurya
Created September 30, 2015 18:20
Show Gist options
  • Save iamsurya/673deb812984d5c7afa6 to your computer and use it in GitHub Desktop.
Save iamsurya/673deb812984d5c7afa6 to your computer and use it in GitHub Desktop.
Calculating Linear Acceleration by subtracting gravity
/* 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