Added linear digital filters

Linear digital filter class based on code from FRC team 341

Change-Id: I4c5198e36a089e08a6d054bf1bf80392def27e23

wpilibc/Athena/include/WPILib.h

@@ -50,6 +50,7 @@
 #include "DriverStation.h"
 #include "Encoder.h"
 #include "ErrorBase.h"
+#include "Filters/LinearDigitalFilter.h"
 #include "GearTooth.h"
 #include "GenericHID.h"
 #include "interfaces/Accelerometer.h"

wpilibc/shared/include/CircularBuffer.h

@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <vector>
+#include <cstddef>
+
+/**
+ * This is a simple circular buffer so we don't need to "bucket brigade" copy
+ * old values.
+ */
+template <class T>
+class CircularBuffer {
+ public:
+  CircularBuffer(size_t size);
+
+  void PushFront(T value);
+  void PushBack(T value);
+  T PopFront();
+  T PopBack();
+  void Reset();
+
+  T& operator[](size_t index);
+  const T& operator[](size_t index) const;
+
+ private:
+  std::vector<T> m_data;
+
+  // Index of element at front of buffer
+  size_t m_front = 0;
+
+  // Number of elements used in buffer
+  size_t m_length = 0;
+
+  size_t ModuloInc(size_t index);
+  size_t ModuloDec(size_t index);
+};
+
+#include "CircularBuffer.inc"

wpilibc/shared/include/CircularBuffer.inc

@@ -0,0 +1,123 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <algorithm>
+
+template <class T>
+CircularBuffer<T>::CircularBuffer(size_t size) : m_data(size, 0) {}
+
+/**
+ * Push new value onto front of the buffer. The value at the back is overwritten
+ * if the buffer is full.
+ */
+template <class T>
+void CircularBuffer<T>::PushFront(T value) {
+  if (m_data.size() == 0) {
+    return;
+  }
+
+  m_front = ModuloDec(m_front);
+
+  m_data[m_front] = value;
+
+  if (m_length < m_data.size()) {
+    m_length++;
+  }
+}
+
+/**
+ * Push new value onto back of the buffer. The value at the front is overwritten
+ * if the buffer is full.
+ */
+template <class T>
+void CircularBuffer<T>::PushBack(T value) {
+  if (m_data.size() == 0) {
+    return;
+  }
+
+  m_data[(m_front + m_length) % m_data.size()] = value;
+
+  if (m_length < m_data.size()) {
+    m_length++;
+  } else {
+    // Increment front if buffer is full to maintain size
+    m_front = ModuloInc(m_front);
+  }
+}
+
+/**
+ * Pop value at front of buffer.
+ */
+template <class T>
+T CircularBuffer<T>::PopFront() {
+  // If there are no elements in the buffer, do nothing
+  if (m_length == 0) {
+    return 0;
+  }
+
+  T& temp = m_data[m_front];
+  m_front = ModuloInc(m_front);
+  m_length--;
+  return temp;
+}
+
+/**
+ * Pop value at back of buffer.
+ */
+template <class T>
+T CircularBuffer<T>::PopBack() {
+  // If there are no elements in the buffer, do nothing
+  if (m_length == 0) {
+    return 0;
+  }
+
+  m_length--;
+  return m_data[(m_front + m_length) % m_data.size()];
+}
+
+template <class T>
+void CircularBuffer<T>::Reset() {
+  std::fill(m_data.begin(), m_data.end(), 0);
+  m_front = 0;
+  m_length = 0;
+}
+
+/**
+ * Returns element at index starting from front of buffer.
+ */
+template <class T>
+T& CircularBuffer<T>::operator[](size_t index) {
+  return m_data[(m_front + index) % m_data.size()];
+}
+
+/**
+ * Returns element at index starting from front of buffer.
+ */
+template <class T>
+const T& CircularBuffer<T>::operator[](size_t index) const {
+  return m_data[(m_front + index) % m_data.size()];
+}
+
+/**
+ * Increment an index modulo the length of the m_data buffer
+ */
+template <class T>
+size_t CircularBuffer<T>::ModuloInc(size_t index) {
+  return (index + 1) % m_data.size();
+}
+
+/**
+ * Decrement an index modulo the length of the m_data buffer
+ */
+template <class T>
+size_t CircularBuffer<T>::ModuloDec(size_t index) {
+  if (index == 0) {
+    return m_data.size() - 1;
+  } else {
+    return index - 1;
+  }
+}

wpilibc/shared/include/Filters/Filter.h

@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include "PIDSource.h"
+
+/**
+ * Interface for filters
+ */
+class Filter : public PIDSource {
+ public:
+  Filter(std::shared_ptr<PIDSource> source);
+  virtual ~Filter() = default;
+
+  // PIDSource interface
+  virtual void SetPIDSourceType(PIDSourceType pidSource) override;
+  PIDSourceType GetPIDSourceType() const;
+  virtual double PIDGet() override = 0;
+
+  /**
+   * Returns the current filter estimate without also inserting new data as
+   * PIDGet() would do.
+   *
+   * @return The current filter estimate
+   */
+  virtual double Get() const = 0;
+
+  /**
+   * Reset the filter state
+   */
+  virtual void Reset() = 0;
+
+ protected:
+  /**
+   * Calls PIDGet() of source
+   *
+   * @return Current value of source
+   */
+  double PIDGetSource();
+
+ private:
+  std::shared_ptr<PIDSource> m_source;
+};

wpilibc/shared/include/Filters/LinearDigitalFilter.h

