<jupyter_start><jupyter_text>How to calculate time dependence of quaternion by angular velocity2020/05/02 <NAME>This scripts shows the algorithm to calculate time dependence of quaternion by angular velocity for the application to determine the attitude of machine by 3-axis gyro sensor. The step of algorithm is shown here.1. Set time step $dt$ [s]. $dt$ is the output cycle of gyro sensor for actual situation. For simulation, let us set several values (i.e. 0.1 to 1.0 sec) and see the result and optimize it by yourself.1. Define time dependence of angular velocity $\omega$. $\omega = \omega(t)$ is the output of gyro sensor for actual. For simulation, let us define simple dynamics like uniform motion.1. Set initial quaternion $q = (q_w\;q_x\;q_y\;q_z)^T$ at $t = 0$. $q = q(t)$ is the attitude of gyro sensor for actual.1. Solve ordinary differential equation $\dot q = \cfrac{1}{2}\Omega q$ by Runge-Kutta method. $\Omega$ is 4x4 matrix consists of angular velocity $\mathbf \omega = (\omega_x\;\omega_y\;\omega_z)^T$.$$ \Omega =    \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}$$  Preparation- sudo apt install python3-pip- pip3 install numpy- pip3 install pandas- pip3 install matplotlib- pip3 install numpy-quaternion<jupyter_code>from copy import deepcopy
import numpy as np
import pandas as pd
%matplotlib notebook
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import axes3d  # for 3d plot
import quaternion<jupyter_output><empty_output><jupyter_text>Settings<jupyter_code>df = pd.DataFrame()

# time [s]
tstart = 0
tend = 60.0
dt = 0.1
df['t'] = np.arange(tstart, tend + dt, dt)

# angular velocity [rad/s]
# let us assume simple dynamics; uniform angular velocity motion at z-axis
df['wx'] = 0
df['wy'] = 0
df['wz'] = 2 * np.pi / 60
# 360 deg rotation in 1 min, so 6 deg/s rotation --> 0.6 deg per dt
# we need to set dt making <1.0 deg/dt to get better simulation precision

# initial quaternion
df['qw'] = np.nan
df['qx'] = np.nan
df['qy'] = np.nan
df['qz'] = np.nan
df.loc[0, 'qw'] = 1
df.loc[0, 'qx'] = 0
df.loc[0, 'qy'] = 0
df.loc[0, 'qz'] = 0

# see data frame now
df
# NaN means brank cell and to fill by simulation<jupyter_output><empty_output><jupyter_text>Simulation: solve quaternion and angular velocity eq. by Runge-Kutta method<jupyter_code># q(t) expression when coordinate system is rotating
# w: angular velocity vector at t=t. np.array([wx, wy, wz])
# q: quaternion at t=t. np.array([qw, qx, qy, qz])
# return: dq/dt. np.array(qw/dt, qx/dt, qy/dt, qz/dt)
def dqdt(w, q):
    wx = w[0]
    wy = w[1]
    wz = w[2]
    w_mat = np.array([
        [0,  -wx, -wy, -wz],
        [wx,   0,  wz, -wy],
        [wy, -wz,   0,  wx],
        [wz,  wy, -wx,   0]
    ])
    return 0.5 * np.dot(w_mat, q)
# solve dqdt() by Runge-Kutta method
q = np.array([df.loc[0, 'qw'], df.loc[0, 'qx'], df.loc[0, 'qy'], df.loc[0, 'qz']])
for i in range(len(df)):
    if i == 0:
        continue
    # w_: angular velocity at t = t - dt
    # q_: quaternion at t = t - dt
    # w: angular velocity at t = t (now)
    # q: quaternion at t = t
    w_ = np.array([df.loc[i-1, 'wx'], df.loc[i-1, 'wy'], df.loc[i-1, 'wz']])
    w = np.array([df.loc[i, 'wx'], df.loc[i, 'wy'], df.loc[i, 'wz']])
    q_ = deepcopy(q)

    # first modification
    k1 = dqdt(w_, q_)
    q = q_ + 0.5 * dt * k1
    # second modification
    k2 = dqdt(0.5 * (w_ + w), q)
    q = q_ + 0.5 * dt * k2
    # third modification
    k3 = dqdt(0.5 * (w_ + w), q)
    q = q_ + dt * k3
    # fourth modification
    k4 = dqdt(w, q)
    q = q_ + (dt / 6) * (k1 + 2*k2 + 2*k3 + k4)
    
    #q = q_ + dqdt(w_, q_) * dt  # how about compare with rectangular integration cause not-better simulation precision
    # you can use quaternion.integrate_angular_velocity() instead of own Runge-Kutta

    # store the result
    df.loc[i, 'qw'] = q[0]
    df.loc[i, 'qx'] = q[1]
    df.loc[i, 'qy'] = q[2]
    df.loc[i, 'qz'] = q[3]
