4d_ObstacleRadarDroneCAN/math/quaternion.c

#include "quaternion.h"
#include "float.h"
#include "helpler_funtions.h"
#include "math.h"
#include "matrix.h"

/**
 * @brief 四元数初始化
 *
 */
void Quaternion(float q[4]) {
  q[0] = 1.0f;
  for (int i = 1; i < 4; ++i) {
    q[i] = 0.0f;
  }
}
/**
 * @brief 四元数单位化
 *
 * @param Q 需要单位化的四元数
 */
void Quaternion_Normalize(float Q[4]) {
  float q_norm = Vector_GetNorm(Q, 4);
  if (!is_zerof(q_norm)) {
    Vector_Scale(Q, 1.0F / q_norm, 4);
  }
}

/**
 * @brief 四元数规范化 保证实部不小于0
 *
 */
void Quaternion_Canonicalize(float Q[4]) {
  for (int i = 0; i < 4; ++i) {
    if (fabsf(Q[i]) > FLT_EPSILON) {
      Vector_Scale(Q, sign(Q[i]), 4);
      return;
    }
  }
}

/**
 * @brief 使用旋转矩阵构造四元数
 *
 * @param Q 四元数
 * @param R 旋转矩阵
 */
void Quaternion_ByDcm(const float R[3][3], float Q[4]) {
  float t = Matrix_GetTrace(&R[0][0], 3);

  if (t > 0.0f) {
    t = sqrtf(1.0f + t);
    Q[0] = 0.5f * t;
    t = 0.5f / t;
    Q[1] = (R[2][1] - R[1][2]) * t;
    Q[2] = (R[0][2] - R[2][0]) * t;
    Q[3] = (R[1][0] - R[0][1]) * t;
  } else if (R[0][0] > R[0][1] && R[0][0] > R[2][2]) {
    t = sqrtf(1.0f + R[0][0] - R[1][1] - R[2][2]);
    Q[1] = 0.5F * t;
    t = 0.5f / t;
    Q[0] = (R[2][1] - R[1][2]) * t;
    Q[2] = (R[1][0] + R[0][1]) * t;
    Q[3] = (R[0][2] + R[2][0]) * t;
  } else if (R[1][1] > R[2][2]) {
    t = sqrtf(1.0f - R[0][0] + R[1][1] - R[2][2]);
    Q[2] = 0.5F * t;
    t = 0.5f / t;
    Q[0] = (R[0][2] - R[2][0]) * t;
    Q[1] = (R[1][0] + R[0][1]) * t;
    Q[3] = (R[2][1] + R[1][2]) * t;
  } else {
    t = sqrtf(1.0f - R[0][0] - R[1][1] + R[2][2]);
    Q[3] = 0.5F * t;
    t = 0.5f / t;
    Q[0] = (R[1][0] - R[0][1]) * t;
    Q[1] = (R[0][2] + R[2][0]) * t;
    Q[2] = (R[2][1] + R[1][2]) * t;
  }
}

/**
 * @brief 使用欧拉角构造四元数, enu_rfu 框架, 右手坐标系
 *
 * @param yaw 航向角 rad
 * @param pitch 俯仰角 rad
 * @param roll 横滚角 rad
 * @param Q 输出四元数
 */
void Quaternion_Enu_ByEuler(float yaw, float pitch, float roll, float Q[4]) {
  float cos_yaw_2 = cosf(yaw / 2);
  float cos_pitch_2 = cosf(pitch / 2);
  float cos_roll_2 = cosf(roll / 2);
  float sin_yaw_2 = sinf(yaw / 2);
  float sin_pitch_2 = sinf(pitch / 2);
  float sin_roll_2 = sinf(roll / 2);

  Q[0] = (cos_yaw_2 * cos_pitch_2 * cos_roll_2 -
          sin_yaw_2 * sin_pitch_2 * sin_roll_2);
  Q[1] = (cos_yaw_2 * sin_pitch_2 * cos_roll_2 -
          sin_yaw_2 * cos_pitch_2 * sin_roll_2);
  Q[2] = (cos_yaw_2 * cos_pitch_2 * sin_roll_2 +
          sin_yaw_2 * sin_pitch_2 * cos_roll_2);
  Q[3] = (cos_yaw_2 * sin_pitch_2 * sin_roll_2 +
          sin_yaw_2 * cos_pitch_2 * cos_roll_2);
}

/**
 * @brief 使用欧拉角构造四元数, ned_frd 框架, 右手坐标系
 *
 * @param yaw 航向角 rad
 * @param pitch 俯仰角 rad
 * @param roll 横滚角 rad
 * @param Q 输出四元数
 */
