Strange acceleration calculations for 6DoF system

Hello all. I am trying to calculate the acceleration in world coordinates of my arduino nano 33 ble sense given the rotation matrix and the accelerometer readings.

I used the formula true Acceleration Matrix = Inverse of rotation matrix * accelerometer readings vector, and I used this method to get the rotation matrix from euler angles.

My final readings seem off however. Some of the time the X reading is always 0 no matter what and some of the time the other two readings are always 0 and the X reading is the only one that changes.

void getRotMatfromEuler(float rBuffer[3][3], float pitch, float yaw, float roll, bool inRadians){

  if(!inRadians){
    pitch = radians(pitch);
    yaw = radians(yaw);
    roll = radians(roll);
  }

  float h = yaw;
  float a = pitch;
  float b = roll;
  
  float toReturn[3][3] =
  {{cos(h) * cos(a), -(cos(h) * sin(a) * cos(b)) + (sin(h) * sin(b)), (cos(h) * sin(a) * sin(b)) + (sin(h) * cos(b))},
   {sin(a), cos(a) * cos(b), -cos(a) * sin(b)},
   {-sin(h) * cos(a), (sin(h) * sin(a) * cos(b)) + (cos(h) + sin(b)), -(sin(h) * sin(a) * sin(b)) + (cos(h) * cos(b))}}; 

  for(int i = 0; i < 3; i++){
    for(int j = 0; j < 3; j++){
      rBuffer[i][j] = toReturn[i][j];
    }
  }

}

void matMult3x3byVector(float rBuffer[3], float mat1[3][3], int c1, int r1, float mat2[3], int c2, int r2){
  float toReturn[3];
   // Initializing elements of matrix mult to 0.
  for(int i = 0; i < r1; ++i){
    toReturn[i] = 0;
  }

  // Multiplying matrix a and b and storing in array mult.
  for(int i = 0; i < r1; ++i){
      for(int j = 0; j < c2; ++j){
          for(int k = 0; k < r1; ++k)
          {
              toReturn[i] += mat1[i][k] * mat2[k];
          }
      }
  }
  for(int i = 0; i < 3; i++)
    rBuffer[i] = toReturn[i];
}

#define N 3

void getCofactor(float A[N][N], float temp[N][N], int p, int q, int n) 
{ 
    int i = 0, j = 0; 
  
    // Looping for each element of the matrix 
    for (int row = 0; row < n; row++) 
    { 
        for (int col = 0; col < n; col++) 
        { 
            //  Copying into temporary matrix only those element 
            //  which are not in given row and column 
            if (row != p && col != q) 
            { 
                temp[i][j++] = A[row][col]; 
  
                // Row is filled, so increase row index and 
                // reset col index 
                if (j == n - 1) 
                { 
                    j = 0; 
                    i++; 
                } 
            } 
        } 
    } 
} 
  
/* Recursive function for finding determinant of matrix. 
   n is current dimension of A[][]. */
int determinant(float A[N][N], int n) 
{ 
    int D = 0; // Initialize result 
  
    //  Base case : if matrix contains single element 
    if (n == 1) 
        return A[0][0]; 
  
    float temp[N][N]; // To store cofactors 
  
    int sign = 1;  // To store sign multiplier 
  
     // Iterate for each element of first row 
    for (int f = 0; f < n; f++) 
    { 
        // Getting Cofactor of A[0][f] 
        getCofactor(A, temp, 0, f, n); 
        D += sign * A[0][f] * determinant(temp, n - 1); 
  
        // terms are to be added with alternate sign 
        sign = -sign; 
    } 
  
    return D; 
} 
  
// Function to get adjoint of A[N][N] in adj[N][N]. 
void adjoint(float A[N][N],float adj[N][N]) 
{ 
    if (N == 1) 
    { 
        adj[0][0] = 1; 
        return; 
    } 
  
    // temp is used to store cofactors of A[][] 
    int sign = 1;
    float temp[N][N]; 
  
    for (int i=0; i<N; i++) 
    { 
        for (int j=0; j<N; j++) 
        { 
            // Get cofactor of A[i][j] 
            getCofactor(A, temp, i, j, N); 
  
            // sign of adj[j][i] positive if sum of row 
            // and column indexes is even. 
            sign = ((i+j)%2==0)? 1: -1; 
  
            // Interchanging rows and columns to get the 
            // transpose of the cofactor matrix 
            adj[j][i] = (sign)*(determinant(temp, N-1)); 
        } 
    } 
} 
  
