From 6983ecd5bbd74501057bce7ed81f99a01a62fc87 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 5 Oct 2023 21:20:01 +0200 Subject: [PATCH] Notch filters (#3128) --- sw/airborne/filters/adaptive_notch_filter.h | 144 ++++++++++++++++++++ sw/airborne/filters/notch_filter.h | 33 ++++- sw/airborne/filters/notch_filter_float.h | 133 ++++++++++++++++++ 3 files changed, 303 insertions(+), 7 deletions(-) create mode 100644 sw/airborne/filters/adaptive_notch_filter.h create mode 100644 sw/airborne/filters/notch_filter_float.h diff --git a/sw/airborne/filters/adaptive_notch_filter.h b/sw/airborne/filters/adaptive_notch_filter.h new file mode 100644 index 0000000000..1e552d31ae --- /dev/null +++ b/sw/airborne/filters/adaptive_notch_filter.h @@ -0,0 +1,144 @@ +/* + * Copyright (C) 2023 Dennis van Wijngaarden + * Ewoud Smeur + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** @file filters/adaptive_notch_filter.h + * @brief Adaptive notch filter + * + */ + +#ifndef ADAPTIVE_NOTCH_FILTER_H +#define ADAPTIVE_NOTCH_FILTER_H + +#include "std.h" +#include "filters/low_pass_filter.h" +#include "filters/notch_filter_float.h" + +struct AdaptiveNotchFilter { + float Ts; // Sample time + float lp_cutoff_f; // Lowpass cutoff frequency + float hp_cutoff_f; // Highpass cutoff frequency + float r; // Stop-band filter's bandwidth + float r2; // Stop-band filter's bandwidth squared + float lambda; // Learning gain + + float a_est; // estimated notch frequency + float y[2]; // Notch filtered measurement vector + float y_est[2]; // Notch filtered estimation vector + + struct SecondOrderNotchFilter output_n_filter; // Output notch filter + Butterworth2LowPass lp_filter; // Lowpass filter for the lower bound of the bandpass filter + Butterworth2LowPass hp_filter; // Lowpass filter for the higher bound of the bandpass filter + + float xband[3]; // Bandpassed measurement signals signals +}; + +/** Init a adaptive notch filter. + * + * @param filter adaptive notch filter structure + * @param sample_time sampling period of the signal + * @param low_freq_bound lower frequency bound of the adaptive notch filter + * @param high_freq_bound higher frequency bound of the adaptive notch filter + * @param bandwidth bandwidth of the adaptive notch filter + * @param lambda learning rate of the adaptive notch filter + * @param a_ini initial band-stop frequency of the adaptive notch filter + * @param value initial value of the filter + */ +static inline void adaptive_notch_filter_init(struct AdaptiveNotchFilter *filter, float sample_time, + float low_freq_bound, float high_freq_bound, float bandwidth, float lambda, float a_ini, float value) +{ + // Init filter parameters + filter->Ts = sample_time; + filter->lp_cutoff_f = low_freq_bound; + filter->hp_cutoff_f = high_freq_bound; + filter->r = bandwidth; + filter->lambda = lambda; + + // Initialize initial filter values + filter->y[0] = filter->y[1] = filter->y_est[0] = filter->y_est[1] = value; + filter->a_est = a_ini; + + filter->xband[0] = filter->xband[1] = filter->xband[2] = value; + + // Init output notch filter + float fs = 1. / filter->Ts; + notch_filter_init(&filter->output_n_filter, filter->a_est, filter->r, fs); + + // Init lowpass filter + float lp_tau = 1.0 / (2.0 * M_PI * filter->lp_cutoff_f); + init_butterworth_2_low_pass(&filter->lp_filter, lp_tau, filter->Ts, 0.0); + + // Init highpass filter + float hp_tau = 1.0 / (2.0 * M_PI * filter->hp_cutoff_f); + init_butterworth_2_low_pass(&filter->hp_filter, hp_tau, filter->Ts, 0.0); + +} + +/** Update a second order adaptive notch filter. + * + * @param filter adaptive notch filter structure + * @param value new input value of the filter + * @param output pointer to float for output + * @return new filtered value + */ +static inline void adaptive_notch_filter_update(struct AdaptiveNotchFilter *filter, float value, float *output_signal) +{ + // Propagate Butterworth lowpass and hightpass filters for a bandpass signal + update_butterworth_2_low_pass(&filter->lp_filter, value); + update_butterworth_2_low_pass(&filter->hp_filter, value); + + filter->xband[2] = filter->xband[1]; + filter->xband[1] = filter->xband[0]; + filter->xband[0] = value - filter->lp_filter.o[0] - (value - filter->hp_filter.o[0]); + + // Run output notch filter + notch_filter_set_filter_frequency(&filter->output_n_filter, filter->a_est); + notch_filter_update(&filter->output_n_filter, &value, output_signal); + + // Run estimation notch filter + float out_est = filter->xband[0] - filter->a_est * filter->xband[1] + filter->xband[2] + filter->r * filter->a_est * + filter->y_est[0] - filter->r2 * filter->y_est[1]; + + float beta = -filter->xband[1] + filter->r * filter->y_est[1]; + filter->a_est = filter->a_est - filter->lambda * filter->Ts * filter->y_est[0] * beta; + // Bound estimation frequency to the bandpass cutoff frequencies to be safe + Bound(filter->a_est, filter->lp_cutoff_f, filter->hp_cutoff_f); + + filter->y[1] = filter->y[0]; + filter->y[0] = *output_signal; + filter->y_est[1] = filter->y_est[0]; + filter->y_est[0] = out_est; + +} + +/** Return the latest second order adaptive notch filter output + * + * @param filter adaptive notch filter structure + * @return latest filtered value + */ + +static inline float adaptive_notch_filter_get_output(struct AdaptiveNotchFilter *filter) +{ + return filter->y_est[0]; +} + + +#endif // ADAPTIVE_NOTCH_FILTER_H \ No newline at end of file diff --git a/sw/airborne/filters/notch_filter.h b/sw/airborne/filters/notch_filter.h index 261fda2ec0..9f5c4fcc79 100644 --- a/sw/airborne/filters/notch_filter.h +++ b/sw/airborne/filters/notch_filter.h @@ -41,30 +41,33 @@ struct SecondOrderNotchFilter { /** Set sampling frequency of the notch filter * + * @param filter filter data structure * @param frequency frequency at which the filter is updated */ static inline void notch_filter_set_sampling_frequency(struct SecondOrderNotchFilter *filter, uint16_t frequency) { - filter->Ts = 1.0/frequency; + filter->Ts = 1.0 / frequency; } /** Set bandwidth of the notch filter * + * @param filter filter data structure * @param bandwidth bandwidth of the filter [Hz] */ static inline void notch_filter_set_bandwidth(struct SecondOrderNotchFilter *filter, float bandwidth) { - float d = expf(-M_PI*bandwidth*filter->Ts); - filter->d2 = d*d; + float d = expf(-M_PI * bandwidth * filter->Ts); + filter->d2 = d * d; } /** Set notch filter frequency in Hz * + * @param filter filter data structure * @param frequency to attenuate [Hz] */ static inline void notch_filter_set_filter_frequency(struct SecondOrderNotchFilter *filter, float frequency) { - float theta = 2.0*M_PI*frequency*filter->Ts; + float theta = 2.0 * M_PI * frequency * filter->Ts; filter->costheta = cosf(theta); } @@ -73,14 +76,17 @@ static inline void notch_filter_set_filter_frequency(struct SecondOrderNotchFilt * Discrete implementation: * y[n] = b * y[n-1] - d^2 * y[n-2] + a * x[n] - b * x[n-1] + a * x[n-2] * + * @param filter filter data structure * @param input_signal input x[n] * @param output_signal output y[n] */ -static inline void notch_filter_update(struct SecondOrderNotchFilter *filter, int32_t *input_signal, int32_t *output_signal) +static inline void notch_filter_update(struct SecondOrderNotchFilter *filter, int32_t *input_signal, + int32_t *output_signal) { float a = (1 + filter->d2) * 0.5; float b = (1 + filter->d2) * filter->costheta; - *output_signal = (b * filter->yn1) - (filter->d2 * filter->yn2) + (a * *input_signal) - (b * filter->xn1) + (a * filter->xn2); + *output_signal = (b * filter->yn1) - (filter->d2 * filter->yn2) + (a * *input_signal) - (b * filter->xn1) + + (a * filter->xn2); /* Update values for next update */ filter->xn2 = filter->xn1; @@ -89,6 +95,18 @@ static inline void notch_filter_update(struct SecondOrderNotchFilter *filter, in filter->yn1 = *output_signal; } +/** Get latest notch filter output + * + * @param filter filter data structure + * @return Latest filtered value + */ + + +static inline int32_t notch_filter_get_output(struct SecondOrderNotchFilter *filter) +{ + return filter->yn1; +} + /** Initialize second order notch filter * * Discrete implementation: @@ -98,7 +116,8 @@ static inline void notch_filter_update(struct SecondOrderNotchFilter *filter, in * @param bandwidth bandwidth of the filter [Hz] * @param sample_frequency frequency at which the filter is updated */ -static inline void notch_filter_init(struct SecondOrderNotchFilter *filter, float cutoff_frequency, float bandwidth, uint16_t sample_frequency) +static inline void notch_filter_init(struct SecondOrderNotchFilter *filter, float cutoff_frequency, float bandwidth, + uint16_t sample_frequency) { notch_filter_set_sampling_frequency(filter, sample_frequency); notch_filter_set_filter_frequency(filter, cutoff_frequency); diff --git a/sw/airborne/filters/notch_filter_float.h b/sw/airborne/filters/notch_filter_float.h new file mode 100644 index 0000000000..557f529718 --- /dev/null +++ b/sw/airborne/filters/notch_filter_float.h @@ -0,0 +1,133 @@ +/* + * Copyright (C) 2016 Bart Slinger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** @file filters/notch_filter.h + * @brief Second order notch filter + * + */ + +#ifndef NOTCH_FILTER_H +#define NOTCH_FILTER_H + +#include "std.h" + +struct SecondOrderNotchFilter { + float Ts; + float d2; + float costheta; + float xn1; + float xn2; + float yn1; + float yn2; +}; + +/** Set sampling frequency of the notch filter + * + * @param filter SecondOrderNotchFilter data + * @param frequency frequency at which the filter is updated + */ +static inline void notch_filter_set_sampling_frequency(struct SecondOrderNotchFilter *filter, uint16_t frequency) +{ + filter->Ts = 1.0 / frequency; +} + +/** Set bandwidth of the notch filter + * + * @param filter SecondOrderNotchFilter data + * @param bandwidth bandwidth of the filter [Hz] + */ +static inline void notch_filter_set_bandwidth(struct SecondOrderNotchFilter *filter, float bandwidth) +{ + float d = expf(-M_PI * bandwidth * filter->Ts); + filter->d2 = d * d; +} + +/** Set notch filter frequency in Hz + * + * @param filter SecondOrderNotchFilter data + * @param frequency to attenuate [Hz] + */ +static inline void notch_filter_set_filter_frequency(struct SecondOrderNotchFilter *filter, float frequency) +{ + float theta = 2.0 * M_PI * frequency * filter->Ts; + filter->costheta = cosf(theta); +} + +/** Notch filter propagate + * + * Discrete implementation: + * y[n] = b * y[n-1] - d^2 * y[n-2] + a * x[n] - b * x[n-1] + a * x[n-2] + * + * @param filter SecondOrderNotchFilter data + * @param input_signal input x[n] + * @param output_signal output y[n] + */ +static inline void notch_filter_update(struct SecondOrderNotchFilter *filter, float *input_signal, float *output_signal) +{ + float a = (1 + filter->d2) * 0.5; + float b = (1 + filter->d2) * filter->costheta; + *output_signal = (b * filter->yn1) - (filter->d2 * filter->yn2) + (a * *input_signal) - (b * filter->xn1) + + (a * filter->xn2); + + /* Update values for next update */ + filter->xn2 = filter->xn1; + filter->xn1 = *input_signal; + filter->yn2 = filter->yn1; + filter->yn1 = *output_signal; +} + +/** Get notch filter output + * + * @param filter SecondOrderNotchFilter data + * @return Laterst filtered value + */ + + +static inline float notch_filter_get_output(struct notch_filter_float *filter) +{ + return filter->yn1; +} + + + +/** Initialize second order notch filter + * + * Discrete implementation: + * y[n] = b * y[n-1] - d^2 * y[n-2] + a * x[n] - b * x[n-1] + a * x[n-2] + * + * @param filter SecondOrderNotchFilter data + * @param cutoff_frequency frequency to attenuate [Hz] + * @param bandwidth bandwidth of the filter [Hz] + * @param sample_frequency frequency at which the filter is updated + */ +static inline void notch_filter_init(struct SecondOrderNotchFilter *filter, float cutoff_frequency, float bandwidth, + uint16_t sample_frequency) +{ + notch_filter_set_sampling_frequency(filter, sample_frequency); + notch_filter_set_filter_frequency(filter, cutoff_frequency); + notch_filter_set_bandwidth(filter, bandwidth); + filter->xn1 = 0; + filter->xn2 = 0; + filter->yn1 = 0; + filter->yn2 = 0; +} + +#endif \ No newline at end of file