[算法]飞控中的低通滤波算法

[算法]飞控中的低通滤波算法

---

以前看匿名飞控的源码的时候经常看见类似下面的算法

thr_lpf+=(1 / (1 + 1/(2.0f * 3.14f * T )))*(height_thr - thr_lpf)

看这个变量名这应该是对油门进行低通滤波，于是我就注释上低通滤波

整理一下上式可以得到
$$Out_1=(\frac{1}{1+(\frac{1}{2 \pi * tf})})*(In_0-Out_0)$$
$$Out_1=(1-A)Out_0+A*In_0$$
$$A=(\frac{1}{1+(\frac{1}{2 \pi * tf})})$$

但是我不知道凭什么这个算法可以低通滤波呀？

电路中的滤波

最早听见低通滤波是在上电路课的时候，这个算法和电路里的滤波电路有什么关系吗？

时域

$$I_c=\frac{d_q}{d_t}=(\frac{ d_{(C*U_o)} }{d_t})=C*\frac{ d_{U_o} }{d_t}$$
$$U_i=RC*(\frac{ d_{U_o} }{d_t})+U_o$$
$$\tau=RC$$
$$U_o(t)=U_i(1-e^{-\frac{t}{RC}})$$

频域

$$H(jw)=(\frac{U_o}{U_i})=(\frac{\frac{1}{jwc}}{ R+\frac{1}{jwc} })=\frac{ 1 }{jwRC}$$
截止频率
$$f_c=\frac{1}{2\pi RC}$$
$$A(f)=\frac{1}{2\pi fRCj+1}$$
$$\frac{U_o}{U_i}=\frac{1}{RCs+1},(s=jw,w=2\pi f)$$

z变换

$$s=\frac{1-z^{-1}} {T}$$
$$\frac{Y(z)}{X(z)}=\frac{1}{RC{\frac{1-z^{-1}}{T}+1}}=\frac{T} {RC(1-Z^{-1})+T}$$
$$Y(n)=\frac{T}{RC+T}X(n)+\frac{RC}{RC+T}Y(n-1)$$
$$Y(n)=A*X(n)+(1-A)Y(n-1)$$
$$A=\frac{T}{RC+T}$$
这个式子已经跟程序里的非常像了，现在就差系数的问题了

为什么
$$A=(\frac{1}{1+(\frac{1}{2 \pi * tf})})$$
因为
$$A=\frac{T}{RC+T}$$
$$f=\frac{1}{2\pi RC}$$
所以
$$RC=\frac{1}{2\pi f}$$
带入
$$A=\frac{T}{\frac{1}{2\pi f}+T}=(\frac{1}{1+(\frac{1}{2 \pi * tf})})$$

所以程序里的公式其实就是一阶RC滤波器的公式

参考资料
https://blog.csdn.net/qq_2733...