// Function to calculate and store inverse, returns false if 
// matrix is singular 
bool inverse(float A[N][N], float inverse[N][N]) 
{ 
    // Find determinant of A[][] 
    int det = determinant(A, N); 
    if (det == 0) 
    { 
        return false; 
    } 
  
    // Find adjoint 
    float adj[N][N]; 
    adjoint(A, adj); 
  
    // Find Inverse using formula "inverse(A) = adj(A)/det(A)" 
    for (int i=0; i<N; i++) 
        for (int j=0; j<N; j++) 
            inverse[i][j] = adj[i][j]/float(det); 
  
    return true; 
}

I think it might be an issue that has to do with variable types and the difference between ints and floats, because I had some issues previously where calculating the determinant as a float would cause rediculously high numbers in the inverse rotation matrix that would over flow the float variable for some reason that I don’t really understand. I’m proper stumped, does anyone have any tips for things to check? Thanks!

TrueAccelOutput.PNG

Always post the complete code. The snippet is useless to diagnose the problem that you report.

First, test all your matrix routines to make sure they work, in cases where you know the answer. For example, invert the identity matrix.

Be aware that there are around a dozen different definitions of Euler angles, so you have to make sure that all conventions are understood and uniformly applied. Not even something a simple as the direction of a positive rotation angle is universally agreed upon.

Where do the Euler angles come from in your Nano 33 BLE Sense, and what angular convention is used to define them?

have you tried testing your code with unit vectors in each axis to verify some of your math?

@dyskord

Other post/duplicate DELETED
Please do NOT cross post / duplicate as it wastes peoples time and efforts to have more than one post for a single topic.

Continued cross posting could result in a time out from the forum.

Could you take a few moments to Learn How To Use The Forum.
It will help you get the best out of the forum in the future.
Other general help and troubleshooting advice can be found here.

Sorry for cross posting ballscrewbob, I wasn’t sure where to post my question and I didn’t know it was against the rules. I wont do it again.

jremington posted below is the full code for my sketch, minus the previous code I posted which fits in at the bottom, and a few lines of declarations and irrelevant debug methods that allowed me to get under the 9000 character limit

// Arduino_LSM9DS1 - Version: Latest 
#include <Arduino_LSM9DS1.h>

// ArduinoBLE - Version: Latest 
#include <ArduinoBLE.h>

#include <MadgwickAHRS.h>

  
int gscale = 8.75;
const int MAX_C_LENGTH = 20;
const float sensorRate = 119;

Madgwick filter;

float roll = 0.0, pitch = 0.0, yaw = 0.0;

/* Sensor Characteristic UUID Dictionary
 * Directions:
 *  Pitch: 1101
 *  Roll: 1102
 *  Yaw: 1103
 *  
 * Acceleration:
 *  X: 1201
 *  Y: 1202
 *  Z: 1203
*/

const int X = 0, Y = 1, Z = 2;
float magDeclination = 10.924;

char pitchChar[MAX_C_LENGTH], rollChar[MAX_C_LENGTH], yawChar[MAX_C_LENGTH];

char tAccelXChar[MAX_C_LENGTH], tAccelYChar[MAX_C_LENGTH], tAccelZChar[MAX_C_LENGTH];

float a[3] = {0,0,0}; //accelerometer data

float g[3] = {0,0,0}; //gyroscope data

float m[3] = {0,0,0}; //magnetometer data

BLEService nanoSensors("19B10010-E8F2-537E-4F6C-D104768A1214");;
BLECharacteristic pitchService("1101", BLERead | BLENotify, '0', MAX_C_LENGTH);
BLECharacteristic rollService("1102", BLERead | BLENotify, '0', MAX_C_LENGTH);
BLECharacteristic yawService("1103", BLERead | BLENotify, '0', MAX_C_LENGTH);

