Question

I’m currently working on a tilt compensated linear accelerometer on an android phone. What i basically want to reach is the acceleration, which is in the earth frame, rather than sensor frame. I also want make my vector gravity free. X-axis of the accelerometer will be alligned seperately with the x-axis of a land vehicle. Therefore yaw compensation (angle between the x axis of the phone and magnetic north) is not wanted.

For this we have 2 sensor readings:

1) 3×3 rotation matrix R (rotation from earth frame to sensor frame)

2) 3×1 acceleration vector a = (x,y,z)^T from sensor frame

To manually remove gravity from a_transposed we can simply define a gravity vector in earth frame:

g = (0,0,9.81)^T

I can now multiply gravity with rotation matrix R and substract it from a vector. As a result a’ is the gravity free acceleration vector.

a’ = a – (R * g)
(at this step a’ has the same values as the software sensor LINEAR_ACCELERATION in API)

Until here everything works fine.

Now i want to rotate my linear acceleration vector a’ in the earth frame without taking Z-axis rotation in to account. In order to do so first i calculate the inverse of the rotation matrix R, which is equal to the tranpose of it:

R^(-1) = R^T

Then i multiply a’ with R^(-1) and rotate it back to earth frame.

a’ = R^(-1) * a’

At this step I have a tilt compensated linear acceleration data, if my phone is facing magnetic north, because i have also rotated around z axis, which is not needed. I have to rerotate it around z-axis to get the final right result. For this purpose i have calculated the euler angle (rotation around z axis) from the rotation matrix, which is yaw. And rotated my vector once in the opposite direction by using the yaw angle. This should be the final correction step but there is a cost using euler angles.

This method does not work if i hold the phone directly upwards, beacause if the roll angle is equal to 90 degrees and it causes a gimbal lock. My calculated yaw angle is then unknown and my back-rotation fails.

My question is how to modify/change the rotation matrix so that i can only rotate around x and y axis by ignoring z axis, without using euler angles?

i have alread checked but it didnt helped me:
How do I remove axis from a rotation matrix?

Answer 1

a_geo = R^-1*( a_phone - R*g ) = R^-1*a_phone - g

so all you did so far is project your unbiased acceleration in local geographical frame (N,E,D or something). If what you want is a projection on horizontal plane, you simply annihilate the 3rd coordinate, lets say your indices are 1 based, like matlab or fortran:

a_horiz=a_geo; a_horiz[3]=0;

Or expressed with a matrix,

a_horiz = [1,0,0;
           0,1,0;
           0,0,0] * a_geo;

You can project this back to your phone axis,

R*a_horiz;

But it’s not clear if it’s really what you want… I guess you want to just apply a heading rotation and have some kind of horizontal forward acceleration and horizontal lateral (starboard) acceleration. I don’t know any easy trick… Just extract the heading (as you noted only possible if your phone x axis is not vertical)

psi = atan2( R(1,2) , R(1,1) );

Then rotate your horizontal acceleration

a_horiz_car = [ cos(psi) ,  sin(psi) , 0;
               -sin(psi) ,  cos(psi) , 0;
                0        ,        0  , 1] * a_horiz;

Only first two coordinates are interesting, so you could use 2×2 matrix.