Merge pull request #10 from kajiLabTeam/issues/#5

Issues/#5

pyproject.toml

@@ -30,6 +30,9 @@ ignore = [
     "N803",  # variable in except block
     "N815", # variable in except block
     "PLR0913", # prefer using f-strings
+    "N802", # function name should be lowercase
+    "D400", # first line should end with a period
+    "D415", # first line should end with a period
 ]
 unfixable = [
     "F401",   # unused import

server/domain/models/estimated_particle/likelihood.py

@@ -6,77 +6,72 @@
 
 if TYPE_CHECKING:
     from domain.models.particle.particle import Particle
+    from numpy.typing import NDArray
+
+import pandas as pd
 
 
 class Likelihood:
-    def __init__(
-        self,
-        particles: list[Particle],
-    ) -> None:
-        self.__particles = particles
-        self.__mean_x = sum(
-            particle.get_coordinate().x for particle in self.__particles
-        ) / len(
-            self.__particles,
-        )
-        self.__mean_y = sum(
-            particle.get_coordinate().y for particle in self.__particles
-        ) / len(
-            self.__particles,
+    def __init__(self, mode_path: str) -> None:
+        self.__rssi_model_df = pd.read_csv(mode_path)
+        self.__x = np.linspace(
+            0, int(max(self.__rssi_model_df["x"])), int(max(self.__rssi_model_df["x"]))
         )
-        self.__std_dev_x = float(
-            np.std([particle.get_coordinate().x for particle in self.__particles]),
+        self.__y = np.linspace(
+            0, int(max(self.__rssi_model_df["y"])), int(max(self.__rssi_model_df["y"]))
         )
-        self.__std_dev_y = float(
-            np.std([particle.get_coordinate().y for particle in self.__particles]),
+        self.__X, self.__Y = np.meshgrid(self.__x, self.__y)
+        self.__rssi = self.__rssi_model_df["rssi"].to_numpy().reshape(self.__X.shape)
+        self.__std_dev = np.std(self.__rssi)
+
+        # キャッシュを初期化
+        self.__likelihood_cache: dict[float, NDArray[np.float64]] = {}
+
+    def __generate_likelihood_function(self, rssi_input: float) -> NDArray[np.float64]:
+        """## rssiを受け取り、rssiモデルを元に座標の確率密度関数を作成する"""
+        # キャッシュをチェック
+        if rssi_input in self.__likelihood_cache:
+            return self.__likelihood_cache[rssi_input]
+
+        # RSSI値と入力されたRSSI値との差を計算
+        rssi_difference = self.__rssi - rssi_input
+
+        # 尤度関数を計算
+        likelihood = np.exp(-0.5 * (rssi_difference / self.__std_dev) ** 2) / (
+            self.__std_dev * np.sqrt(2 * np.pi)
         )
 
-    def get_mean_x(
-        self,
-    ) -> float:
-        return self.__mean_x
+        # 尤度を正規化して総和が1になるようにする
+        likelihood /= np.sum(likelihood)
 
-    def get_mean_y(
-        self,
-    ) -> float:
-        return self.__mean_y
+        # キャッシュに保存
+        self.__likelihood_cache[rssi_input] = likelihood
 
-    def get_std_dev_x(
-        self,
-    ) -> float:
-        return self.__std_dev_x
+        return likelihood
 
-    def get_std_dev_y(
-        self,
-    ) -> float:
-        return self.__std_dev_y
-
-    def calculate_normalized_probability_densities(
-        self,
-    ) -> list[float]:
-        probability_densities: list[float] = [
-            self.__calculate_probability_density(
-                x=particle.get_coordinate().x,
-                y=particle.get_coordinate().y,
-            )
-            for particle in self.__particles
-        ]
-
-        probability_densities_sum = np.sum(probability_densities)
-
-        return [
-            probability_density / probability_densities_sum
-            for probability_density in probability_densities
-        ]
-
-    def __calculate_probability_density(
-        self,
-        x: int,
-        y: int,
+    def __get_likelihood_from_coordinate(
+        self, coordinate: tuple[int, int], likelihood: NDArray[np.float64]
     ) -> float:
-        return (1.0 / (2 * np.pi * self.__std_dev_x * self.__std_dev_y)) * np.exp(
-            -(
-                (x - self.__mean_x) ** 2 / (2 * self.__std_dev_x**2)
-                + (y - self.__mean_y) ** 2 / (2 * self.__std_dev_y**2)
-            ),
+        """## 座標を入力して尤度を取得"""
+        x, y = coordinate
+        # 座標の範囲チェック
+        if (
+            x < self.__X.min()
+            or x > self.__X.max()
+            or y < self.__Y.min()
+            or y > self.__Y.max()
+        ):
+            return 0.0
+
+        # 座標に最も近いインデックスを取得
+        idx_x = (np.abs(self.__X[0, :] - x)).argmin()
+        idx_y = (np.abs(self.__Y[:, 0] - y)).argmin()
+
+        return float(likelihood[idx_y, idx_x])
+
+    def get_likelihood(self, particle: Particle, rssi: float) -> float:
+        """## パーティクルの尤度を取得する."""
+        likelihood = self.__generate_likelihood_function(rssi)
+        return self.__get_likelihood_from_coordinate(
+            (particle.get_coordinate().x, particle.get_coordinate().y), likelihood
         )