MATLABで回転行列とヤコビアンを一つの行列で表現して6自由度を扱えるようにする

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

\begin{align}
\boldsymbol{\eta}=
\begin{pmatrix}
\boldsymbol{\eta}_1 \\ \boldsymbol{\eta}_2
\end{pmatrix}=
\begin{pmatrix}
x \\ y \\ z \\ \phi \\ \theta \\ \psi
\end{pmatrix}
\end{align}

と剛体座標系の速度、角速度ベクトル

\begin{align}
\boldsymbol{v}=
\begin{pmatrix}
\boldsymbol{v}_1 \\ \boldsymbol{v}_2
\end{pmatrix}=
\begin{pmatrix}
u \\ v \\ w \\ p \\ q \\ r
\end{pmatrix}
\end{align}

は\( \boldsymbol{\eta} \)を使って

\begin{align}
\boldsymbol{v}= \boldsymbol{J}_e (\boldsymbol{\eta}) \dot{\boldsymbol{\eta}}
\end{align}

ここで\( \boldsymbol{J}_e (\boldsymbol{\eta}) \)は

\begin{align}
\boldsymbol{J}_e (\boldsymbol{\eta}) =
\begin{pmatrix}
\boldsymbol{R}_I^B & \boldsymbol{O} \\
\boldsymbol{O} & \boldsymbol{J}_e
\end{pmatrix}
\end{align}

と表される。

function J = J_e(eta)
O=zeros(3,3);

eta2=eta(1,4:end);

R=Rpy_to_rot(eta2);
J=J_ko_rpy(eta2);

J=[ R, O;...
    O, J
];
end