整套可直接移植 C 代码

适用：STM32 / ESP32 / 普通32位MCU
IMU：6轴（加速度计+陀螺仪，如MPU6050、ICM42688）
流程：原始数据读取 → 零偏校准 → 一阶低通预处理 → Mahony AHRS融合输出四元数 → 四元数转欧拉角

坐标系约定（行业通用）
X：右，Y：前，Z：下
roll(绕X)横滚，pitch(绕Y)俯仰，yaw(绕Z)偏航
⚠️6轴IMU yaw会漂移，想要稳定航向必须加磁力计扩展9轴

1. 头文件 imu_ahrs.h

#ifndef __IMU_AHRS_H
#define __IMU_AHRS_H

#include <stdint.h>
#include <math.h>

#define PI          3.141592653589793f
#define DEG2RAD     (PI / 180.0f)
#define RAD2DEG     (180.0f / PI)

// IMU采样周期【根据你的实际采样频率修改！】
// 例：200Hz采样 → dt = 0.005f
#define IMU_DT      0.005f

// Mahony参数
#define Kp          2.0f    // 比例增益，调大修正更快，噪声变大
#define Ki          0.005f  // 积分增益，抑制稳态漂移

// IMU数据结构体
typedef struct
{
    // 原始ADC输出
    int16_t ax_raw, ay_raw, az_raw;
    int16_t gx_raw, gy_raw, gz_raw;

    // 物理量
    float ax, ay, az;       // g
    float gx, gy, gz;       // rad/s

    // 零偏（静止校准得到）
    float gx_bias, gy_bias, gz_bias;

    // 姿态四元数
    float q0, q1, q2, q3;

    // 欧拉角 单位：度
    float roll, pitch, yaw;

}IMU_t;

extern IMU_t imu;

// 一阶低通滤波结构体
typedef struct
{
    float alpha;
    float out;
}LPF_t;

// 函数声明
void IMU_Init(void);
void IMU_Gyro_Calibrate(uint16_t sample_cnt);
void IMU_RawDataProcess(void);
float LPF_Update(LPF_t *lpf, float raw);
void Mahony_AHRS_Update(float ax, float ay, float az, float gx, float gy, float gz);
void Quaternion_To_Euler(void);

#endif

2. 源文件 imu_ahrs.c

#include "imu_ahrs.h"

IMU_t imu;

// 低通滤波器实例，加速度使用
static LPF_t lpf_ax = {.alpha = 0.2f, .out = 0};
static LPF_t lpf_ay = {.alpha = 0.2f, .out = 0};
static LPF_t lpf_az = {.alpha = 0.2f, .out = 0};

// Mahony内部积分误差
static float integral_fb_x = 0, integral_fb_y = 0, integral_fb_z = 0;

/**
 * @brief 一阶指数低通滤波
 * @param lpf 滤波器实例
 * @param raw 原始输入
 * @retval 滤波后数值
 */
float LPF_Update(LPF_t *lpf, float raw)
{
    lpf->out = lpf->alpha * raw + (1.0f - lpf->alpha) * lpf->out;
    return lpf->out;
}

/**
 * @brief IMU初始化，四元数初始姿态水平
 */
void IMU_Init(void)
{
    imu.q0 = 1.0f;
    imu.q1 = 0.0f;
    imu.q2 = 0.0f;
    imu.q3 = 0.0f;

    imu.gx_bias = 0;
    imu.gy_bias = 0;
    imu.gz_bias = 0;
}

/**
 * @brief 陀螺仪零偏校准
 * @param sample_cnt 采样点数，建议500~1000
 * 【使用条件：IMU水平静止，不要晃动！】
 */
