/* Rotates the DCM matrix by a given angle.  */
void IMU_dcmRotate(float dcm[3][3], float angle[3])
/* Rotates the DCM matrix by a given angle.  */
void IMU_dcmRotate(float dcm[3][3], float angle[3])
{
   int x, y;
   float um[3][3];
   float tm[3][3];
   um[0][0] =  0.0f;
   um[0][1] = -angle[2];
   um[0][2] =  angle[1];
   um[1][0] =  angle[2];
   um[1][1] =  0.0f;
   um[1][2] = -angle[0];
   um[2][0] = -angle[1];
   um[2][1] =  angle[0];
   um[2][2] =  0.0f;
   IMU_matrixMultiply( tm, dcm, um );
   for( y = 0; y < 3; y++ ) {
      for( x = 0; x < 3; x++ ) {
         dcm[y][x] += tm[y][x];
      }
   }
   return;
}