ArduCopter——ArduPilot——Notch Filter（陷波滤波器）

作为一名通信、电子信息、自动化相关专业的学生或者从业人员，一定对信号处理这门学科非常熟悉，自然就或多或少接触过信号采集、信号噪声、信号滤波等相关专业词汇，本篇博客是想记录和学习一下ArduPilot团队设计的Notch Filter(陷波滤波器)。

百度百科说：“陷波滤波器指的是一种可以在某一个频率点迅速衰减输入信号，以达到阻碍此频率信号通过的滤波效果的滤波器。一个理想陷波滤波器的频率响应是要在消除的信号频率点，其值等于零；而在其他频率处，其值不为零，且要等于1。”

陷波滤波器属于软件滤波的众多中的一种，有软件滤波，就一定有硬件滤波。常用的硬件滤波，比如减震器等，也就是从机械结构的层面上想办法去减去不需要的振动。那我们熟悉的飞控减震板或者IMU减震板就是这样的一种。常见的常规的小的多轴飞机基本上都可以通过安装减震板来解决电机螺旋桨高速旋转后给飞控板和IMU带来的振动，但是比较大型的多轴飞机、直升机、固定翼，振动也是很恐怖的，普通的减震器是无法做到滤除振动的，那么软件滤波的出现无疑是防止炸机的又一利器。(解决振动问题的优先级是：机械结构的优化>硬件滤波>软件滤波)。

下面，我们直接来了解一下Notch filter:

这是我在MATLAB中自己设计的一个陷波滤波器，可以很直观地看得出，我想要滤除的信号的中心频率是6Hz,滤除的信号的衰减大小是50~60之间，下图是该滤波器的相位图。

但是由于我现在手头上没有直升机或者较大飞机的陀螺仪和加速度计的原始输出数据，所以没办法自己亲手做实验来演示，之后有了，会补充上来的。那现在就只好拿网上大神们整理的数据和实验来展示一下Notch Filter的神奇效果了：

上图是设计的滤波器的特性图，可以看得出，中心频率是60Hz，衰减大小是60dB，那应用到实际的数据上的效果就是下图啦。

很明显的滤波，有没有！！！上面的是原始信号，下面的是经过上图特性的notch filter的输出信号。是不是很清晰的感觉！！！如果还是不清晰，那我们看看它信号做了快速傅里叶变换FFT（时域->频域）之后的样子：

上面的60Hz原始信号噪声到下面的就被清晰滤掉，有没有震惊到！！！

该滤波器的传递函数的Z变换形式：

如果写成代码的话呢？？？？很简单，就在下面：

y(n) =1*x(n) - 1.4579*x(n-1)+1*x(n-2)+1.3850*y(n-1)-0.9025*y(n-2).

上面公式中每项的系数就是我们设计滤波器最终需要的关键参数。

细心的小伙伴儿一定发现了，这个图是ECG signal，也就是心电图哦。

引用嘛，一定要给出资料的出处哦——>>>Lizhe Tan, Jean Jiang, in Digital Signal Processing (Third Edition), 2019

别再罗嗦拉！！！我只关心代码是怎么实现的。。。。

① 来自NotchFilter.h，图中给出了一些注释，方便理解和记忆

#include <AP_Math/AP_Math.h>
#include <cmath>
#include <inttypes.h>
#include <AP_Param/AP_Param.h>template <class T>
class NotchFilter {
public:// set parametersvoid init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB);void init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q);T apply(const T &sample);void reset();// calculate attenuation and quality from provided center frequency and bandwidth//根据给定的中心频率和带宽来计算衰减分贝和Q因子static void calculate_A_and_Q(float center_freq_hz, float bandwidth_hz, float attenuation_dB, float& A, float& Q); private:bool initialised;float b0, b1, b2, a1, a2, a0_inv; // Notch Filter的输出公式中的核心系数T ntchsig, ntchsig1, ntchsig2, signal2, signal1;  //2阶的Notch Filter的输入输出信号
};/*notch filter enable and filter parameters*/
class NotchFilterParams {
public:NotchFilterParams(void);static const struct AP_Param::GroupInfo var_info[];float center_freq_hz(void) const { return _center_freq_hz; }float bandwidth_hz(void) const { return _bandwidth_hz; }float attenuation_dB(void) const { return _attenuation_dB; }uint8_t enabled(void) const { return _enable; }protected:
//预先设定的几个全参数AP_Int8 _enable;   // 该滤波器的使能AP_Float _center_freq_hz;  // 想要滤除的信号的中心频率 AP_Float _bandwidth_hz;    // 想要滤除的信号的带宽AP_Float _attenuation_dB;  // 想要滤除的信号的衰减分贝
};

