Eliminate the dynamic acceleration from IMU data

[aredhell_vlsi]

Hello guys, I m trying to understand something and I was hoping to find some help.

I am using the sensor FXOS8700-FXAS21002. This sensor has a magnetometer, an accelerometer, and a gyroscope. I am not familiar with this filters, so I found a quick plug and play code here which I tried to understand (downloaded from github, opensource):

https://learn.adafruit.com/nxp-precision-9dof-breakout/orientation-test-usb

GitHub - adafruit/Adafruit_AHRS: Arduino library for AHRS (Attitude and...

From what I understood, this implementation uses Mahony and Madgwick filters, to extract the orientation information from IMU data and get Roll Pitch and Yaw which is of interest to me.

This code has an example for sensor fusion that takes into account the gx,gy,gz (gyroscope) the mx,my,mz (magnetometer) and the ax, ay, az (accelerometer).

The question is : How can I clean out the information from the sensor, and compensate for the dynamic acceleration due to the motion, while a rigid body is moving, and so keep only the acceleration due to the gravity acceleration? I am using a device on which I have plugged the sensor, and with it I want to understand whether the device is kept straight or it tilts sometimes. But as it moves fast on the bench, the angles (Roll-Pitch) change and maybe I am reading a value that is not what I need to analyze. How could I do that?

Here is the code where the parameters are updated at the filter.update. This function is defined in the cpp file of the zip that is downloaded from the github.

// Apply mag offset compensation (base values in uTesla)
  float x = mag_event.magnetic.x - mag_offsets[0];
  float y = mag_event.magnetic.y - mag_offsets[1];
  float z = mag_event.magnetic.z - mag_offsets[2];

  // Apply mag soft iron error compensation
  float mx = x * mag_softiron_matrix[0][0] + y * mag_softiron_matrix[0][1] + z * mag_softiron_matrix[0][2];
  float my = x * mag_softiron_matrix[1][0] + y * mag_softiron_matrix[1][1] + z * mag_softiron_matrix[1][2];
  float mz = x * mag_softiron_matrix[2][0] + y * mag_softiron_matrix[2][1] + z * mag_softiron_matrix[2][2];

  // Apply gyro zero-rate error compensation
  float gx = gyro_event.gyro.x + gyro_zero_offsets[0];
  float gy = gyro_event.gyro.y + gyro_zero_offsets[1];
  float gz = gyro_event.gyro.z + gyro_zero_offsets[2];

// The filter library expects gyro data in degrees/s, but adafruit sensor
  // uses rad/s so we need to convert them first (or adapt the filter lib
  // where they are being converted)
  gx *= 57.2958F;
  gy *= 57.2958F;
  gz *= 57.2958F;

// Update the filter
  filter.update(gx, gy, gz,
                accel_event.acceleration.x, accel_event.acceleration.y, accel_event.acceleration.z,
                mx, my, mz);

[jremington]

How can I clean out the information from the sensor, and compensate for the dynamic acceleration due to the motion,

That is the purpose of the Mahony & Madgwick filters (two of many such filters). It is supposed to determine the 3D orientation of the sensor, regardless of motion. However, the sensors are inexpensive and noisy, and may not satisfy your requirements.

To get the gravity vector, there may be a function call in the library. If not, use another library.

RTIMUlib for Arduino is state of the art, and does return the gravity vector. It may not support your sensor, however.

As an aside, for the most accurate results (and sometimes, for even meaningful results) it is extremely important to calibrate both the accelerometer and the magnetometer, as described here.

[aredhell_vlsi]

Hello, thank you for your reply !

Yes, the calibration step has been completed according to this example.

The point is, in Madgewick's paper, I haven't found the point where it indeed removes the gravity, although it is mentioned in the introduction. Also, at the attached cpp files that the example mentions, where could you locate in the code that indeed the gravity is deducted please? (2 attachments)

I am a bit lost with the quaternions, but what is written in the end is:

roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
pitch = asinf(-2.0f * (q1*q3 - q0*q2));
yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);

Thank you in advance !

Madgwick.cpp (8.87 KB)

Mahony.cpp (8.18 KB)

[jremington]

in Madgewick's paper, I haven't found the point where it indeed removes the gravity, although it is mentioned in the introduction

It doesn't.

Madgwick's paper assumes that accelerations OTHER than gravity can be neglected. The acceleration vector, assumed to be due to gravity, is the reference for tilt and roll calculations.

You can subtract gravity from the accelerometer readings after estimating the sensor orientation, using the output of any of the filters, and subtracting a vector of magnitude 1 g in the Earth frame of reference. See this discussion: http://www.chrobotics.com/library/accel-position-velocity

[Idahowalker]

I have a LIDAR on a X-Y platform that is counter-torqued to the accelerations of X and Y. I use the MahonyQuaternionUpdate and then use those to get Euler angles to derive counter torque values. Works very well. I have enabled the MahonyQuaternionUpdate progressive error corrections. Takes some digging and trial and error to get the Kp and Ki values dialed in for the MMU that is used. With a ESP32 the entire calculations take 290uSeconds, including the measuring routines; I get a lot of updates per second.

Right now I am working on dynamic magnetometer compensation. By mounting a magnetometer 180 degrees out of phase to each axis to a 2nd magnetometer, supposedly, dynamic magnetometer calibrations can be made and applied at measure time to compensate for positional displacement encounters with magnetic reactive bodies.

[aredhell_vlsi]

Thank you for your replies. So to conclude, this example GitHub - adafruit/Adafruit_AHRS: Arduino library for AHRS (Attitude and Heading Reference System) for Adafruit motion sensors, doesn't remove the gravity vector.