void IMU_Gyro_Calibrate(uint16_t sample_cnt)
{
    int32_t sum_gx = 0, sum_gy = 0, sum_gz = 0;
    uint16_t i;

    for(i = 0; i < sample_cnt; i++)
    {
        // ==========这里需要你自行填充：读取IMU原始gx_raw,gy_raw,gz_raw=========
        // Read_IMU_Raw(&imu.ax_raw, &imu.ay_raw, &imu.az_raw,
        //              &imu.gx_raw, &imu.gy_raw, &imu.gz_raw);

        sum_gx += imu.gx_raw;
        sum_gy += imu.gy_raw;
        sum_gz += imu.gz_raw;
    }

    imu.gx_bias = (float)sum_gx / sample_cnt;
    imu.gy_bias = (float)sum_gy / sample_cnt;
    imu.gz_bias = (float)sum_gz / sample_cnt;
}

/**
 * @brief 原始数据转换、去零偏、低通预处理
 * 【重要】根据你的IMU量程修改系数！示例：MPU6050
 * accel ±2g  scale = 2.0f / 32768.0f
 * gyro ±250°/s scale = 250.0f / 32768.0f
 */
void IMU_RawDataProcess(void)
{
    // 加速度原始值 -> g
    float accel_scale = 2.0f / 32768.0f;
    imu.ax = imu.ax_raw * accel_scale;
    imu.ay = imu.ay_raw * accel_scale;
    imu.az = imu.az_raw * accel_scale;

    // 加速度低通滤波
    imu.ax = LPF_Update(&lpf_ax, imu.ax);
    imu.ay = LPF_Update(&lpf_ay, imu.ay);
    imu.az = LPF_Update(&lpf_az, imu.az);

    // 陀螺仪原始值 -> °/s，减去零偏，再转为 rad/s
    float gyro_scale = 250.0f / 32768.0f;
    float gx_dps = (imu.gx_raw - imu.gx_bias) * gyro_scale;
    float gy_dps = (imu.gy_raw - imu.gy_bias) * gyro_scale;
    float gz_dps = (imu.gz_raw - imu.gz_bias) * gyro_scale;

    imu.gx = gx_dps * DEG2RAD;
    imu.gy = gy_dps * DEG2RAD;
    imu.gz = gz_dps * DEG2RAD;
}

/**
 * @brief Mahony AHRS 6轴融合算法
 * ax,ay,az 单位g；gx,gy,gz 单位rad/s
 */
