Madgwick算法 c语言

根据提供的引用内容，Madgwick算法的源码可以在作者的公司网站上找到。这里提供一个C语言版本的Madgwick算法的实现，供您参考：

#include <math.h>

#define sampleFreq 512.0f // 采样频率
#define betaDef 0.1f // beta默认值

void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float *qw, float *qx, float *qy, float *qz)
{
    // 全局变量
    static float beta = betaDef; // beta
    static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // 四元数
    static float invSampleFreq = 1.0f / sampleFreq; // 采样频率的倒数
    static float invBeta = 1.0f / beta; // beta的倒数

    // 辅助变量
    float recipNorm;
    float s0, s1, s2, s3;
    float qDot1, qDot2, qDot3, qDot4;
    float hx, hy;
    float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;

    // 单位化加速度、磁力计数据
    recipNorm = sqrt(ax * ax + ay * ay + az * az);
    if (recipNorm == 0.0f) return;
    recipNorm = 1.0f / recipNorm;
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;
    recipNorm = sqrt(mx * mx + my * my + mz * mz);
    if (recipNorm == 0.0f) return;
    recipNorm = 1.0f / recipNorm;
    mx *= recipNorm;
    my *= recipNorm;
    mz *= recipNorm;

    // 计算磁场的参考方向
    hx = 2.0f * mx * (0.5f - q2 * q2 - q3 * q3) + 2.0f * my * (q1 * q2 - q0 * q3) + 2.0f * mz * (q1 * q3 + q0 * q2);
    hy = 2.0f * mx * (q1 * q2 + q0 * q3) + 2.0f * my * (0.5f - q1 * q1 - q3 * q3) + 2.0f * mz * (q2 * q3 - q0 * q1);

    // 计算误差
    _2q0mx = 2.0f * q0 * mx;
    _2q0my = 2.0f * q0 * my;
    _2q0mz = 2.0f * q0 * mz;
    _2q1mx = 2.0f * q1 * mx;
    _2q0 = 2.0f * q0;
    _2q1 = 2.0f * q1;
    _2q2 = 2.0f * q2;
    _2q3 = 2.0f * q3;
    _2q0q2 = 2.0f * q0 * q2;
    _2q2q3 = 2.0f * q2 * q3;
    q0q0 = q0 * q0;
    q0q1 = q0 * q1;
    q0q2 = q0 * q2;
    q0q3 = q0 * q3;
    q1q1 = q1 * q1;
    q1q2 = q1 * q2;
    q1q3 = q1 * q3;
    q2q2 = q2 * q2;
    q2q3 = q2 * q3;
    q3q3 = q3 * q3;
    _2bx = sqrt(hx * hx + hy * hy);
    _2bz = -q0q2 * mx + q0q1 * my + q3q1 * mz;
    _4bx = 2.0f * _2bx;
    _4bz = 2.0f * _2bz;

    // 梯度下降算法
    s0 = -_2q2 * (2.0f * (q1q3 - q0q2) - ax) + _2q1 * (2.0f * (q0q1 + q2q3) - ay) + _4bz * q2 * (_4bx * (0.5f - q2q2 - q3q3) + _4bz * (q1q3 - q0q2) - mx) - (_4bx * q3 + _4bz * q1) * (_4bx * (q2q3 - q0q1) + _4bz * (q0q2 + q1q3) - my) + _4bx * q2 * (_4bx * (q0q1 + q2q3) + _4bz * (0.5f - q1q1 - q2q2) - mz);
    s1 = _2q3 * (2.0f * (q1q3 - q0q2) - ax) + _2q0 * (2.0f * (q0q1 + q2q3) - ay) - 4.0f * q1 * (2.0f * (0.5f - q1q1 - q2q2) - az) + _4bz * q3 * (_4bx * (0.5f - q2q2 - q3q3) + _4bz * (q1q3 - q0q2) - mx) + (_4bx * q2 + _4bz * q0) * (_4bx * (q2q3 - q0q1) + _4bz * (q0q2 + q1q3) - my) + (_4bx * q3 - _8bz * q1) * (_4bx * (q0q1 + q2q3) + _4bz * (0.5f - q1q1 - q2q2) - mz);
    s2 = -_2q0 * (2.0f * (q1q3 - q0q2) - ax) + _2q3 * (2.0f * (q0q1 + q2q3) - ay) - 4.0f * q2 * (2.0f * (0.5f - q1q1 - q2q2) - az) + (-_8bx * q2 - _4bz * q0) * (_4bx * (q2q3 - q0q1) + _4bz * (q0q2 + q1q3) - my) + (_4bx * q1 + _4bz * q3) * (_4bx * (q0q1 + q2q3) + _4bz * (0.5f - q1q1 - q2q2) - mz) + (_4bx * q0 - _8bz * q2) * (_4bx * (0.5f - q1q1 - q3q3) + _4bz * (q1q3 - q0q2) - mx);
    s3 = _2q1 * (2.0f * (q1q3 - q0q2) - ax) + _2q2 * (2.0f * (q0q1 + q2q3) - ay) + (-_8bx * q3 + _4bz * q1) * (_4bx * (q2q3 - q0q1) + _4bz * (q0q2 + q1q3) - my) + (-_4bx * q2 + _4bz * q0) * (_4bx * (q0q1 + q2q3) + _4bz * (0.5f - q1q1 - q2q2) - mz) + _4bx * q1 * (_4bx * (0.5f - q2q2 - q3q3) + _4bz * (q1q3 - q0q2) - mx);

    // 归一化梯度
    recipNorm = sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
    if (recipNorm == 0.0f) return;
    recipNorm = 1.0f / recipNorm;
    s0 *= recipNorm;
    s1 *= recipNorm;
    s2 *= recipNorm;
    s3 *= recipNorm;

    // 计算四元数的变化率
    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz) - beta * s0;
    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy) - beta * s1;
    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx) - beta * s2;
    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx) - beta * s3;

    // 更新四元数
    q0 += qDot1 * invSampleFreq;
    q1 += qDot2 * invSampleFreq;
    q2 += qDot3 * invSampleFreq;
    q3 += qDot4 * invSampleFreq;

    // 归一化四元数
    recipNorm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    if (recipNorm == 0.0f) return;
    recipNorm = 1.0f / recipNorm;
    *qw = q0 * recipNorm;
    *qx = q1 * recipNorm;
    *qy = q2 * recipNorm;
    *qz = q3 * recipNorm;
}