あるベクトル２つのベクトルの関係が回転行列によって次のように与えられているとする。
\begin{align}
\boldsymbol{y}= \boldsymbol{R} \boldsymbol{x}
\end{align}
この時逆問題は
\begin{align}
\boldsymbol{x}&= \boldsymbol{R}^{-1} \boldsymbol{y} = \boldsymbol{R}^{T} \boldsymbol{y}
\end{align}
となる。MATLABで書くと次のようになる。この関数はロール、ピッチ、ヨーを与えれば回転行列とその逆行列を返す。
function [R,invR]=Rpy_to_rot(rpy)
phi=rpy(1);
theta=rpy(2);
psi=rpy(3);
R=[...
cos(psi)*cos(theta), -sin(psi)*cos(phi)+cos(psi)*sin(theta)*sin(phi), sin(psi)*sin(phi)+cos(psi)*cos(phi)*sin(theta);...
sin(psi)*cos(theta), cos(psi)*cos(phi)+sin(psi)*sin(theta)*sin(phi), -cos(psi)*sin(phi)+sin(psi)*sin(theta)*cos(phi);...
-sin(theta), sin(phi)*cos(theta), cos(phi)*cos(theta);...
];
invR=[...
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
コメント