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!
