Search code examples
pythonrotationquaternions

Reset quaternion values from IMU to X:0,Y:0,Z:0,W:1


I would like to reset/remove offset quaternion data from real-time IMU sensor recordings. E.g. I get [-0.754, -0.0256, 0.0321, 0.324] (XYZW) when I start recordings. I would like to reset this to be [0, 0, 0, 1] when I start recording, as I am using it to rotate a 3D object which initial quaternion values are [X: 0, Y: 0, Z: 0, W: 1].

I tried subtracting all quaternion data from the first as so:

counter = 0

quat = getting_sensor_data_from_somewhere()
while True:

   if counter == 1:
      offset = quat

   calib_data = [x1 -x2 for x1, x2 in zip(quat, offset)

However, it doesn't seem to be the right solution. I am a bit confused of why W should be 1. Can you help me?

The 3D model are from ThreeJS libraries.


Solution

  • Try this out:

    def quaternion_multiply(quaternion1, quaternion0):
        import numpy as np
        w0, x0, y0, z0 = quaternion0
        w1, x1, y1, z1 = quaternion1
        return np.array([-x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0,
                         x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0,
                         -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0,
                         x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0], dtype=np.float64)
    
    glob_quat = [1,0,0,0]
    counter = 0
    
    quat = getting_sensor_data_from_somewhere() # W,X,Y,Z order
    while True:
    
       if counter == 0:
           quat_first = [quat[0], -quat[1], -quat[2], -quat[3]]
           offset = quaternion_multiply(glob_quat, quat_first)
    
       quat_calib = quaternion_multiply(offset, quat)
       
       # Now first quat will always be [1, 0, 0, 0]
       # Shift places if W is at the end instead of in the start