②下面的代码片段来自NotchFilter.cpp

下面的第一个片段值得大家注意一下，就是想要滤除的信号的中心频率center_freq_hz一定要大于带宽的一半，且小于信号采样频率sample_freq_hz的一半的，原因是什么呢？

学过《信号与系统》或者《通信原理》的小伙伴一定知道奈奎斯特和香农这两个伟人，他们说的采样定理就告诉了我们要小于等于采样频率的一半；

那么为啥一定要大于带宽的一半呢，这里我个人表示也不是非常理解和明白，希望看到的大神可以不吝赐教！！！但是我还是有一点自己的小见解的，就是通过这样的一个设定，我们可以计算出Q因子，而这个Q因子是衡量无用信号的中心频率和其他有用信号频率隔离的一个重要指标，针对一个固定阶次的滤波器，Q因子越大，我们得到的滤波器对于已知的无用信号的滤除就越精准。另外，从数学上看，2的任何次方都等于一个正的数，那么如果中心频率小雨带宽的一半，就会出现负数，这是不对的。

/*initialise filter*/
template <class T>
void NotchFilter<T>::init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB)
{// check center frequency is in the allowable rangeif ((center_freq_hz > 0.5 * bandwidth_hz) && (center_freq_hz < 0.5 * sample_freq_hz)) {float A, Q;calculate_A_and_Q(center_freq_hz, bandwidth_hz, attenuation_dB, A, Q);init_with_A_and_Q(sample_freq_hz, center_freq_hz, A, Q);} else {initialised = false;}
}

template <class T>
void NotchFilter<T>::calculate_A_and_Q(float center_freq_hz, float bandwidth_hz, float attenuation_dB, float& A, float& Q) {A = powf(10, -attenuation_dB / 40.0f);if (center_freq_hz > 0.5 * bandwidth_hz) {const float octaves = log2f(center_freq_hz / (center_freq_hz - bandwidth_hz / 2.0f)) * 2.0f;Q = sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1.0f);} else {Q = 0.0;}
}

上面这个函数就是通过提前设定好的想要滤除的信号的中心频率和带宽来计算我们Notch Filter的两个关键参数A（衰减分贝因子）和Q（质量因子）。

template <class T>
void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q)
{if ((center_freq_hz > 0.0) && (center_freq_hz < 0.5 * sample_freq_hz) && (Q > 0.0)) {float omega = 2.0 * M_PI * center_freq_hz / sample_freq_hz; float alpha = sinf(omega) / (2 * Q);b0 =  1.0 + alpha*sq(A);b1 = -2.0 * cosf(omega);b2 =  1.0 - alpha*sq(A);a0_inv =  1.0/(1.0 + alpha);a1 = b1;a2 =  1.0 - alpha;initialised = true;} else {initialised = false;}
}

上面这个函数是再已经得到A和Q的前提下，去计算滤波器输出公式中的几个关键系数：b0，b1，b2，a0_inv，a1，a2。上面的omega是想要滤除的信号频率（rad）。

再得到上面的几个参数之后，我们就可以得到我们Notch Filter的最终输出形式以及最终输出结果了——下面代码中的output

