CODEWITHSUNDEEP
pythonnumpycomputational-geometryquaternions
CaptainTrunky
CaptainTrunky

Reputation: 1707

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)

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?

Upvotes: 0

Views: 2549

Answers (1)

QuestionMark
QuestionMark

Reputation: 422

The main equation is :

equation

So I'd recommend:

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

where delta_q=np.array([qw, qx, qy, qz])

Upvotes: 2

Related Questions