陷波滤波器设计

飞控课程笔记（二）
（https://zhuanlan.zhihu.com/p/66962045   across说的课程）

       在飞控领域中，主要针对飞行器在某一个高频点幅值较大，换句话来说，就是常见的飞机有共振的现象时。首先考虑通过机械减震或IMU减震来消除这种现象，若实际很难做到就考虑软件方面处理。

       要使用陷波滤波器，首先了解信号的频谱。假设已知有用信号在30Hz以内，在80Hz处有一个高频干扰。

       此时如果使用低通（设计一个二阶IIR）,其幅度响应如下图）：

可看到在80Hz处还没衰减完，

用fdatool设计一个陷波滤波器，

得到它的系数（Analysis----Filter Coefficients)

%fdatool设计 90hz中心 10hz带宽
dat = data2';
b=[0.925417,1.760248,0.925417];
a=[1.0,1.760248,0.850834];
datnotch = filter(b,a,dat);

在C代码中实现：

//1000hz调用   80-100的陷波处理

float Gyro_b1 = 0.9408;
float Gyro_b2 = -1.5918;
float Gyro_b3 = 0.9408;
float Gyro_a2 = -1.5918;
float Gyro_a3 = 0.8816;

void IMUdata_notch_filter(void)
 {
   pIMU_Data_filtered->RollRate   = (Gyro_b1*pIMU_Data->RollRate  + Gyro_b2*pIMU_Data_old->RollRate  + Gyro_b3*pIMU_Data_pre_old->RollRate  - Gyro_a2* pIMU_Data_filtered_old->RollRate  - Gyro_a3*pIMU_Data_filtered_pre_old->RollRate);
   pIMU_Data_filtered->PitchRate  = (Gyro_b1*pIMU_Data->PitchRate + Gyro_b2*pIMU_Data_old->PitchRate + Gyro_b3*pIMU_Data_pre_old->PitchRate - Gyro_a2* pIMU_Data_filtered_old->PitchRate - Gyro_a3*pIMU_Data_filtered_pre_old->PitchRate);
   pIMU_Data_filtered->YawRate    = (Gyro_b1*pIMU_Data->YawRate   + Gyro_b2*pIMU_Data_old->YawRate   + Gyro_b3*pIMU_Data_pre_old->YawRate   - Gyro_a2* pIMU_Data_filtered_old->YawRate   - Gyro_a3*pIMU_Data_filtered_pre_old->YawRate);
   
   pIMU_Data_pre_old->RollRate  = pIMU_Data_old->RollRate;
   pIMU_Data_old->RollRate      = pIMU_Data->RollRate;
   pIMU_Data_pre_old->PitchRate = pIMU_Data_old->PitchRate;
   pIMU_Data_old->PitchRate     = pIMU_Data->PitchRate;
   pIMU_Data_pre_old->YawRate   = pIMU_Data_old->YawRate;
   pIMU_Data_old->YawRate       = pIMU_Data->YawRate;
   pIMU_Data_filtered_pre_old->RollRate  = pIMU_Data_filtered_old->RollRate;
   pIMU_Data_filtered_old->RollRate      = pIMU_Data_filtered->RollRate;
   pIMU_Data_filtered_pre_old->PitchRate = pIMU_Data_filtered_old->PitchRate;
   pIMU_Data_filtered_old->PitchRate     = pIMU_Data_filtered->PitchRate;
   pIMU_Data_filtered_pre_old->YawRate   = pIMU_Data_filtered_old->YawRate;
   pIMU_Data_filtered_old->YawRate       = pIMU_Data_filtered->YawRate;
   
 }

上面的C代码在matlab里的仿真：

Z = dat;
b0 = 0.925417;
b1 = 1.760248;
b2 = 0.925417;
a1 = 1.760248;
a2 = 0.850834;
x_filt2 = zeros(n,1);
x2 = zeros(n,1);
x1 = zeros(n,1);
y2 = zeros(n,1);
y1 = zeros(n,1);
y2(1) = Z(1);
y1(1) = Z(1);
for i=2:n
    x_filt2(i) = b0 * Z(i) + b1*x1(i-1) + b2*x2(i-1) - a1*y1(i-1) -a2*y2(i-1);
    x2(i) = x1(i-1);
    x1(i) = Z(i);
    y2(i) = y1(i-1);
    y1(i) = x_filt2(i);
end

谢谢across说！