Added linear digital filters

Linear digital filter class based on code from FRC team 341

Change-Id: I4c5198e36a089e08a6d054bf1bf80392def27e23

wpilibcIntegrationTests/src/CircularBufferTest.cpp

@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015. 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 <CircularBuffer.h>
+#include "gtest/gtest.h"
+#include <array>
+
+static const std::array<double, 10> values = {751.848, 766.366, 342.657,
+                                              234.252, 716.126, 132.344,
+                                              445.697, 22.727, 421.125,
+                                              799.913};
+
+static const std::array<double, 8> pushFrontOut = {799.913, 421.125, 22.727,
+                                                   445.697, 132.344, 716.126,
+                                                   234.252, 342.657};
+
+static const std::array<double, 8> pushBackOut = {342.657, 234.252, 716.126,
+                                                  132.344, 445.697, 22.727,
+                                                  421.125, 799.913};
+
+TEST(CircularBufferTest, PushFrontTest) {
+  CircularBuffer<double> queue(8);
+
+  for (auto& value : values) {
+    queue.PushFront(value);
+  }
+
+  for (unsigned int i = 0; i < pushFrontOut.size(); i++) {
+    EXPECT_EQ(pushFrontOut[i], queue[i]);
+  }
+}
+
+TEST(CircularBufferTest, PushBackTest) {
+  CircularBuffer<double> queue(8);
+
+  for (auto& value : values) {
+    queue.PushBack(value);
+  }
+
+  for (unsigned int i = 0; i < pushBackOut.size(); i++) {
+    EXPECT_EQ(pushBackOut[i], queue[i]);
+  }
+}
+
+TEST(CircularBufferTest, PushPopTest) {
+  CircularBuffer<double> queue(3);
+
+  // Insert three elements into the buffer
+  queue.PushBack(1.0);
+  queue.PushBack(2.0);
+  queue.PushBack(3.0);
+
+  EXPECT_EQ(1.0, queue[0]);
+  EXPECT_EQ(2.0, queue[1]);
+  EXPECT_EQ(3.0, queue[2]);
+
+  /*
+   * The buffer is full now, so pushing subsequent elements will overwrite the
+   * front-most elements.
+   */
+
+  queue.PushBack(4.0); // Overwrite 1 with 4
+
+  // The buffer now contains 2, 3 and 4
+  EXPECT_EQ(2.0, queue[0]);
+  EXPECT_EQ(3.0, queue[1]);
+  EXPECT_EQ(4.0, queue[2]);
+
+  queue.PushBack(5.0); // Overwrite 2 with 5
+
+  // The buffer now contains 3, 4 and 5
+  EXPECT_EQ(3.0, queue[0]);
+  EXPECT_EQ(4.0, queue[1]);
+  EXPECT_EQ(5.0, queue[2]);
+
+  EXPECT_EQ(5.0, queue.PopBack()); // 5 is removed
+
+  // The buffer now contains 3 and 4
+  EXPECT_EQ(3.0, queue[0]);
+  EXPECT_EQ(4.0, queue[1]);
+
+  EXPECT_EQ(3.0, queue.PopFront()); // 3 is removed
+
+  // Leaving only one element with value == 4
+  EXPECT_EQ(4.0, queue[0]);
+}

wpilibcIntegrationTests/src/FilterNoiseTest.cpp

@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015. 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 <functional>
+#include <memory>
+#include <random>
+#include <thread>
+#include <cmath>
+
+#include <Filters/LinearDigitalFilter.h>
+
+#include "gtest/gtest.h"
+#include "TestBench.h"
+#include "Base.h"
+
+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;
+}
+
+using std::chrono::system_clock;
+
+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 += TestBench::kFilterStep;
+    return m_dataFunc(m_count) + m_noise;
+  }
+
+  void Reset() {
+    m_count = -TestBench::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 = -TestBench::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 * M_PI * 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,
+            TestBench::kSinglePoleIIRTimeConstant,
+            TestBench::kFilterStep));
+        break;
+      }
+
+      case TEST_MOVAVG: {
+        m_filter = std::make_unique<LinearDigitalFilter>(LinearDigitalFilter::MovingAverage(m_noise,
+            TestBench::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 < TestBench::kFilterTime; t += TestBench::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_CASE_P(Test, FilterNoiseTest,
+                        testing::Values(TEST_SINGLE_POLE_IIR, TEST_MOVAVG));

wpilibcIntegrationTests/src/FilterOutputTest.cpp

@@ -0,0 +1,124 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015. 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 <functional>
+#include <memory>
+#include <random>
+#include <thread>
+#include <cmath>
+
+#include <Filters/LinearDigitalFilter.h>
+
+#include "gtest/gtest.h"
+#include "TestBench.h"
+#include "Base.h"
+
+enum FilterOutputTestType { TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS, TEST_MOVAVG };
+
+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;
+  }
+
+  return os;
+}
+
+class DataWrapper : public PIDSource {
+ public:
+  DataWrapper(double (*dataFunc)(double)) {
+    m_dataFunc = dataFunc;
+  }
+
+  virtual void SetPIDSourceType(PIDSourceType pidSource) {}
+
+  virtual double PIDGet() {
+    m_count += TestBench::kFilterStep;
+    return m_dataFunc(m_count);
+  }
+
+  void Reset() {
+    m_count = -TestBench::kFilterStep;
+  }
+
+ private:
+  std::function<double(double)> m_dataFunc;
+
+  // Make sure first call to PIDGet() uses m_count == 0
+  double m_count = -TestBench::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 * M_PI * t) + 20.0 * std::cos(50.0 * M_PI * t);
+  }
+
+  void SetUp() override {
+    m_data = std::make_shared<DataWrapper>(GetData);
+
+    switch (GetParam()) {
+      case TEST_SINGLE_POLE_IIR: {
+        m_filter = std::make_unique<LinearDigitalFilter>(LinearDigitalFilter::SinglePoleIIR(m_data,
+                                                                                            TestBench::kSinglePoleIIRTimeConstant,
+                                                                                            TestBench::kFilterStep));
+        m_expectedOutput = TestBench::kSinglePoleIIRExpectedOutput;
+        break;
+      }
+
+      case TEST_HIGH_PASS: {
+        m_filter = std::make_unique<LinearDigitalFilter>(LinearDigitalFilter::HighPass(m_data,
+                                                                                       TestBench::kHighPassTimeConstant,
+                                                                                       TestBench::kFilterStep));
+        m_expectedOutput = TestBench::kHighPassExpectedOutput;
+        break;
+      }
+
+      case TEST_MOVAVG: {
+        m_filter = std::make_unique<LinearDigitalFilter>(LinearDigitalFilter::MovingAverage(m_data,
+                                                                                            TestBench::kMovAvgTaps));
+        m_expectedOutput = TestBench::kMovAvgExpectedOutput;
+        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 < TestBench::kFilterTime; t += TestBench::kFilterStep) {
+    filterOutput = m_filter->PIDGet();
+  }
+
+  RecordProperty("FilterOutput", filterOutput);
+
+  EXPECT_FLOAT_EQ(m_expectedOutput, filterOutput)
+      << "Filter output didn't match expected value";
+}
+
+INSTANTIATE_TEST_CASE_P(Test, FilterOutputTest,
+                        testing::Values(TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS,
+                                        TEST_MOVAVG));