WPILibC++  2020.3.2
MedianFilter.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2019 FIRST. All Rights Reserved. */
3 /* Open Source Software - may be modified and shared by FRC teams. The code */
4 /* must be accompanied by the FIRST BSD license file in the root directory of */
5 /* the project. */
6 /*----------------------------------------------------------------------------*/
7 
8 #pragma once
9 
10 #include <algorithm>
11 #include <vector>
12 
13 #include <wpi/Algorithm.h>
14 #include <wpi/circular_buffer.h>
15 
16 namespace frc {
23 template <class T>
24 class MedianFilter {
25  public:
31  explicit MedianFilter(size_t size) : m_valueBuffer(size), m_size{size} {}
32 
39  T Calculate(T next) {
40  // Insert next value at proper point in sorted array
41  wpi::insert_sorted(m_orderedValues, next);
42 
43  size_t curSize = m_orderedValues.size();
44 
45  // If buffer is at max size, pop element off of end of circular buffer
46  // and remove from ordered list
47  if (curSize > m_size) {
48  m_orderedValues.erase(std::find(m_orderedValues.begin(),
49  m_orderedValues.end(),
50  m_valueBuffer.pop_back()));
51  curSize = curSize - 1;
52  }
53 
54  // Add next value to circular buffer
55  m_valueBuffer.push_front(next);
56 
57  if (curSize % 2 == 1) {
58  // If size is odd, return middle element of sorted list
59  return m_orderedValues[curSize / 2];
60  } else {
61  // If size is even, return average of middle elements
62  return (m_orderedValues[curSize / 2 - 1] + m_orderedValues[curSize / 2]) /
63  2.0;
64  }
65  }
66 
70  void Reset() {
71  m_orderedValues.clear();
72  m_valueBuffer.reset();
73  }
74 
75  private:
76  wpi::circular_buffer<T> m_valueBuffer;
77  std::vector<T> m_orderedValues;
78  size_t m_size;
79 };
80 } // namespace frc
frc::MedianFilter::MedianFilter
MedianFilter(size_t size)
Creates a new MedianFilter.
Definition: MedianFilter.h:31
frc::MedianFilter::Reset
void Reset()
Resets the filter, clearing the window of all elements.
Definition: MedianFilter.h:70
frc::MedianFilter
A class that implements a moving-window median filter.
Definition: MedianFilter.h:24
frc::MedianFilter::Calculate
T Calculate(T next)
Calculates the moving-window median for the next value of the input stream.
Definition: MedianFilter.h:39
wpi::circular_buffer
This is a simple circular buffer so we don't need to "bucket brigade" copy old values.
Definition: circular_buffer.h:20
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16