FFT add simple median filter

msg/sensor_gyro_fft.msg

@@ -7,6 +7,8 @@ float32 dt                # delta time between samples (microseconds)
 
 float32 resolution_hz
 
-float32[2] peak_frequency_x # x axis peak frequencies
-float32[2] peak_frequency_y # y axis peak frequencies
-float32[2] peak_frequency_z # z axis peak frequencies
+float32[3] peak_frequency # peak frequency per axis
+
+float32[4] peak_frequencies_x # x axis peak frequencies
+float32[4] peak_frequencies_y # y axis peak frequencies
+float32[4] peak_frequencies_z # z axis peak frequencies

src/examples/gyro_fft/GyroFFT.cpp

@@ -137,8 +137,8 @@ static constexpr float tau(float x)
 float GyroFFT::EstimatePeakFrequency(q15_t fft[FFT_LENGTH * 2], uint8_t peak_index)
 {
 	// find peak location using Quinn's Second Estimator (2020-06-14: http://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/)
-	int16_t real[3] {fft[peak_index - 2], fft[peak_index], fft[peak_index + 2]};
-	int16_t imag[3] {fft[peak_index - 2 + 1], fft[peak_index + 1], fft[peak_index + 2 + 1]};
+	int16_t real[3] { fft[peak_index - 2],     fft[peak_index],     fft[peak_index + 2]     };
+	int16_t imag[3] { fft[peak_index - 2 + 1], fft[peak_index + 1], fft[peak_index + 2 + 1] };
 
 	const int k = 1;
 
@@ -255,7 +255,10 @@ void GyroFFT::Run()
 
 					static constexpr uint16_t MIN_SNR = 10; // TODO:
 
-					static constexpr int MAX_NUM_PEAKS = 2;
+					uint32_t max_peak_magnitude = 0;
+					uint8_t max_peak_index = 0;
+
+					static constexpr int MAX_NUM_PEAKS = 4;
 					uint32_t peaks_magnitude[MAX_NUM_PEAKS] {};
 					uint8_t peak_index[MAX_NUM_PEAKS] {};
 
@@ -275,6 +278,12 @@ void GyroFFT::Run()
 							const uint32_t fft_magnitude_squared = real * real + complex * complex;
 
 							if (fft_magnitude_squared > MIN_SNR) {
+
+								if (fft_magnitude_squared > max_peak_magnitude) {
+									max_peak_magnitude = fft_magnitude_squared;
+									max_peak_index = bucket_index;
+								}
+
 								for (int i = 0; i < MAX_NUM_PEAKS; i++) {
 									if (fft_magnitude_squared > peaks_magnitude[i]) {
 										peaks_magnitude[i] = fft_magnitude_squared;
@@ -287,20 +296,25 @@ void GyroFFT::Run()
 						}
 					}
 
+					if (max_peak_index > 0) {
+						_sensor_gyro_fft.peak_frequency[axis] = _median_filter[axis].apply(EstimatePeakFrequency(_fft_outupt_buffer,
+											max_peak_index));
+					}
+
 					if (publish) {
 						float *peak_frequencies;
 
 						switch (axis) {
 						case 0:
-							peak_frequencies = _sensor_gyro_fft.peak_frequency_x;
+							peak_frequencies = _sensor_gyro_fft.peak_frequencies_x;
 							break;
 
 						case 1:
-							peak_frequencies = _sensor_gyro_fft.peak_frequency_y;
+							peak_frequencies = _sensor_gyro_fft.peak_frequencies_y;
 							break;
 
 						case 2:
-							peak_frequencies = _sensor_gyro_fft.peak_frequency_z;
+							peak_frequencies = _sensor_gyro_fft.peak_frequencies_z;
 							break;
 						}
 

src/examples/gyro_fft/GyroFFT.hpp

@@ -33,6 +33,7 @@
 
 #pragma once
 
+#include <lib/mathlib/math/filter/MedianFilter.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/defines.h>
@@ -111,6 +112,8 @@ private:
 
 	unsigned _gyro_last_generation{0};
 
+	math::MedianFilter<float, 3> _median_filter[3] {};
+
 	sensor_gyro_fft_s _sensor_gyro_fft{};
 
 	DEFINE_PARAMETERS(

src/lib/mathlib/CMakeLists.txt

@@ -38,5 +38,6 @@ px4_add_library(mathlib
 	math/filter/LowPassFilter2pVector3f.cpp
 )
 
+px4_add_unit_gtest(SRC math/filter/MedianFilterTest.cpp)
 px4_add_unit_gtest(SRC math/filter/NotchFilterTest.cpp)
 px4_add_unit_gtest(SRC math/FunctionsTest.cpp)

src/lib/mathlib/math/filter/MedianFilter.hpp

@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file MedianFilter.hpp
+ *
+ * @brief Implementation of a median filter with C qsort.
+ *
+ */
+
+#pragma once
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+namespace math
+{
+
+template<typename T, int WINDOW = 3>
+class MedianFilter
+{
+public:
+	static_assert(WINDOW >= 3, "MedianFilter window size must be >= 3");
+	static_assert(WINDOW % 2, "MedianFilter window size must be odd"); // odd
+
+	MedianFilter() = default;
+
+	void insert(const T &sample)
+	{
+		_head = (_head + 1) % WINDOW;
+		_buffer[_head] = sample;
+	}
+
+	T median()
+	{
+		T sorted[WINDOW];
+		memcpy(sorted, _buffer, sizeof(_buffer));
+		qsort(&sorted, WINDOW, sizeof(T), cmp);
+
+		return sorted[WINDOW / 2];
+	}
+
+	T apply(const T &sample)
+	{
+		insert(sample);
+		return median();
+	}
+
+private:
+
+	static int cmp(const void *a, const void *b)
+	{
+		return (*(T *)a >= *(T *)b) ? 1 : -1;
+	}
+
+	T _buffer[WINDOW] {};
+	uint8_t _head{0};
+};
+
+} // namespace math

src/lib/mathlib/math/filter/MedianFilterTest.cpp

@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Test code for the Median filter
+ * Run this test only using make tests TESTFILTER=MedianFilter
+ */
+
+#include <gtest/gtest.h>
+#include <matrix/matrix/math.hpp>
+#include <mathlib/mathlib.h>
+
+#include "MedianFilter.hpp"
+
+using namespace math;
+using matrix::Vector3f;
+
+class MedianFilterTest : public ::testing::Test
+{
+public:
+
+
+};
+
+TEST_F(MedianFilterTest, test3f_simple)
+{
+	MedianFilter<float, 3> median_filter3;
+
+	for (int i = 0; i < 3; i++) {
+		median_filter3.insert(i);
+	}
+
+	EXPECT_EQ(median_filter3.median(), 1); // 0, 1, 2
+}
+
+TEST_F(MedianFilterTest, test3f_100)
+{
+	MedianFilter<float, 3> median_filter3;
+
+	for (int i = 0; i < 100; i++) {
+		EXPECT_EQ(median_filter3.apply(i), max(0, i - 1));
+	}
+}
+
+TEST_F(MedianFilterTest, test5u_simple)
+{
+	MedianFilter<uint16_t, 5> median_filter5;
+
+	for (int i = 0; i < 5; i++) {
+		median_filter5.insert(i);
+	}
+
+	EXPECT_EQ(median_filter5.median(), 2); // 0, 1, 2, 4, 5
+}
+
+TEST_F(MedianFilterTest, test5i_100)
+{
+	MedianFilter<uint16_t, 5> median_filter5;
+
+	for (int i = 0; i < 100; i++) {
+		EXPECT_EQ(median_filter5.apply(i), max(0, i - 2));
+	}
+}