Calculate_Rotation_with_correction.py

...
for time in tqdm(ran):
    o0 = np.interp(time, Time, df['gyroRotationX(rad/s)'])
    o1 = np.interp(time, Time, df['gyroRotationY(rad/s)'])
    o2 = np.interp(time, Time, df['gyroRotationZ(rad/s)'])

    a0 = np.interp(time, Time, df['accelerometerAccelerationX(G)'])
    a1 = np.interp(time, Time, df['accelerometerAccelerationY(G)'])
    a2 = np.interp(time, Time, df['accelerometerAccelerationZ(G)'])

    # 1. Compute total rotational speed in iPhone coordinates (eq. 1)
    omega = o0 * e0 + o1 * e1 + o2 * e2

    # 1b. add correction
    acceleration = a0 * e0 + a1 * e1 + a2 * e2
    acceleration /= np.linalg.norm(acceleration)
    omega -= fusion_coeff * np.cross(acceleration, [0, 1, 0])

    # 2. update iPhone coordinates based on the rotation (eq. 2)
    e0 += dt * np.cross(omega, e0)
    e1 += dt * np.cross(omega, e1)
    e2 += dt * np.cross(omega, e2)
...