Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add LinearFilter::FiniteDifference() (#3900)

Browse Source 
This allows making more general finite difference filters, like central
finite difference. SysId uses this for acceleration filtering.
This commit is contained in:
Tyler Veness
2022-01-15 20:18:11 -08:00
committed by GitHub
parent 63d1fb3bed
commit 7f4265facc
4 changed files with 360 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files

120
wpimath/src/main/native/include/frc/filter/LinearFilter.h
View File

@@ -10,6 +10,7 @@
#include <stdexcept>
#include <vector>
#include <wpi/array.h>
#include <wpi/circular_buffer.h>
#include <wpi/span.h>
@@ -166,6 +167,73 @@ class LinearFilter {
return LinearFilter(gains, {});
}
/**
* Creates a finite difference filter that computes the nth derivative of the
* input given the specified stencil points.
*
* Stencil points are the indices of the samples to use in the finite
* difference. 0 is the current sample, -1 is the previous sample, -2 is the
* sample before that, etc. Don't use positive stencil points (samples from
* the future) if the LinearFilter will be used for stream-based online
* filtering.
*
* @tparam Derivative The order of the derivative to compute.
* @tparam Samples The number of samples to use to compute the given
* derivative. This must be one more than the order of
* derivative or higher.
* @param stencil List of stencil points.
* @param period The period in seconds between samples taken by the user.
*/
template <int Derivative, int Samples>
static LinearFilter<T> FiniteDifference(
const wpi::array<int, Samples> stencil, units::second_t period) {
// See
// https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points
//
// For a given list of stencil points s of length n and the order of
// derivative d < n, the finite difference coefficients can be obtained by
// solving the following linear system for the vector a.
//
// [s₁⁰ ⋯ sₙ⁰ ][a₁] [ δ₀,d ]
// [ ⋮ ⋱ ⋮ ][⋮ ] = d! [ ⋮ ]
// [s₁ⁿ⁻¹ ⋯ sₙⁿ⁻¹][aₙ] [δₙ₋₁,d]
//
// where δᵢ,ⱼ are the Kronecker delta. The FIR gains are the elements of the
// vector a in reverse order divided by hᵈ.
//
// The order of accuracy of the approximation is of the form O(hⁿ⁻ᵈ).
static_assert(Derivative >= 1,
"Order of derivative must be greater than or equal to one.");
static_assert(Samples > 0, "Number of samples must be greater than zero.");
static_assert(Derivative < Samples,
"Order of derivative must be less than number of samples.");
Eigen::Matrix<double, Samples, Samples> S;
for (int row = 0; row < Samples; ++row) {
for (int col = 0; col < Samples; ++col) {
S(row, col) = std::pow(stencil[col], row);
}
}
// Fill in Kronecker deltas: https://en.wikipedia.org/wiki/Kronecker_delta
Eigen::Vector<double, Samples> d;
for (int i = 0; i < Samples; ++i) {
d(i) = (i == Derivative) ? Factorial(Derivative) : 0.0;
}
Eigen::Vector<double, Samples> a =
S.householderQr().solve(d) / std::pow(period.value(), Derivative);
// Reverse gains list
std::vector<double> ffGains;
for (int i = Samples - 1; i >= 0; --i) {
ffGains.push_back(a(i));
}
return LinearFilter(ffGains, {});
}
/**
* Creates a backward finite difference filter that computes the nth
* derivative of the input given the specified number of samples.
@@ -184,56 +252,14 @@ class LinearFilter {
* @param period The period in seconds between samples taken by the user.
*/
template <int Derivative, int Samples>
static auto BackwardFiniteDifference(units::second_t period) {
// See
// https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points
//
// For a given list of stencil points s of length n and the order of
// derivative d < n, the finite difference coefficients can be obtained by
// solving the following linear system for the vector a.
//
// @verbatim
// [s₁⁰ ⋯ sₙ⁰ ][a₁] [ δ₀,d ]
// [ ⋮ ⋱ ⋮ ][⋮ ] = d! [ ⋮ ]
// [s₁ⁿ⁻¹ ⋯ sₙⁿ⁻¹][aₙ] [δₙ₋₁,d]
// @endverbatim
//
// where δᵢ,ⱼ are the Kronecker delta. For backward finite difference, the
// stencil points are the range [-n + 1, 0]. The FIR gains are the elements
// of the vector a in reverse order divided by hᵈ.
//
// The order of accuracy of the approximation is of the form O(hⁿ⁻ᵈ).
static_assert(Derivative >= 1,
"Order of derivative must be greater than or equal to one.");
static_assert(Samples > 0, "Number of samples must be greater than zero.");
static_assert(Derivative < Samples,
"Order of derivative must be less than number of samples.");
Eigen::Matrix<double, Samples, Samples> S;
for (int row = 0; row < Samples; ++row) {
for (int col = 0; col < Samples; ++col) {
double s = 1 - Samples + col;
S(row, col) = std::pow(s, row);
}
}
// Fill in Kronecker deltas: https://en.wikipedia.org/wiki/Kronecker_delta
Eigen::Vector<double, Samples> d;
static LinearFilter<T> BackwardFiniteDifference(units::second_t period) {
// Generate stencil points from -(samples - 1) to 0
wpi::array<int, Samples> stencil{wpi::empty_array};
for (int i = 0; i < Samples; ++i) {
d(i) = (i == Derivative) ? Factorial(Derivative) : 0.0;
stencil[i] = -(Samples - 1) + i;
}
Eigen::Vector<double, Samples> a =
S.householderQr().solve(d) / std::pow(period.value(), Derivative);
// Reverse gains list
std::vector<double> gains;
for (int i = Samples - 1; i >= 0; --i) {
gains.push_back(a(i));
}
return LinearFilter(gains, {});
return FiniteDifference<Derivative, Samples>(stencil, period);
}
/**