ArduCopter——ArduPilot——Harmonic Notch Filter(谐波陷波滤波器)

最新推荐文章于 2024-09-06 23:13:19 发布

慕~

最新推荐文章于 2024-09-06 23:13:19 发布

阅读量2.2k

点赞数

分类专栏： ArduPilot 无人机文章标签：算法

本文链接：https://blog.csdn.net/u012814946/article/details/105440188

版权

无人机同时被 2 个专栏收录

30 篇文章 19 订阅

订阅专栏

ArduPilot

24 篇文章 35 订阅

订阅专栏

之前，我们已经比较详细地学习和分析了Notch Filter陷波滤波器——>这里，点我。

那我们现在趁热打铁，在Notch Filter的基础上，看看如何学习和理解下Harmonic Notch Filter（HNF）谐波陷波滤波器，以及应用与如何使用的。

翻阅了祖国大好河山和国外的美丽景色，也没看到我们最想看到的谐波陷波滤波器的定义，也许是我翻阅的还不够仔细吧......

但是，一会儿通过阅读ArduPilot的源代码，我们可以有一个自己的理解，这可能比课本给的定义更令人记忆深刻！！！

毫不罗嗦，直接上代码来看吧：

①HNF的初始化：

如果你已经看过我上一篇的陷波滤波器的博客，就一定会发现HNF的初始化貌似和NF的初始化基本一样，不同的点在于，这里定义了nyquist_limit = 0.48倍的信号采样频率，是为了遵守我们之前提到的奈奎斯特采样定理（要小于信号采样频率的一半）啊！

/*
  initialise the associated filters with the provided shaping constraints
  the constraints are used to determine attenuation (A) and quality (Q) factors for the filter
 */
template <class T>
void HarmonicNotchFilter<T>::init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB)
{
    // sanity check the input
    if (_filters == nullptr || is_zero(sample_freq_hz) || isnan(sample_freq_hz)) {
        return;
    }

    _sample_freq_hz = sample_freq_hz;
    const float nyquist_limit = sample_freq_hz * 0.48f;

    // adjust the fundamental center frequency to be in the allowable range
    center_freq_hz = constrain_float(center_freq_hz, bandwidth_hz * 0.52f, nyquist_limit);

    // calculate attenuation and quality from the shaping constraints
    NotchFilter<T>::calculate_A_and_Q(center_freq_hz, bandwidth_hz, attenuation_dB, _A, _Q);

    _num_enabled_filters = 0;
    // initialize all the configured filters with the same A & Q and multiples of the center frequency
    for (uint8_t i = 0, filt = 0; i < HNF_MAX_HARMONICS && filt < _num_filters; i++) {
        const float notch_center = center_freq_hz * (i+1);
        if ((1U<<i) & _harmonics) {
            // only enable the filter if its center frequency is below the nyquist frequency
            if (notch_center < nyquist_limit) {
                _filters[filt].init_with_A_and_Q(sample_freq_hz, notch_center, _A, _Q);
                _num_enabled_filters++;
            }
            filt++;
        }
    }
    _initialised = true;
}

另外，这里在计算了衰减大小和Q因子之外，还加入了一个for循环，用于多次调用init_with_A_and_Ｑ这个函数，调用这个函数，聪明的你一定知道就是为了计算滤波器输出所用的公式中的关键系数b0 b1 b2 a0_inv a1 a2，那么为什么要多次调用这个函数呢？

关键点在于 notch_center = 无用信号中心频率*（i+1），这行代码告诉我们每次调用init_with_A_and_Ｑ这个函数之前，都会先更新notch_center，notch_center是这个函数的需要用到的一个参数，且notch_center是跟随i的变化而变化，且是成倍数的变化。说到这里，我大致有点明白了，可能就是几个陷波滤波器的叠加使用呗，且这几个叠加滤波器滤除的频率是倍数相关的。是的，没错，其实这里用到的谐波陷波滤波器就是你的猜想！！！

② HNF的update：

和init初始化基本一致，只是没有计算A和Q的那一步，比较节约硬件资源和时间而已。

/*
  update the underlying filters' center frequency using the current attenuation and quality
  this function is cheaper than init() because A & Q do not need to be recalculated
 */
template <class T>
void HarmonicNotchFilter<T>::update(float center_freq_hz)
{
    if (!_initialised) {
        return;
    }

    // adjust the fundamental center frequency to be in the allowable range
    const float nyquist_limit = _sample_freq_hz * 0.48f;
    center_freq_hz = constrain_float(center_freq_hz, 1.0f, nyquist_limit);

    _num_enabled_filters = 0;
    // update all of the filters using the new center frequency and existing A & Q
    for (uint8_t i = 0, filt = 0; i < HNF_MAX_HARMONICS && filt < _num_filters; i++) {
        const float notch_center = center_freq_hz * (i+1);
        if ((1U<<i) & _harmonics) {
            // only enable the filter if its center frequency is below the nyquist frequency
            if (notch_center < nyquist_limit) {
                _filters[filt].init_with_A_and_Q(_sample_freq_hz, notch_center, _A, _Q);
                _num_enabled_filters++;
            }
            filt++;
        }
    }
}

③ HNF的输出接口：

/*
  apply a sample to each of the underlying filters in turn and return the output
 */
template <class T>
T HarmonicNotchFilter<T>::apply(const T &sample)
{
    if (!_initialised) {
        return sample;
    }

    T output = sample;
    for (uint8_t i = 0; i < _num_enabled_filters; i++) {
        output = _filters[i].apply(output); // 这里调用的是Notch Filter的apply 函数，详见上一篇博客
    }
    return output;
}

看到这里呢，基本上HNF的核心功能已经看到了，其实就是在把NF进行倍数打包使用。当然核心是信号中心频率的1倍、2倍、3倍......

那具体是怎么调用的呢？

接着看。。。

void
AP_InertialSensor::init(uint16_t sample_rate)
{
    // the center frequency of the harmonic notch is always taken from the calculated value so that it can be updated
    // dynamically, the calculated value is always some multiple of the configured center frequency, so start with the
    // configured value
    _calculated_harmonic_notch_freq_hz = _harmonic_notch_filter.center_freq_hz();

    for (uint8_t i=0; i<get_gyro_count(); i++)
   {
        _gyro_harmonic_notch_filter[i].allocate_filters(_harmonic_notch_filter.harmonics());
        // initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro()
        _gyro_harmonic_notch_filter[i].init(_gyro_raw_sample_rates[i], _calculated_harmonic_notch_freq_hz,
             _harmonic_notch_filter.bandwidth_hz(), _harmonic_notch_filter.attenuation_dB());
    }
}

void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
                                                            const Vector3f &gyro,
                                                            uint64_t sample_us)
{
        // apply the harmonic notch filter
        if (gyro_harmonic_notch_enabled()) {
            gyro_filtered = _imu._gyro_harmonic_notch_filter[instance].apply(gyro_filtered);
        }

}

是不是一下就舒服了......

好像还是缺点什么。。。。。。。。。对，虽然我自己没有实验数据，但是用ArduPilot的大神多了去了，他们肯定有。。。

下面就是我找到的使用HNF之前的加速度计和陀螺仪的原始数据的快速傅立叶变换的结果：