Next

Author: FlorianPfaff

pyrecest/filters/kernel_sme_filter.py

@@ -1,6 +1,7 @@
-import filterpy
+import bayesian_filters
 import numpy as np
 import scipy
+from pyrecest.backend import eye, hstack, mean, ones, zeros
 from pyrecest.distributions import GaussianDistribution
 from scipy.linalg import block_diag
 from scipy.spatial.distance import cdist
@@ -40,7 +41,7 @@ def filter_state(self, value: list[GaussianDistribution] | GaussianDistribution)
         self.n_targets = len(value)
         x_list = [prior.mu for prior in value]
         C_list = [prior.C for prior in value]
-        self.x = np.hstack(x_list)
+        self.x = hstack(x_list)
         self.C = scipy.linalg.block_diag(*C_list)
         if self.log_prior_estimates:
             self.store_prior_estimates()
@@ -118,7 +119,7 @@ def update_linear(
         if gating_threshold is None:
             gating_threshold = chi2.ppf(0.99, len(measurements))
         n_meas = measurements.shape[1]
-        kernel_width = np.mean(np.diag(cov_mat_meas)) ** 2
+        kernel_width = mean(np.diag(cov_mat_meas)) ** 2
 
         if enable_gating:
             dists = np.full((self.n_targets, n_meas), np.nan)
@@ -181,10 +182,10 @@ def gen_test_points(measurements, kernel_width):
         meas_dim = measurements.shape[0]
         n_meas = measurements.shape[1]
 
-        # Use high level function from filterpy to generate sigma points
+        # Use high level function from bayesian_filters to generate sigma points
         # Reorder samples to be consistent with the Matlab implementation
         all_sample_points_list = [
-            filterpy.kalman.JulierSigmaPoints(meas_dim).sigma_points(
+            bayesian_filters.kalman.JulierSigmaPoints(meas_dim).sigma_points(
                 measurements[:, i], np.sqrt(kernel_width) * np.eye(meas_dim)
             )[
                 [0, 3, 1, 4, 2], :  # noqa: E203
@@ -199,12 +200,12 @@ def calc_pseudo_meas(testPoints, measurements, kernel_width):
         nMeas = measurements.shape[1]
         measDim = measurements.shape[0]
         nTestPoints = testPoints.shape[1]
-        pseudoMeas = np.zeros(nTestPoints)
+        pseudoMeas = zeros(nTestPoints)
         for i in range(nTestPoints):
             a_i = testPoints[:, i]
             for j in range(nMeas):
                 pseudoMeas[i] += multivariate_normal.pdf(
-                    a_i, mean=measurements[:, j], cov=kernel_width * np.eye(measDim)
+                    a_i, mean=measurements[:, j], cov=kernel_width * eye(measDim)
                 )
         return pseudoMeas
 
@@ -238,35 +239,36 @@ def calc_moments(
 
         # Ensure lambdaMultimeas is a vector of length n_targets
         if isinstance(lambdaMultimeas, (float, int)):
-            lambda_vec = float(lambdaMultimeas) * np.ones(n_targets)
+            lambda_vec = float(lambdaMultimeas) * ones(n_targets)
         else:
             lambda_vec = np.asarray(lambdaMultimeas, dtype=float).reshape(-1)
-            assert lambda_vec.size == n_targets, "lambdaMultimeas must be scalar or length n_targets"
+            assert (
+                lambda_vec.size == n_targets
+            ), "lambdaMultimeas must be scalar or length n_targets"
 
         lam_c = float(falseAlarmRate)
 
         state_dim = x_prior.size // n_targets
 
         # Per-target state blocks and block covariances
         C_blocks_list = [
-            C_prior[i * state_dim : (i + 1) * state_dim,
-                    i * state_dim : (i + 1) * state_dim]
+            C_prior[
+                i * state_dim : (i + 1) * state_dim, i * state_dim : (i + 1) * state_dim
+            ]
             for i in range(n_targets)
         ]
         x_prior_mat = np.reshape(x_prior, (state_dim, n_targets), order="F")
 
-        I_meas = np.eye(meas_dim)
+        I_meas = eye(meas_dim)
 
         # Per-target predicted measurement means and covariances
-        meas_cov_kernel = []       # H C_l H^T + R + Gamma I
+        meas_cov_kernel = []  # H C_l H^T + R + Gamma I
         meas_cov_half_kernel = []  # H C_l H^T + R + 0.5 Gamma I
         meas_means = []
 
         for k in range(n_targets):
             S = (
-                measurement_matrix
-                @ C_blocks_list[k]
-                @ measurement_matrix.T
+                measurement_matrix @ C_blocks_list[k] @ measurement_matrix.T
                 + covMatMeas
             )
             meas_cov_kernel.append(S + kernel_width * I_meas)
@@ -288,7 +290,7 @@ def calc_moments(
         for i in range(n_testpoints):
             clutter_pdf[i] = multivariate_normal.pdf(
                 testPoints[:, i],
-                mean=np.zeros(meas_dim),
+                mean=nzeros(meas_dim),
                 cov=clutter_cov_kernel,
             )
 
@@ -331,9 +333,7 @@ def calc_moments(
                 )
 
                 # --- term 2: kernel_between * sum_l (lambda_l^2 P_l^{Gamma/2}(midpoint)) ---
-                term2 = kernel_between * np.sum(
-                    (lambda_vec ** 2) * Pij[i, j, :]
-                )
+                term2 = kernel_between * np.sum((lambda_vec**2) * Pij[i, j, :])
 
                 # --- clutter-related terms (3)–(6) ---
                 clutter_i = clutter_pdf[i]
@@ -345,39 +345,34 @@ def calc_moments(
                     cov=clutter_cov_kernel,
                 )
 
-                term3 = (lam_c ** 2) * clutter_i * clutter_j
+                term3 = (lam_c**2) * clutter_i * clutter_j
                 term4 = mu_s[i] * lam_c * clutter_j
                 term5 = mu_s[j] * lam_c * clutter_i
                 term6 = lam_c * kernel_between * clutter_mid_pdf
 
                 # Full Sigma^{s_i, s_j}
                 sigma_s[i, j] = (
-                    term1 + term2 + term3 + term4 + term5 + term6
-                    - mu_s[i] * mu_s[j]
+                    term1 + term2 + term3 + term4 + term5 + term6 - mu_s[i] * mu_s[j]
                 )
 
         # Symmetrize Sigma_s
         iu, ju = np.triu_indices(n_testpoints, k=1)
         sigma_s[ju, iu] = sigma_s[iu, ju]
 
         # Cross-covariance Sigma_xs
-        sigma_xs = np.zeros((x_prior.shape[0], n_testpoints))
+        sigma_xs = zeros((x_prior.shape[0], n_testpoints))
 
         # Block columns of C_prior (for K^l)
         C_k_full_columns = np.hsplit(C_prior, n_targets)
         K_list = []
         for k in range(n_targets):
             S_full = (
-                measurement_matrix
-                @ C_blocks_list[k]
-                @ measurement_matrix.T
+                measurement_matrix @ C_blocks_list[k] @ measurement_matrix.T
                 + covMatMeas
                 + kernel_width * I_meas
             )
             K_list.append(
-                C_k_full_columns[k]
-                @ measurement_matrix.T
-                @ np.linalg.inv(S_full)
+                C_k_full_columns[k] @ measurement_matrix.T @ np.linalg.inv(S_full)
             )
 
         # Sigma^{x, s_i}
@@ -390,8 +385,7 @@ def calc_moments(
                     * P_target[i, k]
                     * (
                         x_prior
-                        + K_list[k]
-                        @ (z - measurement_matrix @ x_prior_mat[:, k])
+                        + K_list[k] @ (z - measurement_matrix @ x_prior_mat[:, k])
                     )
                 )
             # clutter term + centering