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

Clean up LinearDigitalFilter class (#782)

Browse Source 
* Renamed LinearDigitalFilter to LinearFilter
* Filter base class removed since it wasn't useful
* C++: std::shared_ptr<> replaced with double parameter
This commit is contained in:
Tyler Veness
2019-06-28 13:35:57 -07:00
committed by Peter Johnson
parent 311e2de4c1
commit 30e936837c
22 changed files with 771 additions and 960 deletions
Download Patch File Download Diff File Expand all files Collapse all files

137
wpilibc/src/test/native/cpp/FilterNoiseTest.cpp
View File

@@ -1,137 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/filters/LinearDigitalFilter.h" // NOLINT(build/include_order)
#include <cmath>
#include <functional>
#include <memory>
#include <random>
#include <thread>
#include "frc/Base.h"
#include "gtest/gtest.h"
/* Filter constants */
static constexpr double kFilterStep = 0.005;
static constexpr double kFilterTime = 2.0;
static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
static constexpr double kSinglePoleIIRExpectedOutput = -3.2172003;
static constexpr double kHighPassTimeConstant = 0.006631;
static constexpr double kHighPassExpectedOutput = 10.074717;
static constexpr int32_t kMovAvgTaps = 6;
static constexpr double kMovAvgExpectedOutput = -10.191644;
static constexpr double kPi = 3.14159265358979323846;
using namespace frc;
enum FilterNoiseTestType { TEST_SINGLE_POLE_IIR, TEST_MOVAVG };
std::ostream& operator<<(std::ostream& os, const FilterNoiseTestType& type) {
switch (type) {
case TEST_SINGLE_POLE_IIR:
os << "LinearDigitalFilter SinglePoleIIR";
break;
case TEST_MOVAVG:
os << "LinearDigitalFilter MovingAverage";
break;
}
return os;
}
constexpr double kStdDev = 10.0;
/**
* Adds Gaussian white noise to a function returning data. The noise will have
* the standard deviation provided in the constructor.
*/
class NoiseGenerator : public PIDSource {
public:
NoiseGenerator(double (*dataFunc)(double), double stdDev)
: m_distr(0.0, stdDev) {
m_dataFunc = dataFunc;
}
void SetPIDSourceType(PIDSourceType pidSource) override {}
double Get() { return m_dataFunc(m_count) + m_noise; }
double PIDGet() override {
m_noise = m_distr(m_gen);
m_count += kFilterStep;
return m_dataFunc(m_count) + m_noise;
}
void Reset() { m_count = -kFilterStep; }
private:
std::function<double(double)> m_dataFunc;
double m_noise = 0.0;
// Make sure first call to PIDGet() uses m_count == 0
double m_count = -kFilterStep;
std::random_device m_rd;
std::mt19937 m_gen{m_rd()};
std::normal_distribution<double> m_distr;
};
/**
* A fixture that includes a noise generator wrapped in a filter
*/
class FilterNoiseTest : public testing::TestWithParam<FilterNoiseTestType> {
protected:
std::unique_ptr<PIDSource> m_filter;
std::shared_ptr<NoiseGenerator> m_noise;
static double GetData(double t) { return 100.0 * std::sin(2.0 * kPi * t); }
void SetUp() override {
m_noise = std::make_shared<NoiseGenerator>(GetData, kStdDev);
switch (GetParam()) {
case TEST_SINGLE_POLE_IIR: {
m_filter = std::make_unique<LinearDigitalFilter>(
LinearDigitalFilter::SinglePoleIIR(
m_noise, kSinglePoleIIRTimeConstant, kFilterStep));
break;
}
case TEST_MOVAVG: {
m_filter = std::make_unique<LinearDigitalFilter>(
LinearDigitalFilter::MovingAverage(m_noise, kMovAvgTaps));
break;
}
}
}
};
/**
* Test if the filter reduces the noise produced by a signal generator
*/
TEST_P(FilterNoiseTest, NoiseReduce) {
double theoryData = 0.0;
double noiseGenError = 0.0;
double filterError = 0.0;
m_noise->Reset();
for (double t = 0; t < kFilterTime; t += kFilterStep) {
theoryData = GetData(t);
filterError += std::abs(m_filter->PIDGet() - theoryData);
noiseGenError += std::abs(m_noise->Get() - theoryData);
}
RecordProperty("FilterError", filterError);
// The filter should have produced values closer to the theory
EXPECT_GT(noiseGenError, filterError)
<< "Filter should have reduced noise accumulation but failed";
}
INSTANTIATE_TEST_SUITE_P(Test, FilterNoiseTest,
testing::Values(TEST_SINGLE_POLE_IIR, TEST_MOVAVG));

157
wpilibc/src/test/native/cpp/FilterOutputTest.cpp
View File

@@ -1,157 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/filters/LinearDigitalFilter.h" // NOLINT(build/include_order)
#include <cmath>
#include <functional>
#include <memory>
#include <random>
#include <thread>
#include "frc/Base.h"
#include "gtest/gtest.h"
/* Filter constants */
static constexpr double kFilterStep = 0.005;
static constexpr double kFilterTime = 2.0;
static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
static constexpr double kSinglePoleIIRExpectedOutput = -3.2172003;
static constexpr double kHighPassTimeConstant = 0.006631;
static constexpr double kHighPassExpectedOutput = 10.074717;
static constexpr int32_t kMovAvgTaps = 6;
static constexpr double kMovAvgExpectedOutput = -10.191644;
static constexpr double kPi = 3.14159265358979323846;
using namespace frc;
enum FilterOutputTestType {
TEST_SINGLE_POLE_IIR,
TEST_HIGH_PASS,
TEST_MOVAVG,
TEST_PULSE
};
std::ostream& operator<<(std::ostream& os, const FilterOutputTestType& type) {
switch (type) {
case TEST_SINGLE_POLE_IIR:
os << "LinearDigitalFilter SinglePoleIIR";
break;
case TEST_HIGH_PASS:
os << "LinearDigitalFilter HighPass";
break;
case TEST_MOVAVG:
os << "LinearDigitalFilter MovingAverage";
break;
case TEST_PULSE:
os << "LinearDigitalFilter Pulse";
break;
}
return os;
}
class DataWrapper : public PIDSource {
public:
explicit DataWrapper(double (*dataFunc)(double)) { m_dataFunc = dataFunc; }
virtual void SetPIDSourceType(PIDSourceType pidSource) {}
virtual double PIDGet() {
m_count += kFilterStep;
return m_dataFunc(m_count);
}
void Reset() { m_count = -kFilterStep; }
private:
std::function<double(double)> m_dataFunc;
// Make sure first call to PIDGet() uses m_count == 0
double m_count = -kFilterStep;
};
/**
* A fixture that includes a consistent data source wrapped in a filter
*/
class FilterOutputTest : public testing::TestWithParam<FilterOutputTestType> {
protected:
std::unique_ptr<PIDSource> m_filter;
std::shared_ptr<DataWrapper> m_data;
double m_expectedOutput = 0.0;
static double GetData(double t) {
return 100.0 * std::sin(2.0 * kPi * t) + 20.0 * std::cos(50.0 * kPi * t);
}
static double GetPulseData(double t) {
if (std::abs(t - 1.0) < 0.001) {
return 1.0;
} else {
return 0.0;
}
}
void SetUp() override {
switch (GetParam()) {
case TEST_SINGLE_POLE_IIR: {
m_data = std::make_shared<DataWrapper>(GetData);
m_filter = std::make_unique<LinearDigitalFilter>(
LinearDigitalFilter::SinglePoleIIR(
m_data, kSinglePoleIIRTimeConstant, kFilterStep));
m_expectedOutput = kSinglePoleIIRExpectedOutput;
break;
}
case TEST_HIGH_PASS: {
m_data = std::make_shared<DataWrapper>(GetData);
m_filter =
std::make_unique<LinearDigitalFilter>(LinearDigitalFilter::HighPass(
m_data, kHighPassTimeConstant, kFilterStep));
m_expectedOutput = kHighPassExpectedOutput;
break;
}
case TEST_MOVAVG: {
m_data = std::make_shared<DataWrapper>(GetData);
m_filter = std::make_unique<LinearDigitalFilter>(
LinearDigitalFilter::MovingAverage(m_data, kMovAvgTaps));
m_expectedOutput = kMovAvgExpectedOutput;
break;
}
case TEST_PULSE: {
m_data = std::make_shared<DataWrapper>(GetPulseData);
m_filter = std::make_unique<LinearDigitalFilter>(
LinearDigitalFilter::MovingAverage(m_data, kMovAvgTaps));
m_expectedOutput = 0.0;
break;
}
}
}
};
/**
* Test if the linear digital filters produce consistent output
*/
TEST_P(FilterOutputTest, FilterOutput) {
m_data->Reset();
double filterOutput = 0.0;
for (double t = 0.0; t < kFilterTime; t += kFilterStep) {
filterOutput = m_filter->PIDGet();
}
RecordProperty("FilterOutput", filterOutput);
EXPECT_FLOAT_EQ(m_expectedOutput, filterOutput)
<< "Filter output didn't match expected value";
}
INSTANTIATE_TEST_SUITE_P(Test, FilterOutputTest,
testing::Values(TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS,
TEST_MOVAVG, TEST_PULSE));

92
wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp Normal file
View File

@@ -0,0 +1,92 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/LinearFilter.h" // NOLINT(build/include_order)
#include <cmath>
#include <memory>
#include <random>
#include "gtest/gtest.h"
// Filter constants
static constexpr double kFilterStep = 0.005;
static constexpr double kFilterTime = 2.0;
static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
static constexpr int32_t kMovAvgTaps = 6;
enum LinearFilterNoiseTestType { TEST_SINGLE_POLE_IIR, TEST_MOVAVG };
std::ostream& operator<<(std::ostream& os,
const LinearFilterNoiseTestType& type) {
switch (type) {
case TEST_SINGLE_POLE_IIR:
os << "LinearFilter SinglePoleIIR";
break;
case TEST_MOVAVG:
os << "LinearFilter MovingAverage";
break;
}
return os;
}
static double GetData(double t) {
constexpr double kPi = 3.14159265358979323846;
return 100.0 * std::sin(2.0 * kPi * t);
}
class LinearFilterNoiseTest
: public testing::TestWithParam<LinearFilterNoiseTestType> {
protected:
std::unique_ptr<frc::LinearFilter> m_filter;
void SetUp() override {
switch (GetParam()) {
case TEST_SINGLE_POLE_IIR: {
m_filter = std::make_unique<frc::LinearFilter>(
frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant,
kFilterStep));
break;
}
case TEST_MOVAVG: {
m_filter = std::make_unique<frc::LinearFilter>(
frc::LinearFilter::MovingAverage(kMovAvgTaps));
break;
}
}
}
};
/**
* Test if the filter reduces the noise produced by a signal generator
*/
TEST_P(LinearFilterNoiseTest, NoiseReduce) {
double noiseGenError = 0.0;
double filterError = 0.0;
std::random_device rd;
std::mt19937 gen{rd()};
std::normal_distribution<double> distr{0.0, 10.0};
for (double t = 0; t < kFilterTime; t += kFilterStep) {
double theory = GetData(t);
double noise = distr(gen);
filterError += std::abs(m_filter->Calculate(theory + noise) - theory);
noiseGenError += std::abs(noise - theory);
}
RecordProperty("FilterError", filterError);
// The filter should have produced values closer to the theory
EXPECT_GT(noiseGenError, filterError)
<< "Filter should have reduced noise accumulation but failed";
}
INSTANTIATE_TEST_SUITE_P(Test, LinearFilterNoiseTest,
testing::Values(TEST_SINGLE_POLE_IIR, TEST_MOVAVG));

