Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51: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 ab0b7e9b03
4 changed files with 360 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -138,42 +138,41 @@ public class LinearFilter {
}
/**
* Creates a backward finite difference filter that computes the nth derivative of the input given
* the specified number of samples.
* Creates a finite difference filter that computes the nth derivative of the input given the
* specified stencil points.
*
* <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>
* <p>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.
*
* @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 stencil List of stencil points.
* @param period The period in seconds between samples taken by the user.
* @return Linear filter.
* @throws IllegalArgumentException if derivative &lt; 1, samples &lt;= 0, or derivative &gt;=
* samples.
*/
@SuppressWarnings("LocalVariableName")
public static LinearFilter backwardFiniteDifference(int derivative, int samples, double period) {
public static LinearFilter finiteDifference(
int derivative, int samples, int[] stencil, 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
// 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ᵈ.
// where δᵢ,ⱼ are the Kronecker delta. 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ⁿ⁻ᵈ).
// The order of accuracy of the approximation is of the form O(hⁿ⁻ᵈ).
if (derivative < 1) {
throw new IllegalArgumentException(
@@ -192,8 +191,7 @@ public class LinearFilter {
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));
S.set(row, col, Math.pow(stencil[col], row));
}
}
@@ -211,9 +209,34 @@ public class LinearFilter {
ffGains[i] = a.get(samples - i - 1, 0);
}
double[] fbGains = new double[0];
return new LinearFilter(ffGains, new double[0]);
}
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.
* @return Linear filter.
*/
public static LinearFilter backwardFiniteDifference(int derivative, int samples, double period) {
// Generate stencil points from -(samples - 1) to 0
int[] stencil = new int[samples];
for (int i = 0; i < samples; ++i) {
stencil[i] = -(samples - 1) + i;
}
return finiteDifference(derivative, samples, stencil, period);
}
/** Reset the filter state. */

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);
}
/**

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

@@ -109,12 +109,84 @@ class LinearFilterTest {
0.0));
}
/** Test central finite difference. */
@Test
void centralFiniteDifferenceTest() {
double h = 0.005;
assertCentralResults(
1,
3,
// f(x) = x²
(double x) -> x * x,
// df/dx = 2x
(double x) -> 2.0 * x,
h,
-20.0,
20.0);
assertCentralResults(
1,
3,
// f(x) = sin(x)
(double x) -> Math.sin(x),
// df/dx = cos(x)
(double x) -> Math.cos(x),
h,
-20.0,
20.0);
assertCentralResults(
1,
3,
// f(x) = ln(x)
(double x) -> Math.log(x),
// df/dx = 1 / x
(double x) -> 1.0 / x,
h,
1.0,
20.0);
assertCentralResults(
2,
5,
// f(x) = x²
(double x) -> x * x,
// d²f/dx² = 2
(double x) -> 2.0,
h,
-20.0,
20.0);
assertCentralResults(
2,
5,
// f(x) = sin(x)
(double x) -> Math.sin(x),
// d²f/dx² = -sin(x)
(double x) -> -Math.sin(x),
h,
-20.0,
20.0);
assertCentralResults(
2,
5,
// 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);
}
/** Test backward finite difference. */
@Test
void backwardFiniteDifferenceTest() {
double h = 0.005;
assertResults(
assertBackwardResults(
1,
2,
// f(x) = x²
@@ -125,7 +197,7 @@ class LinearFilterTest {
-20.0,
20.0);
assertResults(
assertBackwardResults(
1,
2,
// f(x) = sin(x)
@@ -136,7 +208,7 @@ class LinearFilterTest {
-20.0,
20.0);
assertResults(
assertBackwardResults(
1,
2,
// f(x) = ln(x)
@@ -147,7 +219,7 @@ class LinearFilterTest {
1.0,
20.0);
assertResults(
assertBackwardResults(
2,
4,
// f(x) = x²
@@ -158,7 +230,7 @@ class LinearFilterTest {
-20.0,
20.0);
assertResults(
assertBackwardResults(
2,
4,
// f(x) = sin(x)
@@ -169,7 +241,7 @@ class LinearFilterTest {
-20.0,
20.0);
assertResults(
assertBackwardResults(
2,
4,
// f(x) = ln(x)
@@ -181,6 +253,53 @@ class LinearFilterTest {
20.0);
}
/**
* Helper for checking results of central 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 assertCentralResults(
int derivative,
int samples,
DoubleFunction<Double> f,
DoubleFunction<Double> dfdx,
double h,
double min,
double max) {
if (samples % 2 == 0) {
throw new IllegalArgumentException("Number of samples must be odd.");
}
// Generate stencil points from -(samples - 1)/2 to (samples - 1)/2
int[] stencil = new int[samples];
for (int i = 0; i < samples; ++i) {
stencil[i] = -(samples - 1) / 2 + i;
}
var filter = LinearFilter.finiteDifference(derivative, samples, stencil, 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 - samples / 2) * h),
filter.calculate(f.apply(i * h)),
Math.pow(h, samples - derivative));
}
}
/**
* Helper for checking results of backward finite difference.
*
@@ -192,7 +311,7 @@ class LinearFilterTest {
* @param min Minimum of f's domain to test.
* @param max Maximum of f's domain to test.
*/
void assertResults(
void assertBackwardResults(
int derivative,
int samples,
DoubleFunction<Double> f,
@@ -209,6 +328,8 @@ class LinearFilterTest {
continue;
}
// For central finite difference, the derivative computed at this point is
// half the window size in the past.
// The order of accuracy is O(h^(N - d)) where N is number of stencil
// points and d is order of derivative
assertEquals(

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

@@ -9,6 +9,7 @@
#include <memory>
#include <random>
#include <wpi/array.h>
#include <wpi/numbers>
#include "gtest/gtest.h"
@@ -120,8 +121,40 @@ INSTANTIATE_TEST_SUITE_P(Tests, LinearFilterOutputTest,
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) {
void AssertCentralResults(F&& f, DfDx&& dfdx, units::second_t h, double min,
double max) {
static_assert(Samples % 2 != 0, "Number of samples must be odd.");
// Generate stencil points from -(samples - 1)/2 to (samples - 1)/2
wpi::array<int, Samples> stencil{wpi::empty_array};
for (int i = 0; i < Samples; ++i) {
stencil[i] = -(Samples - 1) / 2 + i;
}
auto filter =
frc::LinearFilter<double>::FiniteDifference<Derivative, Samples>(stencil,
h);
for (int i = min / h.value(); i < max / h.value(); ++i) {
// Let filter initialize
if (i < static_cast<int>(min / h.value()) + Samples) {
filter.Calculate(f(i * h.value()));
continue;
}
// For central finite difference, the derivative computed at this point is
// half the window size in the past.
// 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 - (Samples - 1) / 2) * h.value()),
filter.Calculate(f(i * h.value())),
std::pow(h.value(), Samples - Derivative));
}
}
template <int Derivative, int Samples, typename F, typename DfDx>
void AssertBackwardResults(F&& f, DfDx&& dfdx, units::second_t h, double min,
double max) {
auto filter =
frc::LinearFilter<double>::BackwardFiniteDifference<Derivative, Samples>(
h);
@@ -141,12 +174,12 @@ void AssertResults(F&& f, DfDx&& dfdx, units::second_t h, double min,
}
/**
* Test backward finite difference.
* Test central finite difference.
*/
TEST(LinearFilterOutputTest, BackwardFiniteDifference) {
TEST(LinearFilterOutputTest, CentralFiniteDifference) {
constexpr auto h = 5_ms;
AssertResults<1, 2>(
AssertCentralResults<1, 3>(
[](double x) {
// f(x) = x²
return x * x;
@@ -157,7 +190,7 @@ TEST(LinearFilterOutputTest, BackwardFiniteDifference) {
},
h, -20.0, 20.0);
AssertResults<1, 2>(
AssertCentralResults<1, 3>(
[](double x) {
// f(x) = std::sin(x)
return std::sin(x);
@@ -168,7 +201,7 @@ TEST(LinearFilterOutputTest, BackwardFiniteDifference) {
},
h, -20.0, 20.0);
AssertResults<1, 2>(
AssertCentralResults<1, 3>(
[](double x) {
// f(x) = ln(x)
return std::log(x);
@@ -179,7 +212,7 @@ TEST(LinearFilterOutputTest, BackwardFiniteDifference) {
},
h, 1.0, 20.0);
AssertResults<2, 4>(
AssertCentralResults<2, 5>(
[](double x) {
// f(x) = x^2
return x * x;
@@ -190,7 +223,7 @@ TEST(LinearFilterOutputTest, BackwardFiniteDifference) {
},
h, -20.0, 20.0);
AssertResults<2, 4>(
AssertCentralResults<2, 5>(
[](double x) {
// f(x) = std::sin(x)
return std::sin(x);
@@ -201,7 +234,80 @@ TEST(LinearFilterOutputTest, BackwardFiniteDifference) {
},
h, -20.0, 20.0);
AssertResults<2, 4>(
AssertCentralResults<2, 5>(
[](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);
}
/**
* Test backward finite difference.
*/
TEST(LinearFilterOutputTest, BackwardFiniteDifference) {
constexpr auto h = 5_ms;
AssertBackwardResults<1, 2>(
[](double x) {
// f(x) = x²
return x * x;
},
[](double x) {
// df/dx = 2x
return 2.0 * x;
},
h, -20.0, 20.0);
AssertBackwardResults<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);
AssertBackwardResults<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);
AssertBackwardResults<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);
AssertBackwardResults<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);
AssertBackwardResults<2, 4>(
[](double x) {
// f(x) = ln(x)
return std::log(x);