[项目]基于FreeRTOS的STM32四轴飞行器: 十二.角速度加速度滤波

一.滤波介绍

模拟信号滤波：
最常用的滤波方法可以在信号和地之间并联一个电容，因为电容通交隔直，信号突变会给电容充电，电容两端电压不会突变，电容越大越明显。
电容滤电压的毛刺，电感滤电流的毛刺。电感串联进电路中，电流出现毛刺，因为通过电感两端的电流不能突变，实现对电流的滤波。
数字信号滤波：
使用算法进行滤波。
1.均值滤波（滑动窗口滤波）：每来一个值使用前四个值进行平均使用平均值。
2.中值滤波：将数据排序，取数据奇数部分的中值代替取到的数值。
3.一阶低通滤波：结果 = 系数 * 上次的值 + （1 - 系数）X 这次的值
4.卡尔曼滤波：核心5个公式。
5.互补滤波：
加速度对时间积分 速度：响应迅速，结果容易受到外界影响。
加速度对时间微分 速度：结果不容受影响，响应不及时。

二.对角速度进行一阶低通滤波

编写一阶低通滤波函数：

#define ALPHA 0.8
/* 一阶低通率波 */
int16_t Com_Filter_LowPass(int16_t newData, int16_t lastData)
{
    return ALPHA * lastData + (1 - ALPHA) * newData;
}

使用一阶低通滤波：
记住读取数据时使用临界区。

    /* 1. 读取原始数据 */
    taskENTER_CRITICAL();
    Inf_MPU6050_ReadGyroAccelCalibrated(gyroAccel);
    taskEXIT_CRITICAL();

    /* 2. 对角速度做一阶低通低通滤波 */
    static int16_t lastDatas[3] = {0};
    gyroAccel->gyro.gyroX = Com_Filter_LowPass(gyroAccel->gyro.gyroX, lastDatas[0]);
    gyroAccel->gyro.gyroY = Com_Filter_LowPass(gyroAccel->gyro.gyroY, lastDatas[1]);
    gyroAccel->gyro.gyroZ = Com_Filter_LowPass(gyroAccel->gyro.gyroZ, lastDatas[2]);
    lastDatas[0] = gyroAccel->gyro.gyroX;
    lastDatas[1] = gyroAccel->gyro.gyroY;
    lastDatas[2] = gyroAccel->gyro.gyroZ;

一阶低通滤波效果演示：
使用虚拟数字示波器观察滤波前后数据波形，滤波效果明显。
CH1（红）：滤波前
CH2（黄）：滤波后

三.对加速度进行卡尔曼滤波

直接移植下面的卡尔曼滤波代码：
.c:
三个结构体分别是XYZ三轴的参数。
卡尔曼滤波函数参数分别为结构体，对谁做滤波。
返回值为滤波后结果。

/* 卡尔曼滤波参数 */
KalmanFilter_Struct kfs[3] = {
    {0.02, 0, 0, 0, 0.001, 0.543},
    {0.02, 0, 0, 0, 0.001, 0.543},
    {0.02, 0, 0, 0, 0.001, 0.543}};
double Common_Filter_KalmanFilter(KalmanFilter_Struct *kf, double input)
{
    kf->Now_P = kf->LastP + kf->Q;
    kf->Kg    = kf->Now_P / (kf->Now_P + kf->R);
    kf->out   = kf->out + kf->Kg * (input - kf->out);
    kf->LastP = (1 - kf->Kg) * kf->Now_P;
    return kf->out;
}

.h:

/* 卡尔曼滤波器结构体 */
typedef struct
{
    float LastP;   // 上一时刻的状态方差（或协方差）
    float Now_P;   // 当前时刻的状态方差（或协方差）
    float out;     // 滤波器的输出值，即估计的状态
    float Kg;      // 卡尔曼增益，用于调节预测值和测量值之间的权重
    float Q;       // 过程噪声的方差，反映系统模型的不确定性
    float R;       // 测量噪声的方差，反映测量过程的不确定性
} KalmanFilter_Struct;

extern KalmanFilter_Struct kfs[3];

使用卡尔曼滤波：
传入结构体和滤波的对象。

    /* 3. 对加速度做卡尔曼滤波 */
    // OutData[0] = gyroAccel->accel.accelX ;
    gyroAccel->accel.accelX = Common_Filter_KalmanFilter(&kfs[0], gyroAccel->accel.accelX);
    gyroAccel->accel.accelY = Common_Filter_KalmanFilter(&kfs[1], gyroAccel->accel.accelY);
    gyroAccel->accel.accelZ = Common_Filter_KalmanFilter(&kfs[2], gyroAccel->accel.accelZ);
    // OutData[1] = gyroAccel->accel.accelX ;
    // OutPut_Data();

观察滤波前后效果：
效果良好