飞控学习笔记（一）

（参考文章的地址：https://zhuanlan.zhihu.com/p/60896985）

加入滤波器必然会造成延时，不要为了追求滤波效果，对系统造成太大的延迟，通常不要超过一个周期最好。

1.一阶低通滤波器的设计：

一般飞行器陀螺仪的低通滤波的经验值是30Hz，然后计算滤波系数。公式如下：

python 低通滤波 scipy lowpass 低通滤波编程_陀螺仪

其中,T表示采样周期，fc表示截止频率，当T = 0.005,fc = 30Hz时，计算A的值为0.4811

   给出的示例代码（matlab）,是对飞控陀螺仪的单轴数据进行50Hz的低通滤波；

dat = data2';
dat4 = zeros(n,1);
k = Ts1 / (Ts1 + (1 / (2 * pi * 50)));
for i = 2:n
   dat4(i) = dat4(i-1) * (1 - k) + dat(i) * k;
end

2.二阶IIR滤波器的设计：

%2阶IIR
[b a]=butter(2,0.5); %50hz  50 / (200 / 2)
dat2 = filter(b,a,dat);

butter函数是求Butterworth数字滤波器的系数，在求出系数后对信号进行滤波时用filter函数。

设计滤波器就是设计滤波器系数[B,A]。
[B,A] = BUTTER(N,Wn,'high') ---用来设计高通滤波器
[B,A] = BUTTER(N,Wn,'low') designs a lowpass filter.--低通滤波器
[B,A] = BUTTER(N,Wn)--带通滤波器

N是滤波器的阶数，不熟的话，大概取个整数就可以了。Wn的确定跟采样频率Fs有关。

比如说你的采样频率Fs=1000Hz，设计一个8阶、通带为100-200Hz的带通滤波器：

[b,a]=butter(8,[0.2 0.4])=butter(8,[100/(1000/2) 200/(1000/2) ])

得到滤波器系数后，就可以直接用了。y=filter(B,A,x)

3.6阶FIR滤波器的设计

% 6阶fir滤波器  3周期延时  截止频率50hz  衰减40db
h=[0.0403945865293132,0.121212071633126,0.212097140386596,0.252491727065433,0.212097140386596,0.121212071633126,0.0403945865293132];
dat3 = filter(h,1,dat);

设计的滤波器系数如何在C代码中实现？

float Filter(float cur,Filter_BufferData *x,float *h,int len)
{
	float y = 0.0f;
	for(int i=(len-1);i>0;i--)
	{
		x->Input_Butter[i] = x->Input_Butter[i-1];
	}
	x->Input_Butter[0] = cur;
	
	for(int i=0;i<len;i++)
	{
		y += (h[i] * x->Input_Butter[i]);
	}
	return y;
}

4.PX4开源代码中的2阶IIR

void Set_Cutoff_Frequency(float sample_frequent, float cutoff_frequent,Butter_Parameter *LPF)
{
	float fr = sample_frequent / cutoff_frequent;
	float ohm = tanf(M_PI_F / fr);
	float c = 1.0f + 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm;
	if (cutoff_frequent <= 0.0f) {
		// no filtering
		return;
	}
	LPF->b[0] = ohm * ohm / c;
	LPF->b[1] = 2.0f * LPF->b[0];
	LPF->b[2] = LPF->b[0];
        LPF->a[0]=1.0f;
	LPF->a[1] = 2.0f * (ohm * ohm - 1.0f) / c;
	LPF->a[2] = (1.0f - 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm) / c;
}

FIR用的很少

5.IIR与FIR的区别

IIR是非线性相位延迟，FIR是线性相位延迟。FIR无论是在低频段还是在高频段，滤波后的数据延迟是固定的；IIR在低频段的延迟大于在高频段的延迟。如下图所示：

python 低通滤波 scipy lowpass 低通滤波编程_数据_02

python 低通滤波 scipy lowpass 低通滤波编程_陀螺仪_03

magnitude response  幅度响应  ；图中显示的是这项（group delay response群延迟响应）

通过看仿真图，发现一阶滤波后数据在高频处还有噪声，应该是跟滤波器衰减的增益有关，一阶滤波器的衰减增益不够强。

       使用Matlab的FDAtool工具设计了IIR滤波器。使用File菜单中的export选项可以把滤波器的参数输出到Matlab的工作空间中。若滤波器为IIR型，则输出的变量名为G和SOS。它把高阶的IIR滤波器转换为一系列二阶IIR滤波器的级联。SOS为二阶IIR滤波器的系数（b,a），G为各级的增益系数，可用来调节各级通带的衰减。计算IIR滤波器的输出的时候，先将输入数据乘以G, 然后一一通过SOS中的每个IIR滤波器，就得到最终的结果了。 

       SOS的每一行表示一级二阶IIR滤波器系[b0,b1,b2,a0,a1,a2],G的每个元素表示相应级的增益系数。也即将G乘上SOS的b0,b1,b2。得到一组新的滤波器系数SOS'，可调用MATLAB滤波函数sosfilt对输入信号进行滤波。