Added linear digital filters

Linear digital filter class based on code from FRC team 341

Change-Id: I4c5198e36a089e08a6d054bf1bf80392def27e23

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;
+}