@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <initializer_list>
+#include <memory>
+#include <vector>
+#include "Filter.h"
+#include "CircularBuffer.h"
+
+/**
+ * This class implements a linear, digital filter. All types of FIR and IIR
+ * filters are supported. Static factory methods are provided to create commonly
+ * used types of filters.
+ *
+ * Filters are of the form:
+ *  y[n] = (b0*x[n] + b1*x[n-1] + ... + bP*x[n-P) - (a0*y[n-1] + a2*y[n-2] + ... + aQ*y[n-Q])
+ *
+ * Where:
+ *  y[n] is the output at time "n"
+ *  x[n] is the input at time "n"
+ *  y[n-1] is the output from the LAST time step ("n-1")
+ *  x[n-1] is the input from the LAST time step ("n-1")
+ *  b0...bP are the "feedforward" (FIR) gains
+ *  a0...aQ are the "feedback" (IIR) gains
+ * IMPORTANT! Note the "-" sign in front of the feedback term! This is a common
+ *            convention in signal processing.
+ *
+ * What can linear filters do? Basically, they can filter, or diminish, the
+ * effects of undesirable input frequencies. High frequencies, or rapid changes,
+ * can be indicative of sensor noise or be otherwise undesirable. A "low pass"
+ * filter smooths out the signal, reducing the impact of these high frequency
+ * components.  Likewise, a "high pass" filter gets rid of slow-moving signal
+ * components, letting you detect large changes more easily.
+ *
+ * Example FRC applications of filters:
+ *  - Getting rid of noise from an analog sensor input (note: the roboRIO's FPGA
+ *    can do this faster in hardware)
+ *  - Smoothing out joystick input to prevent the wheels from slipping or the
+ *    robot from tipping
+ *  - Smoothing motor commands so that unnecessary strain isn't put on
+ *    electrical or mechanical components
+ *  - If you use clever gains, you can make a PID controller out of this class!
+ *
+ * For more on filters, I highly recommend the following articles:
+ *  http://en.wikipedia.org/wiki/Linear_filter
+ *  http://en.wikipedia.org/wiki/Iir_filter
+ *  http://en.wikipedia.org/wiki/Fir_filter
+ *
+ * Note 1: PIDGet() should be called by the user on a known, regular period.
+ * You can set up a Notifier to do this (look at the WPILib PIDController
+ * class), or do it "inline" with code in a periodic function.
+ *
+ * Note 2: For ALL filters, gains are necessarily a function of frequency. If
+ * you make a filter that works well for you at, say, 100Hz, you will most
+ * definitely need to adjust the gains if you then want to run it at 200Hz!
+ * Combining this with Note 1 - the impetus is on YOU as a developer to make
+ * sure PIDGet() gets called at the desired, constant frequency!
+ */
+class LinearDigitalFilter : public Filter {
+ public:
+  LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+                      std::initializer_list<double> ffGains,
+                      std::initializer_list<double> fbGains);
+  LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+                      std::initializer_list<double> ffGains,
+                      const std::vector<double>& fbGains);
+  LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+                      const std::vector<double>& ffGains,
+                      std::initializer_list<double> fbGains);
+  LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+                      const std::vector<double>& ffGains,
+                      const std::vector<double>& fbGains);
+
+  // Static methods to create commonly used filters
+  static LinearDigitalFilter SinglePoleIIR(std::shared_ptr<PIDSource> source,
+                                           double timeConstant, double period);
+  static LinearDigitalFilter HighPass(std::shared_ptr<PIDSource> source,
+                                      double timeConstant, double period);
+  static LinearDigitalFilter MovingAverage(std::shared_ptr<PIDSource> source,
+                                           unsigned int taps);
+
+  // Filter interface
+  double Get() const override;
+  void Reset() override;
+
+  // PIDSource interface
+  double PIDGet() override;
+
+ private:
+  CircularBuffer<double> m_inputs;
+  CircularBuffer<double> m_outputs;
+  std::vector<double> m_inputGains;
+  std::vector<double> m_outputGains;
+};

wpilibc/shared/src/Filters/Filter.cpp

@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "Filters/Filter.h"
+
+Filter::Filter(std::shared_ptr<PIDSource> source) {
+  m_source = source;
+}
+
+void Filter::SetPIDSourceType(PIDSourceType pidSource) {
+  m_source->SetPIDSourceType(pidSource);
+}
+
+PIDSourceType Filter::GetPIDSourceType() const {
+  return m_source->GetPIDSourceType();
+}
+
+double Filter::PIDGetSource() {
+  return m_source->PIDGet();
+}

wpilibc/shared/src/Filters/LinearDigitalFilter.cpp

