互补滤波器

互补滤波器

从 RC 电路 到 数字滤波器 。

参考：wikiPedia

by luoshi006

欢迎留言交流，或luoshi006@gmail.com

介绍

Created with Raphaël 2.1.0IMU互补滤波结构图：加速度计加速度计陀螺仪陀螺仪低通滤波器数值积分高通滤波器求和输出结果

第一次用这么高级的UML序列图，不太了解，计算到 输出结果 就结束了。请海涵……

原理

低通滤波器

传递函数



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

YU=11+RC⋅S\frac{Y}{U}=\frac{1}{1+RC\cdot S}

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

######################### 该部分内容在Z变换时  ####################################
######################### 未考虑零阶保持器等   ####################################
######################### 得到的结果形似神不似  ###################################
######################### 故，暂时只做参考。    ###################################
#### **Z变换**

> Z变换公式：$Z=e^{st}$
> 可得：$S=\frac{lnZ}{T}$
> 按泰勒一阶展开：$s=\frac{z-1}{T}$

对复域的传函进行Z变换（$s=\frac{z-1}{T}$）：

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

#### **Z反变换**

$$T\cdot U(k)=T \cdot Y(k)+RC\cdot  Y(k+1)-RC\cdot Y(k)$$

化简：

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

将传函转换为微分形式：

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

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

y(k)=RCT+RCy(k−1)+TT+RCx(k)y(k)=\frac{RC}{T+RC}y(k-1)+\frac{T}{T+RC}x(k)

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

C代码

float acc_lpf_factor = 4.0f;//低通滤波因子
static sensor_t accLPF; // 低通滤波后的加速度计值

for (uint8_t axis = 0; axis < 3; axis++)
{//对三轴数据分别滤波
accLPF.data[axis] = accLPF.data[axis] * (1.0f - (1.0f / acc_lpf_factor)) + (float)mpu6500->acc.data[axis] * (1.0f / acc_lpf_factor); // Apply low pass filter
}

高通滤波器

依然使用RC电路为模型。

传递函数为：

G(s)=11+1RC⋅SG(s)=\frac {1}{1+\frac{1}{RC\cdot S}}

=RC⋅SRC⋅S+1=\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⋅dy(t)dt+y(t)=RC⋅dx(t)dtRC\cdot \frac{dy(t)}{dt}+y(t)=RC \cdot \frac{dx(t)}{dt}

转换为差分形式：

y(k)=RCRC+Ty(k−1)+RCRC+T(x(k)−x(k−1))y(k)=\frac{RC}{RC+T}y(k-1)+\frac{RC}{RC+T}(x(k)-x(k-1))

该部分代码未找到实际使用示例，故，在此不列出 C 代码。

互补滤波器

综上，可知：

低通滤波器：

y(k)=RCRC+Ty(k−1)+TRC+Tx(k)y(k)=\frac{RC}{RC+T}y(k-1)+\frac{T}{RC+T}x(k)

高通滤波器：

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

故，互补滤波器：

y(k)=RCRC+T[y(k−1)+Δxgyro(k)]+TT+RCxacc(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 滤波，限于篇幅，另外介绍。

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

互补滤波器
介绍

原理
低通滤波器

高通滤波器

互补滤波器

the end