accelerometercalibrationrotational-matricesimumems

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


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.


Solution

  • 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.