9#include <initializer_list>
83 LinearFilter(std::span<const double> ffGains, std::span<const double> fbGains)
84 : m_inputs(ffGains.size()),
85 m_outputs(fbGains.size()),
86 m_inputGains(ffGains.begin(), ffGains.end()),
87 m_outputGains(fbGains.begin(), fbGains.end()) {
88 for (
size_t i = 0; i < ffGains.size(); ++i) {
89 m_inputs.emplace_front(0.0);
91 for (
size_t i = 0; i < fbGains.size(); ++i) {
92 m_outputs.emplace_front(0.0);
95 static int instances = 0;
108 std::initializer_list<double> fbGains)
110 {fbGains.begin(), fbGains.end()}) {}
128 units::second_t period) {
129 double gain = std::exp(-period.value() / timeConstant);
148 double gain = std::exp(-period.value() / timeConstant);
164 throw std::runtime_error(
"Number of taps must be greater than zero.");
167 std::vector<double> gains(taps, 1.0 / taps);
188 template <
int Derivative,
int Samples>
207 static_assert(Derivative >= 1,
208 "Order of derivative must be greater than or equal to one.");
209 static_assert(Samples > 0,
"Number of samples must be greater than zero.");
210 static_assert(Derivative < Samples,
211 "Order of derivative must be less than number of samples.");
214 for (
int row = 0; row < Samples; ++row) {
215 for (
int col = 0; col < Samples; ++col) {
216 S(row, col) =
std::pow(stencil[col], row);
222 for (
int i = 0; i < Samples; ++i) {
223 d(i) = (i == Derivative) ? Factorial(Derivative) : 0.0;
227 S.householderQr().solve(d) /
std::pow(period.value(), Derivative);
230 std::vector<double> ffGains;
231 for (
int i = Samples - 1; i >= 0; --i) {
232 ffGains.push_back(a(i));
255 template <
int Derivative,
int Samples>
259 for (
int i = 0; i < Samples; ++i) {
260 stencil[i] = -(Samples - 1) + i;
263 return FiniteDifference<Derivative, Samples>(stencil, period);
270 std::fill(m_inputs.begin(), m_inputs.end(), T{0.0});
271 std::fill(m_outputs.begin(), m_outputs.end(), T{0.0});
324 void Reset(std::span<const double> inputBuffer,
325 std::span<const double> outputBuffer) {
329 if (inputBuffer.size() != m_inputGains.size() ||
330 outputBuffer.size() != m_outputGains.size()) {
331 throw std::runtime_error(
332 "Incorrect length of inputBuffer or outputBuffer");
335 for (
size_t i = 0; i < inputBuffer.size(); ++i) {
336 m_inputs.push_front(inputBuffer[i]);
338 for (
size_t i = 0; i < outputBuffer.size(); ++i) {
339 m_outputs.push_front(outputBuffer[i]);
354 if (m_inputGains.size() > 0) {
355 m_inputs.push_front(input);
359 for (
size_t i = 0; i < m_inputGains.size(); ++i) {
360 retVal += m_inputs[i] * m_inputGains[i];
362 for (
size_t i = 0; i < m_outputGains.size(); ++i) {
363 retVal -= m_outputs[i] * m_outputGains[i];
367 if (m_outputGains.size() > 0) {
368 m_outputs.push_front(retVal);
384 std::vector<double> m_inputGains;
385 std::vector<double> m_outputGains;
392 static constexpr int Factorial(
int n) {
396 return n * Factorial(n - 1);
This class implements a linear, digital filter.
Definition: LinearFilter.h:75
LinearFilter(std::span< const double > ffGains, std::span< const double > fbGains)
Create a linear FIR or IIR filter.
Definition: LinearFilter.h:83
void Reset(std::span< const double > inputBuffer, std::span< const double > outputBuffer)
Resets the filter state, initializing internal buffers to the provided values.
Definition: LinearFilter.h:324
T Calculate(T input)
Calculates the next value of the filter.
Definition: LinearFilter.h:350
static LinearFilter< T > HighPass(double timeConstant, units::second_t period)
Creates a first-order high-pass filter of the form: y[n] = gain x[n] + (-gain) x[n-1] + gain y[n-1] ...
Definition: LinearFilter.h:147
static LinearFilter< T > FiniteDifference(const wpi::array< int, Samples > stencil, units::second_t period)
Creates a finite difference filter that computes the nth derivative of the input given the specified ...
Definition: LinearFilter.h:189
static LinearFilter< T > BackwardFiniteDifference(units::second_t period)
Creates a backward finite difference filter that computes the nth derivative of the input given the s...
Definition: LinearFilter.h:256
static LinearFilter< T > MovingAverage(int taps)
Creates a K-tap FIR moving average filter of the form: y[n] = 1/k (x[k] + x[k-1] + … + x[0])
Definition: LinearFilter.h:162
void Reset()
Reset the filter state.
Definition: LinearFilter.h:269
LinearFilter(std::initializer_list< double > ffGains, std::initializer_list< double > fbGains)
Create a linear FIR or IIR filter.
Definition: LinearFilter.h:107
T LastValue() const
Returns the last value calculated by the LinearFilter.
Definition: LinearFilter.h:379
static LinearFilter< T > SinglePoleIIR(double timeConstant, units::second_t period)
Creates a one-pole IIR low-pass filter of the form: y[n] = (1 - gain) x[n] + gain y[n-1] where gain...
Definition: LinearFilter.h:127
This class is a wrapper around std::array that does compile time size checking.
Definition: array.h:26
This is a simple circular buffer so we don't need to "bucket brigade" copy old values.
Definition: circular_buffer.h:20
static void ReportUsage(MathUsageId id, int count)
Definition: MathShared.h:73
FMT_NOINLINE FMT_CONSTEXPR auto fill(OutputIt it, size_t n, const fill_t< Char > &fill) -> OutputIt
Definition: format.h:1779
Definition: AprilTagPoseEstimator.h:15
Eigen::Matrix< double, Rows, Cols, Options, MaxRows, MaxCols > Matrixd
Definition: EigenCore.h:21
Eigen::Vector< double, Size > Vectord
Definition: EigenCore.h:12
auto pow(const UnitType &value) noexcept -> unit_t< typename units::detail::power_of_unit< power, typename units::traits::unit_t_traits< UnitType >::unit_type >::type, typename units::traits::unit_t_traits< UnitType >::underlying_type, linear_scale >
computes the value of value raised to the power
Definition: base.h:2806
constexpr empty_array_t empty_array
Definition: array.h:16
#define S(label, offset, message)
Definition: Errors.h:119