@@ -0,0 +1,166 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "Filters/LinearDigitalFilter.h"
+#include <cassert>
+#include <cmath>
+
+/**
+ * Create a linear FIR or IIR filter
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param ffGains The "feed forward" or FIR gains
+ * @param fbGains The "feed back" or IIR gains
+ */
+LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+                                         std::initializer_list<double> ffGains,
+                                         std::initializer_list<double> fbGains) :
+    Filter(source), m_inputs(ffGains.size()), m_outputs(fbGains.size()),
+    m_inputGains(ffGains), m_outputGains(fbGains) {}
+
+/**
+ * Create a linear FIR or IIR filter
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param ffGains The "feed forward" or FIR gains
+ * @param fbGains The "feed back" or IIR gains
+ */
+LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+                                         std::initializer_list<double> ffGains,
+                                         const std::vector<double>& fbGains) :
+    Filter(source), m_inputs(ffGains.size()), m_outputs(fbGains.size()),
+    m_inputGains(ffGains), m_outputGains(fbGains) {}
+
+/**
+ * Create a linear FIR or IIR filter
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param ffGains The "feed forward" or FIR gains
+ * @param fbGains The "feed back" or IIR gains
+ */
+LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+                                         const std::vector<double>& ffGains,
+                                         std::initializer_list<double> fbGains) :
+    Filter(source), m_inputs(ffGains.size()), m_outputs(fbGains.size()),
+    m_inputGains(ffGains), m_outputGains(fbGains) {}
+
+/**
+ * Create a linear FIR or IIR filter
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param ffGains The "feed forward" or FIR gains
+ * @param fbGains The "feed back" or IIR gains
+ */
+LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+                                         const std::vector<double>& ffGains,
+                                         const std::vector<double>& fbGains) :
+    Filter(source), m_inputs(ffGains.size()), m_outputs(fbGains.size()),
+    m_inputGains(ffGains), m_outputGains(fbGains) {}
+
+/**
+ * Creates a one-pole IIR low-pass filter of the form:
+ *   y[n] = (1-gain)*x[n] + gain*y[n-1]
+ * where gain = e^(-dt / T), T is the time constant in seconds
+ *
+ * This filter is stable for time constants greater than zero
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param timeConstant The discrete-time time constant in seconds
+ * @param period The period in seconds between samples taken by the user
+ */
+LinearDigitalFilter LinearDigitalFilter::SinglePoleIIR(std::shared_ptr<PIDSource> source,
+                                                       double timeConstant,
+                                                       double period) {
+  double gain = std::exp(-period / timeConstant);
+  return LinearDigitalFilter(std::move(source), {1.0 - gain}, {-gain});
+}
+
+/**
+ * Creates a first-order high-pass filter of the form:
+ *   y[n] = gain*x[n] + (-gain)*x[n-1] + gain*y[n-1]
+ * where gain = e^(-dt / T), T is the time constant in seconds
+ *
+ * This filter is stable for time constants greater than zero
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param timeConstant The discrete-time time constant in seconds
+ * @param period The period in seconds between samples taken by the user
+ */
+LinearDigitalFilter LinearDigitalFilter::HighPass(std::shared_ptr<PIDSource> source,
+                                                  double timeConstant,
+                                                  double period) {
+  double gain = std::exp(-period / timeConstant);
+  return LinearDigitalFilter(std::move(source), {gain, -gain}, {-gain});
+}
+
+/**
+ * Creates a K-tap FIR moving average filter of the form:
+ *   y[n] = 1/k * (x[k] + x[k-1] + ... + x[0])
+ *
+ * This filter is always stable.
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param taps The number of samples to average over. Higher = smoother but
+ *        slower
+ */
+LinearDigitalFilter LinearDigitalFilter::MovingAverage(std::shared_ptr<PIDSource> source,
+                                                       unsigned int taps) {
+  assert(taps > 0);
+
+  std::vector<double> gains(taps, 1.0 / taps);
+  return LinearDigitalFilter(std::move(source), gains, {});
+}
+
+/**
+ * {@inheritDoc}
+ */
+double LinearDigitalFilter::Get() const {
+  double retVal = 0.0;
+
+  // Calculate the new value
+  for (unsigned int i = 0; i < m_inputGains.size(); i++) {
+    retVal += m_inputs[i] * m_inputGains[i];
+  }
+  for (unsigned int i = 0; i < m_outputGains.size(); i++) {
+    retVal -= m_outputs[i] * m_outputGains[i];
+  }
+
+  return retVal;
+}
+
+/**
+ * {@inheritDoc}
+ */
+void LinearDigitalFilter::Reset() {
+  m_inputs.Reset();
+  m_outputs.Reset();
+}
+
+/**
+ * Calculates the next value of the filter
+ *
+ * @return The filtered value at this step
+ */
+double LinearDigitalFilter::PIDGet() {
+  double retVal = 0.0;
+
+  // Rotate the inputs
+  m_inputs.PushFront(PIDGetSource());
+
+  // Calculate the new value
+  for (unsigned int i = 0; i < m_inputGains.size(); i++) {
+    retVal += m_inputs[i] * m_inputGains[i];
+  }
+  for (unsigned int i = 0; i < m_outputGains.size(); i++) {
+    retVal -= m_outputs[i] * m_outputGains[i];
+  }
+
+  // Rotate the outputs
+  m_outputs.PushFront(retVal);
+
+  return retVal;
+}

wpilibcIntegrationTests/include/TestBench.h

