Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-01 02:41:48 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add LinearFilter::BackwardFiniteDifference() (#3528)

Browse Source 
This is an alternative to #2344 that handles arbitrary order derivatives
of arbitrary precision. The downside is that since it's part of
LinearFilter, it can't utilize the units type system in the same way to
make Calculate()'s input type different from its output type.
This commit is contained in:
Tyler Veness
2021-08-28 20:50:18 -07:00
committed by GitHub
parent c8fc715fe2
commit 3b5d0d141a
4 changed files with 387 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

96
wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
View File

@@ -8,6 +8,7 @@ import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.util.CircularBuffer;
import java.util.Arrays;
import org.ejml.simple.SimpleMatrix;
/**
* This class implements a linear, digital filter. All types of FIR and IIR filters are supported.
@@ -57,8 +58,8 @@ public class LinearFilter {
/**
* Create a linear FIR or IIR filter.
*
* @param ffGains The "feed forward" or FIR gains.
* @param fbGains The "feed back" or IIR gains.
* @param ffGains The "feedforward" or FIR gains.
* @param fbGains The "feedback" or IIR gains.
*/
public LinearFilter(double[] ffGains, double[] fbGains) {
m_inputs = new CircularBuffer(ffGains.length);
@@ -136,6 +137,84 @@ public class LinearFilter {
return new LinearFilter(ffGains, fbGains);
}
/**
* Creates a backward finite difference filter that computes the nth derivative of the input given
* the specified number of samples.
*
* <p>For example, a first derivative filter that uses two samples and a sample period of 20 ms
* would be
*
* <pre><code>
* LinearFilter.backwardFiniteDifference(1, 2, 0.02);
* </code></pre>
*
* @param derivative The order of the derivative to compute.
* @param 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 period The period in seconds between samples taken by the user.
*/
@SuppressWarnings("LocalVariableName")
public static LinearFilter backwardFiniteDifference(int derivative, int samples, double period) {
// See
// https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points
//
// <p>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.
//
// <pre>
// [s₁⁰ ⋯ sₙ⁰ ][a₁] [ δ₀,d ]
// [ ⋮ ⋱ ⋮ ][⋮ ] = d! [ ⋮ ]
// [s₁ⁿ⁻¹ ⋯ sₙⁿ⁻¹][aₙ] [δₙ₋₁,d]
// </pre>
//
// <p>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ᵈ.
//
// <p>The order of accuracy of the approximation is of the form O(hⁿ⁻ᵈ).
if (derivative < 1) {
throw new IllegalArgumentException(
"Order of derivative must be greater than or equal to one.");
}
if (samples <= 0) {
throw new IllegalArgumentException("Number of samples must be greater than zero.");
}
if (derivative >= samples) {
throw new IllegalArgumentException(
"Order of derivative must be less than number of samples.");
}
var S = new SimpleMatrix(samples, samples);
for (int row = 0; row < samples; ++row) {
for (int col = 0; col < samples; ++col) {
double s = 1 - samples + col;
S.set(row, col, Math.pow(s, row));
}
}
// Fill in Kronecker deltas: https://en.wikipedia.org/wiki/Kronecker_delta
var d = new SimpleMatrix(samples, 1);
for (int i = 0; i < samples; ++i) {
d.set(i, 0, (i == derivative) ? factorial(derivative) : 0.0);
}
var a = S.solve(d).divide(Math.pow(period, derivative));
// Reverse gains list
double[] ffGains = new double[samples];
for (int i = 0; i < samples; ++i) {
ffGains[i] = a.get(samples - i - 1, 0);
}
double[] fbGains = new double[0];
return new LinearFilter(ffGains, fbGains);
}
/** Reset the filter state. */
public void reset() {
m_inputs.clear();
@@ -171,4 +250,17 @@ public class LinearFilter {
return retVal;
}
/**
* Factorial of n.
*
* @param n Argument of which to take factorial.
*/
private static int factorial(int n) {
if (n < 2) {
return 1;
} else {
return n * factorial(n - 1);
}
}
}

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

@@ -13,6 +13,8 @@
#include <wpi/circular_buffer.h>
#include <wpi/span.h>
#include "Eigen/Core"
#include "Eigen/QR"
#include "units/time.h"
#include "wpimath/MathShared.h"
@@ -74,8 +76,8 @@ class LinearFilter {
/**
* Create a linear FIR or IIR filter.
*
* @param ffGains The "feed forward" or FIR gains.
* @param fbGains The "feed back" or IIR gains.
* @param ffGains The "feedforward" or FIR gains.
* @param fbGains The "feedback" or IIR gains.
*/
LinearFilter(wpi::span<const double> ffGains, wpi::span<const double> fbGains)
: m_inputs(ffGains.size()),
@@ -98,8 +100,8 @@ class LinearFilter {
/**
* Create a linear FIR or IIR filter.
*
* @param ffGains The "feed forward" or FIR gains.
* @param fbGains The "feed back" or IIR gains.
* @param ffGains The "feedforward" or FIR gains.
* @param fbGains The "feedback" or IIR gains.
*/
LinearFilter(std::initializer_list<double> ffGains,
std::initializer_list<double> fbGains)
@@ -164,6 +166,76 @@ class LinearFilter {
return LinearFilter(gains, {});
}
/**
* Creates a backward finite difference filter that computes the nth
* derivative of the input given the specified number of samples.
*
* For example, a first derivative filter that uses two samples and a sample
* period of 20 ms would be
*
* <pre><code>
* LinearFilter<double>::BackwardFiniteDifference<1, 2>(20_ms);
* </code></pre>
*
* @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 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::Matrix<double, Samples, 1> d;
for (int i = 0; i < Samples; ++i) {
d(i) = (i == Derivative) ? Factorial(Derivative) : 0.0;
}
Eigen::Matrix<double, Samples, 1> a =
S.householderQr().solve(d) / std::pow(period.to<double>(), Derivative);
// Reverse gains list
std::vector<double> gains;
for (int i = Samples - 1; i >= 0; --i) {
gains.push_back(a(i));
}
return LinearFilter(gains, {});
}
/**
* Reset the filter state.
*/
@@ -208,6 +280,19 @@ class LinearFilter {
wpi::circular_buffer<T> m_outputs;
std::vector<double> m_inputGains;
std::vector<double> m_outputGains;
/**
* Factorial of n.
*
* @param n Argument of which to take factorial.
*/
static int Factorial(int n) {
if (n < 2) {
return 1;
} else {
return n * Factorial(n - 1);
}
}
};
} // namespace frc

109
wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java
View File

@@ -108,4 +108,113 @@ class LinearFilterTest {
(DoubleFunction<Double>) LinearFilterTest::getPulseData,
0.0));
}
/** Test backward finite difference. */
@Test
void backwardFiniteDifferenceTest() {
double h = 0.005;
assertResults(
1,
2,
// f(x) = x²
(double x) -> x * x,
// df/dx = 2x
(double x) -> 2.0 * x,
h,
-20.0,
20.0);
assertResults(
1,
2,
// f(x) = sin(x)
(double x) -> Math.sin(x),
// df/dx = cos(x)
(double x) -> Math.cos(x),
h,
-20.0,
20.0);
assertResults(
1,
2,
// f(x) = ln(x)
(double x) -> Math.log(x),
// df/dx = 1 / x
(double x) -> 1.0 / x,
h,
1.0,
20.0);
assertResults(
2,
4,
// f(x) = x²
(double x) -> x * x,
// d²f/dx² = 2
(double x) -> 2.0,
h,
-20.0,
20.0);
assertResults(
2,
4,
// f(x) = sin(x)
(double x) -> Math.sin(x),
// d²f/dx² = -sin(x)
(double x) -> -Math.sin(x),
h,
-20.0,
20.0);
assertResults(
2,
4,
// f(x) = ln(x)
(double x) -> Math.log(x),
// d²f/dx² = -1 / x²
(double x) -> -1.0 / (x * x),
h,
1.0,
20.0);
}
/**
* Helper for checking results of backward finite difference.
*
* @param derivative The order of the derivative.
* @param samples The number of sample points.
* @param f Function of which to take derivative.
* @param dfdx Derivative of f.
* @param h Sample period in seconds.
* @param min Minimum of f's domain to test.
* @param max Maximum of f's domain to test.
*/
void assertResults(
int derivative,
int samples,
DoubleFunction<Double> f,
DoubleFunction<Double> dfdx,
double h,
double min,
double max) {
var filter = LinearFilter.backwardFiniteDifference(derivative, samples, h);
for (int i = (int) (min / h); i < (int) (max / h); ++i) {
// Let filter initialize
if (i < (int) (min / h) + samples) {
filter.calculate(f.apply(i * h));
continue;
}
// The order of accuracy is O(h^(N - d)) where N is number of stencil
// points and d is order of derivative
assertEquals(
dfdx.apply(i * h),
filter.calculate(f.apply(i * h)),
10.0 * Math.pow(h, samples - derivative));
}
}
}

