Remove an arbitrary rotation axis from a rotation matrix

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) 3x3 rotation matrix R (rotation from earth frame to sensor frame)

2) 3x1 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?

This is unclear. If the phone is facing North and accelerating East, a' will point East; which way do you want it to point? If the phone is facing up and accelerating East, a' will point East; which way do you want it to point? — Beta
The phone will be placed inside a vehicle and x-axis of the phone will be alligned with the x-axis of the car, which is front side of the car (in a seperate calibration step via gps). If the phone is facing north and accelerating east, and x axis of the phone points east, that is fine. — kolpazar
Then when/why do you have to "rerotate to get the final right result"? — Beta
ok, let's assume x-axis of my phone is paired with the car x-axis physically. Then I multiply a' = R^-1 * a'. Now my accelerometer data pitch, roll AND yaw compensated. My phone is parallel to the horizontal plane and my x-axis is pointing to north. So i lost my pairing with the x-axis of the car. Therefore i rotate back around z axis immediately for this sample to make it on pair with the car again. Why i do it? to keep it on pair with the car. When i do it? just after calculating R^-1 * a'. I basically keep the pitch and roll compensation and ignore yaw compensation — kolpazar
I hope this time i could make my self clear, having a difficult time expressing what i need. Cheers!!! :D — kolpazar

aka.nice aka.nice · Accepted Answer · 2012-06-29T22:47:41

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 2x2 matrix.

Remove an arbitrary rotation axis from a rotation matrix

2 Answers