Gradient based sensitivity analysis of roll & pitch Euler Angles (ZYX convention)

88 Views Asked by At

I'm trying to analytically create the sensitivity function for simple Euler Angle in intrinsic ZYX convention (sometimes called Tait Bryan angles), see https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix. Let's start with the projection of a full ZYX rotation matrix to the vector of gravity $[0, 0, 1]^T$, see https://www.nxp.com/files-static/sensors/doc/app_note/AN3461.pdf, page 7. enter image description here Now assume that I have an accelerometer that measures the negative components of this gravity vector so that $$\begin{bmatrix} a_x\\a_y\\a_z \end{bmatrix} = \begin{bmatrix} \sin\theta\\-\cos\theta \sin\phi\\-\cos\theta \cos\phi\end{bmatrix} $$ Please note the change of sign. From that, the so-called roll ($\phi$) and pitch ($\theta$) angles can be calculated: $$ \begin{eqnarray} \tan\phi&=\frac{-a_y}{-a_z}\quad (1) \\\tan\theta&=\frac{a_x\cos\phi}{-a_z} \quad (2) \\ &=\frac{a_x}{\sqrt{a_y^2+a_z^2}} \quad (3) \end{eqnarray} $$

What I like to have is the sensitivity function, i.e. the partial derivatives of $\phi$. This is relatively straight forward in the shown formulation, (1) being a function of $a_y$ and $a_z$. However, I'd like to have this parametrized in terms of $\theta$ and $\phi$ to plot the sensitivity function $J_\phi(\theta,\phi)$ for roll/$\phi$.

Since (2) contains both, $\phi$ and $\theta$, I tried to do something like that (starting from (2)).

$$ \begin{eqnarray} \tan\theta=\frac{a_x\cos\phi}{-a_z} \\ \Leftrightarrow \cos\phi=\frac{-a_z \tan\theta}{a_x} \\ \Leftrightarrow \phi = \arccos\frac{-a_z \tan\theta}{a_x} \\ \end{eqnarray} $$

Let's denote that with $\phi=f(x(\theta))=\arccos(x(\theta))$, with $x=\frac{-a_z \tan\theta}{a_x}$, i.e. I need to apply the chain rule for the partial derivatives: $$ \frac{\partial f}{\partial\theta}= \frac{\partial f}{\partial x}\frac{\partial x}{\partial \theta}= \frac{-1}{\sqrt{1-(\frac{-a_z \tan\theta}{a_x})^2}}\cdot({-(1+\tan^2\theta)\frac{a_z}{a_x}})= \frac{a_z\sec^2(\theta)}{a_x\sqrt{1-\frac{a_z^2 \tan^2\theta}{a_x^2}}} $$

However this seems to be wrong. I know, that the sensitivity should be high for larger values of $\theta$, something like the first plot, but it's the other way round (singularities in the middle)

enter image description here enter image description here

I think, there is conceptional mistake, probably since $a_y$ and $a_z$ are functions of $\theta$, too.

What is the correct way to solve this problem? I guess first $a_y$ and $a_z$ should be eliminated to have a pure function of $\theta$ and $\phi$, is that correct?

Any hints are very much appreciated, thank you!