Any two independent orthonormal coordinate frames can be related by a sequence of rotations (not more than three) about coordinate axes, where no two successive rotations may be about the same axis.
The illustration of the world reference frame, {M}, and the body reference frame, {B}, is shown in the following figure.
Pan angle rotation, α, is acquired using computer vision. And, tilt angle rotation, β, and roll angle rotation, γ, are acquired by sensing the gravity, g, using onboard accelerometers. Let us consider the roll angle first, assuming there are no pan and tilt angles. When the roll angle, γ, is zero Y-axis is upward vertical and X-axis is horizontal pointing according to right handed rule. The gravity sensed by X-axis accelerometer and Y-axis accelerometer are denoted by gx and gy respectively. The definition of the roll angle is illustrated in the following figure.
$$ \gamma = \tan^{-1}\frac{gx}{gy} $$.
Then, it is checked for second and third quadrants as
if(gy<0)
if(gx>=0)
γ=π+γ;
else
γ=-π+γ;
end
end
The roll angle rotation matrix, Rγ, is obtained as follows.$$ \mathbf{R}_{\gamma}= \begin{bmatrix} \cos(\gamma) & -\sin(\gamma) & 0 \\ \sin(\gamma) & \cos(\gamma) & 0 \\ 0 & 0 & 1 \end{bmatrix}$$
After the roll angle is corrected, let us consider for tilt angle. The definition of the tilt angle is illustrated in the following figure.
$$ \begin{bmatrix} gx2 \\ gy2 \\ gz2 \end{bmatrix} = \mathbf{R}_{\gamma} \begin{bmatrix} gx \\ gy \\ gz \end{bmatrix} $$
The tilt angle, β, is calculated as
$$\beta = \tan^{-1}\frac{gy2}{gz2} $$.
Then, it is checked for second and third quadrants as
if(gz2>=0)
if(gy2>=0)
β=-π+β;
else
β=π+β;
end
end
The tilt angle rotation matrix, Rβ, is obtained as follows.$$ \mathbf{R}_{\beta}= \begin{bmatrix} 1 & 0 & 0\\ 0 & \cos(\gamma) & -\sin(\gamma) \\ 0 & \sin(\gamma) & \cos(\gamma) \end{bmatrix}$$
Similarly, calculation of the pan angle rotation matrix, Rα, and description of pan angle definition are as follows.
$$ \mathbf{R}_{\alpha}= \begin{bmatrix} \cos(\alpha) & -\sin(\alpha) & 0 \\ \sin(\alpha) & \cos(\alpha) & 0 \\ 0 & 0 & 1 \end{bmatrix}$$
And, M=RB
Since R is a rotation matrix, inverse of R is obtained by transposing R. The implementation of this 3D rotation in LabVIEW using MabLab code is shown in the following figure.
Quaternion algebra is also easy and popular approach for such 3D transformation.
Reference:
Kuipers, Jack B., Quaternions and rotation sequences : a primer with applications to orbits, aerospace, and virtual reality, Princeton University Press, 1999, ISBN: 0691058725.
