回転行列を微分する

回転行列の微分は外積因子を用いて

\begin{align}
\dot{\boldsymbol{R}}=\boldsymbol{R} \boldsymbol{S}(\boldsymbol{\omega})
\end{align}

ここで\(\omega\)は

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

コードはこれまで作った関数を流用して

omega=Cross_Fact([p,q,r])
[R,~]=Rpy_to_rot([phi theta psi])