I'm trying to come up with an elegant way to compute the Jacobian of a function but having some hard time doing so.
I'll first give a intuitive description of the function and then I'll try to make it more formal.
The Jacobian I come up with has some fundamental problems (biggest example: when there is no rotation i.e $R=I$ I get that the partial derivative does not exist). It seem to me this problem should have an easier solution other than brute force using sympy…
verbal description (i.e hand waving):
The function gets 3 Euler angles (yaw, pitch and roll) and uses some rotation matrix $R$ to change the CS in which these angles are given. (Compute rotation matrix $\hat{R}$ from the angles and extract the Euler angles from $R\cdot \hat{R} \cdot R^T$)
Attempt to be more formal:
Let $$\psi=\{R \ |\ R\in\mathbb{R}^{3×3}, R\cdot R^T=I \\\wedge \\\text{det}(R)=1\}$$
$$f: \mathbb{R}^3\times\psi \rightarrow \mathbb{R}^3$$
$$g: \mathbb{R}^3\ \rightarrow \psi$$
$$h: \psi\ \rightarrow \mathbb{R}^3$$
$$f(v, R)=h(R\cdot g(v) \cdot R^T) $$
to be clear – I'm looking for $J_R(f_R)$ where $f_R(v)=f(v, R)$
I omitted the formal definition of $g$ and $h$. they are basically the functions that compose (g) and decompose (h) rotation matrices from Euler angles.
h is something along this:
$$h(R)^{(1)}=atan2(R_{2, 3}, \sqrt{R_{1, 3}^2 + R_{3, 3}^2})$$
$$h(R)^{(2)}=atan2(R_{1, 3}, R_{3, 3})$$
$$h(R)^{(3)}=atan2(-R_{2, 1}, R_{2, 2})$$
g is something like this:
$$
\left[\begin{matrix}\cos(y) &0& \sin(y)\\ 0& 1& 0\\ -\sin(y)&0& \cos(y)\\\end{matrix} \right]
\times
\left[\begin{matrix}1 &0& 0\\ 0& \cos(p)& \sin(p)\\ 0& -\sin(p)& \cos(p)\\\end{matrix} \right]
\times
\left[\begin{matrix}\cos(r) &\sin(r)& 0\\ -\sin(r)& \cos(r)& 0\\ 0& 0& 1\\\end{matrix}\,\right] $$
Hope it's clear and that I made no typing mistakes..
Thanks.
Best Answer
I was trying to solve the same problem for yaw-pitch-roll rotations but gave up since it's too complicated. Instead, I assumed that the rotation is small, and used the transformation matrix which converts the rotation rates in the global frame to the inertial frame
$$ \begin{bmatrix} \delta \phi \\ \delta \theta \\ \delta \psi \end{bmatrix} = \begin{bmatrix} 1&0&-sin(\theta_1)\\ 0&cos(\phi_1)&sin(\phi_1)cos(\theta_1)\\ 0&-sin(\phi_1)&cos(\phi_1)cos(\theta_1) \end{bmatrix}\begin{bmatrix} \phi_2-\phi_1\\ \theta_2-\theta_1\\ \psi_2-\psi_1\ \end{bmatrix} $$ and then apply jacobian to that matrix. It seems like your rotation sequence is pitch-roll-yaw. You may find the appropriate transformation matrix by following: https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2016/RD2016script.pdf
Hope it helps.