95
wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
View File

@@ -118,3 +118,98 @@ TEST_P(LinearFilterOutputTest, Output) {
INSTANTIATE_TEST_SUITE_P(Test, LinearFilterOutputTest,
testing::Values(kTestSinglePoleIIR, kTestHighPass,
kTestMovAvg, kTestPulse));
template <int Derivative, int Samples, typename F, typename DfDx>
void AssertResults(F&& f, DfDx&& dfdx, units::second_t h, double min,
double max) {
auto filter =
frc::LinearFilter<double>::BackwardFiniteDifference<Derivative, Samples>(
h);
for (int i = min / h.to<double>(); i < max / h.to<double>(); ++i) {
// Let filter initialize
if (i < static_cast<int>(min / h.to<double>()) + Samples) {
filter.Calculate(f(i * h.to<double>()));
continue;
}
// The order of accuracy is O(h^(N - d)) where N is number of stencil
// points and d is order of derivative
EXPECT_NEAR(dfdx(i * h.to<double>()),
filter.Calculate(f(i * h.to<double>())),
10.0 * std::pow(h.to<double>(), Samples - Derivative));
}
}
/**
* Test backward finite difference.
*/
TEST(LinearFilterOutputTest, BackwardFiniteDifference) {
constexpr auto h = 5_ms;
AssertResults<1, 2>(
[](double x) {
// f(x) = x²
return x * x;
},
[](double x) {
// df/dx = 2x
return 2.0 * x;
},
h, -20.0, 20.0);
AssertResults<1, 2>(
[](double x) {
// f(x) = std::sin(x)
return std::sin(x);
},
[](double x) {
// df/dx = std::cos(x)
return std::cos(x);
},
h, -20.0, 20.0);
AssertResults<1, 2>(
[](double x) {
// f(x) = ln(x)
return std::log(x);
},
[](double x) {
// df/dx = 1 / x
return 1.0 / x;
},
h, 1.0, 20.0);
AssertResults<2, 4>(
[](double x) {
// f(x) = x^2
return x * x;
},
[](double x) {
// d²f/dx² = 2
return 2.0;
},
h, -20.0, 20.0);
AssertResults<2, 4>(
[](double x) {
// f(x) = std::sin(x)
return std::sin(x);
},
[](double x) {
// d²f/dx² = -std::sin(x)
return -std::sin(x);
},
h, -20.0, 20.0);
AssertResults<2, 4>(
[](double x) {
// f(x) = ln(x)
return std::log(x);
},
[](double x) {
// d²f/dx² = -1 / x²
return -1.0 / (x * x);
},
h, 1.0, 20.0);
}