void Mahony_AHRS_Update(float ax, float ay, float az, float gx, float gy, float gz)
{
    float q0 = imu.q0;
    float q1 = imu.q1;
    float q2 = imu.q2;
    float q3 = imu.q3;

    float norm;
    float vx, vy, vz;
    float ex, ey, ez;

    // 归一化加速度计
    norm = sqrtf(ax*ax + ay*ay + az*az);
    if(norm == 0.0f) return;
    ax /= norm;
    ay /= norm;
    az /= norm;

    // 预估重力方向（由当前四元数推算）
    vx = 2.0f * (q1*q3 - q0*q2);
    vy = 2.0f * (q0*q1 + q2*q3);
    vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

    // 误差 = 叉乘（测量重力 叉乘 预估重力）
    ex = ay * vz - az * vy;
    ey = az * vx - ax * vz;
    ez = ax * vy - ay * vx;

    // 积分反馈
    integral_fb_x += ex * Ki * IMU_DT;
    integral_fb_y += ey * Ki * IMU_DT;
    integral_fb_z += ez * Ki * IMU_DT;

    // 修正角速度
    gx += Kp * ex + integral_fb_x;
    gy += Kp * ey + integral_fb_y;
    gz += Kp * ez + integral_fb_z;

    // 四元数微分更新
    float q0_dot = 0.5f * (-q1*gx - q2*gy - q3*gz);
    float q1_dot = 0.5f * ( q0*gx + q2*gz - q3*gy);
    float q2_dot = 0.5f * ( q0*gy - q1*gz + q3*gx);
    float q3_dot = 0.5f * ( q0*gz + q1*gy - q2*gx);

    q0 += q0_dot * IMU_DT;
    q1 += q1_dot * IMU_DT;
    q2 += q2_dot * IMU_DT;
    q3 += q3_dot * IMU_DT;

    // 四元数归一化（必须！防止发散）
    norm = sqrtf(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    imu.q0 = q0 / norm;
    imu.q1 = q1 / norm;
    imu.q2 = q2 / norm;
    imu.q3 = q3 / norm;
}

/**
 * @brief 四元数转欧拉角 roll pitch yaw（角度）
 */
void Quaternion_To_Euler(void)
{
    float q0 = imu.q0;
    float q1 = imu.q1;
    float q2 = imu.q2;
    float q3 = imu.q3;

    // Roll X轴
    imu.roll = atan2f(2.0f*(q0*q1 + q2*q3), q0*q0 - q1*q1 - q2*q2 + q3*q3) * RAD2DEG;
    // Pitch Y轴
    imu.pitch = -asinf(2.0f*(q1*q3 - q0*q2)) * RAD2DEG;
    // Yaw Z轴 【6轴会漂移】
    imu.yaw = atan2f(2.0f*(q0*q3 + q1*q2), q0*q0 + q1*q1 - q2*q2 - q3*q3) * RAD2DEG;
}

3. 主循环调用示例（main.c / 定时器中断）

⭐强烈建议放在固定周期定时器中断执行，保证dt稳定！

#include "imu_ahrs.h"

// 外部函数，需要你自己实现I2C/SPI读取IMU原始数据
extern void Read_IMU_Raw(int16_t *ax, int16_t *ay, int16_t *az,
                         int16_t *gx, int16_t *gy, int16_t *gz);

int main(void)
{
    // 硬件初始化：I2C/SPI、IMU寄存器配置省略
    IMU_Init();

    // 【校准步骤！上电后保持IMU静止水平运行一次】
    IMU_Gyro_Calibrate(800);

    while(1)
    {
        // 1.读取原始数据
        Read_IMU_Raw(&imu.ax_raw, &imu.ay_raw, &imu.az_raw,
                     &imu.gx_raw, &imu.gy_raw, &imu.gz_raw);

        // 2.原始数据处理：去零偏+单位转换+低通滤波
        IMU_RawDataProcess();

        // 3.Mahony融合得到四元数
        Mahony_AHRS_Update(imu.ax, imu.ay, imu.az, imu.gx, imu.gy, imu.gz);

        // 4.四元数转欧拉角
        Quaternion_To_Euler();

        // 此时可以使用 imu.roll imu.pitch imu.yaw
        // printf("roll:%.2f pitch:%.2f yaw:%.2f\r\n",imu.roll,imu.pitch,imu.yaw);

        // 严格保证采样周期 和 IMU_DT匹配
        // HAL_Delay(5); // dt=0.005 200Hz
    }
}

重点修改说明（必看！否则数据错乱）

1. IMU量程系数
   代码内是MPU6050 ±2g / ±250dps
   如果你用ICM42688、BMI160，修改 accel_scale、gyro_scale
2. Read_IMU_Raw() 函数
   需要你自行实现I2C/SPI读取寄存器原始int16，这部分和硬件驱动相关
3. IMU_DT
   采样频率200Hz → 0.005；100Hz →0.01；必须和实际一致
4. 坐标轴方向
   如果角度反向、左右颠倒：修改原始数据正负号、调换ax/ay顺序

参数调试指南

1. Kp：越大，加速度修正越强，运动时抖动变大；静止姿态收敛更快
   推荐区间：1.0 ~ 5.0
2. Ki：积分项，抑制长时间静态漂移；太大容易震荡
   推荐：0 ~ 0.01
3. 低通alpha：加速度噪声大 → 减小到0.1；想要响应快 →提高到0.3

常见问题

1. 角度震荡：降低Kp，或者减小低通alpha
2. 静止缓慢漂移：适度增大Ki
3. 运动时角度乱晃：Kp不要太大，剧烈运动加速度计不可信
4. yaw持续漂移：6轴物理限制，只能加磁力计升级9轴

如果你告诉我：
MCU型号、IMU型号、通信方式（I2C/SPI）、采样频率
我可以帮你补上完整IMU读取驱动，直接编译运行。

另外如果你想要 Madgwick 版本代码，我也可以一并给出。