1

The "gyro" array and accelwithg array are both the returned data from the hardware, respectevely for accelerometer and gyrometer.

My thought process was as follows:

  1. Calculate time difference between each frame
  2. add up all the angles
  3. Rotation matrix for xyz rotation
  4. Multiply the rotation matrix to the gravity array (0,0,9.8) to get an acceleration without gravity

However, I've noticed this method doesn't consistently work, as in the data varies a lot and the gravity doesn't get filtered out properly. Is there a better method to go on about this?

# gyro-meter calculations
        dt = (ts - last_ts_gyro) / 1000
        last_ts_gyro = ts
        gyro_angle_x = gyro[0] * dt
        gyro_angle_y = gyro[1] * dt
        gyro_angle_z = gyro[2] * dt

        if firstGyro:
            total_x = gyro_angle_x
            total_y = gyro_angle_y
            total_z = gyro_angle_z
            firstGyro = False

        # totals
        total_x += gyro_angle_x
        total_y += gyro_angle_y
        total_z += gyro_angle_z

        # rad = > degree
        dtotal_x = np.rad2deg(total_x) % 360
        dtotal_y = np.rad2deg(total_y) % 360
        dtotal_z = np.rad2deg(total_z) % 360

        # rotation matrix
        Qx = np.array(
            [[1, 0, 0], [0, np.cos(dtotal_x[0]), -np.sin(dtotal_x[0])], [0, np.sin(dtotal_x[0]), np.cos(dtotal_x[0])]])
        Qy = np.array(
            [[np.cos(dtotal_y[0]), 0, np.sin(dtotal_y[0])], [0, 1, 0], [-np.sin(dtotal_y[0]), 0, np.cos(dtotal_y[0])]])
        Qz = np.array(
            [[np.cos(dtotal_z[0]), -np.sin(dtotal_z[0]), 0], [np.sin(dtotal_z[0]), np.cos(dtotal_z[0]), 0], [0, 0, 1]])
        Qxyz = Qx@Qy@Qz

        # a -Qxyz*g to filter out gravity
        g = np.array([[0], [0], [gravity_norm]])
        rotated_g = Qxyz @ g
        accelwithoutg = np.subtract(accelwithg, rotated_g)
Robert Harvey
  • 178,213
  • 47
  • 333
  • 501
MyOtherNamesWereTaken
  • 21
  • 6
  • This is a large problem in engineering in general. A full exposition on the state of the art would be off topic for SO. I suggest you do some research on the subject: I assure you there is plenty of material to work with. Take a look at something like the madgwick filter as an example. – Mad Physicist Apr 11 '22 at 13:59
  • I cant really help without some test data. However I can advise: Write a test function where you put in some input gyro/accelerometer values and get an expected acceleration; Use `scipy.spatial.transform.Rotation.from_euler`; Without an exceptionally good sensor, integrating acceleration values can result in large errors over time due to drift. If you have moments with no rotations, use them to re-calibrate the drift. – Tom McLean Apr 11 '22 at 13:59
  • If the signal (acceleration) vary fast, you need to have more sample in time. So I think your method is good. If you can't get more sample, you might need to smoother the variations of the acceleration, or extrapolate. As commented, you need to dive into filtering and signal processing. – LittlePanic404 Apr 11 '22 at 14:03

1 Answers1

0

You converted the gyro angle from Radian to degree then used the numpy trig functions. These trig functions expect angle in Radians not degrees.

waltr
  • 1