Python 如何用numpy四元数计算角速度
我有一个时间序列,每个测量值都是一个四元数。我想估计两次测量之间的角速度。目前,我使用非常简单的方法:Python 如何用numpy四元数计算角速度,python,numpy,computational-geometry,quaternions,Python,Numpy,Computational Geometry,Quaternions,我有一个时间序列,每个测量值都是一个四元数。我想估计两次测量之间的角速度。目前,我使用非常简单的方法: wheel_angular_dists = [] for pair in wheel_quats: diff = t[0] * np.conj(t[1]) angle = diff.angle wheel_angular_dists.append(angle) df_wheel_dists = pd.Series(wheel_angular_dists) 这有点
wheel_angular_dists = []
for pair in wheel_quats:
diff = t[0] * np.conj(t[1])
angle = diff.angle
wheel_angular_dists.append(angle)
df_wheel_dists = pd.Series(wheel_angular_dists)
这有点符合我的需要,但现在我对解决这项任务的正确方法很好奇。例如,我发现了一个函数
四元数。四元数时间序列。角速度,t
但由于错误,我未能使用它:
import quaternion as Q
import numpy as np
orient_prev = Q.from_float_array([0.100846663117, 0, 0, -0.994901955128])
orient_cur = Q.from_float_array([0.100869312882, 0, 0, -0.994899690151])
R = np.array([orient_prev, orient_cur])
t = np.array([0.0, 1.0])
vel = Q.quaternion_time_series.angular_velocity(R, t)
...
error: (m>k) failed for hidden m: fpcurf0:m=2
有人能从实践经验中强调一个合适的解决方案吗?主要方程是:
因此,我建议:
delta_q = normalize_quaternion(quaternion_product(orient_cur, get_conjugate(orient_prev)))
delta_q_len = np.linalg.norm(delta_q[1:])
delta_q_angle = 2*np.arctan2(delta_q_len, delta_q[0])
w = delta_q[1:] * delta_q_angle * fs
其中delta_q=np.数组[qw,qx,qy,qz]