嵌入式C语言使用低通滤波、高通滤波、互补滤波算法

一、一阶低通滤波算法

  低通滤波（Low Pass Filter）用于从一个信号中去除高于某个频率的成分。它的基本原理是，信号中高于某个频率的成分在信号传输或接收过程中会发生衰减，而低于该频率的成分则不受影响。因此，通过将信号通过一个低通滤波器，可以去除高频噪声，保留信号中的低频成分。

  一阶低通滤波器是低通滤波的一阶离散形式，用于滤除输入信号中的高频分量，只保留低频分量。它通过减弱高频部分的幅度，从而实现对信号的平滑处理。一阶低通滤波器的基本原理涉及限制信号的变化速率，对快速变化的信号进行衰减，而对缓慢变化的信号保留。

  一阶低通滤波的形式与一阶滞后滤波完全相同。倒不如说一阶滞后滤波其实就是一阶低通滤波，只不过当该滤波器用于不同的作用时，我们将其冠以了不同的称呼。

1.1 公式

$$Y_k=A{X}_k+(1-A)Y_{k-1}$$
其中
$$A=\frac{1}{1+\frac{1}{2\pi f_{c}T}}$$

$Y_k$ ：输出
$Y_{k-1}$ ：上一个输出
$X_k$ ：输入
$f_c$ ：截至频率
$T$ ：采样周期

  在这个方程中，$A$ 越小，时间常数越大，低通滤波器的截止频率就越低，对高频部分的抑制效果就越强。
一阶低通滤波器常用于需要平滑信号或去除高频噪声的应用场景。它们在信号处理、通信系统、控制系统等领域都有广泛的应用。

1.2 C代码

#include <stdio.h>
 
// 定义一阶低通滤波器结构体
typedef struct {
    float alpha;           // 时间常数
    float previous_output; // 上一时刻的输出
} LowPassFilter;
 
// 初始化滤波器
void initializeFilter(LowPassFilter* filter, float alpha) {
    filter->alpha = alpha;
    filter->previous_output = 0.0;
}
 
// 一阶低通滤波函数
float filterValue(LowPassFilter* filter, float input) {
    // 计算输出
    float output = (1.0 - filter->alpha) * filter->previous_output + filter->alpha * input;
 
    // 更新上一次的输出
    filter->previous_output = output;
 
    return output;
}
 
int main() {
    // 初始化滤波器，设置时间常数为0.2
    float alpha = 0.2;
    LowPassFilter myFilter;
    initializeFilter(&myFilter, alpha);
 
    // 使用示例
    float inputValues[] = {10.0, 15.0, 20.0, 18.0, 22.0, 25.0, 17.0};
    int numValues = sizeof(inputValues) / sizeof(inputValues[0]);
 
    printf("Input Values:\tFiltered Values:\n");
 
    for (int i = 0; i < numValues; ++i) {
        float filteredValue = filterValue(&myFilter, inputValues[i]);
        printf("%.2f\t\t%.2f\n", inputValues[i], filteredValue);
    }
 
    return 0;
}

  MedianAverageFilter 结构体用于存储滤波器的状态信息，包括窗口和平均滤波的权重系数。initializeFilter 函数用于初始化滤波器，而 filterValue 函数实现了中位值平均滤波的操作。在 main 函数中，我们创建了一个 MedianAverageFilter 实例，并对一系列输入值进行滤波处理，输出滤波后的值。

二、一阶高通滤波算法

  高通滤波（High Pass Filter）可以滤除信号中的低频部分，保留高频部分。高通滤波器的应用非常广泛，例如在音频处理中可以用来去除低频噪声、在图像处理中可以用来增强图像的边缘等。
  高通滤波算法的基本思想是：将信号分解成高频和低频两部分，去掉低频部分，只保留高频部分。高通滤波的实现可以通过频域方法和时域方法两种方式实现。
  频域方法是将信号转换到频域进行处理，常用的有傅里叶变换和小波变换等。通过滤波器在频域中滤除低频成分，然后再将信号转换回时域。
  时域方法则是通过差分等方式，直接在时域中滤除低频部分。
  一阶高通滤波器是高通滤波的一阶差分形式，用于滤除输入信号中的低频分量，同时保留高频分量。高通滤波器的作用是弱化或消除信号中的低频成分，从而突出高频变化或忽略缓慢变化的部分。一阶高通滤波器的设计原理涉及对低频分量进行衰减，保留高频部分。

2.1 公式

