Problem using this library with the LSM9DS1 IMU.

[HyFrow]

Hello, I'm having a problem.

I'm trying to use the velocity example that you provide with the data from the LSM9DS1 IMU. I am gathering the accelerometer and gyroscope data thanks to the Arduino_LSM9DS1 Library, as it can be seen on the script below. According to the documentation and my previous experimentations, the functions readAccelerometer and readGyroscope return float values for each axis, just like the imu data that you can get from the basicMPU6050.

I'm kind of disappointed, because the only thing I changed compared to your example is to put my values instead of yours in fusion.setup, fusion.update and fusion.updateVel functions, and I'm getting "nan nan nan" in the Serial Console for vel.x, vel.y and vel.z.

Moreover, I tried to send static values (i.e send {2.0,2.0,2.0} for gyro and {1.0,0.5,0.5} for accel), and it still gives the same result.

Do you have any ideas where the problem comes from ?

/*
 This sketch shows to integrate acceleration to obtain an estimate of velocity
*/

/* NOTE: The accelerometer MUST be calibrated to integrate its output. Adjust the 
   AX, AY, AND AZ offsets until the sensor reads (0,0,0) at rest. 
*/

#include <Arduino_LSM9DS1.h>
#include <accIntegral.h>

// ========== IMU sensor ==========

// Gyro settings:
#define         LP_FILTER   3           // Low pass filter.                    Value from 0 to 6
#define         GYRO_SENS   0           // Gyro sensitivity.                   Value from 0 to 3
#define         ACCEL_SENS  0           // Accelerometer sensitivity.          Value from 0 to 3
#define         ADDRESS_A0  LOW         // I2C address from state of A0 pin.   A0 -> GND : ADDRESS_A0 = LOW
                                        //                                     A0 -> 5v  : ADDRESS_A0 = HIGH
// Accelerometer offset:
constexpr int   AX_OFFSET =  552;       // Use these values to calibrate the accelerometer. The sensor should output 1.0g if held level. 
constexpr int   AY_OFFSET = -241;       // These values are unlikely to be zero.
constexpr int   AZ_OFFSET = -3185;

// =========== Settings ===========
accIntegral fusion;
                                            //  Unit            
constexpr float GRAVITY = 9.81e3;           //  mm/s^2          Magnitude of gravity at rest. Determines units of velocity.
constexpr float SD_ACC  = 200;              //  mm/s^2          Standard deviation of acceleration. Deviations from zero are suppressed.
constexpr float SD_VEL  = 200;              //  mm/s            Standard deviation of velocity. Deviations from target value are suppressed.
float acceleration[3];
float gyrom[3];

void setup() {
  Serial.begin(38400);

  if(!IMU.begin())
    while(1);
  if(IMU.accelerationAvailable()){
      IMU.readAcceleration(acceleration[0],acceleration[1],acceleration[2]);
      Serial.print("acceleration : ");
      Serial.println(acceleration[0]);
    }
  // initialize sensor fusion
  fusion.setup(acceleration[0],acceleration[1],acceleration[2]);   // set initial heading 
  fusion.reset();                                 // zero velocity estimate
}

void loop() {   
  /* NOTE: The heading must be updated along with the velocity estimate for accurate results.
           Use the following steps to ensure proper integration */

  // 1. measure state
  if(IMU.accelerationAvailable() && IMU.gyroscopeAvailable()){
      IMU.readAcceleration(acceleration[0],acceleration[1],acceleration[2]);
      IMU.readGyroscope(gyrom[0], gyrom[1], gyrom[2]);

  vec3_t accel = { acceleration[0],acceleration[1],acceleration[2] };
  vec3_t gyro = { gyrom[0], gyrom[1], gyrom[2] };

  // 2. update heading

  fusion.update( gyro, accel );

  // 3. update velocity estimate

  vec3_t vel_t = {0,0,0};   // Known measured velocity (target state)
  vec3_t vel;               // placeholder variable 

  fusion.updateVel( gyro, accel, vel_t, SD_ACC, SD_VEL, GRAVITY );
  vel = fusion.getVel();

  // Display velocity components [view with serial plotter]

  Serial.print( vel.x);
  Serial.print( " " );
  Serial.print( vel.y);
  Serial.print( " " );
  Serial.print( vel.z);
  Serial.println();
  }
}

[RCmags]

Hi HyFrow,

I suspect the problem lies in how you're initializing or updating the imu values. Mainly, the gyroscope and accelerometer measurements are not initialized to a particular value. Rather, they're probably taking on random values, as this clause at the start of the setup() or loop() function

