:::info
# Orthogonal & Non-Orthogonal Gimbal Kinematics Derivation

> [name=Nikolai Nekrutenko, Freefly Systems]
:::

Using the axis convention as shown below, we derive the equations from translating motion from the gimbal body frame to that of the camera frame.


Using the following variable convention:

$\begin{bmatrix}
x \\
y \\
z
\end{bmatrix} =
\begin{bmatrix}
\text{roll} \\
\text{tilt} \\
\text{pan}
\end{bmatrix}$

We define the following variables:

$\theta{_x}$ = roll joint angle [rad]
$\theta{_y}$ = tilt joint angle [rad]
$\theta{_z}$ = pan joint angle [rad]

$s{_x}$ = roll joint rate [rad/s]
$s{_y}$ = tilt joint rate [rad/s]
$s{_z}$ = pan join rate [rad/s]

$\omega{_x}$ = roll IMU rate [rad/s]
$\omega{_y}$ = tilt IMU rate [rad/s]
$\omega{_z}$ = pan IMU rate [rad/s]

## Orthogonal Gimbal Kinematics Derivation

The jacobian is defined as:

$$
J = R_y(\theta_y) \cdot
\begin{bmatrix} 1 \\ 0 \\ 0 \\ \end{bmatrix} +
\begin{bmatrix} 0 \\ 1 \\ 0 \\ \end{bmatrix} +
R_y(\theta_y) \cdot R_x(\theta_x) \cdot
\begin{bmatrix} 0 \\ 0 \\ 1 \\ \end{bmatrix}
$$

Derivation from pg. 49 of the ETH Zurich lecture notes, found in resources on rotation matricies.

However, the standard rotational matricies do not work with our coordinate axes as the z-axis is flipped in the Freefly convention. As a result the rotation matricies are modified to take this into account:

$$
R_x(\theta_x) =
\begin{bmatrix}
1 & 0 & 0 \\
0 & cos(\theta_x) & -sin(\theta_x) \\
0 & sin(\theta_x) & cos(\theta_x)
\end{bmatrix} \Rightarrow
\begin{bmatrix}
1 & 0 & 0 \\
0 & cos(\theta_x) & sin(\theta_x) \\
0 & -sin(\theta_x) & cos(\theta_x)
\end{bmatrix}
$$

$$
R_y(\theta_y) =
\begin{bmatrix}
cos(\theta_y) & 0 & sin(\theta_y) \\
0 & 1 & 0 \\
-sin(\theta_y) & 0 & cos(\theta_y)
\end{bmatrix} \Rightarrow
\begin{bmatrix}
cos(\theta_y) & 0 & -sin(\theta_y) \\
0 & 1 & 0 \\
sin(\theta_y) & 0 & cos(\theta_y)
\end{bmatrix}
$$

$$
R_z(\theta_z) =
\begin{bmatrix}
cos(\theta_z) & -sin(\theta_z) & 0 \\
sin(\theta_z) & cos(\theta_z) & 0 \\
0 & 0 & 1
\end{bmatrix} \Rightarrow
\begin{bmatrix}
cos(\theta_z) & sin(\theta_z) & 0 \\
-sin(\theta_z) & cos(\theta_z) & 0 \\
0 & 0 & 1
\end{bmatrix}
$$

We multiply $R_y(\theta_y)$ by $R_x(\theta_x)$ and get the following:

$$R_y(\theta_y) \cdot R_x(\theta_x) =
\begin{bmatrix}
cos(\theta_y) & sin(\theta_y)sin(\theta_x) & -sin(\theta_y)cos(\theta_x) \\
0 & cos(\theta_x) & sin(\theta_x) \\
sin(\theta_y) & -cos(\theta_y)sin(\theta_x) & cos(\theta_y)cos(\theta_x)
\end{bmatrix}
$$

Then, we evaluate our expression for the jacobian:

$$
J =
\begin{bmatrix}
cos(\theta_y) & 0 & -sin(\theta_y) \\
0 & 1 & 0 \\
sin(\theta_y) & 0 & cos(\theta_y)
\end{bmatrix} \cdot
\begin{bmatrix} 1 \\ 0 \\ 0 \\ \end{bmatrix} +
\begin{bmatrix} 0 \\ 1 \\ 0 \\ \end{bmatrix} +
\begin{bmatrix}
cos(\theta_y) & sin(\theta_y)sin(\theta_x) & -sin(\theta_y)cos(\theta_x) \\
0 & cos(\theta_x) & sin(\theta_x) \\
sin(\theta_y) & -cos(\theta_y)sin(\theta_x) & cos(\theta_y)cos(\theta_x)
\end{bmatrix} \cdot
\begin{bmatrix} 0 \\ 0 \\ 1 \\ \end{bmatrix}
$$

$$
J =
\begin{bmatrix}
cos(\theta_y) & 0 & -sin(\theta_y)cos(\theta_x) \\
0 & 1 & sin(\theta_x) \\
sin(\theta_y) & 0 & cos(\theta_y)cos(\theta_x)\\
\end{bmatrix}
$$

The following relationship can be established between the rate from the camera and world frames using the jacobian:

$$
\begin{bmatrix}
w_x \\
w_y \\
w_z
\end{bmatrix} =
\begin{bmatrix}
cos(\theta_y) & 0 & -sin(\theta_y)cos(\theta_x) \\
0 & 1 & sin(\theta_x) \\
sin(\theta_y) & 0 & cos(\theta_y)cos(\theta_x)\\
\end{bmatrix} \cdot
\begin{bmatrix}
s_x \\
s_y \\
s_z
\end{bmatrix}
$$

Evaluating the matrix expression results in the following system of equations:

$$\omega_x = s_xcos(\theta_y) - s_zsin(\theta_y)cos(\theta_x)$$

