度/秒至轴角

Question

我有一张陀螺仪读数，单位为度/秒（X、Y、Z）。我需要在 C 中将其转换为轴角格式（X、Y、Z、THETA）。 这是我想到的：

void gyroToAxisAngle(double gyro[3], double axis_angle[4]) {
    double gyroRad[3];
    // Convert gyro values from degrees per second to radians per second
    gyroRad[0] = gyro[0] * M_PI / 180.0;
    gyroRad[1] = gyro[1] * M_PI / 180.0;
    gyroRad[2] = gyro[2] * M_PI / 180.0;

    // Calculate the angular velocity magnitude
    double gyroMagnitude = sqrt(gyroRad[0] * gyroRad[0] + gyroRad[1] * gyroRad[1] + gyroRad[2] * gyroRad[2]);

    // Calculate the axis of rotation
    axis_angle[0] = gyroRad[0] / gyroMagnitude;
    axis_angle[1] = gyroRad[1] / gyroMagnitude;
    axis_angle[2] = gyroRad[2] / gyroMagnitude;

    // Calculate the rotation angle (theta)
    axis_angle[3] = gyroMagnitude;
}

让我们以这些陀螺仪值为例

{-22.767, 145.174, -47.1}

，结果值为

XYZ: (-0.147539, 0.940781, -0.305225)

，

Theta: 2.693255

。

有人可以确认我是否正确转换，或者是否有更准确的转换方法？谢谢！

Answer 1

有没有更准确的换算方法？

是的，使用更广泛的数学（当

long double

比

double

更宽时）和

FLT_EVAL_METHOD < 2

。

也许这不是 OP 寻求的改进，但它就是：

// long double constant - degrees to radians
#defined D2R (3.1415926535897932384626433832795L / 180)

void gyroToAxisAngle(const double gyro[3], double axis_angle[4]) {
    // Convert gyro values from degrees per second to radians per second
    long double qr0 = gyro[0] * D2R;
    long double qr1 = gyro[1] * D2R;
    long double qr2 = gyro[2] * D2R;

    // Calculate the angular velocity magnitude
    long double gyroMagnitude = sqrtl(gr0*gr0 + gr1*gr1 + gr2*gr2);

    // Calculate the axis of rotation
    axis_angle[0] = (double) (qr0 / gyroMagnitude);
    axis_angle[1] = (double) (qr1 / gyroMagnitude);
    axis_angle[2] = (double) (gr2 / gyroMagnitude);

    // Calculate the rotation angle (theta)
    axis_angle[3] = (double) gyroMagnitude;
}

我估计OP的结果会达到1或2左右ULP更好。