【MATLAB】クォータニオンと回転行列の相互変換を定義する

回転行列とクォータニオンは相互に変換することができる。いまクォータニオンが\(\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

としても良い。

コメント

タイトルとURLをコピーしました