if(IMU.accelerationAvailable() && IMU.gyroscopeAvailable()){ IMU.readAcceleration(acceleration[0],acceleration[1],acceleration[2]); IMU.readGyroscope(gyrom[0], gyrom[1], gyrom[2]);

Might not be executed immediately after the arduino turns on. Try modifying the variable declarations as such:

float acceleration[3] = {0,0,1}; float gyrom[3] = {0,0,0};

Alternatively, try calling the filter update function without the gyroscope data:

fusion.setup()

Let me know if this fixed the problem.

[HyFrow]

Hello RCmags,

I tried your idea but the problem does not seem to come from here. I tried to find exactly where in the code the problem happens, and from my experimentations I think the issue comes from the updateVel function Indeed, by adding these prints to the accIntegral.cpp :

I am getting this.

That's why I think the problem might come from the rotate function from quaternion_type.cpp file.

As I said, even when i try to send a constant value for the acceleration, like {0,0,0}, this is still giving me "nan" for the vel_local.x value after the Remove centrifugal step in accIntegral.cpp.

[RCmags]

If the quaternion is messing up, it may be the heading is not being updated properly via the fusion.update() function. It expects the angular velocity to be in radians/second, and the acceleration to be normalized with respect to 1g (earth gravity).

Is the output of the acceleterometer normalized? ( (0,0,1) output at rest ) What units does the gyroscope output have?

That being said, try checking the output of the heading using the "heading" example sketch. If its working, you should be able to read the pitch/yaw/roll angles of the sensor. If it fails, then we're closer to identifying the problem.

[RCmags]

Any updates on this issue? How did it go?

[HyFrow]

Sorry, I didn't check your answer since Friday.

Indeed, I wasn't sending radian/second but degree/second. I changed that. For the acceleration, I am sending in g, but it can go up to +-4g, I don't know if this may cause a problem ? According to your advices, I have modified the heading example with my inputs and it goes like this :

/*
 This sketch shows to rotate the heading (yaw angle) of the estimated orientation.
 */

#include <Arduino_LSM9DS1.h>
#include <imuFilter.h>

float acceleration[3] = {0,0,1};
float gyrom[3] = {0,0,0};
imuFilter fusion;

void setup() {
  Serial.begin(38400);

  if(!IMU.begin())
    while(1);
  if(IMU.accelerationAvailable()){
      IMU.readAcceleration(acceleration[0],acceleration[1],acceleration[2]);
      Serial.print("acceleration : ");
      Serial.println(acceleration[0]);
    }

  // Initialize filter: 
  fusion.setup( acceleration[0], acceleration[1], acceleration[2] );     

  // Rotate heading:
  float angle = 45 * DEG_TO_RAD;                // angle in radians to rotate heading about z-axis
  fusion.rotateHeading( angle, LARGE_ANGLE );   // Can choose LARGE_ANGLE or SMALL_ANGLE approximation
}

void loop() {  
  // Update filter:
  if(IMU.accelerationAvailable() && IMU.gyroscopeAvailable()){
      IMU.readAcceleration(acceleration[0],acceleration[1],acceleration[2]);
      IMU.readGyroscope(gyrom[0], gyrom[1], gyrom[2]);

  vec3_t gyro;
  gyrom[0] *= DEG_TO_RAD;
  gyrom[1] *= DEG_TO_RAD;
  gyrom[2] *= DEG_TO_RAD;
  // 2. update heading
  fusion.update(  gyrom[0], gyrom[1], gyrom[2], acceleration[0],acceleration[1],acceleration[2] );    

  // Display angles:
  Serial.print( fusion.pitch() );
  Serial.print( " " );
  Serial.print( fusion.yaw() );
  Serial.print( " " );
  Serial.print( fusion.roll() );
  Serial.println();
  }
}

However, I am still getting nan for the 3 angles...

[RCmags]

Hi HyFrow,

Sorry for the delay in response. Unfortunately, I don't have an answer for your problem. Have you installed the latest version of "vector_datatype"? That's the only thing, code-wise, that I can think of that would cause the quaterion to fail.

Nonetheless, I was able to replicate your (nan,nan,nan) error using the Arduino IDE v1.8.19.

• If I had two .ino files opened and I tried to upload the imuFilter code, the aformentioned error would appear.
• Once I closed all .ino files, except for the imuFilter code, the error didn't seem to appear any more. The sketch worked as intended.

It seems the problem lay while uploading the sketch to the board. Try only opening an example, and check your board connection. Maybe this will resolve the problem?

Besides this, I've updated the library to make the "accIntegral" class simpler. It explicitly mentions the required gyroscope and accelerometer scales. Try upgrading the library. It may help.