一阶高通滤波器的差分方程一般表示为：
$$Y_k=A{Y}_{k-1}+A(X_k-X_{k-1})$$
其中
$$A=\frac{1}{1+\frac{1}{2\pi f_{c}T}}$$
$Y_k$ ：输出
$Y_{k-1}$ ：上一个输出
$X_k$ ：输入
$X_{k-1}$ ：上一个输入
$f_c$ ：截至频率
$T$ ：采样周期

  在这个方程中，$A$ 越小，时间常数越大，高通滤波器的截止频率就越低，对低频部分的抑制效果就越弱。
  一阶高通滤波器通常应用于需要突出信号中快速变化或高频成分的应用场景。在图像处理、音频处理、传感器信号处理等领域，高通滤波器被广泛用于去除低频噪声或趋势成分。

2.2 C代码

#include <stdio.h>
 
// 定义一阶高通滤波器结构体
typedef struct {
    float alpha;           // 时间常数
    float previous_output; // 上一时刻的输出
} HighPassFilter;
 
// 初始化滤波器
void initializeFilter(HighPassFilter* filter, float alpha) {
    filter->alpha = alpha;
    filter->previous_output = 0.0;
}
 
// 一阶高通滤波函数
float filterValue(HighPassFilter* filter, float input) {
    // 计算输出
    float output = filter->alpha * (input - filter->previous_output) + filter->previous_output;
 
    // 更新上一次的输出
    filter->previous_output = output;
 
    return output;
}
 
int main() {
    // 初始化滤波器，设置时间常数为0.1
    float alpha = 0.1;
    HighPassFilter myFilter;
    initializeFilter(&myFilter, alpha);
 
    // 使用示例
    float inputValues[] = {10.0, 15.0, 20.0, 18.0, 22.0, 25.0, 17.0};
    int numValues = sizeof(inputValues) / sizeof(inputValues[0]);
 
    printf("Input Values:\tFiltered Values:\n");
 
    for (int i = 0; i < numValues; ++i) {
        float filteredValue = filterValue(&myFilter, inputValues[i]);
        printf("%.2f\t\t%.2f\n", inputValues[i], filteredValue);
    }
 
    return 0;
}

   HighPassFilter 结构体用于存储滤波器的状态信息，包括时间常数和上一次的输出。initializeFilter 函数用于初始化滤波器，而 filterValue 函数实现了一阶高通滤波的操作。在 main 函数中，我们创建了一个 HighPassFilter 实例，并对一系列输入值进行滤波处理，输出滤波后的值。

三、互补滤波算法

   你上网看了无数的互补滤波解读教程，始终不理解，为什么算法原理和代码可以没有任何关系？，那这个算法是怎么写成代码的呢？
我直接给出结论吧，造成这样的原因是因为：
   网上大部分互补滤波原理介绍的是传统的 线性互补滤波(Classical Complementary Filters)， 而Mahony用来算解姿态的滤波是经过改进的 非线性互补滤波，
   非线性互补滤波里有两种形式：直接互补滤波(Direct complementary filter)和无源互补滤波(Passive complementary filter)， 你在网上看到的开源代码都是基于无源互补滤波器的显式误差版本-显式互补滤波器(Explicit complementary filter).
你拿着两个完全不一样的东西，那肯定对应不上呀。

3.1 前言

   一般的互补公式用在六轴传感器的数据融合，其他的行业和领域现在还没有涉及到，这里只说六轴传感器的融合。以后遇到融合的话，可以直接用。

3.2 公式

$$y_n=K{\theta }_n+(1-K)[y_{n-1}+\Delta t{\Omega }_n]$$
$y_n$：输出角度
${\theta }_n$：加速度的角度
$\Delta t$：采样周期
${\Omega }_n$：陀螺仪角度
$y_{n-1}$：上一次输出角度

可以看出，互补滤波就是通过加速度计获取的角度对陀螺仪积分的角度进行校准，从而积分的角度逐步跟踪到加速度传感器所得到的角度。K1 与1-K1 是对这两个角度取不同的权重，可以表示我们对不同数据的信任程度。

3.2 C代码

/****************************** BEFIN ********************************
**
**@Name       : Complementary_Filter_x
**@Brief      : 一阶互补滤波   
**@Param angle_m: 加速度算出的角度 
**		gyro_m: 陀螺仪的角速度
**@Return     : None
**@Author     : @mayuxin
**@Data	      : 2022-06-04
******************************** END *********************************/
float Complementary_Filter_x(float angle_m, float gyro_m)
{
	 static float angle;
	 float K1 =0.02; 
   angle = K1 * angle_m+ (1-K1) * (angle + gyro_m * dt);
	 return angle;
}

---

文章是自己总结而记录，有些知识点没说明白的，请各位看官多多提意见，多多交流，欢迎大家留言
如果技术交流可以加以下群，方便沟通