diff --git a/wpilibc/src/main/native/include/frc/MedianFilter.h b/wpilibc/src/main/native/include/frc/MedianFilter.h new file mode 100644 index 0000000000..b5d499b238 --- /dev/null +++ b/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 +#include + +#include +#include + +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 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 m_valueBuffer; + std::vector m_orderedValues; + size_t m_size; +}; +} // namespace frc diff --git a/wpilibc/src/test/native/cpp/MedianFilterTest.cpp b/wpilibc/src/test/native/cpp/MedianFilterTest.cpp new file mode 100644 index 0000000000..7fe7d2e2a4 --- /dev/null +++ b/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 filter{10}; + + filter.Calculate(3); + filter.Calculate(0); + filter.Calculate(4); + + EXPECT_EQ(filter.Calculate(1000), 3.5); +} + +TEST(MedianFilterTest, MedianFilterNotFullTestOdd) { + frc::MedianFilter 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 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 filter{5}; + + filter.Calculate(3); + filter.Calculate(0); + filter.Calculate(5); + filter.Calculate(4); + filter.Calculate(1000); + + EXPECT_EQ(filter.Calculate(99), 5); +} diff --git a/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp index 2cc21318dc..9f9c01d539 100644 --- a/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp +++ b/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 +#include #include #include #include @@ -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 m_filter{10}; + frc::AnalogInput m_ultrasonic{kUltrasonicPort}; frc::PWMVictorSPX m_left{kLeftMotorPort}; diff --git a/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp index 745fc10e89..47bd62d6be 100644 --- a/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp @@ -6,6 +6,7 @@ /*----------------------------------------------------------------------------*/ #include +#include #include #include #include @@ -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 m_filter{5}; + frc::AnalogInput m_ultrasonic{kUltrasonicPort}; frc::PWMVictorSPX m_left{kLeftMotorPort}; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java new file mode 100644 index 0000000000..a4a3a95372 --- /dev/null +++ b/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 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(); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java new file mode 100644 index 0000000000..00b100d3c7 --- /dev/null +++ b/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)); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java index e03c66c2b9..2cae2883d9 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java +++ b/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; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java index 225f3adfa8..b0234295f2 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java +++ b/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); } diff --git a/wpiutil/src/main/native/include/wpi/Algorithm.h b/wpiutil/src/main/native/include/wpi/Algorithm.h new file mode 100644 index 0000000000..ec8dc95218 --- /dev/null +++ b/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 + +namespace wpi { + +// Binary insortion into vector; std::log(n) efficiency. +template +typename std::vector::iterator insert_sorted(std::vector& vec, + T const& item) { + return vec.insert(std::upper_bound(vec.begin(), vec.end(), item), item); +} +} // namespace wpi