Integration methods of acceleration values

Using Arduino Programming Questions
1

Hello!
I would like to compare different integration methods for the calculation of velocity values from acceleration values. I know which problems arise when calculating velocities from acceleration values only and that they are quite inaccurate or useless values, but this is not relevant for my work. For this I use a M5Stack (esp32). I have stored and recalled data on a SD card and would like to integrate it using the trapezoid method (or any other useful method), but I have difficulties in the implementation. If someone could help me, I would be very grateful. Therefore I post my previous integration method, to which I would like to implement the trapezoid method, is this even possible? (I will leave out the rest of the code for now.)
Thanks a lot
Jan

void readFile(fs::FS &fs, const char * path){
    Serial.printf("Reading file: %s\n", path);

    File file = fs.open(path);
    if(!file){
        Serial.println("Failed to open file for reading");
        return;
    }

    Serial.print("Read from file:\n ");
    //file.readBytesUntil('\n');

    // Initialize the totals
    float testX = 0.0;
    float testY = 0.0;
    float testZ = 0.0;

    float norm_acc = 0.0;
    float integral_vel = 0.0;
    float integral_dis = 0.0;
   
  // dt: Zeitschritt zwischen den Datenwerten:
    float delta = 0.125;
  
    while(file.available()){
    //Serial.write(file.read());

 // Ausgeben der gespeicherten zugehörigen Zeiten:
      float time = file.parseFloat();

 // Berechnung Variante 2:
    testX += ((file.parseFloat()));
    testY += ((file.parseFloat()));
    testZ += ((file.parseFloat()));

    norm_acc = sqrt((sq(testX)) + (sq(testY)) + (sq(testZ)));
    integral_vel += ((norm_acc*delta));
    integral_dis += ((integral_vel*delta));

    
    Serial.print(" ");
    Serial.println(time); // To show progress
    }

 // File all read.  Display the totals.
    Serial.print("testX: ");
    Serial.print(testX);
    Serial.print("\ttestY: ");
    Serial.print(testY);
    Serial.print("\ttestZ: ");
    Serial.println(testZ);
    M5.Lcd.setCursor(0, 20);
    M5.Lcd.printf("Acceleration: %5.2f",norm_acc);
    //M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", testX, testY, testZ);
    M5.Lcd.setCursor(0,65);
    M5.Lcd.printf("Velocity: %5.2f ",integral_vel); 
    M5.Lcd.setCursor(0,110);
    M5.Lcd.printf("Distance: %5.2f ",integral_dis);
    file.close(
2

You didn't say what your difficulties are... also it will be difficult to help you without seeing all your code. Mainly, you need to explain what is wrong. Very few people have the same hardware, so we can't just test it. Proof reading the code would just be a tiresome guessing game.

3

your formula is wrong

Suppose you are on one axis only and not moving at first. if you have a positive acceleration to the right for 1 second and then the same acceleration in the opposite direction your speed should be back to 0 (assuming no other forces)

you compute the length [color=red]L[/color] of the acceleration vector in 3D space using the square root of the sum of squares (its norm in 3 dimensional Euclidean space) and then you do

Speed[sub](t+1)[/sub] = Speed[tt][sub](t)[/sub] + L.∆t[/tt]

As L will always be positive and ∆t is always 0.125 (sampling at 8Hz I assume) your speed can only increase... (and you always have the Gravity on earth so if your vehicle is actually in 2D space then how its weight is compensated/taken into account ?)

4

If your accelerometer isn't always in the same horizontal plane pointing in the same direction you have to know the direction in order to determine the new position and speed from the old position, speed, and acceleration. If the accelerometer is NOT constrained to a perfectly horizontal plane you also need the 3D orientation so you can subtract out the gravity vector, otherwise, the position will appear to accelerate downward at 9.8 m/s^2.

5

Hello!
Sorry for the late answer, I've rethought my whole project and started over, so I now have more specific questions about my project, as well as the complete code!
About my task: I would like to record acceleration values with an M5Stack (esp32), save them on an SD card (for possible post-processing on an external PC). I am only looking at pure one-dimensional translational movements (without turning or screwing movements). I then want to integrate them (with different methods) and thus get the speed.

@ J-M-L: thanks for the tip, I have now increased my sampling rate so that it should currently be 100HZ.
I am aware that this task is very error-prone, because I keep adding up the error during integration. Therefore I would like to save my reopened data ( X-, Y-, Z-axis ) in arrays and use a moving average filter of "MedianFilterLib.h" afterwards.

My first question is: How can I read out the data in my function again and save them in arrays without pre-defining its size ( how much data should be written in )? The measurements will be stopped not after a specific data size, but after the time it takes to complete the movement.

Second question: So far I have specified the array size in my code. The problem is, that if I want to define the abort criterion for my loop to sum up the array elements, the sizeof - operator returns the given number as length value, although I do not know how many values are stored in the array. Would it be logical to run a counter in my loop (in the main program) and use it as abort criterion?

Third question: Is it correct at all to muliplicate the single array-elements with the given delta, sum them up and then build the norm? So far I only get 0.0 for the "integral values" for all measurements

Fourth question: How can I determine my delta in my program? For this I have to calculate 1/time step, but how can I calculate this value in my program (so far my code reads the corresponding time values via
"float time = file.parseFloat();" and displays them to me. Up to now I only defined my value by reading the "time"?

I would be glad about help :slight_smile:
Thanks for the answers so far!!!!
This time I will post my complete code!

6 
© Jan-Soeren Henning
#define M5STACK_MPU6886 
#include <M5Stack.h>
#include "FS.h"
#include "SD.h" 
#include "SPI.h"
#include "time.h"
#include "math.h"
#include "MedianFilterLib.h"
float accX = 0.0F;
float accY = 0.0F;
float accZ = 0.0F;
float calX = 0.0F;
float calY = 0.0F;
float calZ = 0.0F;
float sumX = 0.0F;
float sumY = 0.0F;
float sumZ = 0.0F;
float zeit = 0.0F;
int zaehler = 0;
char msg[50];
unsigned long zeit_start = 0.0;
unsigned long zeit_end = 0.0;
unsigned long zeit_cal = 0.0;

boolean button_a = false;
boolean messung = false;
boolean button_c = false;
boolean calibration = false;
boolean start_aussen = true;
boolean start_innen = false;


void writeFile(fs::FS &fs, const char * path, const char * message){
    Serial.printf("Writing file: %s\n", path);

    File file = fs.open(path, FILE_WRITE);
    if(!file){
        Serial.println("Failed to open file for writing");
        return;
    }
    if(file.print(message)){
        Serial.println("File written");
    } else {
        Serial.println("Write failed");
    }
    file.close();
}

void appendFile(fs::FS &fs, const char * path, const char * message){
    Serial.printf("Appending to file: %s\n", path);

    File file = fs.open(path, FILE_APPEND);
    if(!file){
        Serial.println("Failed to open file for appending");
        return;
    }
    if(file.print(message)){
        Serial.println("Message appended");
    } else {
        Serial.println("Append failed");
    }
    file.close();
}

void readFile(fs::FS &fs, const char * path){
    Serial.printf("Reading file: %s\n", path);

    File file = fs.open(path);
    if(!file){
        Serial.println("Failed to open file for reading");
        return;
    }

    Serial.print("Read from file:\n ");
    //file.readBytesUntil('\n'); // Strip off the header

 // Initialize the totals
    float integralX = 0.0;
    float integralY = 0.0;
    float integralZ = 0.0;
    
 // set only one value so far
    float fieldX[50]; 
    float fieldY[50];
    float fieldZ[50]; 
  
    float norm_vel = 0.0;
    
 // set only one value so far 
    float delta = 0.01;

    MedianFilter<float> medianFilter(5);
    
    while(file.available()){
    //Serial.write(file.read());

    float time = file.parseFloat();

    float fieldX[50] = file.parseFloat();
    float fieldY[50] = file.parseFloat();
    float fieldZ[50] = file.parseFloat();

    size_t valuesLength = sizeof(fieldX) / sizeof(fieldX[0]); // of course 50 ?!
 
    float medianX = medianFilter.AddValue(fieldX[50]);
    float medianY = medianFilter.AddValue(fieldY[50]);
    float medianZ = medianFilter.AddValue(fieldZ[50]);
    
    for (int i=0; i<= valuesLength; i++)
        { integralX += (medianX * delta);
          integralY += (medianY * delta);
          integralZ += (medianZ * delta);
        }
    norm_vel = sqrt((sq(integralX)) + (sq(integralY)) + (sq(integralZ)));


 // Serial.print(" ");
 // Serial.println(time); // To show progress                
 // Serial.print(integralX);
 // Serial.print(integralY);
 // Serial.println(integralZ);

  }

  M5.Lcd.printf("Velocity: %5.2f ",norm_vel); 
  file.close();
}

void setup() {
  M5.begin();
  M5.Power.begin();
  M5.IMU.Init();

  M5.Lcd.println("Press button C to\nstart calibration");

 Serial.begin(115200);
    if(!SD.begin()){
        Serial.println("Card Mount Failed");
        return;
    }
    uint8_t cardType = SD.cardType();

    if(cardType == CARD_NONE){
        Serial.println("No SD card attached");
        return;
    }

    Serial.print("SD Card Type: ");
    if(cardType == CARD_MMC){
        Serial.println("MMC");
    } else if(cardType == CARD_SD){
        Serial.println("SDSC");
    } else if(cardType == CARD_SDHC){
        Serial.println("SDHC");
    } else {
        Serial.println("UNKNOWN");
    }
    
      writeFile(SD, "/acceleration.txt", "   Time    aX       aY        aZ"); // Aufruf der Funktion (void writeFile)
    
    
// KALIBRATION:
while (start_aussen){

      if (M5.BtnC.wasReleasefor(70))
      { 
        button_c = (!(button_c));
        calibration = (!(calibration));
        M5.Lcd.clear();
      }

      if (button_c)
      {
        zeit_cal = millis();
        while (calibration){
          

                 M5.IMU.getAccelData(&accX,&accY,&accZ);
                 M5.Lcd.setCursor(0, 20);
                 M5.Lcd.printf("Calibration in\nprogress...");

                 sumX = ((sumX + accX));
                 sumY = ((sumY + accY));
                 sumZ = ((sumZ + accZ));

                 zaehler+=1;

             if (zaehler == 3000)
             {
                 calibration = (!(calibration));
                 start_aussen = (!(start_aussen));
                 start_innen = (!(start_innen));
                 M5.Lcd.clear();
                 M5.Lcd.setCursor(0, 20);
                 M5.Lcd.println("Press button A to\nstart measurement");
             }

                      M5.update();
                      if (M5.BtnC.wasReleasefor(70))
                         { 
                           button_c = (!(button_c));
                           calibration = (!(calibration));
                           start_aussen = (!(start_aussen));
                           start_innen = (!(start_innen));
                           M5.Lcd.clear();
                           M5.Lcd.setCursor(0, 20);
                           M5.Lcd.println("Press button A to\nstart measurement");
                         }
                }          
      }
}

     calX = (abs(sumX) / zaehler); 
     calY = (abs(sumY) / zaehler); 
     calZ = (abs(sumZ) / zaehler); 
     
// PROGRAMM:
while (start_innen){
   
    zeit_start = millis();

     M5.update();

     if (M5.BtnA.wasReleasefor(70))
         {
             button_a = (!(button_a));
             messung = (!(messung));
             M5.Lcd.clear();
          }  


      if (button_a)
      {
        while (messung)
          { 
              zeit_end = millis()-zeit_start;
              
              M5.IMU.getAccelData(&accX,&accY,&accZ);

              accX = accX - calX;
              accY = accY - calY;
              accZ = accZ - calZ;
                            
              zeit = (float) zeit_end / 1000.00; // vielleicht auf milli sekunden lassen

              snprintf(msg, 50, "\r\n  %5.2lf   %5.2lf     %5.2lf     %5.2lf",zeit, accX, accY, accZ); 
              appendFile(SD, "/acceleration.txt", msg);


              M5.update();
              if (M5.BtnA.wasReleasefor(70))
                 {
                    messung = (!(messung));
                    readFile(SD, "/acceleration.txt");    
                 }
            }
        }
    }   
}
7

Since the RAM is very limited and your sketch is not sharing RAM with any other programs, just increase the array size until you are using 75% of dynamic memory.

The best solution is to do your calculations while you read the data and don't try to keep more than a line or two of data in memory.

If you REALLY need to have access to the data in random order, create ONE array to hold the file position of each line (file.position()). Then, when you need the data from that line, use file.seek(position) to position to the beginning of the line and read the data again. That will allow you to have about three times as many lines before memory fills up.

8

Thanks for the answer @johnwasser
I simply adjusted the field size accordingly, so that I am now at 500 field entries (but can still be increased).

Unfortunately I can't do the calculations in the main program, because this is at the expense of my sampling frequency and therefore the integrated values would be even higher.

I have created the termination condition for my summation loop as well as for my "dt" by a counter and a calculation in my main program.

At the moment all values are displayed in fieldX[], fieldY[], fieldZ[], but in my summation loop there is no summation. Only the value 0 is displayed for all. Unfortunately I don't know why the field elements are not summed up? That's why the filter library does not work so far!

void readFile(fs::FS &fs, const char * path){
    Serial.printf("Reading file: %s\n", path);

    File file = fs.open(path);
    if(!file){
        Serial.println("Failed to open file for reading");
        return;
    }

    Serial.print("Read from file:\n ");
    //file.readBytesUntil('\n'); // Strip off the header

    // Initialize the totals
    unsigned int index;
    float integralX = 0.0;
    float integralY = 0.0;
    float integralZ = 0.0;
    float norm_vel = 0.0;
    float fieldX[500];
    float fieldY[500];
    float fieldZ[500]; 


    MedianFilter<float> medianFilter(5);
    
    while(file.available()){
    //Serial.write(file.read());

 // Ausgeben der gespeicherten zugehörigen Zeiten:
    float time = file.parseFloat();


 // Berechnung Variante 1:
    fieldX[500] = file.parseFloat();
    fieldY[500] = file.parseFloat();
    fieldZ[500] = file.parseFloat();

 
 // Ausgabe im seriellen Monitor:
 // Serial.print(time); // To show progress  
  
    Serial.print(fieldX[500]);
    Serial.print("\t");
    Serial.print(fieldY[500]);
    Serial.print("\t");
    Serial.println(fieldZ[500]);

               
 // Berechnung durch Updateregel als Feldelemente:
    for ( index=0; index!= zaehler2; index++)
        { 
          integralX += (fieldX[index]);
          integralY += (fieldY[index]);
          integralZ += (fieldZ[index]);
        }
 
    norm_vel = sqrt((sq(integralX)) + (sq(integralY)) + (sq(integralZ)));      
           
    Serial.print("X:");
    Serial.print(integralX);
    Serial.print("\tY:");
    Serial.print(integralY);
    Serial.print("\tZ:");
    Serial.println(integralZ);

  }

    M5.Lcd.setCursor(0,65);
    M5.Lcd.printf("Velocity: %5.2f ",norm_vel); 

    file.close();
}

I would be greatful for any help!