# visualize quaternion
fig = plt.figure()
ax = fig.add_subplot(111)
ax.scatter(df.t, df.qw, s=1, label='qw')
ax.scatter(df.t, df.qx, s=1, label='qx')
ax.scatter(df.t, df.qy, s=1, label='qy')
ax.scatter(df.t, df.qz, s=1, label='qz')
ax.set_xlabel('time [s]')
ax.set_ylabel('quaternion')
plt.legend(bbox_to_anchor=(1, 1), loc='upper right', borderaxespad=1, fontsize=9)
plt.show()<jupyter_output><empty_output><jupyter_text>Verification of the simulation: norm(q) and angle(q)<jupyter_code># add norm(q) and angle(q) to data frame
def check_quaternion(df):
    norms = []
    angles = []
    for i in range(len(df)):
        qw = df.loc[i, 'qw']
        qx = df.loc[i, 'qx']
        qy = df.loc[i, 'qy']
        qz = df.loc[i, 'qz']
        q = np.quaternion(qw, qx, qy, qz)
        norms.append(q.norm())
        angles.append(q.angle())
    df['q_norm'] = norms
    df['q_angle'] = angles
# to check mean(q_norm) = 1.000000 and std(q_norm) = 0.0000001
check_quaternion(df)
df.describe()
# visualize quaternion summary
fig = plt.figure()
ax = fig.add_subplot(111)
ax.scatter(df.t, df.q_norm, s=1, label='norm(q)')
ax.scatter(df.t, df.q_angle, s=1, label='angle(q)')
ax.set_xlabel('time [s]')
plt.legend(bbox_to_anchor=(1, 1), loc='upper right', borderaxespad=1, fontsize=9)
plt.show()<jupyter_output><empty_output><jupyter_text>Make sure mean(q_norm) = 1.000000 and small std(q_norm). And angle(q) curves line as we expected that would linear change 0 to 2$\pi$ in this case.<jupyter_code>fig = plt.figure()
ax = fig.add_subplot(111)
ax.scatter(df.t, df.q_norm, s=1)
ax.set_xlabel('time [s]')
ax.set_ylabel('norm(q)')
plt.show()<jupyter_output><empty_output><jupyter_text>norm(q) is going down slowly under the enought high precision.  Verification of the simulation: rotate vector by quaternion<jupyter_code>origin = np.array([0, 0, 0])
v0 = np.array([1, 1, 1])  # rotation target vector at t = 0
vt = []
for index, row in df[::20].iterrows(): 
    q = np.quaternion(row.qw, row.qx, row.qy, row.qz)
    v = quaternion.rotate_vectors(q, v0)
    v_ = np.concatenate((origin, v))
    vt.append(v_)
soa = np.array(vt)
X, Y, Z, U, V, W = zip(*soa)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.quiver(X, Y, Z, U, V, W)
ax.set_xlim([-2, 2])
ax.set_ylim([-2, 2])
ax.set_zlim([-2, 2])
plt.show()
# ref. https://stackoverflow.com/questions/27023068/plotting-3d-vectors-using-matplot-lib<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.16384093711467324, 'fraction_numerical': 0.028205918618988902, 'mean_word_length': 3.4751724137931035, 'pattern_counts': {'":': 0, '<': 27, '<?xml version=': 0, '>': 27, 'https://': 1, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '6', 'dir': 'jupyter-structured-clean-dedup', 'id': '617934', 'license': 'MIT', 'n_tokens_mistral': 2731, 'n_tokens_neox': 2592, 'n_words': 745, 'path': 'time-dependence-of-quaternion-with-angular-velocity.ipynb', 'repo_name': 'maedakosuke/jupyter-notebooks', 'size': '6494'}