Get vertical acceleration from rotation vector and accelerometer values

My goal here is to calculate the acceleration towards the ground, which I call "vertical acceleration". This seems to be working OK for the most part, except when there is a lot of rotation going on in many axis at the same time (see graphs below).

What I am most unsure about here is the math for calculating the vertical acceleration - more specifically, the implementation of matrixMult3x9() and rotationMatrixTranspose() and getVerticalAcceleration().

I would appreciate if someone who knows their way around coordinate systems and matrixes could verify that my implementations look correct. Then I can at least rule out errors in the calculations, and assume that any inaccurate results probably come from the off-center placement of the sensors and/or noise/lag.

Also, I'm wondering what the acceleration in the other two dimensions of accelerationInWorldFrame are relative to.

private float[] rotationVector = {0,0,0};
private float[] accelerometerValues = {0,0,0};
private double verticalAcceleration = 0;

@Override
public void onSensorChanged(SensorEvent event) {
if( event.sensor.getType() == ROTATION_SENSOR_TYPE ) {
rotationVector = lowPass(lowPassAlpha, event.values, rotationVector);
}
else if( event.sensor.getType() == Sensor.TYPE_ACCELEROMETER ) {
accelerometerValues = lowPass(lowPassAlpha, event.values, accelerometerValues);
verticalAcceleration = getVerticalAcceleration(); // Account for rotations
}
}

private double getVerticalAcceleration(){
// Based on http://stackoverflow.com/questions/31268852/measuring-vertical-movement-of-non-fixed-android-device
float[] rotationMatrix = new float[9]; // Both 9 and 16 works, depending on what you're doing with it
SensorManager.getRotationMatrixFromVector(rotationMatrix, rotationVector);

float[] accelerationInWorldFrame = matrixMult3x9(
accelerometerValues,
rotationMatrixTranspose(rotationMatrix)
);

return accelerationInWorldFrame[2];
}

private float[] matrixMult3x9(float[] a, float[] b){
float[] result = new float[3];
result[0] = a[0]*b[0] + a[1]*b[3] + a[2]*b[6];
result[1] = a[0]*b[1] + a[1]*b[4] + a[2]*b[7];
result[2] = a[0]*b[2] + a[1]*b[5] + a[2]*b[8];
return result;
}

private float[] rotationMatrixTranspose(float[] rotationMatrix){
float[] result = new float[9];
result[0] = rotationMatrix[0];
result[1] = rotationMatrix[3];
result[2] = rotationMatrix[6];
result[3] = rotationMatrix[1];
result[4] = rotationMatrix[4];
result[5] = rotationMatrix[7];
result[6] = rotationMatrix[2];
result[7] = rotationMatrix[5];
result[8] = rotationMatrix[8];
return result;
}

private float[] lowPass(float alpha, float[] input, float[] output ) {
for ( int i=0; i<3; i++ ) {
output[i] = alpha*output[i] + (1-alpha)*input[i];
}
return output;
}


This is a graph of the calculated vertical acceleration of a drop from 1.16m with rotation around both X- and Y-axis. Since I know the exact fall height, it's easy to calculate how long time the drop should take using this, which gives roughly 490ms:

$$d = \frac{(9.82 * t^2)}{2}$$

As you can see, the impact is preceded by a small "bump", and the actual impact spike doesn't even reach above $\frac{0m}{s^2}$ (which is of course impossible).

Below is a similar graph from a drop (of the same height) where the phone is not rotating (screen facing upwards). Here the vertical acceleration is spot on as you would expect; $\frac{-9.82m}{s^2}$ throughout the fall and a spike up to +$\frac{5m}{s^2}$ at the moment of impact.

The graphs for rotation along only one axis at a time roughly look the same as the one with no rotation, displaying a clear positive spike on impact.

• What hardware did you test your code on? I suspect your inaccuracies come from how the sensor delivers his data, but I can't say for sure without knowing what hardware you used. – Mast May 18 '16 at 14:04
• Tested on a Samsung Galaxy S5. I should also add that if I drop the phone straight down (no rotation), the values are 100% correct. – Magnus W May 18 '16 at 14:17
• This seems like a debugging question which wouldn't be on topic – Raystafarian May 18 '16 at 15:22
• Well, I would just like to verify that I've implemented the methods correctly. The inconsistent results on rotation could be the result of sensor inaccuracy and noise, or the code could be improved. – Magnus W May 18 '16 at 16:09
• Well perhaps a better way to present this is as if it is code for dropping without rotation, maybe some improvements will come that can lead you down the right path for rotation. – Raystafarian May 18 '16 at 17:07