【MATLAB】２つの座標間の角速度ベクトルの関係をクォータニオンを使って定義する

２つの座標間の角速度ベクトルをクォータニオンを使って表すと

\begin{align}
\dot{\boldsymbol{q}} = \boldsymbol{T}_q (\boldsymbol{q}) \boldsymbol{\omega}
\end{align}

となる。ここで\(\boldsymbol{T}_{q} (\boldsymbol{q}) \)は

\begin{align}
\boldsymbol{T}_{q} (\boldsymbol{q}) = \frac{1}{2}
\begin{pmatrix}
– \boldsymbol{\varepsilon}^T \\ \eta \boldsymbol{I} + \boldsymbol{S} (\boldsymbol{\varepsilon})
\end{pmatrix}
=\frac{1}{2}
\begin{pmatrix}
-\varepsilon_1 & -\varepsilon_2 & -\varepsilon_3 \\
\eta & -\varepsilon_3 & \varepsilon_2 \\
\varepsilon_3 & \eta & -\varepsilon_1 \\
-\varepsilon_2 & \varepsilon_1 & \eta \\
\end{pmatrix}
\end{align}

である。

function[Tq ,trTq]=J_q_quat(q)
eta=q(1);
e1=q(2);
e2=q(3);
e3=q(4);

T11=-e1;
T12=-e2;
T13=-e3;

T21=eta;
T22=-e3;
T23=e2;

T31=e3;
T32=eta;
T33=-e1;

T41=-e2;
T42=e1;
T43=eta;

Tq=1/2*[...
    T11 T12 T13;...
    T21 T22 T23;...
    T31 T32 T33;...
    T41 T42 T43;...
    ];

trTq=1/2*[...
    T11 T21 T31 T41;...
    T12 T22 T32 T42;...
    T13 T23 T33 T43;...
    ];

end