void Quaternion_Ned_ByEuler(float yaw, float pitch, float roll, float Q[4]) {
  float cos_yaw_2 = cosf(yaw / 2);
  float cos_pitch_2 = cosf(pitch / 2);
  float cos_roll_2 = cosf(roll / 2);
  float sin_yaw_2 = sinf(yaw / 2);
  float sin_pitch_2 = sinf(pitch / 2);
  float sin_roll_2 = sinf(roll / 2);

  Q[0] = cos_roll_2 * cos_pitch_2 * cos_yaw_2 +
         sin_roll_2 * sin_pitch_2 * sin_yaw_2;
  Q[1] = sin_roll_2 * cos_pitch_2 * cos_yaw_2 -
         cos_roll_2 * sin_pitch_2 * sin_yaw_2;
  Q[2] = cos_roll_2 * sin_pitch_2 * cos_yaw_2 +
         sin_roll_2 * cos_pitch_2 * sin_yaw_2;
  Q[3] = cos_roll_2 * cos_pitch_2 * sin_yaw_2 -
         sin_roll_2 * sin_pitch_2 * cos_yaw_2;
}

/**
 * @brief 使用轴角构造四元数
 *
 * @param anxisAngle 旋转轴角向量, 模为转角 rad, 方向为转轴
 * @param Q 四元数
 */
void Quaternion_ByAxisAngle(const float anxisAngle[3], float Q[4]) {
  float angle = Vector_GetNorm(anxisAngle, 3);

  if (angle < 1e-10f) {
    /* 如果 angle 太小, 相当于没转 */
    Quaternion(Q);
  } else {
    float axis[3] = {anxisAngle[0], anxisAngle[1], anxisAngle[2]};
    Vector_Scale(axis, 1.0f / angle, 3);
    float magnitude = sinf(angle * 0.5f);
    Q[0] = cosf(angle * 0.5f);
    for (int i = 0; i < 3; i++) {
      Q[i + 1] = axis[i] * magnitude;
    }
  }
}

/**
 * @brief 使用向量, 构造由 V1->V2 的最短旋转四元数
 *
 * @param Vsrc 旋转前的向量
 * @param Vdst 旋转后的向量
 * @param Q V1->V2 的最速旋转四元数
 */
void Quaternion_ByTwoVectors(const float Vsrc[3], const float Vdst[3],
                             float Q[4]) {
  float Vcr[3], dot;
  const float eps = 1e-5;
  Vector_CrossProduct_3D(Vsrc, Vdst, Vcr);
  dot = Vector_DotProduct(Vsrc, Vdst, 3);

  if (dot < 0.0f && Vector_GetNorm(Vcr, 3) < eps) {
    /* 判断 Vsrc Vdst 是接近 180 度 */
    float Vtmp[3] = {fabsf(Vcr[0]), fabsf(Vcr[1]), fabsf(Vcr[2])};
    if (Vtmp[0] < Vtmp[1]) {
      if (Vtmp[0] < Vtmp[2]) {
        Vtmp[0] = 1;
        Vtmp[1] = 0;
        Vtmp[2] = 0;
      } else {
        Vtmp[0] = 0;
        Vtmp[1] = 0;
        Vtmp[2] = 1;
      }
    } else {
      if (Vtmp[1] < Vtmp[2]) {
        Vtmp[0] = 0;
        Vtmp[1] = 1;
        Vtmp[2] = 0;
      } else {
        Vtmp[0] = 0;
        Vtmp[1] = 0;
        Vtmp[2] = 1;
      }
    }
    Q[0] = 0;
    Vector_CrossProduct_3D(Vsrc, Vtmp, Vcr);
  } else {
    Q[0] = dot + sqrt(Vector_DotProduct(Vsrc, Vsrc, 3) *
                      Vector_DotProduct(Vdst, Vdst, 3));
  }

  Q[1] = Vcr[0];
  Q[2] = Vcr[1];
  Q[3] = Vcr[2];

  Quaternion_Normalize(Q);
}

/**
 * @brief 单位四元数乘法
 *
 * @param Q1
 * @param Q2
 * @param Q Q1 X Q2
 */
void Quaternion_Multiplication(const float Q1[4], const float Q2[4],
                               float Q[4]) {
  float Q_temp[4];
  Q_temp[0] = Q1[0] * Q2[0] - Q1[1] * Q2[1] - Q1[2] * Q2[2] - Q1[3] * Q2[3];
  Q_temp[1] = Q1[1] * Q2[0] + Q1[0] * Q2[1] - Q1[3] * Q2[2] + Q1[2] * Q2[3];
  Q_temp[2] = Q1[2] * Q2[0] + Q1[3] * Q2[1] + Q1[0] * Q2[2] - Q1[1] * Q2[3];
  Q_temp[3] = Q1[3] * Q2[0] - Q1[2] * Q2[1] + Q1[1] * Q2[2] + Q1[0] * Q2[3];
  for (int i = 0; i < 4; i++) {
    Q[i] = Q_temp[i];
  }
}

/**
 * @brief 四元数求逆
 *
 * @param Q
 * @param Q_inv
 */
