How to calculate rotation matrix for an accelerometer using only basic algebraic operations

Question

There is a C function that gets the acceleration values of x,y,z of an accelerometer sensor (MEMS IMU) as input, and calculates the rotation matrix in a way that the z axis is aligned with the gravity. It is being used for calibrating the accelerometer data.


#define X_AXIS (0u)
#define Y_AXIS (1u)
#define Z_AXIS (2u)

static float matrix[3][3];
void calculate_rotation_matrix(float raw_x, float raw_y, float raw_z)
{ 
  const float r = sqrtf(raw_x * raw_x + raw_y * raw_y + raw_z * raw_z);
  const float x = raw_x / r;
  const float y = raw_y / r;
  const float z = raw_z / r;

  const float x2 = x * x;
  const float y2 = y * y;

  matrix[X_AXIS][X_AXIS] = (y2 - (x2 * z)) / (x2 + y2);
  matrix[X_AXIS][Y_AXIS] = ((-x * y) - (x * y * z)) / (x2 + y2);
  matrix[X_AXIS][Z_AXIS] = x;
  matrix[Y_AXIS][X_AXIS] = ((-x * y) - (x * y * z)) / (x2 + y2);
  matrix[Y_AXIS][Y_AXIS] = (x2 - (y2 * z)) / (x2 + y2);
  matrix[Y_AXIS][Z_AXIS] = y;
  matrix[Z_AXIS][X_AXIS] = -x;
  matrix[Z_AXIS][Y_AXIS] = -y;
  matrix[Z_AXIS][Z_AXIS] = -z;
}



float result[3];
void apply_rotation(float x, float y, float z)
{
  result[AXIS_X] = matrix[X_AXIS][X_AXIS] * x
                 + matrix[X_AXIS][Y_AXIS] * y
                 + matrix[X_AXIS][Z_AXIS] * z;
  
  result[AXIS_Y] = matrix[Y_AXIS][X_AXIS] * x
                 + matrix[Y_AXIS][Y_AXIS] * y
                 + matrix[Y_AXIS][Z_AXIS] * z;
  
  result[AXIS_Z] = matrix[Z_AXIS][X_AXIS] * x
                 + matrix[Z_AXIS][Y_AXIS] * y
                 + matrix[Z_AXIS][Z_AXIS] * z;

}

I'm trying to wrap my head around how it works and why there is no use of trigonometric functions here? is it just simplifying the trigonometric functions by normalizing the input values and using the equivalent equations to calculate the trigonometric functions?

What are the limitations of this method? for example when the denominator calculated is zero, we will have division by zero. Anything else?

Tried to search on the internet and stackoverflow, but couldn't find a similar method to calculate the rotation matrix.

UPDATE:

Just simplified the calculations so they are more readable. To add more context this code is used to rotate the readings of an accelerometer in a way that regardless of the orientation of the device, the z-axis is perpendicular to the ground.

The calculate_rotation_matrix() is called when we know that the object is stationary and is on a flat surface. This results in calculating the 3x3 matrix. Then the apply_rotation() is used to rotate subsequent readings.

imuengine.io · Accepted Answer · 2023-06-06T22:03:37.623

0

A quaternion representation can apply rotations without trig functions. But this appears to be a version of: https://math.stackexchange.com/a/476311 . The math appears to be a variation thereof, where "a" and "b" are the accelerometer and gravity vectors.

The method also appears to assume measurements will not be perfect. And MEMS sensors fit that description. Otherwise, as you stated, if x and y are both zero then you have a divide by zero condition.

edited Jun 06 '23 at 22:03

answered Jun 01 '23 at 19:15

imuengine.io

51
3

IMHO it doesn't seems like a quaternion. As it is producing a rotation matrix with 9 elements. isn't it just simplifying the trigonometric functions by using the normalized values? – XExplo Jun 01 '23 at 22:39
See this: https://math.stackexchange.com/a/476311 . The math appears to be a variation thereof, where "a" and "b" are the accelerometer and gravity vectors. What do you think? – imuengine.io Jun 03 '23 at 03:16
Yes I think that is the method used here! So my understanding is that since it is using the rotation matrices (in contrast with using quaternions) the method suffers from gimbal lock. Do you think there is any other issues with this method? – XExplo Jun 05 '23 at 22:50
Once formed, rotation matrices themselves don't have any limitation. Unlike Euler angles (e.g. Z-Y-X) which have a singularity at 90 deg pitch, which I'm guessing you are calling "gimbal lock". So any limitation will come in how the rotation matrix is formed. A divide by zero condition may happen if the accelerometer x & y are zero. What other issues are you concerned about? – imuengine.io Jun 06 '23 at 21:58
I'm going to update the answer to reference the link discussed. – imuengine.io Jun 06 '23 at 22:00
XExplo, can you conclude this question? Either accept if answer is complete or explain any further issues. – imuengine.io Jun 14 '23 at 14:38

How to calculate rotation matrix for an accelerometer using only basic algebraic operations

1 Answers1