あるベクトル2つのベクトルの関係がヤコビアンによって次のように与えられているとする。
\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で書くと次のようになる。この関数はロール、ピッチ、ヨーを与えればヤコビアンとその逆行列を返す。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 | 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 |
コメント