回転行列とクォータニオンは相互に変換することができる。いまクォータニオンが\(\boldsymbol{q} \in \mathbb{Q}\)と定義されているとすると回転行列とクォータニオンは
\begin{align}
\boldsymbol{R}(\boldsymbol{q})=\boldsymbol{I}+2 \boldsymbol{\eta} \boldsymbol{S}(\boldsymbol{\varepsilon})+2 \boldsymbol{S}^2 (\boldsymbol{\varepsilon})
\end{align}
で変換できる。ここで\(\boldsymbol{S}\)は外積因子である。
MATLAB書くと次のようになる。
function R=quat_to_R(q)
eta=q(1);
e1=q(2);
e2=q(3);
e3=q(4);
R=eye(3)+2*eta.*Cross_Fact([e1 e2 e3])+2*Cross_Fact([e1 e2 e3])^2;
end
あるいは要素ごとに
function R=quat_to_R(q)
eta=q(1);
e1=q(2);
e2=q(3);
e3=q(4);
R11=1-2*(e2^2+e3^2);
R12=2*(e1*e2-e3*eta);
R13=2*(e1*e3+e2*eta);
R21=2*(e1*e2+e3*eta);
R22=1-2*(e1^2+e3^2);
R23=2*(e2*e3-e1*eta);
R31=2*(e1*e3-e2*eta);
R32=2*(e2*e3+e1*eta);
R33=1-2*(e1^2+e2^2);
R=[...
R11 R12 R13;...
R21 R22 R23;...
R31 R32 R33;...
];
end
としても良い。
コメント