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.

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