BLECharacteristic tAccelXService("1201", BLERead | BLENotify, '0', MAX_C_LENGTH);
BLECharacteristic tAccelYService("1202", BLERead | BLENotify, '0', MAX_C_LENGTH);
BLECharacteristic tAccelZService("1203", BLERead | BLENotify, '0', MAX_C_LENGTH);

float magBias[3] = {38.63278,-1.660625,-.2446365};
float accelBias[3] = {.02492126,-.03985823,.9853607};
float gyroBias[3] = {.1145883,.730336,.09468156};
float magScale[3] = {2.691642,3.181818,3.182749};

void setup() {
  Serial.begin(9600);
  //Serial.println("Started");
  
  if (!IMU.begin()) {
    //Serial.println("Failed to initialize IMU!");
    while(1);
  }
  
  if(!BLE.begin()) {
    //Serial.println("Failed to initialize BLE!");
    while(1);
  }

  getAres();
  getGres();
  getMres();
  //Serial.print("accel sensitivity is "); Serial.print(1./(1000.*aRes)); Serial.println(" LSB/mg");
  //Serial.print("gyro sensitivity is "); Serial.print(1./(1000.*gRes)); Serial.println(" LSB/mdps");
  //Serial.print("mag sensitivity is "); Serial.print(1./(1000.*mRes)); Serial.println(" LSB/mGauss");

  //Serial.println("Biases: ");
  for(int i = 0; i < 3; i++){
    //Serial.print("Accel: ");
    //Serial.print(accelBias[i]);
    //Serial.print(" Gyro: ");
    //Serial.print(gyroBias[i]);
    //Serial.print(" Mag: ");
    //Serial.print(magBias[i]); 
  }
  //Serial.print("\n");
  
  
  pinMode(LED_BUILTIN, OUTPUT);
  
  BLE.setLocalName("NANO_BLE_SENSE");
  BLE.setAdvertisedServiceUuid(nanoSensors.uuid());
  nanoSensors.addCharacteristic(pitchService);
  nanoSensors.addCharacteristic(rollService);
  nanoSensors.addCharacteristic(yawService);
  
  nanoSensors.addCharacteristic(tAccelXService);
  nanoSensors.addCharacteristic(tAccelYService);
  nanoSensors.addCharacteristic(tAccelZService);
  BLE.addService(nanoSensors);
  
  BLE.advertise();

  if(Serial)
    Serial.println("Bluetooth is now active, waiting for connections...");

  filter.begin(119);
  delay(100);
}



void loop() {
  BLEDevice central = BLE.central();
  getSensorData();
  if(central){
    if(Serial){
      Serial.print("Connected to central: ");
      Serial.println(central.address());
    }
    digitalWrite(LED_BUILTIN, HIGH);
    while(central.connected()){
      getSensorData();

      tAccelXService.writeValue(tAccelXChar, MAX_C_LENGTH);
      tAccelYService.writeValue(tAccelYChar, MAX_C_LENGTH);
      tAccelZService.writeValue(tAccelZChar, MAX_C_LENGTH);
      
      pitchService.writeValue(pitchChar, MAX_C_LENGTH);
      yawService.writeValue(yawChar, MAX_C_LENGTH);
      rollService.writeValue(rollChar, MAX_C_LENGTH);
      
      if(Serial)
        printSensorData();
    }
    if(Serial){
      Serial.print("Disconnected from central: ");
      Serial.println(central.address());
    }
    digitalWrite(LED_BUILTIN, LOW);
  }
}

