Two-rotation coordinate transformation

I've designed an electronic device that uses a 3-axis accelerometer to measure the acceleration of an automobile. I'm only interested in accelerations in the plane of the road surface, so I want to ignore gravity and any transient bumps and such in the vertical axis. The problem is I have no control of how the device will be mounted in the vehicle. It could be bolted to the vehicle's frame at any arbitrary orientation.

On power-up, I have the device sample the accelerometer. I assume the vehicle is stationary and on a flat surface at power-up. That gives me an acceleration vector of 1g in the vertical axis. With this knowledge, I want to perform a coordinate transformation on all of the readings from the accelerometer's thereafter such that one of the axes lines up with the newly established "down" direction. That way, the other two axes will form the horizontal plane of the vehicle and I can ignore the vertical axis.

I made an 3-axis system and drew an arbitrary vector, A, that represents the 1g reading from the accelerometer. I then decided to do a rotation (angle θ) about the y-axis such that the new z' axis was parallel with the projection of A on the x-z plane. Then I rotated about the new x' axis so that z'' was co-linear with A (angle φ). For the first rotation, the transformation matrix is: $$\begin{bmatrix}x'\\y'\\z'\end{bmatrix} = \begin{bmatrix}cosθ&0&-sinθ\\0&1&0\\sinθ&0&cosθ\end{bmatrix}\begin{bmatrix}x\\y\\z\end{bmatrix}$$ And the second: $$\begin{bmatrix}x''\\y''\\z''\end{bmatrix} = \begin{bmatrix}1&0&0\\0&cosφ&sinφ\\0&-sinφ&cosφ\end{bmatrix}\begin{bmatrix}x'\\y'\\z'\end{bmatrix}$$ Putting them together: $$\begin{bmatrix}x''\\y''\\z''\end{bmatrix} = \begin{bmatrix}cosθ&sinθsinφ&-sinθcosφ\\0&cosφ&sinφ\\sinθ&-cosθsinφ&cosθcosφ\end{bmatrix}\begin{bmatrix}x\\y\\z\end{bmatrix}$$ And, finally, the angles are defined as: $$θ = sin^{-1}\frac{A_x}{\sqrt{A_x^2+A_z^2}}$$ $$φ = sin^{-1}A_y$$

I would expect, then, that if I plug in an arbitrary vector for A, the result should be $$\begin{bmatrix}x''\\y''\\z''\end{bmatrix} = \begin{bmatrix}0\\0\\1\end{bmatrix}$$ since the z'' axis in the new coordinate system should be co-linear with A. I'm getting something completely different though, so I've done something wrong along the way. For example, I tried an A vector of [0.6, -0.7, 0.3873] and got: $$\begin{bmatrix}x''\\y''\\z''\end{bmatrix} = \begin{bmatrix}0.5047\\-0.7710\\0.3884\end{bmatrix}$$ Is my approach fundamentally wrong or am I just making a silly computational error?

• 1. It would be a good idea to first normalize your arbitrary vector if you aim to transform it into $v_\text{new}=(0,0,1)$ 2. It should certainly be possible to always rotate such that a normalized vector is transformed into $v_\text{new}$ 3. We don't allow "check my work please"questions here, so I'm voting to close 4. You may consider posting a similar question on Mathematics, stripping off the physical motivation. – Danu Sep 27 '14 at 8:02

Instead of working with angles (which are very easy to mess up in 3D), I would just go directly to a transformation matrix. I had to solve a very similar problem to this when tracking charged particles through magnetic fields and all the angles and rotations kept messing me up (probably due to subtle sign errors). So, I went with pure linear algebra.

You are looking for a matrix $T$ that, at the very least, maps the downward unit vector in the arbitrary coordinate system of the accelerometer to $\begin{bmatrix}0\\0\\1\end{bmatrix}$. That is, $$T\begin{bmatrix}x\\y\\z\end{bmatrix} = \begin{bmatrix}0\\0\\1\end{bmatrix}$$ where $x$, $y$, and $z$ are the normalized initial measurements from the accelerometers--normalized meaning: $$\sqrt{x^2 + y^2 + z^2} = 1.$$