void Quaternion_Inversed(const float Q[4], float Q_inv[4]) {
  float q_norm = Vector_GetNorm(Q, 4);
  Q_inv[0] = Q[0] / q_norm;
  Q_inv[1] = -Q[1] / q_norm;
  Q_inv[2] = -Q[2] / q_norm;
  Q_inv[3] = -Q[3] / q_norm;
}

/**
 * @brief 将四元数求逆
 *
 * @param Q
 */
void Quaternion_Invert(float Q[4]) {
  float Q_inv[4];
  Quaternion_Inversed(Q, Q_inv);

  for (int i = 0; i < 4; ++i) {
    Q[i] = Q_inv[i];
  }
}

/**
 * @brief 根据 frame1 的角速率, 求 q21 的微分
 *  比如 frame1 为机体坐标系, frame2 为东北天坐标系
 *  V1 为向量在 frame1 的坐标, V2 为向量在 fram2 的坐标
 *  [0, V2] = q21 * [0, V1] * q21^-1
 *  q21_deriv = q21 * [0, 0.5 * angle_rate1]
 *
 * @param q21 当前四元数
 * @param angle_rate1 frame 1 下的三轴角速率
 * @param q21_deriv q21 的微分
 */
void Quaternion_Derivative1(const float q21[4], const float angle_rate1[3],
                            float q21_deriv[4]) {
  float v[4] = {0, 0.5f * angle_rate1[0], 0.5f * angle_rate1[1],
                0.5f * angle_rate1[2]};

  Quaternion_Multiplication(q21, v, q21_deriv);
}

/**
 * @brief 根据 frame2 的角速率, 求 q21 的微分
 *  比如 frame1 为机体坐标系, frame2 为东北天坐标系
 *  V1 为向量在 frame1 的坐标, V2 为向量在 fram2 的坐标
 *  q21 为 [0,V2] = q21 * [0,V1] * q21^-1
 *  q21_deriv = [0, 0.5 * angle_rate1] * q21
 *
 * @param q21 当前四元数
 * @param angle_rate1 frame 1 下的三轴角速率
 * @param q21_deriv q21 的微分
 */
void Quaternion_Derivative2(const float q21[4], const float angle_rate2[3],
                            float q21_deriv[4]) {
  float v[4] = {0, 0.5f * angle_rate2[0], 0.5f * angle_rate2[1],
                0.5f * angle_rate2[2]};

  Quaternion_Multiplication(v, q21, q21_deriv);
}

/**
 * @brief 将四元数按照轴角进行旋转
 *
 * @param axisAngle 轴角向量
 * @param Q 四元数
 */
void Quaternion_RotateByAxisAngle(const float axisAngle[3], float Q[4]) {
  /* 根据轴角向量计算旋转四元数 */
  float q_rot[4], q_temp[4];
  Quaternion_ByAxisAngle(axisAngle, q_rot);

  Quaternion_Multiplication(q_rot, Q, q_temp);

  for (int i = 0; i < 4; ++i) {
    Q[i] = q_temp[i];
  }
}

/**
 * @brief 通过四元数将 frame1 下的 V1 转到 frame2 下的 V2
 *
 * @param Q 四元数
 * @param V1
 * @param V2
 */
void Quaternion_Conj(const float Q[4], const float V1[3], float V2[3]) {
  float v_temp1[4] = {0, V1[0], V1[1], V1[2]};
  float v_temp2[4], v_temp3[4];

  Quaternion_Multiplication(Q, v_temp1, v_temp2);
  Quaternion_Inversed(Q, v_temp1);
  Quaternion_Multiplication(v_temp2, v_temp1, v_temp3);

  V2[0] = v_temp3[1];
  V2[1] = v_temp3[2];
  V2[2] = v_temp3[3];
}

/**
 * @brief 通过四元数将 frame2 下的 V2 转到 frame1 下的 V1
 *
 * @param Q 四元数
 * @param V1
 * @param V2
 */
void Quaternion_ConjInv(const float Q[4], const float V2[3], float V1[3]) {
  float v_temp1[4] = {0, V2[0], V2[1], V2[2]};
  float v_temp2[4], v_temp3[4];

  Quaternion_Multiplication(v_temp1, Q, v_temp2);
  Quaternion_Inversed(Q, v_temp1);
  Quaternion_Multiplication(v_temp1, v_temp2, v_temp3);

  V1[0] = v_temp3[1];
  V1[1] = v_temp3[2];
  V1[2] = v_temp3[3];
}

/**
 * @brief 通过四元数求其对应的旋转矩阵的 Z 轴
 * 可参考由四元数构造旋转矩阵
 *
 * @param Q 四元数
 * @param dcm_z 旋转矩阵 z 轴
 */
void Quaternion_GetDcmZ(const float Q[4], float dcm_z[3]) {
  float a = Q[0];
  float b = Q[1];
  float c = Q[2];
  float d = Q[3];

  dcm_z[0] = 2 * (a * c + b * d);
  dcm_z[1] = 2 * (c * d - a * b);
  dcm_z[2] = a * a - b * b - c * c + d * d;
}