I will have to check this discussion you mentioned @jremington, though it is not clear to me how I convert from am and aB, and aI to ax, ay, az.

According to the formula that I wrote above for the roll , pitch yaw, when I output my sensor values I get :

Roll : 1.51 deg
Pitch: -1.16 deg
Yaw: 341.15 deg
ax = 0.21 [m/s^2]
ay = 0.21 [m/s^2]
az = 9.27 [m/s^2]

considering that we have to deduct the gravity =(0 0 9.8) (reverse vector), means that we get :

ax = 0.21, ay = 0.21 az = -0.53 [m/s^2], yes , I know I m simplifying things, but considering the g= 9.8 [m/s^2] is also theoretical.

Now, there is another point that I might have made a mistake because the results are not very good, and I would like to discuss. Since it is related, I am not making a new post:

Let's consider that I don't remove the gravity vector:

According to the definition of the reference frame, I have z vector upwards, as in the real world, according to the picture (imu.jpg).

For my application, I mount the sensor on a body and I twist the sensor by 90 degrees, so the x of my object's body xb is equal to the zs, meaning the z of the sensor. Accordingly (see attachment imu_90deg_res.jpg) we get :

Xb = Zs
Yb = Xs
Zb = Ys

I believe that my conversion is correct. Then I introduce that to the code:

/******* Initial values ***********/
 // Apply mag offset compensation (base values in uTesla)
  float x = mag_event.magnetic.x - mag_offsets[0];
  float y = mag_event.magnetic.y - mag_offsets[1];
  float z = mag_event.magnetic.z - mag_offsets[2];

  // Apply mag soft iron error compensation
  float mx = x * mag_softiron_matrix[0][0] + y * mag_softiron_matrix[0][1] + z * mag_softiron_matrix[0][2];
  float my = x * mag_softiron_matrix[1][0] + y * mag_softiron_matrix[1][1] + z * mag_softiron_matrix[1][2];
  float mz = x * mag_softiron_matrix[2][0] + y * mag_softiron_matrix[2][1] + z * mag_softiron_matrix[2][2];

  // Apply gyro zero-rate error compensation
  float gx = gyro_event.gyro.x + gyro_zero_offsets[0];
  float gy = gyro_event.gyro.y + gyro_zero_offsets[1];
  float gz = gyro_event.gyro.z + gyro_zero_offsets[2];

  // The filter library expects gyro data in degrees/s, but adafruit sensor
  // uses rad/s so we need to convert them first (or adapt the filter lib
  // where they are being converted)
  gx *= 57.2958F;
  gy *= 57.2958F;
  gz *= 57.2958F;

/*****Now do the conversions according to your reference system converting for magnetometer, accelerometer and gyroscope******/

float mSx = mz;
float mSy = mx;
float mSz = my;

float gSx = gz;
float gSy = gx;
float gSz = gy;

float aSx = accel_event.acceleration.z;
float aSy = accel_event.acceleration.x;
float aSz = accel_event.acceleration.y ;

// Update the filter
filter.update(gSx, gSy, gSz,
              aSx, aSy, aSz,
              mSx, mSy, mSz);

Isn't it correct? After this, I expected that the sensor will give me the absolute values for Roll, Pitch Yaw. Although the Roll and Pitch angles they look correct, the Yaw is always stuck at 340 degrees. This is not normal. The truth is that, if I rotate the body, I see the yaw changing instantly (so thought that it is correct), but it comes back after a while at 340 degrees, so definitely something is not calculated properly (see the output values above). I have done the calibration step earlier that is required and no metallic object was nearby to affect the magnetometer readings.

Thank you for your time and advice !

Edit: I tried to use the original reference system, so I placed the sensor as by its reference frame, and loaded the original firmware without my conversion. The yaw shows the same behavior, it changes instantly but comes back to the same value, ie. 300 degrees. It is always stuck there. Why do I notice this? It looks like a changing rate of yaw across time if I rotate fast. How could I calculate the absolute yaw please?

[jremington]

considering that we have to deduct the gravity =(0 0 9.8 ) (reverse vector), means that we get :
ax = 0.21, ay = 0.21 az = -0.53 [m/s^2]

That is not the way to do the subtraction. You first rotate (0 0 9.8 ) into the reference frame of the sensor, and subtract all three components of the result from all three components of the measurement. If you want the results in the Earth frame, rotate back.

However, if the measurements quoted below are for the sensor at rest, then the accelerometer is not correctly calibrated.

ax =  0.21  [m/s^2]
ay = 0.21  [m/s^2]
az = 9.27  [m/s^2]

g, as measured by that sensor, is 9.2748 m/s^2, and that would be the magnitude of the vector to subtract. Better to work with correctly scaled and normalized values, where the magnitude of the acceleration vector (and g) = 1 when the sensor is at rest.

As to this question:

I want to understand whether the device is kept straight or it tilts sometimes. But as it moves fast on the bench, the angles (Roll-Pitch) change and maybe I am reading a value that is not what I need to analyze.

After experimentation, most people conclude that that consumer grade sensors and 3D orientation algorithms don't work well enough to do this sort of compensation.

[aredhell_vlsi]

Thank you for your reply ! Well, I tried with another setup, and the values are good. The absolute values of all the angles are correct, so I believe that the conversions that I made with the reference frames are correct in the algorithm.

That makes me doubt about the calibration of the initial sensor. Thank you anyway, I will need to invest some time to also understand this gravity elimination, but at least I have retrieved correct absolute values. The gravitational acceleration is my next step.

[Souf21]

Well, I tried with another setup
Hello I would like to know what did you do to eliminate the offset by: Ax = 0.21, Ay = 0.21. I have the same issue could you help me out please