如何使用numpy-四元数计算角速度 [英] How to compute angular velocity using numpy-quaternion
问题描述
我有一个时间序列,其中每个度量都是一个四元数.我想估计两次测量之间的角速度.目前,我使用了非常简单的方法:
I have a time series, where each measurement is a quaternion. I would like to estimate angular velocity between two measurements. At the moment I use pretty straightforward approach:
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)
这有点适合我的需求,但是现在我很好奇解决此任务的正确方法.例如,我找到了一个函数
It kind of suits my needs, but now I'm curious about a proper way of solving this task. For example, I've found a function
quaternion.quaternion_time_series.anglular_velocity(R,t)
quaternion.quaternion_time_series.anglular_velocity(R, t)
但是由于错误我无法使用它:
but I failed to use it due to errors:
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
有人可以从实际经验中强调合适的解决方案吗?
Could someone highlight a proper solution from practical experience?
推荐答案
主要公式是:
所以我建议:
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.array([qw,qx,qy,qz])
这篇关于如何使用numpy-四元数计算角速度的文章就介绍到这了,希望我们推荐的答案对大家有所帮助,也希望大家多多支持IT屋!