[算法]飛控中的低通濾波算法

[算法]飛控中的低通濾波算法

---

之前看匿名飛控的源碼的時候常常看見相似下面的算法

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...