From 0f445a19a62e7db315218fcb7847297eb4129fe7 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Fri, 2 Dec 2022 00:30:05 +0100 Subject: [PATCH] Fix size of frequencies and amplitude spectrum We need to drop the zero-frequency term. According to the `numpy` documentation, the zero-frequency term is the *sum of the signal*. That's not what we want. The range of frequencies and the amplitudes now look meaningful for test data. --- cartesian_controller_utilities/scripts/stability_observer.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/cartesian_controller_utilities/scripts/stability_observer.py b/cartesian_controller_utilities/scripts/stability_observer.py index d18b2526..21ba3297 100755 --- a/cartesian_controller_utilities/scripts/stability_observer.py +++ b/cartesian_controller_utilities/scripts/stability_observer.py @@ -71,13 +71,16 @@ def sensor_cb(self, data): # Amplitude spectrum. # We are interested only in the first half (= positive frequencies). + # Also drop the 0th element that holds the sum over all signals. amplitude_spec = np.abs(np.fft.fftn(self.data)) - amplitude_spec = amplitude_spec[0:int(self.N / 2)] + amplitude_spec = amplitude_spec[1:int(self.N / 2)] # Compute the index where the frequencies are greater than the cross over frequency. # The frequency spectrum is defined by the sensor rate and the sample count. + # Like with the amplitudes, we select only the relevant frequencies. timestep = 1.0 / self.rate # sensor freq = np.fft.fftfreq(self.N, d=timestep) + freq = freq[1:int(self.N / 2)] if max(freq) > self.wc: index = next(x[0] for x in enumerate(freq) if x[1] > self.wc) else: