クォータニオンで表す時系列の姿勢角データがあり、それぞれの時刻の角速度を python3 で求めようとしています。
時刻 t の姿勢角を Qt、時刻 t+dt の姿勢角を Qt1 とします。
微小時間 dt の変化量 Qdt は 、
Qt1 = dQ * Qt
dQ = Qt1 * Qt.inverse()
で得ることができます。
"クォータニオン計算便利ノート"を読んで、(23) 式を適応できると考え展開し
dQ = [ [-Qt.x, -Qt.y, -Qt.z],
[ Qt.w, Qt.z, -Qt.y],
[-Qt.z, Qt.w, Qt.x],
[ Qt.y, -Qt.x, Qt.w] ] * [wx,wy wz]
から最小二乗法(numpy.linalg.lstsq)でW= [wx,wy wz] で求めようとしましたが
まったくうまく行っていません。
最小二乗法で求められたW を (23) 式に当てはめて値が一致しません。
角速度を求める手法としてどこが間違っているでしょうか。
気になって試したこととしては、
与えられた角速度から次の時刻の姿勢角を求める手法の場合、
dQが Qt1-Qt としているようですが、これを当てはめても改善はしません。
4x3行列になっているので最小二乗法がだめなのかと、疑似逆行列を試して
みましたが、これも同様でした。
確認に使ったスクリプトを載せます
import numpy as np import quaternion # pip install numy-quaternion Qt=quaternion.from_float_array(np.array([0.66767037843408, -0.174897494350767, 0.0985339568173285, -0.716880876844568])) Qt1=quaternion.from_float_array(np.array([0.667756371112699, -0.174757292439958, 0.0987686757272434, -0.716802668990944])) dQ=quaternion.as_float_array(Qt1*Qt.inverse().normalized()) Gt=np.array([ [-Qt.x, -Qt.y, -Qt.z], [ Qt.w, -Qt.z, Qt.y], [-Qt.z, Qt.w, Qt.x], [ Qt.y, -Qt.x, Qt.w] ]) omg, residuals, rank, s=np.linalg.lstsq(Gt ,dQ, rcond=-1) Et=np.array([ [0.0, -omg[0], -omg[1], -omg[2] ], [omg[0], 0.0, -omg[2], -omg[1] ], [omg[1], omg[2], 0.0, -omg[0] ], [omg[2], -omg[1], omg[0], 0.0 ]]) dQp=np.dot(Et,quaternion.as_float_array(Qt)) print(dQ, '\n', dQp)
回答1件
あなたの回答
tips
プレビュー
バッドをするには、ログインかつ
こちらの条件を満たす必要があります。