欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页  >  IT编程

陷波滤波器设计

程序员文章站 2022-03-22 08:18:35
飞控课程笔记(二)(https://zhuanlan.zhihu.com/p/66962045 across说的课程) 在飞控领域中,主要针对飞行器在某一个高频点幅值较大,换句话来说,就是常见的飞机有共振的现象时。首先考虑通过机械减震或IMU减震来消除这种现象,若实际很难做到就考虑软件方面处理。 要使用陷波滤波器,首先了解信号的频谱。假设已知有用信号在30Hz以内,在80Hz处有一个高频干扰。 此时如果使用低通(设计一个二阶IIR),其幅度响应如下图):......

飞控课程笔记(二)
(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说!

本文地址:https://blog.csdn.net/weixin_40525909/article/details/107283873