First, we see that $$\begin{bmatrix}x\\y\\z\end{bmatrix} = T^{-1}\begin{bmatrix}0\\0\\1\end{bmatrix}.$$ This means that $T^{-1}$ is of the form $$T^{-1}=\begin{bmatrix}|&|&x\\\vec{e_1}&\vec{e_2}&y\\|&|&z\end{bmatrix}$$ where $\vec{e_1}$ and $\vec{e_2}$ are unit vectors perpendicular to each other and $\begin{bmatrix}x\\y\\z\end{bmatrix}$. We know this because the transform is a pure rotation, which is an orthogonal transform, and thus the columns of the corresponding matrix form an orthonormal basis.

If you don't care about the directions of the non-vertical axes, pick $$\vec{e_1} = \frac{1}{\sqrt{x^2 + y^2}}\begin{bmatrix}-y\\x\\0\end{bmatrix}$$ and $$\vec{e_2} = \begin{bmatrix}x\\y\\z\end{bmatrix}\times\vec{e_1} = \frac{1}{\sqrt{x^2 + y^2}}\begin{bmatrix}-xz\\-yz\\x^2 + y^2\end{bmatrix}.$$ I picked these because

1. They are easy to calculate/program. (Most importantly)
2. They are guaranteed to be orthogonal to each other and $\begin{bmatrix}x\\y\\z\end{bmatrix}.$
3. They are guaranteed to form a right-handed coordinate system, which makes any subsequent math easier.

Thus, $$T^{-1} = \begin{bmatrix} \frac{-y}{\sqrt{x^2 + y^2}} & \frac{-xz}{\sqrt{x^2 + y^2}} & x\\ \frac{x}{\sqrt{x^2 + y^2}} & \frac{-yz}{\sqrt{x^2 + y^2}} & y\\ 0 & \sqrt{x^2 + y^2} & z \end{bmatrix}$$

A nice property of orthogonal matrices/transformations is that the inverse is the same as the transpose. $$T = (T^{-1})^T = \begin{bmatrix} \frac{-y}{\sqrt{x^2 + y^2}} & \frac{x}{\sqrt{x^2 + y^2}} & 0\\ \frac{-xz}{\sqrt{x^2 + y^2}} & \frac{-yz}{\sqrt{x^2 + y^2}} & \sqrt{x^2 + y^2}\\ x & y & z \end{bmatrix}$$

To check this, I'll use your initial unit vector for the direction of gravity. $$\begin{bmatrix}x\\y\\z\end{bmatrix} = \begin{bmatrix}0.6\\-0.7\\0.3873\end{bmatrix}$$ This results in the matrix $$T = \begin{bmatrix} 0.7593 & 0.6508 & 0 \\ -0.2521 & 0.2941 & 0.9220 \\ 0.6000 & -0.7000 & 0.3873 \\ \end{bmatrix}$$ which gives the required result: $$\begin{bmatrix}x''\\y''\\z''\end{bmatrix} = \begin{bmatrix} 0.7593 & 0.6508 & 0 \\ -0.2521 & 0.2941 & 0.9220 \\ 0.6000 & -0.7000 & 0.3873 \\ \end{bmatrix} \begin{bmatrix}0.6\\-0.7\\0.3873\end{bmatrix} = \begin{bmatrix}0\\0\\1\end{bmatrix}$$

• Excellent technique, thank you. I think I'll end up using this in my algorithm instead of my original approach. – Dan Laks Sep 27 '14 at 23:28
• @DanLaks One caveat: as the automobile accelerates, whether by turning, braking, or accelerating, it will probably tilt one way or another. This will tilt the accelerometers and spoil the relation between the accelerometer coordinate system and the outside world coordinate system. Unless the car goes up on two wheels or flips over, the effect should be small, but it will introduce errors. (As a physicist, I've had to do a lot of learning from engineers about tolerances, clearances, inefficiencies, and other just-because-it-works-in-math-doesn't-mean-it-can-be-built factors.) – Mark H Sep 28 '14 at 0:41

Is my approach fundamentally wrong or am I just making a silly computational error?

You made a number of computational errors. The purpose of this answer is to show what you did wrong, and how to fix it. That said, I recommend using Mark H's approach.