void getSensorData(){
  if(IMU.gyroscopeAvailable() && IMU.accelerationAvailable()){
    IMU.readGyroscope(g[X],g[Y],g[Z]);
    /*if(g[X] > 0)
      g[X] -= gyroBias[X];
    else if(g[X] < 0)
      g[X] += gyroBias[X];

    if(g[Y] > 0)
      g[Y] -= gyroBias[Y];
    else if(g[Y] < 0)
      g[Y] += gyroBias[Y];

    if(g[Z] > 0)
      g[Z] -= gyroBias[Z];
    else if(g[Z] < 0)
      g[Z] += gyroBias[Z];*/
    
    g[X] = g[X] * gscale;
    g[Y] = g[Y] * gscale;
    g[Z] = g[Z] * gscale;
    
    IMU.readAcceleration(a[X],a[Y],a[Z]);
    a[X] = a[X];
    a[Y] = a[Y];
    a[Z] = a[Z];
      
    if(IMU.magneticFieldAvailable()){
      IMU.readMagneticField(m[X],m[Y],m[Z]);
      m[X] = (m[X] - magBias[X]); //* magScale[X];
      m[Y] = (m[Y] - magBias[Y]); //* magScale[Y];
      m[Z] = (m[Z] - magBias[Z]); // * magScale[Z];

      m[X] = -m[X];
      m[Y] = m[Y];
      m[Z] = m[Z];
    }
  
    
  
    filter.update(g[X],g[Z],g[Y],a[X],a[Z],a[Y],m[X],m[Z],m[Y]);
  }  
  roll = filter.getRoll();
  pitch = filter.getPitch();
  yaw = filter.getYaw();

/*
 float yawBias = .01;
  if(yaw < 0)
    yaw += yawBias; 
  else if(yaw > 0)
    yaw -= yawBias;*/

  for (int i = 0; i < MAX_C_LENGTH; i++){
      pitchChar[i] = 0;
      rollChar[i] = 0;
      yawChar[i] = 0;
      tAccelXChar[i] = 0;
      tAccelYChar[i] = 0;
      tAccelZChar[i] = 0;  
  }
  
  sprintf(pitchChar, "%f", pitch);
  sprintf(rollChar, "%f", -roll);
  sprintf(yawChar, "%f", -yaw);

  float tAccel[3];
  getTrueAccel(tAccel);

  sprintf(tAccelXChar, "%f", tAccel[X]);
  sprintf(tAccelYChar, "%f", tAccel[Y]);
  sprintf(tAccelZChar, "%f", tAccel[Z]);
}

void getTrueAccel(float trueAccel[3]){
  float rotationMatrix[3][3];
  getRotMatfromEuler(rotationMatrix, pitch, yaw, roll, false);

  printAccelCheck(rotationMatrix);

  float rotMatInverse[3][3];
  inverse(rotationMatrix, rotMatInverse);
  
  matMult3x3byVector(trueAccel, rotMatInverse,3,3,a,1,3);
  float tempZ = trueAccel[Z];
  trueAccel[Z] = (trueAccel[Z]);
  trueAccel[Y] = tempZ;

  char trueAccelChar[35];
  
  sprintf(trueAccelChar, "TrueAccel:\nX: %f Y: %f Z: %f", trueAccel[X], trueAccel[Y], trueAccel[Z]); 
  Serial.println(trueAccelChar);
  Serial.println("\n\n\n");
}
 
}

I tested my rotation matrix calculation with a vector of [0,0,1] which I expected to approximately equal the at rest acceleration values when multiplied by the rotation matrix, and it did, but I didn’t think to test the inverse methods on the identity matrix, thank you guys for that idea.

I am now having another issue however, where writing the acceleration values to their ble characteristics causes the pitch yaw and roll to behave different in testing. 3Dof works fine in my unity project when I comment out the lines

      tAccelXService.writeValue(tAccelXChar, MAX_C_LENGTH);
      tAccelYService.writeValue(tAccelYChar, MAX_C_LENGTH);
      tAccelZService.writeValue(tAccelZChar, MAX_C_LENGTH);

even with the lines that calculate the acceleration and the declarations of the characteristics included.
but if I add them in, without reading any of the values from them and only reading the 3DoF values, the simulation performs very poorly. the 3DoF is jerky and laggy, the directions seem wrong, and a full turn of the device seems to move the scene no more than about 60 degrees.

Has anyone seen a problem like this before? The only thing I can think of is that the ble library write value method takes too long and that calling it 6 times slows down the reading of values which causes the filter to update poorly, or that I’m some how writing over a buffer or something like that, but it seems like the writeValue method would be the cheapest operation in the acceleration calculation and the program works fine with all other components running. I am proper stumped.

It’s also worth noting that my arduino crashes intermittently after receiving an upload of this sketch, but I can’t debug it because it works fine with the above symptoms after I reset it and reupload without changing anything.

