Next

Author: FlorianPfaff

pyrecest/filters/kernel_sme_filter.py

@@ -1,4 +1,4 @@
-import filterpy
+import bayesian_filters
 import numpy as np
 import scipy
 from pyrecest.distributions import GaussianDistribution
@@ -8,6 +8,7 @@
 
 from .abstract_multitarget_tracker import AbstractMultitargetTracker
 
+from pyrecest.backend import zeros, hstack, mean, ones, eye
 
 class KernelSMEFilter(AbstractMultitargetTracker):
     def __init__(
@@ -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,7 +239,7 @@ 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"
@@ -255,7 +256,7 @@ def calc_moments(
         ]
         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
@@ -288,7 +289,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,
             )
 
@@ -361,7 +362,7 @@ def calc_moments(
         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)