Add Butterworth bandpass filter

src/config.py

@@ -2,6 +2,10 @@ import os
 
 PREPROCESSING_SHORT_TERM_WINDOW_SIZE = 5
 PREPROCESSING_LONG_TERM_ALPHA = 0.01
+
+PREPROCESSING_BANDPASS_LOW_CUTOFF = 2
+PREPROCESSING_BANDPASS_HIGH_CUTOFF = 40
+
 AOA_SLIDING_WINDOW_SIZE = 20
 
 RECEIVE_IP_ADDRESS = os.getenv("IP_ADDRESS", "10.0.12.64")
@@ -10,18 +14,16 @@ FEITCSI_PORT = 8008
 
 SAMPLE_RATE = 100  # Hz
 
-EIGVAL_THRESHOLD = 1e4
+EIGVAL_THRESHOLD = 1000
 
 
+# DELTA_F = 78_125  # Spacing between subcarriers in Hz
 DELTA_F = 312_500  # Spacing between subcarriers in Hz
-CENTRAL_FREQUENCY_MHZ = 6195
+CENTRAL_FREQUENCY_MHZ = 5220
-CENTRAL_FREQUENCY_HZ = CENTRAL_FREQUENCY_MHZ * 1_000_000
+ANTENNA_SPACING = 0.0285  # 2.85 cm
-
-ANTENNA_SPACING = 0.0285
-# ANTENNA_SPACING = 0.0285 * 3
-
 CHANNEL_WIDTH = 20
 FRAME_FORMAT = "HT"
 
 
-C = 299_792_458
+CENTRAL_FREQUENCY_HZ = CENTRAL_FREQUENCY_MHZ * 1_000_000
+C = 299_792_458  # m/s

src/preprocess.py

@@ -5,6 +5,7 @@ from queue import Queue
 from . import config
 import numpy.typing as npt
 from scipy.signal import correlate
+from scipy.signal import butter, sosfilt_zi, sosfilt
 
 
 logger = logging.getLogger(__name__)
@@ -18,29 +19,52 @@ class Preprocessor:
         self.prev_entries: Queue[npt.NDArray[np.complex128]] = Queue(maxsize=100)
         self.short_term_avg = np.zeros((1,), dtype=np.complex128)
         self.long_term_avg = np.zeros((1,), dtype=np.complex128)
+        self.filter = butter(
+            5,
+            [
+                config.PREPROCESSING_BANDPASS_LOW_CUTOFF,
+                config.PREPROCESSING_BANDPASS_HIGH_CUTOFF,
+            ],
+            fs=config.SAMPLE_RATE,
+            btype="band",
+            output="sos",
+        )
 
     def preprocess(self, csi: CSI):
         h = csi.matrix
-        h_hat = np.multiply(h, h.conj() / abs(h.conj()))
+
+        # CSI data is not available for pilot subcarriers.
+        h_hat = np.where(
+            np.expand_dims(h[:, 0, 0] == 0, axis=(1, 2)),
+            correlate(h, [[[1 / 2]], [[0]], [[1 / 2]]], mode="same"),
+            h,
+        )
+        h_hat = np.multiply(h_hat, h_hat.conj() / abs(h_hat.conj()))
+        h_hat = np.nan_to_num(h_hat)
+
+        # h_hat = correlate(h_hat, np.ones((3, 1, 1)) / 3)
+        h_hat = correlate(h_hat, [[[1 / 4]], [[1 / 2]], [[1 / 4]]], mode="valid")
 
         # Assume that all csi matrices will have the same shape
-        if self.short_term_avg.shape != h_hat.shape:
+        if self.long_term_avg.shape != h_hat.shape:
-            self.short_term_avg = np.zeros(h_hat.shape, dtype=np.complex128)
             self.long_term_avg = np.zeros(h_hat.shape, dtype=np.complex128)
-            self.prev_entries = Queue(maxsize=100)
 
-        while self.prev_entries.full():
-            old_value = self.prev_entries.get()
-            self.short_term_avg -= (
-                old_value / config.PREPROCESSING_SHORT_TERM_WINDOW_SIZE
-            )
-        self.prev_entries.put(h_hat)
-        self.short_term_avg += h_hat / config.PREPROCESSING_SHORT_TERM_WINDOW_SIZE
         self.long_term_avg = (
             self.long_term_avg * (1 - config.PREPROCESSING_LONG_TERM_ALPHA)
-            + self.short_term_avg * config.PREPROCESSING_LONG_TERM_ALPHA
+            + h_hat * config.PREPROCESSING_LONG_TERM_ALPHA
         )
 
-        # Remove static components
+        # Remove long term average, to remove static paths
-        current_measurement = self.short_term_avg - self.long_term_avg
+        h_hat -= self.long_term_avg
-        return current_measurement
+
+        # Apply bandpass filter to remove low and high frequency noise
+        if not hasattr(self, "filter_zi"):
+            self.filter_zi = (
+                np.expand_dims(sosfilt_zi(self.filter), axis=(-1, -2, -3)) * h_hat
+            )
+
+        h_hat_filt, self.filter_zi = sosfilt(
+            self.filter, [h_hat], zi=self.filter_zi, axis=0
+        )
+
+        return h_hat_filt[0]