Align sensor data rate and sample rate for accelerometer

Hello,

I'm using a 6DOF IMU and working to mitigate gyro drift using a "zero velocity filter". I'm using an ESP32 micro and an ST ISM330dhcx IMU. The idea is to reset the gryo integration to zero when the device is "not moving". I'm attempting to judge if it's moving by calculating the RMS error of the XYZ accelerometers. You might disagree with this approach and I'm all ears on that subject, but here's my question... (if you suggest use Quaternions, try Kalman, try Mahoney etc I've read 10,000 examples and will do that eventually, trying step by step to build my knowledge with a complementary filter first)

I'm using the micros() function to take a look at the current sample and the previous sample from each of the accel XYZ and compute the root mean square variance of the change across that pair of samples. This appears to be about 0.3 degrees at rest, which somewhat makes sense. But I discovered that the variance sits in this range a lot, which I was not expecting, even when I'm moving it around quite a bit! I think what's happening is that for the speed of rotation from my hands, which will be typical for the project, the accel is outputting the same values multiple times, and sequential samples mostly don't change therefore the gryo is resetting over and over, and basically not working to correct drift. If I print the variance to the serial plotter, I can see spikes in the readout that seem to occur every 10 samples or so, with the values hovering under 0.3 degrees between those spikes. (I know the plotter will actually impact the measured intervals, but FYI)

So far in this project I haven't had to consider the output data rate for the accel, nor have I needed to micros() time window and only read/compare every n microseconds. There is a lot of flexibility here and I'm not sure how to logic my way through what should be the best configuration. With all of my atan/sqrt/pow computations and whatnot, the time between samples is about 2500 microseconds, as printed to serial. The accel output data rate is currently set to 6667Hz, which would be an output every 150 micros. I'm not sure where that leaves me because they are just not aligned - I would think that if I slow the data rate way down I might get more samples read but what's the point of running this fancy IMU at 26Hz, besides I've tried it and it's worse - the accel sits in that deadband even more and just resets to 0 continuously. I've tried every combo of data rate, accel low pass filter, and even a few micros() time windows, but nothing is improving the situation.

Not really sure what to do next, hence the lengthy post.

Any guidance / pointers appreciated!

Thanks

Indeed a lengthy post, but nothing for forum members to go on. Please read and follow the directions in the "How to get the best out of the forum" post.

A couple of comments:

Spikes in the sensor data are probably due to errors in your code, which you forgot to post. Please do so, using code tags.

This is a difficult project and the problems to overcome depend on the sensor environment (which you did not describe), your definition of "not moving", magnitude of inherent sensor noise (see the data sheet), cross talk between gyro and accelerometer, etc. You need to separate all these contributions and understand them one by one.

Hello jremington, thanks for the reply. I didn't post code because I don't really have a coding question, more of a "how should one approach aligning sensor data rate and sampling frequency of the micro and the code you run on it?" question. I will post code here though in case I have made an error which moots the question somehow.

I don't have spikes in the sensor data. I have spikes in the accel data variance as compared from one sample to the next. I am expecting this variance to be small when the device is resting (which it is) and large when it is moving (which is seems to mostly - by time - not be). I see the same small variance for 9 samples in a row, then the 10th sample has the variance I'm more or less expecting. So the issue feels like a misalignment of sampling times versus data rates, but it could be something else entirely.

Here's most of the code, there are a few routines not shown that mostly animate LEDs or print a set of variables:

#include <Wire.h>
#include <SPI.h>
#include <SparkFun_ISM330DHCX.h>

#define FASTLED_ALL_PINS_HARDWARE_SPI
#define FASTLED_ESP32_SPI_BUS HSPI
#define NO_CORRECTION 0 // Removes interrupt time compensation for millis()

#include <FastLED.h>

#define DATA_PIN    13
#define CLK_PIN     14
#define LED_TYPE    APA102
#define COLOR_ORDER BGR
#define NUM_LEDS    60
#define BRIGHTNESS          30
#define FRAMES_PER_SECOND  208
CRGB leds[NUM_LEDS];

FASTLED_USING_NAMESPACE

SparkFun_ISM330DHCX myISM; 

// Structs for X,Y,Z data
sfe_ism_data_t accelData; 
sfe_ism_data_t gyroData; 

unsigned long TimeNow=0, TimePrev=0, TimeDelta=0;

float Level = 1; // degrees from zero tilt in Z to consider level
float Still = 20; // accel data variance below which the device is judged not moving
float AdegX=0, AdegY=0, AdegZ=0, AdegR=0, AdegA=0, GdegA=0;
float GdegX=0, GdegY=0, GdegZ=0;
float Roll=0, Pitch=0, Yaw=0, Tilt = 0;

float AXOff = 9, AYOff = 27.4, AZOff = -7;
float GXOff = 35, GYOff = 825, GZOff = 68;

float AXprev=0, AYprev=0, AZprev=0;
float AXnow=0, AYnow=0, AZnow=0;
float powAX=0, powAY=0, powAZ=0;
float AccelVariance=0;
float AXdelta=0, AYdelta=0;
float rad2deg = 180/PI;

int TargetLED = 0;
int PrevTarget = 0;
static uint8_t gHue = 0;

void setup(){

	Wire.begin();
	Serial.begin(115200);
	if( !myISM.begin() ){
		Serial.println("Did not begin.");
		while(1);
	}

	// Reset the device to default settings.
	myISM.deviceReset();

	// Wait for it to finish reseting
	while( !myISM.getDeviceReset() ){ 
		delay(1);
	} 
	delay(100);
	
	myISM.setDeviceConfig();
	myISM.setBlockDataUpdate();
	
	// Set the output data rate and precision of the accelerometer (104 is default)
	myISM.setAccelDataRate(ISM_XL_ODR_6667Hz);
	myISM.setAccelFullScale(ISM_2g); 
	// Set the output data rate and precision of the gyroscope
	myISM.setGyroDataRate(ISM_GY_ODR_6667Hz);
	myISM.setGyroFullScale(ISM_2000dps); 
	// Turn on the accelerometer's filter and apply settings. 
	myISM.setAccelFilterLP2();
	myISM.setAccelSlopeFilter(ISM_LP_ODR_DIV_100); //100 default
	// Turn on the gyroscope's filter and apply settings. 
	myISM.setGyroFilterLP1();
	myISM.setGyroLP1Bandwidth(ISM_MEDIUM);

  delay(2000);  // gryo has spurious values a second after startup

  // Check if both gyroscope and accelerometer data is available.
  if( myISM.checkStatus() ){
    //Calibrate();           // Calculates and prints offset values for gyro and accel
    }

FastLED.addLeds<LED_TYPE,DATA_PIN,CLK_PIN,COLOR_ORDER>(leds, NUM_LEDS).setCorrection(TypicalLEDStrip);
FastLED.setBrightness(BRIGHTNESS);   // set master brightness control
}


void loop(){

    TimePrev = TimeNow;
    TimeNow = micros();
    TimeDelta = TimeNow-TimePrev;
    // Serial.println(TimeDelta);
       ComputePitchRoll();
    if (AdegZ < Level){
      ShowDone();          // LED display code
    }
    else {
    ShowTilt();            // LED display code
    }
     //PrintRawA();
     //PrintRawG();
     PrintAngles();
     //PrintDeltaAngles();
     //PrintPitchRoll();

}


void ComputePitchRoll(){
    
    myISM.getAccel(&accelData);
    myISM.getGyro(&gyroData);

    AXprev = AXnow; 
    AYprev = AYnow; 
    AZprev = AZnow;
    AXnow = accelData.xData+AXOff;  // Accelerometer values in mg
    AYnow = accelData.yData+AYOff; 
    AZnow = accelData.zData+AZOff;
    powAX = pow(AXnow,2);
    powAY = pow(AYnow,2);
    powAZ = pow(AZnow,2);

    // Root mean square variance of accelerometer axes
    AccelVariance = sqrt( pow(AXnow-AXprev,2) + pow(AYnow-AYprev,2) + pow(AZnow-AZprev,2) );
    // Serial.print("Max: ");
    // Serial.print(500);
    // Serial.print(", ");
    // Serial.print("Min: ");
    // Serial.print(0);
    // Serial.print(", ");
    // Serial.print("AccelVariance: ");
    // Serial.println(AccelVariance);

    if (AccelVariance > Still){  // If the device is still, reset gyro to counter drift
      GdegX = GdegX + (gyroData.xData + GXOff) * TimeDelta / 1000000000;
      GdegY = GdegY + (gyroData.yData + GYOff) * TimeDelta / 1000000000;
    }
    else{
      // Serial.println("In else");
      GdegX = 0;
      GdegY = 0;
    }
      
    AdegX = atan( AXnow / sqrt(powAY+powAZ) )      * rad2deg;
    AdegY = atan( -1 * AYnow / sqrt(powAX+powAZ) ) * rad2deg;
    AdegZ = atan( sqrt(powAX+powAY) / AZnow )      * rad2deg;
    AdegR = atan( AXnow/AYnow )                    * rad2deg; // Project tilt angle to XY plane

    AXdelta = (AXnow - AXprev) / TimeDelta;
    AYdelta = (AYnow - AYprev) / TimeDelta;

    GdegA = abs(atan( GdegZ/GdegY ))           *180/PI;   // axis of tilt
    AdegA = abs(atan( AXdelta/AYdelta ))       *180/PI;   // alternate axis of tilt
    
    Roll = (0.90 * GdegX) + (0.10 * AdegX);
    Pitch = (0.90 * GdegY) + (0.10 * AdegY);
   
    Yaw = Yaw + (gyroData.zData + GZOff) * TimeDelta / 1000000;    
  }

Well, one thing I can say after thinking about it when it's not midnight is that the change in the accel reading from sample to sample is not velocity, it's acceleration, so I need to take the derivative.

I'm already computing that for a different purpose, I will try comparing that value from sample to sample. I still don't quite understand the spikes in the data, I have noted that if I just print the micros() value, with no other code in the loop, I get spikes there too, so that delta time may have some patterns in it from some unknown (yet, to me) source. Maybe that spike is the actual printing of the micros(), and maybe the proper derivative will better detect lack of (much) motion regardless.

Thanks again for your eyeballs

That is nonphysical, so it is a problem with the approach.

If this were my project, I would write a separate program with the sole purpose of collecting, processing and calibrating the sensor data, in particular to make sure that I understand the noise, offsets and possible sensor artifacts. No distracting LEDs.

As I understand it, one of your goals is to cancel drift and/or sensibly integrate the gyro rate data. For what purpose? If it is just to compute 3D orientation, I would use the Mahony AHRS filter. It beats all other approaches for speed and simplicity.

Yes, I have already done the computation of the calibration and at this point am putting the resulting values in manually. At some point, I'll make that inline to the setup. The change to use the derivative works as expected although that doesn't mean it works well.

The calibration has all but eliminated the slow drift of the gryo, but I still have jumps in the gyro integration that cause them to settle to a non-zero rate, which the above is lopping off back to zero. In the end, I think what I'm building is a poor man's Mahony, so I might as well bite the bullet and refactor to use that approach.

On the other hand, I think the IMU has very sophisticated built-in filtering and motion processing, but the examples for this IMU don't use those, they just do the IMU equivalent of "Hello World!" and print out values. The IMU has state machines, motion processing, FIFO etc etc but will take a while to get that deep. Thanks again

You are using the "complementary filter", which is not even technically correct for anything other than nearly level flight.

Most people abandoned that long ago, but it lives on in outdated tutorials on the web.

    Roll = (0.90 * GdegX) + (0.10 * AdegX);
    Pitch = (0.90 * GdegY) + (0.10 * AdegY);

Yes, my application is nearly stationary (there is no flight - it's mounted on something that mostly doesn't move, but should show fine resolution of adjustments when made). It works fine with just the accel - I wanted to add the gyro to smooth out the signal a bit so that the LEDs are not flickering with handheld vibrations. The continuous drift that all report is actually well handled by fine tuning the gyro offsets and the complementary filter, but with larger angular movements, such as one might see during setup of the device, the gyro doesn't return to zero properly and that's what I'm fighting.

BTW, I'm still interested in any counsel about aligning sensor data rates and microprocessor sample rates - that was the question I posted. I suspect that one might solve that conclusively by collecting data using an interrupt scheme on demand, and with controlled time delta not subject to variation from other code branching etc. No one has said that yet but I was worried they might :slight_smile:

My application doesn't involve flight - the device is mounted on something that mostly doesn't move but the device needs to show fine adjustments when they are made. Basically it works OK with just the accel, I wanted to add the gyro to improve responsiveness, reduce the led flickering when vibrations occur and potentially to enable future features.

The careful offset calibration handles the slow drift from gyro noise very well but when larger movements occur, such as might be seen when setting up the device initially the gryo doesn't return to zero. That behavior doesn't impact the result (could just ignore the gyro), but doesn't look good at all. A few of those outdated examples, including an Analog Devices application note, call out the zero velocity reset idea but don't give details, I have about convinced myself that such an approach would only work if you have been dead still for a while and essentially "no one's looking" because it's ugly when the gyro lands on 40 degrees instead of zero and you just slam it back to zero in the code. Not sure if Mahony/Madgwick/Kalman will solve that problem either but one can hope!

BTW, I am still interested in any feedback someone might have on how to deal with sensor data rates and processor sample rates, which was the original question. I'm guessing some might say to do the reads in an interrupt where you could just get the data when you need it and control the delta time so that it doesn't vary from branches in the code. But I hope not because that sounds hard :slight_smile:

Solve sampling problems by reading the data from the sensor only when the data ready flag is set. With the ISM330DHCX, there are separate data ready flags for the gyro and the accelerometer.

do the reads in an interrupt

Never attempt to do serial I/O in an interrupt. That usually hangs the processor, and even if not (Arduino makes an exception for UART serial) is always a bad idea.

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.