Your first error was that you multiplied the matrices in the wrong order. I'll start with your second rotation, $$\begin{bmatrix}x''\\y''\\z''\end{bmatrix} = \begin{bmatrix}1 & 0 & 0 \\ 0&\cos φ & \sin φ \\ 0&-\sin φ & \cos φ \end{bmatrix} \begin{bmatrix}x'\\y'\\z'\end{bmatrix}$$ Now substitute your first rotation for $(x', y', z')$: $$\begin{bmatrix}x''\\y''\\z''\end{bmatrix} = \begin{bmatrix}1 & 0 & 0 \\ 0&\cos φ & \sin φ \\ 0&-\sin φ & \cos φ \end{bmatrix} \begin{bmatrix}\cosθ & 0 & -\sinθ \\ 0 & 1 & 0 \\ \sin θ & 0 & \cos θ \end{bmatrix} \begin{bmatrix}x\\y\\z\end{bmatrix}$$ or $$\begin{bmatrix} x'' \\ y'' \\ z'' \end{bmatrix} = \begin{bmatrix} \cos θ & 0 & -\sin θ \\ \sin φ \sin θ & \cos φ & \cos θ \sin φ \\ \cos φ \sin θ & -\sin φ & \cos φ \cos θ \end{bmatrix} \begin{bmatrix} x \\ y \\ z \end{bmatrix}$$

Transformation matrices formed in this way chain right-to-left, not left-to-right.

Your next mistakes were in how you calculated the angles. You did it very wrong. You used inverse sine to compute theta, it looks like you read from the last column of your matrix, and if that's what you did, you ignored signs. These mistakes will get you in trouble one day.

From above, the desired transformation is $$\begin{bmatrix} x'' \\ y'' \\ z'' \end{bmatrix} = \begin{bmatrix} \cos θ & 0 & -\sin θ \\ \sin φ \sin θ & \cos φ & \cos θ \sin φ \\ \cos φ \sin θ & -\sin φ & \cos φ \cos θ \end{bmatrix} \begin{bmatrix} x \\ y \\ z \end{bmatrix}$$

You want $(x'', y'', z'') = (0, 0, 1)$ for $(x, y, z) = (A_x, A_y, A_z)$. This means you want $\cos φ \sin θ = A_x, -\sin φ = A_y, \cos φ \cos θ = A_z$. This leads to \begin{aligned} \sin φ &= -A_y & \quad\text{or}\quad && φ &= \arcsin(-A_y) \\ \frac{\cos φ \sin θ}{\cos φ \cos θ} &= \frac {A_x}{A_z} & \quad\text{or}\quad && θ &= \arctan \left(\frac {A_x}{A_z}\right) \end{aligned} You need to be very, very careful with that inverse tangent calculation. In this case, using a simple arctan happens to work. Here are some of the things that can go wrong:

• What if one or both of your $A_x$ and $A_z$ are negative?
You should be using the two argument version of the inverse tangent.

• What if you have an even longer chain of matrices and you find that $\sin φ = 1$?
This means $\cos φ = 0$, which in turn means that $\frac{\cos φ \sin θ}{\cos φ \cos θ}$ is undefined. This is called "gimbal lock," and if it happens with a robotic arm, you might have already broken it by getting into this gimbal lock predicament.

• What if it turns out that $\cos φ$ is negative?
That can't happen here because of how inverse sine works, but you need to be wary. Ignoring that $\cos φ$ is negative will put $θ$ in the wrong quadrant.

With the above values for $A_x$, $A_y$, and $A_z$, $φ = 0.775397$ and $θ = 0.997591$, which in turn leads to the transformation matrix $$\begin{bmatrix} 0.542328 & 0. & -0.840167 \\ 0.588117 & 0.714143 & 0.379629 \\ 0.599999 & -0.7 & 0.3873 \end{bmatrix}$$ As a sanity check, $$\begin{bmatrix} 0.542328 & 0. & -0.840167 \\ 0.588117 & 0.714143 & 0.379629 \\ 0.599999 & -0.7 & 0.3873 \end{bmatrix} \begin{bmatrix} 0.6 \\ -0.7 \\ 0.3873\end{bmatrix} = \begin{bmatrix} 1.209\times10^{-7} \\ 4.117\times10^{-7} \\ 1.\end{bmatrix}$$

or essentially $(0,0,1)$, as desired.

Note that Mark H's approach avoids all the Euler angle nonsense, the seemingly backwards way in which matrices chain, and having to be very careful with trigonometry.

• David, while I did end up using Mark H's method for my application, I was hoping someone would be able to point out the errors of my original method. This answer was very helpful and I appreciate the time you took to figure what I did wrong. – Dan Laks Nov 10 '14 at 19:15

I believe you have a math error in your matrix multiply. Substituting the single prime vector into the second rotation and multiplying the matrices doesn't give that third matrix.

• Yes, it looks like he multiplied the matrices in the wrong order. – CuriousKev Sep 27 '14 at 7:17