互补滤波器

互补滤波器

从 RC 电路 到 数字滤波器 。

参考：wikiPedia

by luoshi006

原理

低通滤波器

一阶低通滤波器

传递函数

常见的 RC 电路构成的一阶低通滤波器的输入(U) 输出(Y)关系如下：

$$\frac{Y}{U}=\frac{1}{1+RC\cdot S}$$

其中，滤波器的截止频率为：$w_c=\frac{1}{RC}$。

将传函转换为微分形式：

$$y(t)+RC \frac{dy(t)}{dt}=x(t)$$

$\frac{dy(t)}{dt}=差分=\frac{y(k)-y(k-1)}{\Delta t}$，代入得到差分形式：

$$y(k)=\frac{RC}{\Delta t+RC}y(k-1)+\frac{\Delta t}{\Delta t+RC}x(k)$$

由近似公式：

$$\frac{1}{1+\Delta t/RC}\dot= 1-\frac{\Delta t}{RC}$$

可得：

$$\color{blue}{y(k) = (1-\frac{\Delta t}{RC})y(k-1) + \frac{\Delta t}{RC} x(k)}$$

即，一阶低通滤波的差分形式。

---

二阶低通滤波

过程略；

$$y(k)=\frac{2(1+\sigma\Delta t)}{1+2\sigma \Delta t+ \omega^2_0 \Delta t^2} y(k-1) - \frac{1}{1+2 \sigma \Delta t+\omega^2_0 \Delta t^2} y(k-2) + \frac{\omega^2_0 \Delta t^2}{1+2 \sigma \Delta t + \omega^2_0 \Delta t^2}x(k)$$

其中，$\sigma = \frac{R}{2L} , \omega^2_0 = \frac{1}{LC}$。

---

高通滤波器

依然使用RC电路为模型。
传递函数为：

$$G(s)=\frac {1}{1+\frac{1}{RC\cdot S}}$$

$$=\frac {RC\cdot S}{RC \cdot S+1}$$

******************* 内容仅作参考 *******************************
由 $s=\frac {Z-1}{T}$变换：

$$U\cdot RC \cdot Z - U\cdot RC=Y\cdot RC \cdot Z- Y\cdot RC+Y\cdot T$$

Z反变换：

$$Y(k+1)=U(k+1)-U(k)+(1-\frac{T}{RC}) Y(k)$$

将传函转化为微分形式：

$$RC\cdot \frac{dy(t)}{dt}+y(t)=RC \cdot \frac{dx(t)}{dt}$$

转换为差分形式：

$$y(k)=\frac{RC}{RC+T}y(k-1)+\frac{RC}{RC+T}(x(k)-x(k-1))$$

---

互补滤波器

综上，可知：
低通滤波器：

$$y(k)=\frac{RC}{RC+T}y(k-1)+\frac{T}{RC+T}x(k)$$

高通滤波器：

$$y(k)=\frac{RC}{RC+T}[y(k-1)+\Delta x(k)]$$

故，互补滤波器：

$$y(k)=\frac{RC}{RC+T}[y(k-1)+\Delta x_{gyro}(k)]+\frac{T}{T+RC}x_{acc}(k)$$

angle = (factor) * (angle + gyro * dt ) + (1 - factor) * (x_acc);
其中，factor 为互补滤波因子，定义域：( 0 , 1 )。

---

the end

本文简单介绍了一阶互补滤波的理论和实现，以期望对刚开始接触数字滤波的朋友有所帮助。

互补滤波使用较多的 mahony 滤波，限于篇幅，另外介绍。

如果在文中，发现错误或不妥当的地方，请直接留言，或 邮箱 交流。不胜感激~

---