通过四元数计算IMU角度的代码

以下是通过四元数计算IMU角度的示例代码，其中 `q` 是一个四元数，`roll`、`pitch` 和 `yaw` 分别是对应的欧拉角：

#include <cmath>

// 计算 IMU 角度
void calculate_imu_angles(double q[4], double& roll, double& pitch, double& yaw) {
    // 计算重力加速度在 IMU 坐标系中的方向向量
    double gx = 2.0 * (q[1] * q[3] - q[0] * q[2]);
    double gy = 2.0 * (q[0] * q[1] + q[2] * q[3]);
    double gz = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];

    // 计算 IMU 坐标系中的重力加速度方向向量与参考方向向量之间的夹角
    double acc_magnitude = std::sqrt(gx * gx + gy * gy + gz * gz);
    double cos_roll = gx / acc_magnitude;
    double cos_pitch = gy / acc_magnitude;
    double cos_yaw = gz / acc_magnitude;

    // 转换成欧拉角
    roll = std::atan2(cos_roll, std::sqrt(1.0 - cos_roll * cos_roll));
    pitch = std::atan2(cos_pitch, std::sqrt(1.0 - cos_pitch * cos_pitch));
    yaw = std::atan2(cos_yaw, std::sqrt(1.0 - cos_yaw * cos_yaw));
}

这里假设 IMU 坐标系的 z 轴方向与重力加速度方向重合。如果实际情况与此不同，需要对计算过程作出相应的调整。另外，需要注意四元数的顺序，以及欧拉角的单位。