diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
index 7a568c8613..c26270fa49 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
@@ -246,6 +246,43 @@ public class LinearFilter {
m_outputs.clear();
}
+ /**
+ * Resets the filter state, initializing internal buffers to the provided values.
+ *
+ *
These are the expected lengths of the buffers, depending on what type of linear filter used:
+ *
+ *
+ * | Type | Input Buffer Length | Output Buffer Length |
+ * | Unspecified | length of {@code ffGains} | length of {@code fbGains} |
+ *
+ * | Single Pole IIR | 1 | 1 |
+ * | High-Pass | 2 | 1 |
+ * | Moving Average | {@code taps} | 0 |
+ * | Finite Difference | length of {@code stencil} | 0 |
+ * | Backward Finite Difference | {@code samples} | 0 |
+ *
+ *
+ * @param inputBuffer Values to initialize input buffer.
+ * @param outputBuffer Values to initialize output buffer.
+ * @throws IllegalArgumentException if length of inputBuffer or outputBuffer does not match the
+ * length of ffGains and fbGains provided in the constructor.
+ */
+ public void reset(double[] inputBuffer, double[] outputBuffer) {
+ // Clear buffers
+ reset();
+
+ if (inputBuffer.length != m_inputGains.length || outputBuffer.length != m_outputGains.length) {
+ throw new IllegalArgumentException("Incorrect length of inputBuffer or outputBuffer");
+ }
+
+ for (double input : inputBuffer) {
+ m_inputs.addFirst(input);
+ }
+ for (double output : outputBuffer) {
+ m_outputs.addFirst(output);
+ }
+ }
+
/**
* Calculates the next value of the filter.
*
diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
index fc558dfd58..051a6ddd26 100644
--- a/wpimath/src/main/native/include/frc/filter/LinearFilter.h
+++ b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
@@ -271,6 +271,75 @@ class LinearFilter {
std::fill(m_outputs.begin(), m_outputs.end(), T{0.0});
}
+ /**
+ * Resets the filter state, initializing internal buffers to the provided
+ * values.
+ *
+ * These are the expected lengths of the buffers, depending on what type of
+ * linear filter used:
+ *
+ *
+ *
+ * | Type |
+ * Input Buffer Size |
+ * Output Buffer Size |
+ *
+ *
+ * | Unspecified |
+ * size of {@code ffGains} |
+ * size of {@code fbGains} |
+ *
+ *
+ * | Single Pole IIR |
+ * 1 |
+ * 1 |
+ *
+ *
+ * | High-Pass |
+ * 2 |
+ * 1 |
+ *
+ *
+ * | Moving Average |
+ * {@code taps} |
+ * 0 |
+ *
+ *
+ * | Finite Difference |
+ * size of {@code stencil} |
+ * 0 |
+ *
+ *
+ * | Backward Finite Difference |
+ * {@code Samples} |
+ * 0 |
+ *
+ *
+ *
+ * @param inputBuffer Values to initialize input buffer.
+ * @param outputBuffer Values to initialize output buffer.
+ * @throws std::runtime_error if size of inputBuffer or outputBuffer does not
+ * match the size of ffGains and fbGains provided in the constructor.
+ */
+ void Reset(std::span inputBuffer,
+ std::span outputBuffer) {
+ // Clear buffers
+ Reset();
+
+ if (inputBuffer.size() != m_inputGains.size() ||
+ outputBuffer.size() != m_outputGains.size()) {
+ throw std::runtime_error(
+ "Incorrect length of inputBuffer or outputBuffer");
+ }
+
+ for (size_t i = 0; i < inputBuffer.size(); ++i) {
+ m_inputs.push_front(inputBuffer[i]);
+ }
+ for (size_t i = 0; i < outputBuffer.size(); ++i) {
+ m_outputs.push_front(outputBuffer[i]);
+ }
+ }
+
/**
* Calculates the next value of the filter.
*