MATLABで位置ベクトルと速度ベクトルを変換するための回転行列を定義する

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

\begin{align}
\boldsymbol{\eta}_1=
\begin{pmatrix}
x \\ y \\ z
\end{pmatrix}
\end{align}

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

\begin{align}
\boldsymbol{v}_1=
\begin{pmatrix}
u \\ v \\ w
\end{pmatrix}
\end{align}

は角度ベクトル

\begin{align}
\boldsymbol{\eta}_2=
\begin{pmatrix}
\phi \\ \theta \\ \psi
\end{pmatrix}
\end{align}

を使って

\begin{align}
\boldsymbol{v}_1= \boldsymbol{R}_I^B (\boldsymbol{\eta}_{2}) \dot{\boldsymbol{\eta}_{1}}
\end{align}

なる関係がある。\(\boldsymbol{R}_I^B (\boldsymbol{\eta}_{2}) \)は

\begin{align}\boldsymbol{R}_I^B(\boldsymbol{\eta}_{2}) =
\begin{pmatrix}
\mathrm{C}_\psi \mathrm{C}_\theta & \mathrm{S}_\psi \mathrm{C}_\theta & -\mathrm{S}_\theta \\
-\mathrm{S}_\psi \mathrm{C}_\phi+\mathrm{C}_\phi \mathrm{S}_\theta \mathrm{S}_\phi & \mathrm{C}_\psi \mathrm{C}_\phi+\mathrm{S}_\psi \mathrm{S}_\theta \mathrm{S}_\phi & \mathrm{S}_\phi \mathrm{C}_\theta \\
\mathrm{S}_\psi \mathrm{S}_\phi+\mathrm{C}_\psi \mathrm{S}_\theta \mathrm{S}_\phi & -\mathrm{C}_\psi \mathrm{S}_\phi+\mathrm{S}_\psi \mathrm{S}_\theta \mathrm{C}_\phi & \mathrm{C}_\phi \mathrm{C}_\theta
\end{pmatrix}
\end{align}

で与えられる。ここで\(S=\sin,C=\cos\)を表す。

function R=Rpy_to_rot(rpy)
phi=rpy(1);
theta=rpy(2);
psi=rpy(3);

R=[...
   cos(psi)*cos(theta),                                sin(psi)*cos(theta),                                -sin(theta);... 
    -sin(psi)*cos(phi)+cos(psi)*sin(theta)*sin(phi),    cos(psi)*cos(phi)+sin(psi)*sin(theta)*sin(phi),     sin(phi)*cos(theta);...  
    sin(psi)*sin(phi)+cos(psi)*cos(phi)*sin(theta),     -cos(psi)*sin(phi)+sin(psi)*sin(theta)*cos(phi),    cos(phi)*cos(theta);...
];
end