MATLABで角度ベクトルと角速度ベクトルを変換するためのヤコビアンを定義する

剛体に固定された剛体座標系と地球上に固定されたグローバル座標系を考える。いまグローバル座標系上で定義されるオイラー角ベクトルと

\begin{align}
\boldsymbol{\eta_{2}}=
\begin{pmatrix}
\phi \\ \theta \\ \psi
\end{pmatrix}
\end{align}

と剛体座標系上で定義される角速度ベクトル

\begin{align}
\boldsymbol{v_{2}}=
\begin{pmatrix}
p \\ q \\ r
\end{pmatrix}
\end{align}

との変換は、ヤコビアンを使って

\begin{align}
\boldsymbol{v_{2}}= \boldsymbol{T}_e^{-1} (\boldsymbol{\eta_2}) \boldsymbol{\dot{\eta_2}}
\end{align}

とできる。ヤコビアンは

\begin{align}
\boldsymbol{T}_e^{-1}=
\begin{pmatrix}
1 & 0 & -\sin \theta \\
0 & \cos \phi &\cos \theta \sin \phi \\
0 & -\sin\phi &\cos \theta \cos \phi
\end{pmatrix}
\end{align}

で与えられる。Matlabでは次のように実装できる。

function Te=rpy_to_invTe(rpy)
phi=rpy(1);
theta=rpy(2);
psi=rpy(3);

Te=[...
    1   0           -sin(theta);...
    0   cos(phi)    cos(theta)*sin(phi);...
    0   -sin(phi)   cos(theta)*cos(phi);...
];
end