$$\omega_y = s_y + s_zsin(\theta_x)$$

$$\omega_z = s_xcos(\theta_y) + s_ycos(\theta_y)cos(\theta_x)$$

---

# Non-Orthogonal Kinematics Derivation

To account for the non-orthogonality of the gimbal, a rotation matrix is introduced, with the angle from normal defined as $\phi$.

This makes the sequence of going from the body frame to the camera frame look like this:

$$\text{1. pan} \rightarrow \phi \rightarrow \text{roll} \rightarrow \text{tilt}$$

Going from camera frame to body frame is accomplished with the following:

$$\text{1. tilt} \rightarrow \text{roll} \rightarrow \phi \rightarrow \text{pan}$$

Thus, the non-orthogonal Jacobian would look like the following:

$$
J_1 = R_y(\theta_y) \cdot
\begin{bmatrix} 1 \\ 0 \\ 0 \\ \end{bmatrix} +
\begin{bmatrix} 0 \\ 1 \\ 0 \\ \end{bmatrix} +
R_y(\theta_y) \cdot R_x(\theta_x) \cdot R_{\phi}(\phi) \cdot
\begin{bmatrix} 0 \\ 0 \\ 1 \\ \end{bmatrix}
$$

Where $R_{\phi}(\phi)$ is the transformation that accounts for the non-orthogonality of the x-axis is dervied from a rotation about the y axis in the -$\phi$ direction:

$$
R_{\phi}(\phi) = R_y(-\phi) = \begin{bmatrix}
cos(-\phi) & 0 & -sin(-\phi) \\
0 & 1 & 0 \\
sin(-\phi) & 0 & cos(-\phi)
\end{bmatrix} =
\begin{bmatrix}
cos(\phi) & 0 & sin(\phi) \\
0 & 1 & 0 \\
-sin(\phi) & 0 & cos(\phi)
\end{bmatrix}
$$

Now, we move onto computing the Jacobians:

$$R_y(\theta_y) \cdot R_x(\theta_x) \cdot R_{\phi}(\phi) =
\begin{bmatrix}
cos(\theta_y) & sin(\theta_y)sin(\theta_x) & -sin(\theta_y)cos(\theta_x) \\
0 & cos(\theta_x) & sin(\theta_x) \\
sin(\theta_y) & -cos(\theta_y)sin(\theta_x) & cos(\theta_y)cos(\theta_x)
\end{bmatrix} \cdot
\begin{bmatrix}
cos(\phi) & 0 & sin(\phi) \\
0 & 1 & 0 \\
-sin(\phi) & 0 & cos(\phi)
\end{bmatrix}
$$

$$
= \begin{bmatrix}
cos(\theta_y)cos(\phi) + sin(\theta_y)cos(\theta_x)sin(\phi) &
sin(\theta_y)sin(\theta_x) &
cos(\theta_y)sin(\phi) - sin(\theta_y)cos(\theta_x)cos(\phi) \\
-sin(\theta_y)sin(\phi) &
cos(\theta_x) &
sin(\theta_x)cos(\phi) \\
sin(\theta_y)cos(\phi) - cos(\theta_y)cos(\theta_x)sin(\phi)&
-cos(\theta_y)sin(\theta_x) &
sin(\theta_y)sin(\phi) + cos(\theta_y)cos(\theta_x)cos(\phi)
\end{bmatrix}
$$

Thus, $J_1$ can be expressed as:

$$
J_1 =
\begin{bmatrix}
cos(\theta_y) & 0 & cos(\theta_y)sin(\phi) - sin(\theta_y)cos(\theta_x)cos(\phi) \\
0 & 1 & sin(\theta_x)cos(\phi) \\
sin(\theta_y) & 0 & sin(\theta_y)sin(\phi) + cos(\theta_y)cos(\theta_x)cos(\phi)\\
\end{bmatrix}
$$

The resulting equations of motion for J1 is as follows:

$$
\begin{bmatrix}
w_x \\
w_y \\
w_z
\end{bmatrix} =
J_1 \cdot
\begin{bmatrix}
s_x \\
s_y \\
s_z
\end{bmatrix}
$$

$$
\begin{bmatrix}
w_x \\
w_y \\
w_z
\end{bmatrix} =
\begin{bmatrix}
cos(\theta_y) & 0 & cos(\theta_y)sin(\phi) - sin(\theta_y)cos(\theta_x)cos(\phi) \\
0 & 1 & sin(\theta_x)cos(\phi) \\
sin(\theta_y) & 0 & sin(\theta_y)sin(\phi) + cos(\theta_y)cos(\theta_x)cos(\phi)\\
\end{bmatrix} \cdot
\begin{bmatrix}
s_x \\
s_y \\
s_z
\end{bmatrix}
$$

The resulting equations are:

$$w_x = s_xcos(\theta_y) + s_z[cos(\theta_y)sin(\phi) - sin(\theta_y)cos(\theta_x)cos(\phi)]$$

$$w_y = s_y + s_zsin(\theta_x)cos(\phi)$$

$$w_z = s_xsin(\theta_y) + s_z[sin(\theta_y)sin(\phi) + cos(\theta_y)cos(\theta_x)cos(\phi)]$$

---

## Resources:

- Medium post about applying rotation matricies: https://dominicplein.medium.com/extrinsic-intrinsic-rotation-do-i-multiply-from-right-or-left-357c38c1abfd
- NASA paper and 3 and 4 axis gimbals:
https://ntrs.nasa.gov/api/citations/19680020485/downloads/19680020485.pdf
- Robot dynamics lecture notes from ETC Zurich, goes into Jacobians and how to apply rotation matricies in the context of robots:
https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2016/RD2016script.pdf