mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
Notch filters (#3128)
This commit is contained in:
Christophe De Wagter
committed by
GitHub
GitHub
parent
889521b53e
commit
6983ecd5bb
@@ -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
|
||||
@@ -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);
|
||||
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user