基于stm32的一阶互补滤波算法

一阶互补滤波算法是一种常用的姿态解算算法，可以用于测量加速度计和陀螺仪的数据，实现姿态角的估计。下面是基于STM32的一阶互补滤波算法的示例代码：

float Accel_angle, Gyro_rate, Com_angle;
float Kp = 0.5; // 加速度计权重
float Ki = 0.5; // 陀螺仪权重
float dt = 0.01; // 采样周期

void ComplementaryFilter(float accel, float gyro)
{
    Accel_angle = atan2(accel, sqrt(pow(accel, 2) + pow(gyro, 2))); // 加速度计测量的角度
    Gyro_rate = Gyro_rate + gyro * dt; // 陀螺仪积分得到的角速度
    Com_angle = Kp * Accel_angle + Ki * Gyro_rate; // 互补滤波得到的角度
}

其中，Accel_angle为加速度计测量的角度，Gyro_rate为陀螺仪积分得到的角速度，Com_angle为互补滤波得到的角度。Kp和Ki分别为加速度计和陀螺仪的权重系数，dt为采样周期。该算法的基本思想是将加速度计测量的角度和陀螺仪积分得到的角速度进行加权平均，其中加速度计占比较大，陀螺仪占比较小，通过调整Kp和Ki的值可以得到更好的滤波效果。