# Subtract gravity from 3D object in different orientations

I'm currently working on a project that involves calculating a sensor module's distance and orientation. The problem I'm running into is the fact that once the sensor is, for example, rotated around its pitch axis, gravity starts pulling on the sensor's X axis, causing the measured X acceleration to go up even though the sensor is motionless. So I am looking for a way to dynamically subtract gravity's influence from the, depending on the sensor's rotation, desired accelerations.

So my question is: If I were to know an objects yaw/pitch/roll and its acceleration in all three different axis, how can I subtract gravity's influence from its three different accelerations in any orientation?

Any help would be highly appreciated!

EDIT: Hopefully made the case and question a little more clear :).

-
Dear Steven, a detail, a typo: the operation is called "subtraction", not "substraction". ;-) Unfortunately, I don't understand what the question wants to say unless your desire is to subtract one 3-vector from another 3-vector. – Luboš Motl May 23 '11 at 10:59

If I'm reading this right, you want to know how gravity influences your sensor based on its rotation and then remove the effect of that influence. I think the best way to go about this is to rotate the gravity vector to correspond with your sensor's rotation and then add the vector that is opposite in magnitude to add out gravity. Like so:

Let $\vec{G} = \langle 0, 0, g \rangle$ where $g$ is the force of gravity, typically $-9.81 ^{m}/_{s}$.

The matrix for rotating a vector by pitch $\alpha$, yaw $\beta$, and roll $\gamma$ is: (from http://planning.cs.uiuc.edu/node102.html).

$$$$\begin{split} R(\alpha,& \beta,\gamma) = R_z(\alpha) \, R_y(\beta) \, R_x(\gamma) = \\ & \begin{pmatrix} \cos\alpha \cos\beta & \cos\alpha \sin\beta \sin\gamma - \sin\alpha \cos\gamma & \cos\alpha \sin\beta \cos\gamma + \sin\alpha \sin\gamma \\ \sin\alpha \cos\beta & \sin\alpha \sin\beta \sin\gamma + \cos\alpha \cos\gamma & \sin\alpha \sin\beta \cos\gamma - \cos\alpha \sin\gamma \\ -\sin\beta & \cos\beta \sin\gamma & \cos\beta \cos\gamma \\ \end{pmatrix}. \end{split}$$$$

Thus, the transformed gravity vector is:
$\vec{G}\,' = \vec{G}*R(\alpha, \beta,\gamma) = \langle -g \sin(\beta) , \;\; g \cos(\beta) \sin(\gamma) , \;\; g \cos(\beta) \cos(\gamma) \rangle$

You can then take your acceleration vector $\vec{A}$ and add the vector of opposite magnitude, which is the same as subtracting $\vec{G}\,'$. Thus, your final formula is:

$\vec{A}\,' = \vec{A} - \langle -g \sin(\beta) , \;\; g \cos(\beta) \sin(\gamma) , \;\; g \cos(\beta) \cos(\gamma) \rangle$

Is that what you want?

EDIT2: This matrix depends on the order in which rotations are applied.

-
Yes, this is exactly what I needed. Thanks alot! – Steven May 24 '11 at 7:29
Is it possible that the "-g * sin(yaw)" part should be "-g * sin(pitch)"? When testing your original I was still getting some acceleration when rotating the sensor around its yaw axis. When I changed yaw to pitch in that part, it worked perfectly. – Steven May 24 '11 at 8:06
@Steven: Huh, interesting. I didn't do any testing myself, I just found this matrix and did the relevant multiplication...so I suppose it's possible they had a typo or something. – El'endia Starman May 24 '11 at 12:12
After some further testing it appeared there was a problem with pitch rotations too. So I decided to properly read the link you gave and saw "It is important to note that performs the roll first, then the pitch, and finally the yaw. If the order of these operations is changed, a different rotation matrix would result.". I am now thinking this might be the problem I am having now, but not sure how to fix it :). – Steven May 24 '11 at 13:06
@Steven: Yes, there is that important caveat. Well, if you know how to do matrix multiplication, then you can derive the matrix yourself by multiplying the three simpler rotation matrices in the proper order. – El'endia Starman May 24 '11 at 13:20