132
wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp Normal file
View File

@@ -0,0 +1,132 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/LinearFilter.h" // NOLINT(build/include_order)
#include <cmath>
#include <functional>
#include <memory>
#include <random>
#include "gtest/gtest.h"
// Filter constants
static constexpr double kFilterStep = 0.005;
static constexpr double kFilterTime = 2.0;
static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
static constexpr double kSinglePoleIIRExpectedOutput = -3.2172003;
static constexpr double kHighPassTimeConstant = 0.006631;
static constexpr double kHighPassExpectedOutput = 10.074717;
static constexpr int32_t kMovAvgTaps = 6;
static constexpr double kMovAvgExpectedOutput = -10.191644;
enum LinearFilterOutputTestType {
TEST_SINGLE_POLE_IIR,
TEST_HIGH_PASS,
TEST_MOVAVG,
TEST_PULSE
};
std::ostream& operator<<(std::ostream& os,
const LinearFilterOutputTestType& type) {
switch (type) {
case TEST_SINGLE_POLE_IIR:
os << "LinearFilter SinglePoleIIR";
break;
case TEST_HIGH_PASS:
os << "LinearFilter HighPass";
break;
case TEST_MOVAVG:
os << "LinearFilter MovingAverage";
break;
case TEST_PULSE:
os << "LinearFilter Pulse";
break;
}
return os;
}
static double GetData(double t) {
constexpr double kPi = 3.14159265358979323846;
return 100.0 * std::sin(2.0 * kPi * t) + 20.0 * std::cos(50.0 * kPi * t);
}
static double GetPulseData(double t) {
if (std::abs(t - 1.0) < 0.001) {
return 1.0;
} else {
return 0.0;
}
}
/**
* A fixture that includes a consistent data source wrapped in a filter
*/
class LinearFilterOutputTest
: public testing::TestWithParam<LinearFilterOutputTestType> {
protected:
std::unique_ptr<frc::LinearFilter> m_filter;
std::function<double(double)> m_data;
double m_expectedOutput = 0.0;
void SetUp() override {
switch (GetParam()) {
case TEST_SINGLE_POLE_IIR: {
m_filter = std::make_unique<frc::LinearFilter>(
frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant,
kFilterStep));
m_data = GetData;
m_expectedOutput = kSinglePoleIIRExpectedOutput;
break;
}
case TEST_HIGH_PASS: {
m_filter = std::make_unique<frc::LinearFilter>(
frc::LinearFilter::HighPass(kHighPassTimeConstant, kFilterStep));
m_data = GetData;
m_expectedOutput = kHighPassExpectedOutput;
break;
}
case TEST_MOVAVG: {
m_filter = std::make_unique<frc::LinearFilter>(
frc::LinearFilter::MovingAverage(kMovAvgTaps));
m_data = GetData;
m_expectedOutput = kMovAvgExpectedOutput;
break;
}
case TEST_PULSE: {
m_filter = std::make_unique<frc::LinearFilter>(
frc::LinearFilter::MovingAverage(kMovAvgTaps));
m_data = GetPulseData;
m_expectedOutput = 0.0;
break;
}
}
}
};
/**
* Test if the linear filters produce consistent output for a given data set.
*/
TEST_P(LinearFilterOutputTest, Output) {
double filterOutput = 0.0;
for (double t = 0.0; t < kFilterTime; t += kFilterStep) {
filterOutput = m_filter->Calculate(m_data(t));
}
RecordProperty("LinearFilterOutput", filterOutput);
EXPECT_FLOAT_EQ(m_expectedOutput, filterOutput)
<< "Filter output didn't match expected value";
}
INSTANTIATE_TEST_SUITE_P(Test, LinearFilterOutputTest,
testing::Values(TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS,
TEST_MOVAVG, TEST_PULSE));