拡張カルマンフィルタ
これまでの不具合
hrpsysに入っていたKFは，内部状態をオイラー角にするために加速度センサの値からオイラー角を求めているが，これのせいで特異点が出てしまう．
ので，加速度をRPYに変換しなければ良くて，そうするには加速度センサの値を変換すること無く使えば良い．
そもそも加速度センサをRPYに変換することで線形になって（？）いて嬉しかったが，拡張カルマンフィルタを使えば非線形でもOKなのでそれを使う．
拡張カルマンフィルタ
$$ \begin{eqnarray} 加速度センサの出力 &：& \boldsymbol{a_k}\\ ジャイロセンサの出力 &：& \boldsymbol{\omega_k}\\ ほしい物 &：& ルートリンクに対する姿勢\boldsymbol{q_k}\\ 状態量\boldsymbol{x_{k}} &：& \boldsymbol{q_k}（これにプラスでドリフト項も必要？）\\ 状態方程式 &：& \boldsymbol{x_{k}} = f(\boldsymbol{x_{k-1}}, \boldsymbol{u_{k}}, \boldsymbol{w_{k}})\\ 入力 &：& \boldsymbol{u_{k}} = \boldsymbol{\omega_k} （ジャイロ）\\ 時間変移に関する雑音 process noise &：& \boldsymbol{w_{k}}\\ 観測方程式 &：& \boldsymbol{z_{k}} = h(\boldsymbol{x_{k}}, \boldsymbol{v_{k}})\\ 出力 &:& \boldsymbol{z_{k}} = \boldsymbol{a_{k}} （加速度）\\ 観測雑音 observation noise &：& \boldsymbol{v_{k}}\\ \end{eqnarray} $$
式
状態方程式
$$ x_{k+1} = f(x_{k}, u_{k}) + w_{k}\\ w_k \sim N(0, Q_k) $$
観測方程式
$$ z_k = h(x_k) + v_k\\ v_k \sim N(0, R_k) $$
f と h の詳細
状態量\(x\)はworld座標から見たIMUセンサの位置姿勢，入力\(u\)はジャイロセンサ，出力\(z\)は加速度センサとする．
fは内部状態である現在の姿勢と入力であるジャイロから次の時刻の姿勢を求める関数であり，なんでこうなるかは不明だが，A_sensor_fusion_algorithm_for_an_integrated_angular_position_estimation_with_inertial_measurement_unitsの式（1）によると，
$$ \begin{eqnarray} &&\dot{x_k} = \frac{1}{2}\begin{pmatrix} 0 & -{\omega}_x & -{\omega}_y & -{\omega}_z \\ {\omega}_x & 0 & {\omega}_z & -{\omega}_y \\ {\omega}_y & -{\omega}_z & 0 & {\omega}_x \\ {\omega}_z & {\omega}_y & -{\omega}_x & 0 \end{pmatrix}_k x_k \\ & \Leftrightarrow & \frac{x_{k+1} - x_k}{dt} = \frac{1}{2} \begin{pmatrix} 0 & -{\omega}_x & -{\omega}_y & -{\omega}_z \\ {\omega}_x & 0 & {\omega}_z & -{\omega}_y \\ {\omega}_y & -{\omega}_z & 0 & {\omega}_x \\ {\omega}_z & {\omega}_y & -{\omega}_x & 0 \end{pmatrix}_k x_k \\ & \Leftrightarrow & x_{k+1} = x_k + \frac{dt}{2} \begin{pmatrix} 0 & -{\omega}_x & -{\omega}_y & -{\omega}_z \\ {\omega}_x & 0 & {\omega}_z & -{\omega}_y \\ {\omega}_y & -{\omega}_z & 0 & {\omega}_x \\ {\omega}_z & {\omega}_y & -{\omega}_x & 0 \end{pmatrix}_k x_k \\ & \Leftrightarrow & x_{k+1} = \begin{pmatrix} 1 & -{\omega}_x \cdot \frac{dt}{2} & -{\omega}_y \cdot \frac{dt}{2} & -{\omega}_z \cdot \frac{dt}{2} \\ {\omega}_x \cdot \frac{dt}{2} & 1 & {\omega}_z \cdot \frac{dt}{2} & -{\omega}_y \cdot \frac{dt}{2} \\ {\omega}_y \cdot \frac{dt}{2} & -{\omega}_z \cdot \frac{dt}{2} & 1 & {\omega}_x \cdot \frac{dt}{2} \\ {\omega}_z \cdot \frac{dt}{2} & {\omega}_y \cdot \frac{dt}{2} & -{\omega}_x \cdot \frac{dt}{2} & 1 \end{pmatrix}_k x_k \\ & \Leftrightarrow & x_{k+1} = f(x_{k}, u_{k}) \end{eqnarray} $$
となるので， $$ F_k = \frac{\partial f}{\partial x} = \begin{pmatrix} 1 & -{\omega}_x \cdot \frac{dt}{2} & -{\omega}_y \cdot \frac{dt}{2} & -{\omega}_z \cdot \frac{dt}{2} \\ {\omega}_x \cdot \frac{dt}{2} & 1 & {\omega}_z \cdot \frac{dt}{2} & -{\omega}_y \cdot \frac{dt}{2} \\ {\omega}_y \cdot \frac{dt}{2} & -{\omega}_z \cdot \frac{dt}{2} & 1 & {\omega}_x \cdot \frac{dt}{2} \\ {\omega}_z \cdot \frac{dt}{2} & {\omega}_y \cdot \frac{dt}{2} & -{\omega}_x \cdot \frac{dt}{2} & 1 \end{pmatrix}_k $$ となる．
ポイントとして，クォータニオン計算便利ノートの2枚目の右上二行目にあるように， $$ \begin{eqnarray} &&x_{k+1} = \begin{pmatrix} 1 & -{\omega}_x \cdot \frac{dt}{2} & -{\omega}_y \cdot \frac{dt}{2} & -{\omega}_z \cdot \frac{dt}{2} \\ {\omega}_x \cdot \frac{dt}{2} & 1 & {\omega}_z \cdot \frac{dt}{2} & -{\omega}_y \cdot \frac{dt}{2} \\ {\omega}_y \cdot \frac{dt}{2} & -{\omega}_z \cdot \frac{dt}{2} & 1 & {\omega}_x \cdot \frac{dt}{2} \\ {\omega}_z \cdot \frac{dt}{2} & {\omega}_y \cdot \frac{dt}{2} & -{\omega}_x \cdot \frac{dt}{2} & 1 \end{pmatrix}_k x_k\\ &\Leftrightarrow& x_{k+1} = x_k * quaternion\left(1, {\omega}_x \frac{dt}{2}, {\omega}_y \frac{dt}{2}, {\omega}_z \frac{dt}{2}\right) \end{eqnarray} $$ となること．
一方，hは内部状態である姿勢から出力である加速度センサの値を求める関数なので，直立時の加速度センサの値が重力であることを使って， $$ x_k = \left( \begin{array}{c} w \\ x \\ y \\ z \end{array} \right) $$
としたとき，ここの「クォータニオン→回転行列」を参考にして，
$$ \begin{eqnarray} &&z_k = \begin{pmatrix} w^2 + x^2 - y^2 - z^2 & 2xy + 2wz & 2xz - 2wy \\ 2xy - 2wz & w^2 - x^2 + y^2 - z^2 & 2yz + 2wx \\ 2xz + 2wy & 2yz - 2wx & w^2 - x^2 - y^2 + z^2 \end{pmatrix}_k^T \left( \begin{array}{c} 0 \\ 0 \\ 9.8 \end{array} \right)\\ &\Leftrightarrow& z_k = \left( \begin{array}{c} (2xz + 2wy) * 9.8 \\ (2yz - 2wx) * 9.8 \\ (w^2 - x^2 - y^2 + z^2) * 9.8 \end{array} \right)\\ &\Leftrightarrow& z_k = h(x_k) \end{eqnarray} $$ となる．
ここに転置が入るかどうかがポイントで，Quaternion-Based_Extended_Kalman_Filter_for_Determining_Orientation_by_Inertial_and_Magnetic_Sensingの式(7)の真ん中やA_sensor_fusion_algorithm_for_an_integrated_angular_position_estimation_with_inertial_measurement_unitsの式(3)では回転行列に転置が入っていないが，四元数を使ってみよう！の式(2)では回転行列を転置して使っていて，自分調べでは後者が正しい．
よって， $$ H_k = \frac{\partial h}{\partial x} = 2 * 9.8 * \begin{pmatrix} y & z & w & x \\ -x & -w & z & y \\ w & -x & -y & z \end{pmatrix} $$
ちなみに，An_Introduction_to_the_Kalman_Filterによると，
$$ \begin{eqnarray} x_k &=& \tilde{x_k} + F(x_{k-1}-\hat{x_{k-1}}) + Ww_{k-1}\\ z_k &=& \tilde{z_k} + H(x_k - \tilde{x_k}) + Vv_k\\ F &=& \frac{\partial f}{\partial x} \\ W &=& \frac{\partial f}{\partial w} \\ H &=& \frac{\partial h}{\partial x} \\ V &=& \frac{\partial h}{\partial v} \end{eqnarray} $$
であるが，
$$ x_{k+1} = \begin{pmatrix} 1 & -{\omega}_x \cdot \frac{dt}{2} & -{\omega}_y \cdot \frac{dt}{2} & -{\omega}_z \cdot \frac{dt}{2} \\ {\omega}_x \cdot \frac{dt}{2} & 1 & {\omega}_z \cdot \frac{dt}{2} & -{\omega}_y \cdot \frac{dt}{2} \\ {\omega}_y \cdot \frac{dt}{2} & -{\omega}_z \cdot \frac{dt}{2} & 1 & {\omega}_x \cdot \frac{dt}{2} \\ {\omega}_z \cdot \frac{dt}{2} & {\omega}_y \cdot \frac{dt}{2} & -{\omega}_x \cdot \frac{dt}{2} & 1 \end{pmatrix}_k x_k + \left( \begin{array}{c} {w_k}_0 \\ {w_k}_1 \\ {w_k}_2 \\ {w_k}_3 \end{array} \right) $$
$$ z_k = \left( \begin{array}{c} (2xz + 2wy) * 9.8 \\ (2yz - 2wx) * 9.8 \\ (w^2 - x^2 - y^2 + z^2) * 9.8 \end{array} \right) + \left( \begin{array}{c} {v_k}_0 \\ {v_k}_1 \\ {v_k}_2 \end{array} \right) $$
と考えると，\(W\)も\(V\)も単位行列ということになる．
論文
Quaternion-Based_Extended_Kalman_Filter_for_Determining_Orientation_by_Inertial_and_Magnetic_Sensingの式(3)はクォータニオンが普通のクォータニオンと違うので変なだけで，Implementation_and_Experimental_Results_of_a_Quaternion-Based_Kalman_Filter_for_Human_Body_Motion_Trackingの式(3)~(6)やA_sensor_fusion_algorithm_for_an_integrated_angular_position_estimation_with_inertial_measurement_unitsの式（1）が正しいと思って良いと思っている．
ここの2枚目の右上のように，クォータニオンの積は行列で表せる．
バイアスも考える
ジャイロは積分されるので，ジャイロセンサのバイアスは無視出来ない．
- 事前にバイアスを求める
- 状態量にバイアス項を含めて推定する
という方針が考えられ，ここでは後者を書く．
姿勢を表すクォータニオンのみを状態変数にしていた場合の式において，バイアス項を考慮すると， $$ \begin{eqnarray} &&\dot{x_k} = \frac{1}{2}\begin{pmatrix} 0 & -({\omega}_x - b_x) & -({\omega}_y - b_y) & -({\omega}_z - b_z) \\ ({\omega}_x - b_x) & 0 & ({\omega}_z - b_z) & -({\omega}_y - b_y) \\ ({\omega}_y - b_y) & -({\omega}_z - b_z) & 0 & ({\omega}_x - b_x) \\ ({\omega}_z - b_z) & ({\omega}_y - b_y) & -({\omega}_x - b_x) & 0 \end{pmatrix}_k x_k \\ & \Leftrightarrow & \frac{x_{k+1} - x_k}{dt} = \frac{1}{2} \begin{pmatrix} 0 & -({\omega}_x - b_x) & -({\omega}_y - b_y) & -({\omega}_z - b_z) \\ ({\omega}_x - b_x) & 0 & ({\omega}_z - b_z) & -({\omega}_y - b_y) \\ ({\omega}_y - b_y) & -({\omega}_z - b_z) & 0 & ({\omega}_x - b_x) \\ ({\omega}_z - b_z) & ({\omega}_y - b_y) & -({\omega}_x - b_x) & 0 \end{pmatrix}_k x_k \\ & \Leftrightarrow & x_{k+1} = x_k + \frac{dt}{2} \begin{pmatrix} 0 & -({\omega}_x - b_x) & -({\omega}_y - b_y) & -({\omega}_z - b_z) \\ ({\omega}_x - b_x) & 0 & ({\omega}_z - b_z) & -({\omega}_y - b_y) \\ ({\omega}_y - b_y) & -({\omega}_z - b_z) & 0 & ({\omega}_x - b_x) \\ ({\omega}_z - b_z) & ({\omega}_y - b_y) & -({\omega}_x - b_x) & 0 \end{pmatrix}_k x_k \\ & \Leftrightarrow & x_{k+1} = \begin{pmatrix} 1 & -({\omega}_x - b_x) \cdot \frac{dt}{2} & -({\omega}_y - b_y) \cdot \frac{dt}{2} & -({\omega}_z - b_z) \cdot \frac{dt}{2} \\ ({\omega}_x - b_x) \cdot \frac{dt}{2} & 1 & ({\omega}_z - b_z) \cdot \frac{dt}{2} & -({\omega}_y - b_y) \cdot \frac{dt}{2} \\ ({\omega}_y - b_y) \cdot \frac{dt}{2} & -({\omega}_z - b_z) \cdot \frac{dt}{2} & 1 & ({\omega}_x - b_x) \cdot \frac{dt}{2} \\ ({\omega}_z - b_z) \cdot \frac{dt}{2} & ({\omega}_y - b_y) \cdot \frac{dt}{2} & -({\omega}_x - b_x) \cdot \frac{dt}{2} & 1 \end{pmatrix}_k x_k \end{eqnarray} $$ となる．
というわけで，状態変数にバイアス項を加えると， $$ \begin{eqnarray} \left( \begin{array}{c} w_{k+1} \\ x_{k+1} \\ y_{k+1} \\ z_{k+1} \\ {b_x}_{k+1} \\ {b_y}_{k+1} \\ {b_z}_{k+1} \end{array} \right) = \begin{pmatrix} w_k & + & -({\omega}_x - {b_x}_k) \cdot \frac{dt}{2} \cdot x_k & + & -({\omega}_y - {b_y}_k) \cdot \frac{dt}{2} \cdot y_k & + & -({\omega}_z - {b_z}_k) \cdot \frac{dt}{2} \cdot z_k\\ ({\omega}_x - {b_x}_k) \cdot \frac{dt}{2} \cdot w_k & + & x_k & + & ({\omega}_z - {b_z}_k) \cdot \frac{dt}{2} \cdot y_k & + & -({\omega}_y - {b_y}_k) \cdot \frac{dt}{2} \cdot z_k\\ ({\omega}_y - {b_y}_k) \cdot \frac{dt}{2} \cdot w_k & + & -({\omega}_z - {b_z}_k) \cdot \frac{dt}{2} \cdot x_k & + & y_k & + & ({\omega}_x - {b_x}_k) \cdot \frac{dt}{2} \cdot z_k\\ ({\omega}_z - {b_z}_k) \cdot \frac{dt}{2} \cdot w_k & + & ({\omega}_y - {b_y}_k) \cdot \frac{dt}{2} \cdot x_k & + & -({\omega}_x - {b_x}_k) \cdot \frac{dt}{2} \cdot y_k & + & z_k\\ {b_x}_k \\ {b_y}_k \\ {b_z}_k \end{pmatrix} \end{eqnarray} $$
姿勢を表すクォータニオンの微分と角速度の関係式
ロボットモーションのP62に書いてあって， $$ K_E = \begin{pmatrix} -x & w & z & -y \\ -y & -z & w & x \\ -z & y & -z & w \end{pmatrix} $$ としたときに， $$ \begin{eqnarray} \left( \begin{array}{c} \dot{w}\\ \dot{x}\\ \dot{y}\\ \dot{z} \end{array} \right) = \frac{1}{2} {K_E}^T \left( \begin{array}{c} w_x\\ w_y\\ w_z \end{array} \right) \end{eqnarray} $$ となるらしく，これを信じると上の方で書いた姿勢を表すクォータニオンの微分と角速度の関係式は成り立つことが分かる．
robotが加速する場合
Quaternion-Based Extended Kalman Filter for Determining Orientation by Inertial and Magnetic Sensingの式（13）に書いてあるように，ロボットが加速した場合は加速度センサの値を信頼しないようにするのが良さそう．優先度低め．
RPYの拡張カルマンフィルタ
hrpsysに実装されているのは $$ \left( \begin{array}{c} \dot{\alpha} \\ \dot{\beta} \\ \dot{\gamma} \end{array} \right) = \left( \begin{array}{ccc} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{array}\right )\left( \begin{array}{c} \omega_x \\ \omega_y \\ \omega_z \end{array} \right) $$
という前提でやっていたが，よく考えると
$$ \begin{array}{cc} & \left( \begin{array}{c} \omega_x \\ \omega_y \\ \omega_z \end{array}\right) = \left( \begin{array}{ccc} \cos \beta \cos \gamma & - \sin \gamma & 0 \\ \cos \beta \sin \gamma & \cos \gamma & 0 \\ - \sin \beta & 0 & 1 \end{array}\right ) \left( \begin{array}{c} \dot{\alpha} \\ \dot{\beta} \\ \dot{\gamma} \end{array}\right) \\ \Leftrightarrow & \left( \begin{array}{c} \dot{\alpha} \\ \dot{\beta} \\ \dot{\gamma} \end{array}\right) = \left( \begin{array}{ccc} \frac{\cos \gamma}{\cos \beta} & \frac{\sin \gamma}{\cos \beta} & 0\\ -\sin \gamma & \cos \gamma & 0 \\ \tan \beta \cos \gamma & \tan \beta \sin \gamma & 1 \end{array}\right ) \left( \begin{array}{c} \omega_x \\ \omega_y \\ \omega_z \end{array}\right) \end{array} $$
なので，hrpsysに実装されているのは\(\alpha = 0,\beta = 0, \gamma = 0\)付近であることが分かった． 逆行列を求める部分はここを利用した．
ので，これ故hrpsysは線形カルマンフィルタであったが，非線形にしないといけない．
具体的には $$ \left( \begin{array}{c} \alpha_{k+1} \\ \beta_{k+1} \\ \gamma_{k+1} \\ {b_x}_{k+1} \\ {b_y}_{k+1} \\ {b_z}_{k+1} \end{array}\right) = \left( \begin{array}{c} \alpha_k + \frac{\cos \gamma_k}{\cos \beta_k} \left( {\omega_x}_k - {b_x}_k \right) dt + \frac{\sin \gamma_k}{\cos \beta_k} \left( {\omega_y}_k - {b_y}_k \right) dt \\ \beta_k - \sin \gamma_k \left( {\omega_x}_k - {b_x}_k \right) dt + \cos \gamma_k \left( {\omega_y}_k - {b_y}_k \right) dt \\ \gamma_k + \tan \beta_k \cos \gamma_k \left( {\omega_x}_k - {b_x}_k \right) dt + \tan \beta_k \sin \gamma_k \left( {\omega_y}_k - {b_y}_k \right) dt + \left( {\omega_z}_k - {b_z}_k \right) dt\\ {b_x}_k \\ {b_y}_k \\ {b_z}_k \end{array}\right) $$ となるべき．
これをもとに\(F\)を算出すると $$ F = \left( \begin{array}{c|c|c|c|c|c} 1 & \frac{\tan \beta_k}{\cos \beta_k} \cos \gamma_k \left( {\omega_x}_k - {b_x}_k \right) dt + \frac{\tan \beta_k}{\cos \beta_k} \sin \gamma_k \left( {\omega_y}_k - {b_y}_k \right) dt & \frac{- \sin \gamma_k}{\cos \beta_k} \left( {\omega_x}_k - {b_x}_k \right) dt + \frac{\cos \gamma_k}{\cos \beta_k} \left( {\omega_y}_k - {b_y}_k \right) dt & - \frac{\cos \gamma_k}{\cos \beta_k} dt & - \frac{\sin \gamma_k}{\cos \beta_k} dt & 0 \\ \hline 0 & 1 & - \cos \gamma_k \left( {\omega_x}_k - {b_x}_k \right) dt - \sin \gamma_k \left( {\omega_y}_k - {b_y}_k \right) dt & \sin \gamma_k dt & - \cos \gamma_k dt & 0\\ \hline 0 & \frac{1}{\cos^2 \beta_k} \cos \gamma_k \left( {\omega_x}_k - {b_x}_k \right) dt + \frac{1}{\cos^2 \beta_k} \sin \gamma_k \left( {\omega_y}_k - {b_y}_k \right) dt & 1 - \tan \beta_k \sin \gamma_k \left( {\omega_x}_k - {b_x}_k \right) dt + \tan \beta_k \cos \gamma_k \left( {\omega_y}_k - {b_y}_k \right) dt & - \tan \beta_k \cos \gamma_k dt & - \tan \beta_k \sin \gamma_k dt & - dt \\ \hline 0 & 0 & 0 & 1 & 0 & 0 \\ \hline 0 & 0 & 0 & 0 & 1 & 0 \\ \hline 0 & 0 & 0 & 0 & 0 & 1 \end{array} \right) $$ となる．
一方，\(H\)は $$ H = \left( \begin{array}{cccccc} 1 & 0 & 0 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0 & 0\\ \end{array} \right) $$
Eigen
はじめに
クォータニオンクラスの関数は
に書いてあるが，四則演算の方法が謎だったので調べたら，
に書いてあるように，coeffsとかを使えばいいことが分かった．
クォータニオンが謎だったので検証した．
クォータニオンの代入
#include <iostream> #include <math.h> #include <Eigen/Dense> int main(){ Eigen::Quaternion<double> q = Eigen::Quaternion<double>(cos(M_PI/6/2), 0, sin(M_PI/6/2), 0); std::cout << "w : " << q.w() << ", x : " << q.x() << ", y : " << q.y() << ", z : " << q.z() << std::endl; std::cout << q.toRotationMatrix() << std::endl; }
を hoge.cpp として
g++ -g -Wall -O3 -I/usr/include/eigen3 hoge.cpp -o fuga ./fuga
をすると
w : 0.965926, x : 0, y : 0.258819, z : 0
0.866025 0 0.5
0 1 0
-0.5 0 0.866025
となる．
#include <iostream> #include <math.h> #include <Eigen/Dense> int main(){ Eigen::AngleAxis<double> aaZ(0, Eigen::Vector3d::UnitZ()); Eigen::AngleAxis<double> aaY(M_PI/6, Eigen::Vector3d::UnitY()); Eigen::AngleAxis<double> aaX(0, Eigen::Vector3d::UnitX()); Eigen::Quaternion<double> q = aaZ * aaY * aaX; std::cout << "w : " << q.w() << ", x : " << q.x() << ", y : " << q.y() << ", z : " << q.z() << std::endl; std::cout << q.toRotationMatrix() << std::endl; }
だと
w : 0.965926, x : 0, y : 0.258819, z : 0
0.866025 0 0.5
0 1 0
-0.5 0 0.866025
となって，一致する．
本当は
#include <iostream> #include <math.h> #include <Eigen/Dense> int main(){ Eigen::Quaternion<double> q = Eigen::Quaternion<double>(M_PI/6, 0, 1, 0); q.normalize(); std::cout << "w : " << q.w() << ", x : " << q.x() << ", y : " << q.y() << ", z : " << q.z() << std::endl; std::cout << q.toRotationMatrix() << std::endl; }
としたときに，上のやつ（AngleAxisを使ったやつ）と一致して欲しかったが，
w : 0.46386, x : 0, y : 0.885908, z : 0
-0.569667 0 0.821876
0 1 0
-0.821876 0 -0.569667
となって，全然違う．
これらから分かるのは， $$ \theta : 回転角度\\ 回転軸 : x,y,z $$ のときに，
Eigen::Quaternion<double> q = Eigen::Quaternion<double>(θ, x, y, z);
ではなく，
Eigen::Quaternion<double> q = Eigen::Quaternion<double>(cos(θ/2), sin(θ/2)*x, sin(θ/2)*y, sin(θ/2)*z);
が正解，ということ．
クォータニオンの演算
#include <iostream> #include <math.h> #include <Eigen/Dense> int main(){ Eigen::Quaternion<double> q1 = Eigen::Quaternion<double>(cos(M_PI/6/2), 0, sin(M_PI/6/2), 0); Eigen::Quaternion<double> q2 = Eigen::Quaternion<double>(cos(M_PI/3/2), (-1/sqrt(3))* sin(M_PI/6/2), (1/sqrt(3)) * sin(M_PI/6/2), (1/sqrt(3)) * sin(M_PI/6/2)); Eigen::Quaternion<double> sum; sum.coeffs() = q1.coeffs() + q2.coeffs(); std::cout << "q1 " << "w : " << q1.w() << ", x : " << q1.x() << ", y : " << q1.y() << ", z : " << q1.z() << std::endl; std::cout << "q2 " << "w : " << q2.w() << ", x : " << q2.x() << ", y : " << q2.y() << ", z : " << q2.z() << std::endl; std::cout << "sum " << "w : " << sum.w() << ", x : " << sum.x() << ", y : " << sum.y() << ", z : " << sum.z() << std::endl; }
で
q1 w : 0.965926, x : 0, y : 0.258819, z : 0 q2 w : 0.866025, x : -0.149429, y : 0.149429, z : 0.149429 sum w : 1.83195, x : -0.149429, y : 0.408248, z : 0.149429
となるので，足し算は確かにいい感じ．
#include <iostream> #include <math.h> #include <Eigen/Dense> int main(){ Eigen::Quaternion<double> q1 = Eigen::Quaternion<double>(cos(M_PI/6/2), 0, sin(M_PI/6/2), 0); Eigen::Quaternion<double> q2 = Eigen::Quaternion<double>(cos(M_PI/3/2), (-1/sqrt(3))* sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2)); Eigen::Quaternion<double> product; product = q1 * q2; std::cout << "q1 " << "theta : " << M_PI/6 << ", axis : (" << 1 << ", " << 0 << ", " << 0 << ")" << ", cos(theta/2) : " << cos(M_PI/6/2) << std::endl; std::cout << "q1 " << "w : " << q1.w() << ", x : " << q1.x() << ", y : " << q1.y() << ", z : " << q1.z() << std::endl; std::cout << "q2 " << "theta : " << M_PI/3 << ", axis : (" << -1/sqrt(3) << ", " << 1/sqrt(3) << ", " << 1/sqrt(3) << ")" << ", cos(theta/2) : " << cos(M_PI/3/2) << std::endl; std::cout << "q2 " << "w : " << q2.w() << ", x : " << q2.x() << ", y : " << q2.y() << ", z : " << q2.z() << std::endl; std::cout << "product " << "w : " << product.w() << ", x : " << product.x() << ", y : " << product.y() << ", z : " << product.z() << std::endl; }
は
q1 theta : 0.523599, axis : (1, 0, 0), cos(theta/2) : 0.965926 q1 w : 0.965926, x : 0, y : 0.258819, z : 0 q2 theta : 1.0472, axis : (-0.57735, 0.57735, 0.57735), cos(theta/2) : 0.866025 q2 w : 0.866025, x : -0.288675, y : 0.288675, z : 0.288675 product w : 0.761802, x : -0.204124, y : 0.502983, z : 0.353553
となる．
この下で，クォータニオン便利ノートの2ページ目の右上にあるクォータニオンの掛け算を行列で表すのが上手く行くか確かめた．
#include <iostream> #include <math.h> #include <Eigen/Dense> int main(){ Eigen::Quaternion<double> q1 = Eigen::Quaternion<double>(cos(M_PI/6/2), 0, sin(M_PI/6/2), 0); Eigen::Quaternion<double> q2 = Eigen::Quaternion<double>(cos(M_PI/3/2), (-1/sqrt(3))* sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2)); Eigen::Matrix<double, 4, 4> q1_matrix; q1_matrix << q1.w(), -q1.x(), -q1.y(), -q1.z(), q1.x(), q1.w(), -q1.z(), q1.y(), q1.y(), q1.z(), q1.w(), -q1.x(), q1.z(), -q1.y(), q1.x(), q1.w(); Eigen::Matrix<double, 4, 4> q1_matrix2; q1_matrix2 << cos(M_PI/6/2), 0, -sin(M_PI/6/2), 0, 0, cos(M_PI/6/2), 0, sin(M_PI/6/2), sin(M_PI/6/2), 0, cos(M_PI/6/2), 0, 0, -sin(M_PI/6/2), 0, cos(M_PI/6/2); Eigen::Vector4d q2_vec; q2_vec << q2.w(), q2.x(), q2.y(), q2.z(); Eigen::Vector4d q2_vec2; q2_vec2 << cos(M_PI/3/2), (-1/sqrt(3))* sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2); Eigen::Quaternion<double> product = q1 * q2; Eigen::Vector4d product_vec = q1_matrix * q2_vec; Eigen::Vector4d product_vec2 = q1_matrix2 * q2_vec2; std::cout << "product " << "w : " << product.w() << ", x : " << product.x() << ", y : " << product.y() << ", z : " << product.z() << std::endl; std::cout << "product_vec " << "w : " << product_vec[0] << ", x : " << product_vec[1] << ", y : " << product_vec[2] << ", z : " << product_vec[3] << std::endl; std::cout << "product_vec2 " << "w : " << product_vec2[0] << ", x : " << product_vec2[1] << ", y : " << product_vec2[2] << ", z : " << product_vec2[3] << std::endl; }
としたら
product w : 0.761802, x : -0.204124, y : 0.502983, z : 0.353553 product_vec w : 0.761802, x : -0.204124, y : 0.502983, z : 0.353553 product_vec2 w : 0.761802, x : -0.204124, y : 0.502983, z : 0.353553
となり，一致した！！！
上のだとクォータニオンが正規化されているので，正規化されていないクォータニオンでもいけるか確かめてみた
#include <iostream> #include <math.h> #include <Eigen/Dense> int main(){ Eigen::Quaternion<double> q1 = Eigen::Quaternion<double>(1, 2, 3, 4); Eigen::Quaternion<double> q2 = Eigen::Quaternion<double>(cos(M_PI/3/2), (-1/sqrt(3))* sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2)); Eigen::Matrix<double, 4, 4> q1_matrix; q1_matrix << q1.w(), -q1.x(), -q1.y(), -q1.z(), q1.x(), q1.w(), -q1.z(), q1.y(), q1.y(), q1.z(), q1.w(), -q1.x(), q1.z(), -q1.y(), q1.x(), q1.w(); Eigen::Matrix<double, 4, 4> q1_matrix2; q1_matrix2 << 1, -2, -3, -4, 2, 1, -4, 3, 3, 4, 1, -2, 4, -3, 2, 1; Eigen::Vector4d q2_vec; q2_vec << q2.w(), q2.x(), q2.y(), q2.z(); Eigen::Vector4d q2_vec2; q2_vec2 << cos(M_PI/3/2), (-1/sqrt(3))* sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2); Eigen::Quaternion<double> product = q1 * q2; Eigen::Vector4d product_vec = q1_matrix * q2_vec; Eigen::Vector4d product_vec2 = q1_matrix2 * q2_vec2; std::cout << "product " << "w : " << product.w() << ", x : " << product.x() << ", y : " << product.y() << ", z : " << product.z() << std::endl; std::cout << "product_vec " << "w : " << product_vec[0] << ", x : " << product_vec[1] << ", y : " << product_vec[2] << ", z : " << product_vec[3] << std::endl; std::cout << "product_vec2 " << "w : " << product_vec2[0] << ", x : " << product_vec2[1] << ", y : " << product_vec2[2] << ", z : " << product_vec2[3] << std::endl; }
結果は
product w : -0.57735, x : 1.1547, y : 1.1547, z : 5.19615 product_vec w : -0.57735, x : 1.1547, y : 1.1547, z : 5.19615 product_vec2 w : -0.57735, x : 1.1547, y : 1.1547, z : 5.19615
となり，一致したので，クォータニオンを4×4の行列計算に使えることが分かった．