Help is much appreciated. Thanks.

It is hard to see how that code could ever produce sensible values of yaw, pitch and roll.

You aren't subtracting the gyro bias, and the commented out code wouldn't do that properly anyway.

The handedness of the gyro and the accelerometer need to be inverted, not the magnetometer.

The issues were discussed recently in this thread.

See you would think that lol but my pitch yaw and roll values actually work quite well and I've seen that thread and tried it that way and what I have now seems to work better, although there is some drift.

I think it might have something to do with the fact that my arduino is mounted on its side so perhaps the axes are different, but I'm honestly quite bewildered as well. I've tried to calibrate by subtracting average at rest values for gyro and accelerometer (as well as attempt to calibrate for mag soft iron bias) but I can't really discern any measurable improvement from doing so and in some cases it seems like the calibration makes the drift worse, which is why it is commented out currently in my above code.

Also, even though the datasheet says and it is discussed in that thread that the gyro readings are in mdps, attempting to convert them by scaling by 1000 after scaling by the lsb causes the 3dof tracking to not move at all bc the pitch yaw and roll values are way too tiny. The only way I can explain it is that the datasheet is straight up wrong and that the readings are in dps.

I have tried just subtracting the gyro bias instead of subtracting or adding based on the sign as well, but I'm not really sure the proper way to do so, how am I supposed to do it?
Thanks

For the Madgwick and Mahony filters, the rate gyro data must be correctly scaled to radians per second.

Most people calibrate the gyro by collecting 500 or so readings while the sensor is still, averaging those to get the bias values, and then subtracting the bias values from subsequent readings.

If you invert the magnetometer handedness, as in the code you posted, the 3D orientation and Euler angles you get will be defined in a left handed coordinate system. No big problem, as long as you are aware of that.

PS: your sensor is 9DOF.

Ah okay thank you. I did not know that my coordinate system was left handed, but it seems to work the best in my simulation. Could the left handed coordinate system cause problems in the future if I try to implement more graphical things that have a right handed coordinate system standard?

Doesnt 6DoF also refer to pitch, yaw, roll and an acceleration vector in the global coordinate plane? So that the sensor can sense the direction that the object is facing and where the object is moving.

my gyro data is converted from degrees to radians in the madgwick filter code. I will try to calibrate again using 500 readings and see if I get a better calibration. Before I calibrated by getting a reading every frame for 1 minute abut I will try it with exactly 500 readings and it's possible that I messed up my average by adding to my dataset before there was an actual new reading.

But do you have any ideas for how to fix my writeValue() problem? I was thinking I would try lowering the given update speed for the madgwick filter and see if the issue is that the function slows down the speed in which the filter is updated

it seems to work the best in my simulation

The results obtained using either a left or right handed coordinate system will be exactly the same in terms of "quality". If you tried to fix the left handed issue and got qualitatively different results, you probably made a mistake.

Could the left handed coordinate system cause problems in the future if I try to implement more graphical things that have a right handed coordinate system standard?

Of course it will.

The "9" in a 9DOF sensor means the 9 independent variables that it measures. When people on this forum see 6DOF, they usually think of it as applying to sensors that measure 6 quantities.

For an object in a 3D coordinate system, you have many more degrees of freedom, including position, velocity, acceleration and the same three concepts applied to its orientation.

Okay, Understood thank you. Any ideas on how to debug my other problem? It sort of impedes progress on the other stuff.

With regard to your "other problem", your description doesn't make much sense to me, and I don't understand either what you are trying to do or what the problem is.

The sticky post "How to use this forum" gives suggestions for how to post an intelligible question.

So I'm trying to create a system where, when the arduino is mounted to a person's head, it is able to track head orientation as well as where the person is moving, like they do in VR. My problem is basically:

When I add the three lines

      tAccelXService.writeValue(tAccelXChar, MAX_C_LENGTH);
      tAccelYService.writeValue(tAccelYChar, MAX_C_LENGTH);
      tAccelZService.writeValue(tAccelZChar, MAX_C_LENGTH);

Into my code, the madgwick filter gets a lot worse, even though these 3 lines seemingly have nothing to do with the madgwick filter.