ArduCopter——ArduPilot——Harmonic Notch Filter(諧波陷波濾波器)

版權聲明：本文爲博主原創博文，未經允許不得轉載，若要轉載，請說明出處並給出博文鏈接 

之前，我們已經比較詳細地學習和分析了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之前的加速度計和陀螺儀的原始數據的快速傅立葉變換的結果：

 經過HNF後的數據是：

對比之後，是不是發現第一個圖加速度數據中的180Hz和360Hz的噪聲幾乎完美濾除呢！！！  神奇的神器！！！

OK，本次關於諧波陷波濾波器的介紹就到此爲止了，歡迎大家批評指教！！！