From d15b82f0319c718b66a63170e2ad9d753ab412a0 Mon Sep 17 00:00:00 2001
From: Christos Falas <raspberrymaker@gmail.com>
Date: Wed, 14 May 2025 16:54:18 +0100
Subject: [PATCH] add data denoising

---
 where_fi/collection/csi_frame.py  |  4 ++--
 where_fi/config/models.py         | 10 ++++++++
 where_fi/processing/preprocess.py | 38 +++++++++++++++++++++++++++++--
 3 files changed, 48 insertions(+), 4 deletions(-)

diff --git a/where_fi/collection/csi_frame.py b/where_fi/collection/csi_frame.py
index 4d84eef..b806227 100644
--- a/where_fi/collection/csi_frame.py
+++ b/where_fi/collection/csi_frame.py
@@ -43,8 +43,8 @@ class CSIHeader:
         self.num_rx = data[46]
         self.num_tx = data[47]
         self.num_subcarriers = struct.unpack("I", data[52:56])[0]
-        self.rssi1 = struct.unpack("I", data[60:64])[0]
-        self.rssi2 = struct.unpack("I", data[64:68])[0]
+        self.rssi1: int = struct.unpack("I", data[60:64])[0]
+        self.rssi2: int = struct.unpack("I", data[64:68])[0]
         self.source_mac = struct.unpack("BBBBBB", data[68:74])
         self.source_mac_string = "%02x:%02x:%02x:%02x:%02x:%02x" % struct.unpack(
             "BBBBBB", data[68:74]
diff --git a/where_fi/config/models.py b/where_fi/config/models.py
index 845b002..3e8fb58 100644
--- a/where_fi/config/models.py
+++ b/where_fi/config/models.py
@@ -18,6 +18,8 @@ class Preprocessing(BaseModel):
 
     bandpass: Bandpass
     subcarrier_step: int
+    denoising: Literal["none", "median", "mean"] = "median"
+    denoising_period: float = 1
 
 
 class MUSIC(BaseModel):
@@ -36,6 +38,10 @@ class Antennas(BaseModel):
     spacing: float
     order: list[tuple[Host, int]]
 
+    @property
+    def count(self) -> int:
+        return len(self.order)
+
 
 class Config(BaseModel):
     receive_hosts: list[Host]
@@ -66,6 +72,10 @@ class Config(BaseModel):
             return "6"
         raise ValueError(f"{self.central_freq} is not a valid Wi-Fi channel")
 
+    @property
+    def subcarriers(self) -> int:
+        return int((self.channel_width * 1e6) // self.delta_f) - 8
+
     @property
     def delta_f(self) -> int:
         if self.frame_format == "HESU":
diff --git a/where_fi/processing/preprocess.py b/where_fi/processing/preprocess.py
index 170eb7c..4f4c70a 100644
--- a/where_fi/processing/preprocess.py
+++ b/where_fi/processing/preprocess.py
@@ -7,8 +7,10 @@ import numpy.typing as npt
 from scipy.signal import butter, correlate, sosfilt, sosfilt_zi
 
 from where_fi.collection import CSIMatrix
+from where_fi.collection.csi_frame import CSI
+from where_fi.collection.protocols import CSIHost
+from where_fi.config import config
 
-from ..config import config
 from ..visualise import server as visualise
 
 logger = logging.getLogger(__name__)
@@ -28,13 +30,41 @@ class Preprocessor:
         #     output="sos",
         # )
         self._last_sample = None
+        self.denoising_samples = int(
+            config.preprocessing.denoising_period * config.collection_sample_rate
+        )
+        self.circular_buffer = np.zeros(
+            (
+                self.denoising_samples,  # Number of samples
+                config.subcarriers,  # Number of subcarriers
+                config.antennas.count,  # Number of RX antennas
+                1,  # Number of TX antennas
+            ),
+            dtype=np.complex64,
+        )
+        self.sample_index = 0
 
     @property
     def last_sample(self) -> None | CSIMatrix:
         """
         The last sample of the preprocessor. This is used for low frequency processing
         """
-        return self._last_sample
+        match config.preprocessing.denoising:
+            case "none":
+                return self._last_sample
+            case "median":
+                # Return the median of the last samples
+                median_abs = np.median(np.abs(self.circular_buffer), axis=0)
+                median_angle = np.median(np.angle(self.circular_buffer), axis=0)
+                ans = median_abs * np.exp(1j * median_angle)
+                return ans
+            case "mean":
+                # Return the mean of the last samples
+                return np.mean(self.circular_buffer, axis=0)
+            case _:
+                raise ValueError(
+                    f"Invalid denoising method: {config.preprocessing.denoising}"
+                )
 
     def remove_sto(self, csi: CSIMatrix) -> CSIMatrix:
         """
@@ -134,6 +164,10 @@ class Preprocessor:
         # h_hat -= self.long_term_avg
 
         self._last_sample = h_hat
+
+        if config.preprocessing.denoising != "none":
+            self.circular_buffer[self.sample_index] = h_hat
+            self.sample_index = (self.sample_index + 1) % self.denoising_samples
         return h_hat
 
         # Apply bandpass filter to remove low and high frequency noise