template <class T>
T NotchFilter<T>::apply(const T &sample)
{if (!initialised) {// if we have not been initialised when return the input// sample as output and update delayed samplesntchsig2 = ntchsig1;ntchsig1 = ntchsig;ntchsig = sample;signal2 = signal1;signal1 = sample;return sample;}ntchsig2 = ntchsig1;ntchsig1 = ntchsig;ntchsig = sample;T output = (ntchsig*b0 + ntchsig1*b1 + ntchsig2*b2 - signal1*a1 - signal2*a2) * a0_inv;signal2 = signal1;signal1 = output;return output;
}

OK，上面我们把原理和具体实现都列举出来了，那ArduPilot中的Copter是怎么调用的呢？？？

下面的代码，我只截取了和本博客有关的部分。

/*common gyro update function for all backends*/
void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
{ // possily update the notch filter parametersif (!is_equal(_last_notch_center_freq_hz, _gyro_notch_center_freq_hz()) ||!is_equal(_last_notch_bandwidth_hz, _gyro_notch_bandwidth_hz()) ||!is_equal(_last_notch_attenuation_dB, _gyro_notch_attenuation_dB()) ||sensors_converging()) {_imu._gyro_notch_filter[instance].init(_gyro_raw_sample_rate(instance), _gyro_notch_center_freq_hz(), _gyro_notch_bandwidth_hz(), _gyro_notch_attenuation_dB());_last_notch_center_freq_hz = _gyro_notch_center_freq_hz();_last_notch_bandwidth_hz = _gyro_notch_bandwidth_hz();_last_notch_attenuation_dB = _gyro_notch_attenuation_dB();}}

void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,const Vector3f &gyro,uint64_t sample_us)
{     // apply the notch filterif (_gyro_notch_enabled()) {gyro_filtered = _imu._gyro_notch_filter[instance].apply(gyro_filtered);}}

追踪到这两个函数，再追踪下去就是各种不同型号的IMU传感器的初始化与迭代更新了，我们就不再贴了。

今天就先扯到这里吧，如果有哪里有问题，欢迎大家批评指教，一定专心受教！！！

ArduCopter——ArduPilot——Notch Filter（陷波滤波器）相关推荐

ArduCopter——ArduPilot——Harmonic Notch Filter(谐波陷波滤波器)
版权声明:本文为博主原创博文,未经允许不得转载,若要转载,请说明出处并给出博文链接之前,我们已经比较详细地学习和分析了Notch Filter陷波滤波器-->这里,点我. 那我们现在趁热打铁, ...
Simulink 窄带陷波滤波器（Notch filter)仿真到代码生成
目录窄带陷波滤波器(Notch filter)原理设计一个Notch filter 生成代码离散传递函数生成代码配置差分方程生成代码方程推导构建差分方程模型总结窄带陷波滤波器(Notc ...
陷波滤波器（Notch Filter）的离散化设计
陷波滤波器(Notch Filter)的离散化设计符号说明 wbww_{bw}wbw - 陷波宽度,单位:rad/srad/srad/s wcw_cwc - 陷波中央频率,单位:rad/srad ...
常见三种陷波滤波器（Notch Filter）的离散化设计
为了学习陷波器,几乎把全网的陷波器有关文章都看了一遍,整理下,加深印象,也给后来的朋友作参考,文章内容大部分直接复制过来,但是所有内容代码我都测试了一遍,其中有些错误之处,也作了修改. 陷波滤波器形式 ...
基于gnuradio的自适应陷波滤波器OOT模块（notch filter）
基于gnuradio的自适应陷波滤波器OOT模块 Introduction 模型工具gr_modtool 创建OOT Module 创建OOT block 修改impl.h 修改impl.cc 修改a ...
陷波滤波器（Notch filter）
陷波滤波器: 指的是一种可以在某一个频率点迅速衰减输入信号,以达到阻碍此频率信号通过的滤波效果的滤波器.陷波滤波器属于带阻滤波器的一种,只是它的阻带非常狭窄,起阶数必须是二阶(含二阶)以上. 当带阻滤 ...
自适应陷波滤波器（Notch滤波器）
自适应滤波器以其自身调节参数的能力以及对信号和噪声的先验知识要求少的优势得到了广泛的应用. 自适应陷波滤波器相比于普通的数字滤波器具有较窄的阻带和通带,带外衰减较快.具有滤波输出(Y输出)和陷波输出( ...
【OpenCV 例程200篇】90. 频率域陷波滤波器
[OpenCV 例程200篇]90. 频率域陷波滤波器欢迎关注『OpenCV 例程200篇』系列,持续更新中欢迎关注『Python小白的OpenCV学习课』系列,持续更新中 5.2 陷波滤 ...
基于反馈结构的窄带宽陷波滤波器
IEEE Signal Processing Magazine 是信号处理领域一本著名的杂志.它偶尔会出一个名叫 Tips & Tricks 的专栏.这个专栏的文章没有高深的理论知识,简洁实用 ...
matlab中陷波滤波器消除,matlab – 找到合适的陷波滤波器以从图像中移除图案
如果您查看图像的fft,您可以清楚地看到导致图像中图案的强频率. 您需要创建一个陷波滤波器,将这些高峰周围的区域归零.我尝试使用高斯陷波滤波器进行此操作,得到的频谱看起来像这样. ifft图像(对比度 ...

ArduCopter——ArduPilot——Notch Filter（陷波滤波器）

ArduCopter——ArduPilot——Notch Filter（陷波滤波器）相关推荐

最新文章

热门文章