MATLABで定義したヤコビアンを計算する関数を改造して逆回転を扱えるようにする

あるベクトル２つのベクトルの関係がヤコビアンによって次のように与えられているとする。

\begin{align}
\boldsymbol{y}= \boldsymbol{J} \boldsymbol{x}
\end{align}

この時逆問題は

\begin{align}
\boldsymbol{x}&= \boldsymbol{J}^{-1} \boldsymbol{y}　
\end{align}

となる。ここで\(J^{-1}\)は

\begin{align}\boldsymbol{J}^{-1}＝
\begin{pmatrix}
1 & \sin \phi \tan \theta & \cos \phi \tan \theta\\
0 & \cos \phi & -\sin \phi \\
0 & \dfrac{\sin \phi}{\cos \theta}& \dfrac{\cos \phi }{\cos \theta }\\
\end{pmatrix}
\end{align}

である。MATLABで書くと次のようになる。この関数はロール、ピッチ、ヨーを与えればヤコビアンとその逆行列を返す。

function [J,invJ]=J_ko_rpy(rpy)
phi=rpy(1);
theta=rpy(2);
psi=rpy(3);

J=[...
    1,   sin(phi)*tan(theta),         cos(phi)*tan(theta);...
    0,   cos(phi),                    -sin(phi);...
    0,   sin(phi)/cos(theta),         cos(phi)/cos(theta);...
];

invJ=[...
    1   0           -sin(theta);...
    0   cos(phi)    cos(theta)*sin(phi);...
    0   -sin(phi)   cos(theta)*cos(phi);...
];
end