@@ -63,4 +63,14 @@ class TestBench {
   /* PCM channels */
   static const int32_t kSolenoidChannel1 = 0;
   static const int32_t kSolenoidChannel2 = 1;
+
+  /* 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 int kMovAvgTaps = 6;
+  static constexpr double kMovAvgExpectedOutput = -10.191644;
 };

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));

wpilibj/src/shared/java/edu/wpi/first/wpilibj/CircularBuffer.java

@@ -0,0 +1,125 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj;
+
+/**
+ * This is a simple circular buffer so we don't need to "bucket brigade" copy
+ * old values.
+ */
+public class CircularBuffer {
+  private double[] m_data;
+  private int m_front = 0;
+  private int m_length = 0;
+
+  public CircularBuffer(int size) {
+    m_data = new double[size];
+    for (double i : m_data) {
+      i = 0.0;
+    }
+  }
+
+  /**
+   * Push new value onto front of the buffer. The value at the back is
+   * overwritten if the buffer is full.
+   */
+  public void pushFront(double value) {
+    if (m_data.length == 0) {
+      return;
+    }
+
+    m_front = moduloDec(m_front);
+
+    m_data[m_front] = value;
+
+    if (m_length < m_data.length) {
+      m_length++;
+    }
+  }
+
+  /**
+   * Push new value onto back of the buffer. The value at the front is
+   * overwritten if the buffer is full.
+   */
+  public void pushBack(double value) {
+    if (m_data.length == 0) {
+      return;
+    }
+
+    m_data[(m_front + m_length) % m_data.length] = value;
+
+    if (m_length < m_data.length) {
+      m_length++;
+    } else {
+      // Increment front if buffer is full to maintain size
+      m_front = moduloInc(m_front);
+    }
+  }
+
+  /**
+   * Pop value at front of buffer.
+   *
+   * @return value at front of buffer
+   */
+  public double popFront() {
+    // If there are no elements in the buffer, do nothing
+    if (m_length == 0) {
+      return 0.0;
+    }
+
+    double temp = m_data[m_front];
+    m_front = moduloInc(m_front);
+    m_length--;
+    return temp;
+  }
+
+
+  /**
+   * Pop value at back of buffer.
+   */
+  public double popBack() {
+    // If there are no elements in the buffer, do nothing
+    if (m_length == 0) {
+      return 0.0;
+    }
+
+    m_length--;
+    return m_data[(m_front + m_length) % m_data.length];
+  }
+
+  public void reset() {
+    for (double i : m_data) {
+      i = 0.0;
+    }
+    m_front = 0;
+    m_length = 0;
+  }
+
+  /**
+   * @return element at index starting from front of buffer.
+   */
+  public double get(int index) {
+    return m_data[(m_front + index) % m_data.length];
+  }
+
+  /**
+   * Increment an index modulo the length of the m_data buffer
+   */
+  private int moduloInc(int index) {
+    return (index + 1) % m_data.length;
+  }
+
+  /**
+   * Decrement an index modulo the length of the m_data buffer
+   */
+  private int moduloDec(int index) {
+    if (index == 0) {
+      return m_data.length - 1;
+    } else {
+      return index - 1;
+    }
+  }
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/filters/Filter.java

@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj.filters;
+
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+
+/**
+ * Superclass for filters
+ */
+public abstract class Filter implements PIDSource {
+  private PIDSource m_source;
+
+  public Filter(PIDSource source) {
+    m_source = source;
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  @Override
+  public void setPIDSourceType(PIDSourceType pidSource) {
+    m_source.setPIDSourceType(pidSource);
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  public PIDSourceType getPIDSourceType() {
+    return m_source.getPIDSourceType();
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  @Override
+  public abstract double pidGet();
+
+  /**
+   * Returns the current filter estimate without also inserting new data as
+   * pidGet() would do.
+   *
+   * @return The current filter estimate
+   */
+  public abstract double get();
+
+  /**
+   * Reset the filter state
+   */
+  public abstract void reset();
+
+  /**
+   * Calls PIDGet() of source
+   *
+   * @return Current value of source
+   */
+  protected double pidGetSource() {
+    return m_source.pidGet();
+  }
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java

@@ -0,0 +1,204 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj.filters;
+
+import edu.wpi.first.wpilibj.filters.Filter;
+import edu.wpi.first.wpilibj.CircularBuffer;
+import edu.wpi.first.wpilibj.PIDSource;
+
+/**
+ * This class implements a linear, digital filter. All types of FIR and IIR
+ * filters are supported. Static factory methods are provided to create commonly
+ * used types of filters.
+ *
+ * Filters are of the form:
+ *  y[n] = (b0*x[n] + b1*x[n-1] + ... + bP*x[n-P) - (a0*y[n-1] + a2*y[n-2] + ... + aQ*y[n-Q])
+ *
+ * Where:
+ *  y[n] is the output at time "n"
+ *  x[n] is the input at time "n"
+ *  y[n-1] is the output from the LAST time step ("n-1")
+ *  x[n-1] is the input from the LAST time step ("n-1")
+ *  b0...bP are the "feedforward" (FIR) gains
+ *  a0...aQ are the "feedback" (IIR) gains
+ * IMPORTANT! Note the "-" sign in front of the feedback term! This is a common
+ *            convention in signal processing.
+ *
+ * What can linear filters do? Basically, they can filter, or diminish, the
+ * effects of undesirable input frequencies. High frequencies, or rapid changes,
+ * can be indicative of sensor noise or be otherwise undesirable. A "low pass"
+ * filter smooths out the signal, reducing the impact of these high frequency
+ * components.  Likewise, a "high pass" filter gets rid of slow-moving signal
+ * components, letting you detect large changes more easily.
+ *
+ * Example FRC applications of filters:
+ *  - Getting rid of noise from an analog sensor input (note: the roboRIO's FPGA
+ *    can do this faster in hardware)
+ *  - Smoothing out joystick input to prevent the wheels from slipping or the
+ *    robot from tipping
+ *  - Smoothing motor commands so that unnecessary strain isn't put on
+ *    electrical or mechanical components
+ *  - If you use clever gains, you can make a PID controller out of this class!
+ *
+ * For more on filters, I highly recommend the following articles:
+ *  http://en.wikipedia.org/wiki/Linear_filter
+ *  http://en.wikipedia.org/wiki/Iir_filter
+ *  http://en.wikipedia.org/wiki/Fir_filter
+ *
+ * Note 1: PIDGet() should be called by the user on a known, regular period.
+ * You can set up a Notifier to do this (look at the WPILib PIDController
+ * class), or do it "inline" with code in a periodic function.
+ *
+ * Note 2: For ALL filters, gains are necessarily a function of frequency. If
+ * you make a filter that works well for you at, say, 100Hz, you will most
+ * definitely need to adjust the gains if you then want to run it at 200Hz!
+ * Combining this with Note 1 - the impetus is on YOU as a developer to make
+ * sure PIDGet() gets called at the desired, constant frequency!
+ */
+public class LinearDigitalFilter extends Filter {
+  private CircularBuffer m_inputs;
+  private CircularBuffer m_outputs;
+  private double[] m_inputGains;
+  private double[] m_outputGains;
+
+  /**
+   * Create a linear FIR or IIR filter
+   *
+   * @param source The PIDSource object that is used to get values
+   * @param ffGains The "feed forward" or FIR gains
+   * @param fbGains The "feed back" or IIR gains
+   */
+  public LinearDigitalFilter(PIDSource source, double[] ffGains,
+                             double[] fbGains) {
+    super(source);
+    m_inputs = new CircularBuffer(ffGains.length);
+    m_outputs = new CircularBuffer(fbGains.length);
+    m_inputGains = ffGains;
+    m_outputGains = fbGains;
+  }
+
+  /**
+   * Creates a one-pole IIR low-pass filter of the form:
+   *   y[n] = (1-gain)*x[n] + gain*y[n-1]
+   * where gain = e^(-dt / T), T is the time constant in seconds
+   *
+   * This filter is stable for time constants greater than zero
+   *
+   * @param source The PIDSource object that is used to get values
+   * @param timeConstant The discrete-time time constant in seconds
+   * @param period The period in seconds between samples taken by the user
+   */
+  public static LinearDigitalFilter singlePoleIIR(PIDSource source,
+                                                  double timeConstant,
+                                                  double period) {
+    double gain = Math.exp(-period / timeConstant);
+    double[] ffGains = {1.0 - gain};
+    double[] fbGains = {-gain};
+
+    return new LinearDigitalFilter(source, ffGains, fbGains);
+  }
+
+  /**
+   * Creates a first-order high-pass filter of the form:
+   *   y[n] = gain*x[n] + (-gain)*x[n-1] + gain*y[n-1]
+   * where gain = e^(-dt / T), T is the time constant in seconds
+   *
+   * This filter is stable for time constants greater than zero
+   *
+   * @param source The PIDSource object that is used to get values
+   * @param timeConstant The discrete-time time constant in seconds
+   * @param period The period in seconds between samples taken by the user
+   */
+  public static LinearDigitalFilter highPass(PIDSource source,
+                                             double timeConstant,
+                                             double period) {
+    double gain = Math.exp(-period / timeConstant);
+    double[] ffGains = {gain, -gain};
+    double[] fbGains = {-gain};
+
+    return new LinearDigitalFilter(source, ffGains, fbGains);
+  }
+
+  /**
+   * Creates a K-tap FIR moving average filter of the form:
+   *   y[n] = 1/k * (x[k] + x[k-1] + ... + x[0])
+   *
+   * This filter is always stable.
+   *
+   * @param source The PIDSource object that is used to get values
+   * @param taps The number of samples to average over. Higher = smoother but
+   *        slower
+   * @throws IllegalArgumentException if number of taps is less than 1
+   */
+  public static LinearDigitalFilter movingAverage(PIDSource source, int taps) {
+    if (taps <= 0) {
+      throw new IllegalArgumentException("Number of taps was not at least 1");
+    }
+
+    double[] ffGains = new double[taps];
+    for (int i = 0; i < ffGains.length; i++) {
+      ffGains[i] = 1.0 / taps;
+    }
+
+    double[] fbGains = new double[0];
+
+    return new LinearDigitalFilter(source, ffGains, fbGains);
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  @Override
+  public double get() {
+    double retVal = 0.0;
+
+    // Calculate the new value
+    for (int i = 0; i < m_inputGains.length; i++) {
+      retVal += m_inputs.get(i) * m_inputGains[i];
+    }
+    for (int i = 0; i < m_outputGains.length; i++) {
+      retVal -= m_outputs.get(i) * m_outputGains[i];
+    }
+
+    return retVal;
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  @Override
+  public void reset() {
+     m_inputs.reset();
+     m_outputs.reset();
+  }
+
+  /**
+   * Calculates the next value of the filter
+   *
+   * @return The filtered value at this step
+   */
+  @Override
+  public double pidGet() {
+    double retVal = 0.0;
+
+    // Rotate the inputs
+    m_inputs.pushFront(pidGetSource());
+
+    // Calculate the new value
+    for (int i = 0; i < m_inputGains.length; i++) {
+      retVal += m_inputs.get(i) * m_inputGains[i];
+    }
+    for (int i = 0; i < m_outputGains.length; i++) {
+      retVal -= m_outputs.get(i) * m_outputGains[i];
+    }
+
+    // Rotate the outputs
+    m_outputs.pushFront(retVal);
+
+    return retVal;
+  }
+}

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CircularBufferTest.java

@@ -0,0 +1,101 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2014. 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. */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj;
+
+import static org.junit.Assert.assertEquals;
+
+import java.util.logging.Logger;
+
+import org.junit.Test;
+
+import edu.wpi.first.wpilibj.CircularBuffer;
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+
+public class CircularBufferTest extends AbstractComsSetup {
+  private static final Logger logger = Logger.getLogger(CircularBufferTest.class.getName());
+  private double[] values = {751.848, 766.366, 342.657, 234.252, 716.126,
+                             132.344, 445.697, 22.727, 421.125, 799.913};
+  private double[] pushFrontOut = {799.913, 421.125, 22.727, 445.697, 132.344,
+                              716.126, 234.252, 342.657};
+  private double[] pushBackOut = {342.657, 234.252, 716.126, 132.344, 445.697,
+                                  22.727, 421.125, 799.913};
+
+  @Test
+  public void pushFrontTest() {
+    CircularBuffer queue = new CircularBuffer(8);
+
+    for (double value : values) {
+      queue.pushFront(value);
+    }
+
+    for (int i = 0; i < pushFrontOut.length; i++) {
+      assertEquals(pushFrontOut[i], queue.get(i), 0.00005);
+    }
+  }
+
+  @Test
+  public void pushBackTest() {
+    CircularBuffer queue = new CircularBuffer(8);
+
+    for (double value : values) {
+      queue.pushBack(value);
+    }
+
+    for (int i = 0; i < pushBackOut.length; i++) {
+      assertEquals(pushBackOut[i], queue.get(i), 0.00005);
+    }
+  }
+
+  @Test
+  public void pushPopTest() {
+    CircularBuffer queue = new CircularBuffer(3);
+
+    // Insert three elements into the buffer
+    queue.pushBack(1.0);
+    queue.pushBack(2.0);
+    queue.pushBack(3.0);
+
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+    assertEquals(3.0, queue.get(2), 0.00005);
+
+    /*
+     * 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
+    assertEquals(2.0, queue.get(0), 0.00005);
+    assertEquals(3.0, queue.get(1), 0.00005);
+    assertEquals(4.0, queue.get(2), 0.00005);
+
+    queue.pushBack(5.0); // Overwrite 2 with 5
+
+    // The buffer now contains 3, 4, and 5
+    assertEquals(3.0, queue.get(0), 0.00005);
+    assertEquals(4.0, queue.get(1), 0.00005);
+    assertEquals(5.0, queue.get(2), 0.00005);
+
+    assertEquals(5.0, queue.popBack(), 0.00005); // 5 is removed
+
+    // The buffer now contains 3 and 4
+    assertEquals(3.0, queue.get(0), 0.00005);
+    assertEquals(4.0, queue.get(1), 0.00005);
+
+    assertEquals(3.0, queue.popFront(), 0.00005); // 3 is removed
+
+    // Leaving only one element with value == 4
+    assertEquals(4.0, queue.get(0), 0.00005);
+  }
+
+  @Override
+  protected Logger getClassLogger() {
+    return logger;
+  }
+}

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/FilterNoiseTest.java

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj;
+
+import static org.junit.Assert.assertTrue;
+
+import java.util.Arrays;
+import java.util.Collection;
+import java.util.logging.Logger;
+
+import org.junit.After;
+import org.junit.AfterClass;
+import org.junit.Before;
+import org.junit.Test;
+import org.junit.runner.RunWith;
+import org.junit.runners.Parameterized;
+import org.junit.runners.Parameterized.Parameters;
+
+import edu.wpi.first.wpilibj.fixtures.FilterNoiseFixture;
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import edu.wpi.first.wpilibj.test.TestBench;
+
+
+@RunWith(Parameterized.class)
+public class FilterNoiseTest extends AbstractComsSetup {
+  private static final Logger logger = Logger.getLogger(FilterNoiseTest.class.getName());
+
+  private static FilterNoiseFixture<?> me = null;
+
+  @Override
+  protected Logger getClassLogger() {
+    return logger;
+  }
+
+  public FilterNoiseTest(FilterNoiseFixture<?> mef) {
+    logger.fine("Constructor with: " + mef.getType());
+    if (me != null && !me.equals(mef))
+      me.teardown();
+    me = mef;
+  }
+
+  @Parameters(name = "{index}: {0}")
+  public static Collection<FilterNoiseFixture<?>[]> generateData() {
+    return Arrays.asList(new FilterNoiseFixture<?>[][] {
+                         {TestBench.getInstance().getSinglePoleIIRNoiseFixture()},
+                         {TestBench.getInstance().getMovAvgNoiseFixture()}});
+  }
+
+  @Before
+  public void setUp() {
+    me.setup();
+  }
+
+  @After
+  public void tearDown() throws Exception {
+    me.reset();
+  }
+
+  @AfterClass
+  public static void tearDownAfterClass() {
+    // Clean up the fixture after the test
+    me.teardown();
+    me = null;
+  }
+
+  /**
+   * Test if the filter reduces the noise produced by a signal generator
+   */
+  @Test
+  public void testNoiseReduce() {
+    double theoryData = 0.0;
+    double noiseGenError = 0.0;
+    double filterError = 0.0;
+
+    FilterNoiseFixture.NoiseGenerator noise = me.getNoiseGenerator();
+
+    noise.reset();
+    for (double t = 0; t < TestBench.kFilterTime; t += TestBench.kFilterStep) {
+      theoryData = noise.getData(t);
+      filterError += Math.abs(me.getFilter().pidGet() - theoryData);
+      noiseGenError += Math.abs(noise.get() - theoryData);
+    }
+
+    assertTrue(me.getType() + " should have reduced noise accumulation from " + noiseGenError + " but failed. The filter error was " + filterError, noiseGenError > filterError);
+  }
+}

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/FilterOutputTest.java

@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj;
+
+import static org.junit.Assert.assertEquals;
+
+import java.util.Arrays;
+import java.util.Collection;
+import java.util.logging.Logger;
+
+import org.junit.After;
+import org.junit.AfterClass;
+import org.junit.Before;
+import org.junit.Test;
+import org.junit.runner.RunWith;
+import org.junit.runners.Parameterized;
+import org.junit.runners.Parameterized.Parameters;
+
+import edu.wpi.first.wpilibj.fixtures.FilterOutputFixture;
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import edu.wpi.first.wpilibj.test.TestBench;
+
+
+@RunWith(Parameterized.class)
+public class FilterOutputTest extends AbstractComsSetup {
+  private static final Logger logger = Logger.getLogger(FilterOutputTest.class.getName());
+
+  private double expectedOutput;
+
+  private static FilterOutputFixture<?> me = null;
+
+  @Override
+  protected Logger getClassLogger() {
+    return logger;
+  }
+
+  public FilterOutputTest(FilterOutputFixture<?> mef) {
+    logger.fine("Constructor with: " + mef.getType());
+    if (me != null && !me.equals(mef))
+      me.teardown();
+    me = mef;
+    expectedOutput = me.getExpectedOutput();
+  }
+
+  @Parameters(name = "{index}: {0}")
+  public static Collection<FilterOutputFixture<?>[]> generateData() {
+    return Arrays.asList(new FilterOutputFixture<?>[][] {
+                         {TestBench.getInstance().getSinglePoleIIROutputFixture()},
+                         {TestBench.getInstance().getHighPassOutputFixture()},
+                         {TestBench.getInstance().getMovAvgOutputFixture()}});
+  }
+
+  @Before
+  public void setUp() {
+    me.setup();
+  }
+
+  @After
+  public void tearDown() throws Exception {
+    me.reset();
+  }
+
+  @AfterClass
+  public static void tearDownAfterClass() {
+    // Clean up the fixture after the test
+    me.teardown();
+    me = null;
+  }
+
+  /**
+   * Test if the filter produces consistent output for a given data set
+   */
+  @Test
+  public void testOutput() {
+    me.reset();
+
+    double filterOutput = 0.0;
+    for (double t = 0.0; t < TestBench.kFilterTime; t += TestBench.kFilterStep) {
+      filterOutput = me.getFilter().pidGet();
+    }
+
+    assertEquals(me.getType() + " output was incorrect.", expectedOutput, filterOutput, 0.00005);
+  }
+}

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java

@@ -19,10 +19,12 @@ import edu.wpi.first.wpilibj.test.AbstractTestSuite;
  */
 @RunWith(Suite.class)
 @SuiteClasses({AnalogCrossConnectTest.class, AnalogPotentiometerTest.class,
-    BuiltInAccelerometerTest.class, CANTalonTest.class, CounterTest.class,
-    DigitalGlitchFilterTest.class, DIOCrossConnectTest.class, EncoderTest.class,
-    GyroTest.class, MotorEncoderTest.class, MotorInvertingTest.class,
-    PCMTest.class, PDPTest.class, PIDTest.class, PreferencesTest.class,
-    RelayCrossConnectTest.class, SampleTest.class, TimerTest.class})
+    BuiltInAccelerometerTest.class, CANTalonTest.class,
+    CircularBufferTest.class, CounterTest.class, DigitalGlitchFilterTest.class,
+    DIOCrossConnectTest.class, EncoderTest.class, FilterNoiseTest.class,
+    FilterOutputTest.class, GyroTest.class, MotorEncoderTest.class,
+    MotorInvertingTest.class, PCMTest.class, PDPTest.class, PIDTest.class,
+    PreferencesTest.class, RelayCrossConnectTest.class, SampleTest.class,
+    TimerTest.class})
 public class WpiLibJTestSuite extends AbstractTestSuite {
 }

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FilterNoiseFixture.java

@@ -0,0 +1,167 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2014. 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. */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj.fixtures;
+
+import java.lang.reflect.ParameterizedType;
+import java.util.logging.Logger;
+import java.util.Random;
+
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+import edu.wpi.first.wpilibj.test.TestBench;
+
+/**
+ * Represents a physically connected Motor and Encoder to allow for unit tests
+ * on these different pairs<br>
+ * Designed to allow the user to easily setup and tear down the fixture to allow
+ * for reuse. This class should be explicitly instantiated in the TestBed class
+ * to allow any test to access this fixture. This allows tests to be mailable so
+ * that you can easily reconfigure the physical testbed without breaking the
+ * tests.
+ */
+public abstract class FilterNoiseFixture<T extends PIDSource> implements ITestFixture {
+  private static final Logger logger = Logger.getLogger(FilterNoiseFixture.class.getName());
+  private boolean initialized = false;
+  private boolean tornDown = false;
+  protected T filter;
+  private NoiseGenerator data;
+
+  /**
+   * Where the implementer of this class should pass the filter constructor
+   *$
+   * @return
+   */
+  abstract protected T giveFilter(PIDSource source);
+
+  final private void initialize() {
+    synchronized (this) {
+      if (!initialized) {
+        initialized = true; // This ensures it is only initialized once
+
+        data = new NoiseGenerator(TestBench.kStdDev) {
+          @Override
+          public double getData(double t) {
+            return 100.0 * Math.sin(2.0 * Math.PI * t);
+          }
+        };
+        filter = giveFilter(data);
+      }
+    }
+  }
+
+  @Override
+  public boolean setup() {
+    initialize();
+    return true;
+  }
+
+  /**
+   * Gets the filter for this Object
+   *$
+   * @return the filter this object refers too
+   */
+  public T getFilter() {
+    initialize();
+    return filter;
+  }
+
+  /**
+   * Gets the noise generator for this object
+   *$
+   * @return the noise generator that this object refers too
+   */
+  public NoiseGenerator getNoiseGenerator() {
+    initialize();
+    return data;
+  }
+
+  /**
+   * Retrieves the name of the filter that this object refers to
+   *$
+   * @return The simple name of the filter {@link Class#getSimpleName()}
+   */
+  public String getType() {
+    initialize();
+    return filter.getClass().getSimpleName();
+  }
+
+  // test here?
+
+  @Override
+  public boolean reset() {
+    return true;
+  }
+
+  @Override
+  public boolean teardown() {
+    return true;
+  }
+
+  @Override
+  public String toString() {
+    StringBuilder string = new StringBuilder("FilterNoiseFixture<");
+    // Get the generic type as a class
+    @SuppressWarnings("unchecked")
+    Class<T> class1 =
+        (Class<T>) ((ParameterizedType) getClass().getGenericSuperclass()).getActualTypeArguments()[0];
+    string.append(class1.getSimpleName());
+    string.append(">");
+    return string.toString();
+  }
+
+  /**
+   * Adds Gaussian white noise to a function returning data. The noise will have
+   * the standard deviation provided in the constructor.
+   */
+  public abstract class NoiseGenerator implements PIDSource {
+    private double noise = 0.0;
+
+    // Make sure first call to pidGet() uses count == 0
+    private double count = -TestBench.kFilterStep;
+
+    private double stdDev;
+    private Random gen = new Random();
+
+    NoiseGenerator(double stdDev) {
+      this.stdDev = stdDev;
+    }
+
+    abstract public double getData(double t);
+
+    /**
+     * {@inheritDoc}
+     */
+    @Override
+    public void setPIDSourceType(PIDSourceType pidSource) {}
+
+    /**
+     * {@inheritDoc}
+     */
+    @Override
+    public PIDSourceType getPIDSourceType() {
+      return PIDSourceType.kDisplacement;
+    }
+
+    public double get() {
+      return getData(count) + noise;
+    }
+
+    /**
+     * {@inheritDoc}
+     */
+    @Override
+    public double pidGet() {
+      noise = gen.nextGaussian() * stdDev;
+      count += TestBench.kFilterStep;
+      return getData(count) + noise;
+    }
+
+    public void reset() {
+      count = -TestBench.kFilterStep;
+    }
+  }
+}

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FilterOutputFixture.java

@@ -0,0 +1,159 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2014. 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. */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj.fixtures;
+
+import java.lang.reflect.ParameterizedType;
+import java.util.logging.Logger;
+
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+import edu.wpi.first.wpilibj.test.TestBench;
+
+/**
+ * Represents a filterphysically connected Motor and Encoder to allow for unit tests
+ * on these different pairs<br>
+ * Designed to allow the user to easily setup and tear down the fixture to allow
+ * for reuse. This class should be explicitly instantiated in the TestBed class
+ * to allow any test to access this fixture. This allows tests to be mailable so
+ * that you can easily reconfigure the physical testbed without breaking the
+ * tests.
+ */
+public abstract class FilterOutputFixture<T extends PIDSource> implements ITestFixture {
+  private static final Logger logger = Logger.getLogger(FilterOutputFixture.class.getName());
+  private boolean initialized = false;
+  private boolean tornDown = false;
+  protected T filter;
+  private DataWrapper data;
+  private double expectedOutput;
+
+  public FilterOutputFixture(double expectedOutput) {
+    this.expectedOutput = expectedOutput;
+  }
+
+  /**
+   * Get expected output of fixture
+   */
+  public double getExpectedOutput() {
+    return expectedOutput;
+  }
+
+  /**
+   * Where the implementer of this class should pass the filter constructor
+   *$
+   * @return
+   */
+  abstract protected T giveFilter(PIDSource source);
+
+  final private void initialize() {
+    synchronized (this) {
+      if (!initialized) {
+        initialized = true; // This ensures it is only initialized once
+
+        data = new DataWrapper() {
+          @Override
+          public double getData(double t) {
+            return 100.0 * Math.sin(2.0 * Math.PI * t) + 20.0 * Math.cos(50.0 * Math.PI * t);
+          }
+        };
+        filter = giveFilter(data);
+      }
+    }
+  }
+
+  @Override
+  public boolean setup() {
+    initialize();
+    return true;
+  }
+
+  /**
+   * Gets the filter for this Object
+   *$
+   * @return the filter this object refers too
+   */
+  public T getFilter() {
+    initialize();
+    return filter;
+  }
+
+  /**
+   * Gets the data wrapper for this object
+   *$
+   * @return the data wrapper  that this object refers too
+   */
+  public DataWrapper getDataWrapper() {
+    initialize();
+    return data;
+  }
+
+  /**
+   * Retrieves the name of the filter that this object refers to
+   *$
+   * @return The simple name of the filter {@link Class#getSimpleName()}
+   */
+  public String getType() {
+    initialize();
+    return filter.getClass().getSimpleName();
+  }
+
+  @Override
+  public boolean reset() {
+    data.reset();
+    return true;
+  }
+
+  @Override
+  public boolean teardown() {
+    return true;
+  }
+
+  @Override
+  public String toString() {
+    StringBuilder string = new StringBuilder("FilterOutputFixture<");
+    // Get the generic type as a class
+    @SuppressWarnings("unchecked")
+    Class<T> class1 =
+        (Class<T>) ((ParameterizedType) getClass().getGenericSuperclass()).getActualTypeArguments()[0];
+    string.append(class1.getSimpleName());
+    string.append(">");
+    return string.toString();
+  }
+
+  public abstract class DataWrapper implements PIDSource {
+    // Make sure first call to pidGet() uses count == 0
+    private double count = -TestBench.kFilterStep;
+
+    abstract public double getData(double t);
+
+    /**
+     * {@inheritDoc}
+     */
+    @Override
+    public void setPIDSourceType(PIDSourceType pidSource) {}
+
+    /**
+     * {@inheritDoc}
+     */
+    @Override
+    public PIDSourceType getPIDSourceType() {
+      return PIDSourceType.kDisplacement;
+    }
+
+    /**
+     * {@inheritDoc}
+     */
+    @Override
+    public double pidGet() {
+      count += TestBench.kFilterStep;
+      return getData(count);
+    }
+
+    public void reset() {
+      count = -TestBench.kFilterStep;
+    }
+  }
+}

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java

@@ -19,13 +19,17 @@ import edu.wpi.first.wpilibj.CANJaguar;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.DigitalOutput;
 import edu.wpi.first.wpilibj.Jaguar;
+import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.Relay;
 import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj.Talon;
 import edu.wpi.first.wpilibj.Victor;
+import edu.wpi.first.wpilibj.filters.LinearDigitalFilter;
 import edu.wpi.first.wpilibj.fixtures.AnalogCrossConnectFixture;
 import edu.wpi.first.wpilibj.fixtures.CANMotorEncoderFixture;
 import edu.wpi.first.wpilibj.fixtures.DIOCrossConnectFixture;
+import edu.wpi.first.wpilibj.fixtures.FilterNoiseFixture;
+import edu.wpi.first.wpilibj.fixtures.FilterOutputFixture;
 import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
 import edu.wpi.first.wpilibj.fixtures.RelayCrossConnectFixture;
 import edu.wpi.first.wpilibj.fixtures.TiltPanCameraFixture;
@@ -79,6 +83,17 @@ public final class TestBench {
   public static final int DIOCrossConnectA2 = 7;
   public static final int DIOCrossConnectA1 = 6;
 
+  // Filter constants
+  public static final double kStdDev = 10.0;
+  public static final double kFilterStep = 0.005;
+  public static final double kFilterTime = 2.0;
+  public static final double kSinglePoleIIRTimeConstant = 0.015915;
+  public static final double kSinglePoleIIRExpectedOutput = -3.2172003;
+  public static final double kHighPassTimeConstant = 0.006631;
+  public static final double kHighPassExpectedOutput = 10.074717;
+  public static final int kMovAvgTaps = 6;
+  public static final double kMovAvgExpectedOutput = -10.191644;
+
   /** The Singleton instance of the Test Bench */
   private static TestBench instance = null;
 
@@ -410,6 +425,86 @@ public final class TestBench {
     return encoderPortPairs;
   }
 
+  /**
+   * Constructs a new set of objects representing a single-pole IIR filter with
+   * a noisy data source
+   *
+   * @return a single-pole IIR filter with a noisy data source
+   */
+  public FilterNoiseFixture<LinearDigitalFilter> getSinglePoleIIRNoiseFixture() {
+    return new FilterNoiseFixture<LinearDigitalFilter>() {
+      @Override
+      protected LinearDigitalFilter giveFilter(PIDSource source) {
+        return LinearDigitalFilter.singlePoleIIR(source,
+                                                 kSinglePoleIIRTimeConstant,
+                                                 kFilterStep);
+      }
+    };
+  }
+
+  /**
+   * Constructs a new set of objects representing a moving average filter with a
+   * noisy data source using a linear digital filter
+   *
+   * @return a moving average filter with a noisy data source
+   */
+  public FilterNoiseFixture<LinearDigitalFilter> getMovAvgNoiseFixture() {
+    return new FilterNoiseFixture<LinearDigitalFilter>() {
+      @Override
+      protected LinearDigitalFilter giveFilter(PIDSource source) {
+        return LinearDigitalFilter.movingAverage(source, kMovAvgTaps);
+      }
+    };
+  }
+
+  /**
+   * Constructs a new set of objects representing a single-pole IIR filter with
+   * a repeatable data source
+   *
+   * @return a single-pole IIR filter with a repeatable data source
+   */
+  public FilterOutputFixture<LinearDigitalFilter> getSinglePoleIIROutputFixture() {
+    return new FilterOutputFixture<LinearDigitalFilter>(kSinglePoleIIRExpectedOutput) {
+      @Override
+      protected LinearDigitalFilter giveFilter(PIDSource source) {
+        return LinearDigitalFilter.singlePoleIIR(source,
+                                                 kSinglePoleIIRTimeConstant,
+                                                 kFilterStep);
+      }
+    };
+  }
+
+  /**
+   * Constructs a new set of objects representing a high-pass filter with a
+   * repeatable data source
+   *
+   * @return a high-pass filter with a repeatable data source
+   */
+  public FilterOutputFixture<LinearDigitalFilter> getHighPassOutputFixture() {
+    return new FilterOutputFixture<LinearDigitalFilter>(kHighPassExpectedOutput) {
+      @Override
+      protected LinearDigitalFilter giveFilter(PIDSource source) {
+        return LinearDigitalFilter.highPass(source, kHighPassTimeConstant,
+                                            kFilterStep);
+      }
+    };
+  }
+
+  /**
+   * Constructs a new set of objects representing a moving average filter with a
+   * repeatable data source using a linear digital filter
+   *
+   * @return a moving average filter with a repeatable data source
+   */
+  public FilterOutputFixture<LinearDigitalFilter> getMovAvgOutputFixture() {
+    return new FilterOutputFixture<LinearDigitalFilter>(kMovAvgExpectedOutput) {
+      @Override
+      protected LinearDigitalFilter giveFilter(PIDSource source) {
+        return LinearDigitalFilter.movingAverage(source, kMovAvgTaps);
+      }
+    };
+  }
+
   /**
    * Gets the singleton of the TestBench. If the TestBench is not already
    * allocated in constructs an new instance of it. Otherwise it returns the