Add MedianFilter class for moving-window median (#2136)

This kind of filter is extremely useful for signals that are susceptible to sudden
outliers - ultrasonics, 1-D LIDAR, and results from vision processing are all
good use-cases.

This also modifies the existing ultrasonic examples accordingly.

wpilibc/src/main/native/include/frc/MedianFilter.h

@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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 <algorithm>
+#include <vector>
+
+#include <wpi/Algorithm.h>
+#include <wpi/circular_buffer.h>
+
+namespace frc {
+/**
+ * A class that implements a moving-window median filter.  Useful for reducing
+ * measurement noise, especially with processes that generate occasional,
+ * extreme outliers (such as values from vision processing, LIDAR, or ultrasonic
+ * sensors).
+ */
+template <class T>
+class MedianFilter {
+ public:
+  /**
+   * Creates a new MedianFilter.
+   *
+   * @param size The number of samples in the moving window.
+   */
+  explicit MedianFilter(size_t size) : m_valueBuffer(size), m_size{size} {}
+
+  /**
+   * Calculates the moving-window median for the next value of the input stream.
+   *
+   * @param next The next input value.
+   * @return The median of the moving window, updated to include the next value.
+   */
+  T Calculate(T next) {
+    // Insert next value at proper point in sorted array
+    wpi::insert_sorted(m_orderedValues, next);
+
+    size_t curSize = m_orderedValues.size();
+
+    // If buffer is at max size, pop element off of end of circular buffer
+    // and remove from ordered list
+    if (curSize > m_size) {
+      m_orderedValues.erase(std::find(m_orderedValues.begin(),
+                                      m_orderedValues.end(),
+                                      m_valueBuffer.pop_back()));
+      curSize = curSize - 1;
+    }
+
+    // Add next value to circular buffer
+    m_valueBuffer.push_front(next);
+
+    if (curSize % 2 == 1) {
+      // If size is odd, return middle element of sorted list
+      return m_orderedValues[curSize / 2];
+    } else {
+      // If size is even, return average of middle elements
+      return (m_orderedValues[curSize / 2 - 1] + m_orderedValues[curSize / 2]) /
+             2.0;
+    }
+  }
+
+  /**
+   * Resets the filter, clearing the window of all elements.
+   */
+  void Reset() {
+    m_orderedValues.clear();
+    m_valueBuffer.reset();
+  }
+
+ private:
+  wpi::circular_buffer<T> m_valueBuffer;
+  std::vector<T> m_orderedValues;
+  size_t m_size;
+};
+}  // namespace frc

wpilibc/src/test/native/cpp/MedianFilterTest.cpp

@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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 "frc/MedianFilter.h"
+#include "gtest/gtest.h"
+
+TEST(MedianFilterTest, MedianFilterNotFullTestEven) {
+  frc::MedianFilter<double> filter{10};
+
+  filter.Calculate(3);
+  filter.Calculate(0);
+  filter.Calculate(4);
+
+  EXPECT_EQ(filter.Calculate(1000), 3.5);
+}
+
+TEST(MedianFilterTest, MedianFilterNotFullTestOdd) {
+  frc::MedianFilter<double> filter{10};
+
+  filter.Calculate(3);
+  filter.Calculate(0);
+  filter.Calculate(4);
+  filter.Calculate(7);
+
+  EXPECT_EQ(filter.Calculate(1000), 4);
+}
+
+TEST(MedianFilterTest, MedianFilterFullTestEven) {
+  frc::MedianFilter<double> filter{6};
+
+  filter.Calculate(3);
+  filter.Calculate(0);
+  filter.Calculate(0);
+  filter.Calculate(5);
+  filter.Calculate(4);
+  filter.Calculate(1000);
+
+  EXPECT_EQ(filter.Calculate(99), 4.5);
+}
+
+TEST(MedianFilterTest, MedianFilterFullTestOdd) {
+  frc::MedianFilter<double> filter{5};
+
+  filter.Calculate(3);
+  filter.Calculate(0);
+  filter.Calculate(5);
+  filter.Calculate(4);
+  filter.Calculate(1000);
+
+  EXPECT_EQ(filter.Calculate(99), 5);
+}

wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp

@@ -1,11 +1,12 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2019 FIRST. 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 <frc/AnalogInput.h>
+#include <frc/MedianFilter.h>
 #include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/drive/DifferentialDrive.h>
@@ -22,7 +23,10 @@ class Robot : public frc::TimedRobot {
    */
   void TeleopPeriodic() override {
     // Sensor returns a value from 0-4095 that is scaled to inches
-    double currentDistance = m_ultrasonic.GetValue() * kValueToInches;
+    // returned value is filtered with a rolling median filter, since
+    // ultrasonics tend to be quite noisy and susceptible to sudden outliers
+    double currentDistance =
+        m_filter.Calculate(m_ultrasonic.GetVoltage()) * kValueToInches;
     // Convert distance error to a motor speed
     double currentSpeed = (kHoldDistance - currentDistance) * kP;
     // Drive robot
@@ -43,6 +47,9 @@ class Robot : public frc::TimedRobot {
   static constexpr int kRightMotorPort = 1;
   static constexpr int kUltrasonicPort = 0;
 
+  // median filter to discard outliers; filters over 10 samples
+  frc::MedianFilter<double> m_filter{10};
+
   frc::AnalogInput m_ultrasonic{kUltrasonicPort};
 
   frc::PWMVictorSPX m_left{kLeftMotorPort};

wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp

@@ -6,6 +6,7 @@
 /*----------------------------------------------------------------------------*/
 
 #include <frc/AnalogInput.h>
+#include <frc/MedianFilter.h>
 #include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/controller/PIDController.h>
@@ -27,7 +28,8 @@ class Robot : public frc::TimedRobot {
   }
 
   void TeleopPeriodic() override {
-    double output = m_pidController.Calculate(m_ultrasonic.GetAverageVoltage());
+    double output =
+        m_pidController.Calculate(m_filter.Calculate(m_ultrasonic.GetValue()));
     m_robotDrive.ArcadeDrive(output, 0);
   }
 
@@ -51,6 +53,9 @@ class Robot : public frc::TimedRobot {
   static constexpr int kRightMotorPort = 1;
   static constexpr int kUltrasonicPort = 0;
 
+  // median filter to discard outliers; filters over 5 samples
+  frc::MedianFilter<double> m_filter{5};
+
   frc::AnalogInput m_ultrasonic{kUltrasonicPort};
 
   frc::PWMVictorSPX m_left{kLeftMotorPort};

wpilibj/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java

@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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 java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+import edu.wpi.first.wpiutil.CircularBuffer;
+
+/**
+ * A class that implements a moving-window median filter.  Useful for reducing measurement noise,
+ * especially with processes that generate occasional, extreme outliers (such as values from
+ * vision processing, LIDAR, or ultrasonic sensors).
+ */
+public class MedianFilter {
+  private final CircularBuffer m_valueBuffer;
+  private final List<Double> m_orderedValues;
+  private final int m_size;
+
+  /**
+   * Creates a new MedianFilter.
+   *
+   * @param size The number of samples in the moving window.
+   */
+  public MedianFilter(int size) {
+    // Circular buffer of values currently in the window, ordered by time
+    m_valueBuffer = new CircularBuffer(size);
+    // List of values currently in the window, ordered by value
+    m_orderedValues = new ArrayList<>(size);
+    // Size of rolling window
+    m_size = size;
+  }
+
+  /**
+   * Calculates the moving-window median for the next value of the input stream.
+   *
+   * @param next The next input value.
+   * @return The median of the moving window, updated to include the next value.
+   */
+  public double calculate(double next) {
+    // Find insertion point for next value
+    int index = Collections.binarySearch(m_orderedValues, next);
+
+    // Deal with binarySearch behavior for element not found
+    if (index < 0) {
+      index = Math.abs(index + 1);
+    }
+
+    // Place value at proper insertion point
+    m_orderedValues.add(index, next);
+
+    int curSize = m_orderedValues.size();
+
+    // If buffer is at max size, pop element off of end of circular buffer
+    // and remove from ordered list
+    if (curSize > m_size) {
+      m_orderedValues.remove(m_valueBuffer.removeLast());
+      curSize = curSize - 1;
+    }
+
+    // Add next value to circular buffer
+    m_valueBuffer.addFirst(next);
+
+    if (curSize % 2 == 1) {
+      // If size is odd, return middle element of sorted list
+      return m_orderedValues.get(curSize / 2);
+    } else {
+      // If size is even, return average of middle elements
+      return (m_orderedValues.get(curSize / 2 - 1) + m_orderedValues.get(curSize / 2)) / 2.0;
+    }
+  }
+
+  /**
+   * Resets the filter, clearing the window of all elements.
+   */
+  public void reset() {
+    m_orderedValues.clear();
+    m_valueBuffer.clear();
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java

@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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 org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class MedianFilterTest {
+  @Test
+  void medianFilterNotFullTestEven() {
+    MedianFilter filter = new MedianFilter(10);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(4);
+
+    assertEquals(3.5, filter.calculate(1000));
+  }
+
+  @Test
+  void medianFilterNotFullTestOdd() {
+    MedianFilter filter = new MedianFilter(10);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(4);
+    filter.calculate(7);
+
+    assertEquals(4, filter.calculate(1000));
+  }
+
+  @Test
+  void medianFilterFullTestEven() {
+    MedianFilter filter = new MedianFilter(6);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(0);
+    filter.calculate(5);
+    filter.calculate(4);
+    filter.calculate(1000);
+
+    assertEquals(4.5, filter.calculate(99));
+  }
+
+  @Test
+  void medianFilterFullTestOdd() {
+    MedianFilter filter = new MedianFilter(5);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(5);
+    filter.calculate(4);
+    filter.calculate(1000);
+
+    assertEquals(5, filter.calculate(99));
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2019 FIRST. 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.                                                               */
@@ -8,6 +8,7 @@
 package edu.wpi.first.wpilibj.examples.ultrasonic;
 
 import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.MedianFilter;
 import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -31,6 +32,9 @@ public class Robot extends TimedRobot {
   private static final int kRightMotorPort = 1;
   private static final int kUltrasonicPort = 0;
 
+  // median filter to discard outliers; filters over 10 samples
+  private final MedianFilter m_filter = new MedianFilter(10);
+
   private final AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort);
   private final DifferentialDrive m_robotDrive
       = new DifferentialDrive(new PWMVictorSPX(kLeftMotorPort),
@@ -43,7 +47,9 @@ public class Robot extends TimedRobot {
   @Override
   public void teleopPeriodic() {
     // sensor returns a value from 0-4095 that is scaled to inches
-    double currentDistance = m_ultrasonic.getValue() * kValueToInches;
+    // returned value is filtered with a rolling median filter, since ultrasonics
+    // tend to be quite noisy and susceptible to sudden outliers
+    double currentDistance = m_filter.calculate(m_ultrasonic.getValue()) * kValueToInches;
 
     // convert distance error to a motor speed
     double currentSpeed = (kHoldDistance - currentDistance) * kP;

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java

@@ -8,6 +8,7 @@
 package edu.wpi.first.wpilibj.examples.ultrasonicpid;
 
 import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.MedianFilter;
 import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.controller.PIDController;
@@ -37,6 +38,9 @@ public class Robot extends TimedRobot {
   private static final int kRightMotorPort = 1;
   private static final int kUltrasonicPort = 0;
 
+  // median filter to discard outliers; filters over 5 samples
+  private final MedianFilter m_filter = new MedianFilter(5);
+
   private final AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort);
   private final DifferentialDrive m_robotDrive
       = new DifferentialDrive(new PWMVictorSPX(kLeftMotorPort),
@@ -51,8 +55,10 @@ public class Robot extends TimedRobot {
 
   @Override
   public void teleopPeriodic() {
+    // returned value is filtered with a rolling median filter, since ultrasonics
+    // tend to be quite noisy and susceptible to sudden outliers
     double pidOutput
-        = m_pidController.calculate(m_ultrasonic.getAverageVoltage());
+        = m_pidController.calculate(m_filter.calculate(m_ultrasonic.getVoltage()));
 
     m_robotDrive.arcadeDrive(pidOutput, 0);
   }

wpiutil/src/main/native/include/wpi/Algorithm.h

@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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>
+
+namespace wpi {
+
+// Binary insortion into vector; std::log(n) efficiency.
+template <typename T>
+typename std::vector<T>::iterator insert_sorted(std::vector<T>& vec,
+                                                T const& item) {
+  return vec.insert(std::upper_bound(vec.begin(), vec.end(), item), item);
+}
+}  // namespace wpi