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 parameters
    void 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 range
    if ((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 samples
        ntchsig2 = 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 parameters
    if (!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 filter
        if (_gyro_notch_enabled()) {
            gyro_filtered = _imu._gyro_notch_filter[instance].apply(gyro_filtered);
        }

}

追蹤到這兩個函數，再追蹤下去就是各種不同型號的IMU傳感器的初始化與迭代更新了，我們就不再貼了。

今天就先扯到這裏吧，如果有哪裏有問題，歡迎大家批評指教，一定專心受教！！！