四元数转旋转矩阵stm32示例

在 STM32 上进行四元数转旋转矩阵的示例代码如下：

#include "math.h"

#define PI 3.14159265358979323846f

typedef struct {
    float q0, q1, q2, q3; // 四元数
} Quaternion;

typedef struct {
    float m11, m12, m13;
    float m21, m22, m23;
    float m31, m32, m33; // 旋转矩阵
} RotationMatrix;

// 四元数转旋转矩阵
void quaternionToRotationMatrix(const Quaternion q, RotationMatrix *rm)
{
    float q0q0 = q.q0 * q.q0;
    float q0q1 = q.q0 * q.q1;
    float q0q2 = q.q0 * q.q2;
    float q0q3 = q.q0 * q.q3;
    float q1q1 = q.q1 * q.q1;
    float q1q2 = q.q1 * q.q2;
    float q1q3 = q.q1 * q.q3;
    float q2q2 = q.q2 * q.q2;
    float q2q3 = q.q2 * q.q3;
    float q3q3 = q.q3 * q.q3;

    rm->m11 = q0q0 + q1q1 - q2q2 - q3q3;
    rm->m12 = 2.0f * (q1q2 - q0q3);
    rm->m13 = 2.0f * (q1q3 + q0q2);
    rm->m21 = 2.0f * (q1q2 + q0q3);
    rm->m22 = q0q0 - q1q1 + q2q2 - q3q3;
    rm->m23 = 2.0f * (q2q3 - q0q1);
    rm->m31 = 2.0f * (q1q3 - q0q2);
    rm->m32 = 2.0f * (q2q3 + q0q1);
    rm->m33 = q0q0 - q1q1 - q2q2 + q3q3;
}

int main()
{
    Quaternion q = {0.9239f, 0.3827f, 0.0f, 0.0f}; // 四元数 q = cos(theta/2) + sin(theta/2) * i
    RotationMatrix rm;

    quaternionToRotationMatrix(q, &rm); // 四元数转旋转矩阵

    return 0;
}

注意，上述代码中的四元数 `q` 是一个具体的示例，可以根据需要进行修改。同时，需要确保在正在使用的开发环境中包含了 math.h 头文件。