YAGSL/swervelib/math/IMULinearMovingAverageFilter.java

package swervelib.math;

import edu.wpi.first.util.DoubleCircularBuffer;

/**
 * A linear filter that does not calculate() each time a value is added to the DoubleCircularBuffer.
 */
public class IMULinearMovingAverageFilter
{

  /**
   * Circular buffer storing the current IMU readings
   */
  private final DoubleCircularBuffer m_inputs;
  /**
   * Gain on each reading.
   */
  private final double               m_inputGain;

  /**
   * Construct a linear moving average fitler
   *
   * @param bufferLength The number of values to average across
   */
  public IMULinearMovingAverageFilter(int bufferLength)
  {
    m_inputs = new DoubleCircularBuffer(bufferLength);
    m_inputGain = 1.0 / bufferLength;
  }

  /**
   * Add a value to the DoubleCircularBuffer
   *
   * @param input Value to add
   */
  public void addValue(double input)
  {
    m_inputs.addFirst(input);
  }

  /**
   * Calculate the average of the samples in the buffer
   *
   * @return The average of the values in the buffer
   */
  public double calculate()
  {
    double returnVal = 0.0;

    for (int i = 0; i < m_inputs.size(); i++)
    {
      returnVal += m_inputs.get(i) * m_inputGain;
    }

    return returnVal;
  }
}