Test

Author: FlorianPfaff

pyrecest/filters/kernel_sme_filter.py

@@ -202,8 +202,9 @@ def update_linear(
             lambda_multimeas,
         )
 
-        if linalg.cond(sigma_s) > 1e10:
-            sigma_s = sigma_s + 1e-5 * eye(sigma_s.shape[0])
+        # Add jitter to sigma_s for numerical stability
+        jitter = 1e-6 * eye(sigma_s.shape[0]) 
+        sigma_s = sigma_s + jitter
 
         x_posterior = self.x + sigma_xs @ linalg.solve(sigma_s, pseudo_meas - mu_s)
         self.C = self.C - sigma_xs @ linalg.solve(sigma_s, sigma_xs.T)
@@ -236,9 +237,8 @@ def calc_pseudo_meas(testPoints, measurements, kernel_width):
         for i in range(nTestPoints):
             a_i = testPoints[:, i]
             for j in range(nMeas):
-                pseudoMeas[i] += random.multivariate_normal.pdf(
-                    a_i, mean=measurements[:, j], cov=kernel_width * eye(measDim)
-                )
+                gaussian_curr = GaussianDistribution(measurements[:, j], kernel_width * eye(measDim))
+                pseudoMeas[i] += gaussian_curr.pdf(a_i)
         return pseudoMeas
 
     # pylint: disable=too-many-arguments,too-many-locals,too-many-positional-arguments,too-many-statements,too-many-branches
@@ -313,19 +313,15 @@ def calc_moments(
         for i in range(n_testpoints):
             z = testPoints[:, i]
             for k in range(n_targets):
-                P_target[i, k] = random.multivariate_normal.pdf(
-                    z, mean=meas_means[k], cov=meas_cov_kernel[k]
-                )
+                gd_curr = GaussianDistribution(meas_means[k], meas_cov_kernel[k])
+                P_target[i, k] = gd_curr.pdf(z) 
 
         # Clutter pdf at testpoints: N(a_i; 0, clutterCov + Gamma I)
         clutter_cov_kernel = clutterCov + kernel_width * I_meas
         clutter_pdf = empty(n_testpoints)
+        gd_clutter = GaussianDistribution(zeros(meas_dim), clutter_cov_kernel)
         for i in range(n_testpoints):
-            clutter_pdf[i] = random.multivariate_normal.pdf(
-                testPoints[:, i],
-                mean=zeros(meas_dim),
-                cov=clutter_cov_kernel,
-            )
+            clutter_pdf[i] = gd_clutter.pdf(testPoints[:, i])
 
         # Mean of pseudo-measurements:
         # mu_s[i] = sum_l lambda_l P_l^Gamma(a_i) + lambda_c * clutter_pdf[i]
@@ -335,14 +331,11 @@ def calc_moments(
         # Pij[i, j, k] = P_l^{Gamma/2}((a_i + a_j)/2)
         Pij = zeros((n_testpoints, n_testpoints, n_targets))
         for k in range(n_targets):
+            gd_curr = GaussianDistribution(meas_means[k], meas_cov_half_kernel[k])
             for i in range(n_testpoints):
                 for j in range(n_testpoints):
                     midpoint = 0.5 * (testPoints[:, i] + testPoints[:, j])
-                    Pij[i, j, k] = random.multivariate_normal.pdf(
-                        midpoint,
-                        mean=meas_means[k],
-                        cov=meas_cov_half_kernel[k],
-                    )
+                    Pij[i, j, k] = gd_curr.pdf(midpoint)
 
         # Covariance of pseudo-measurements Sigma_s
         sigma_s = zeros((n_testpoints, n_testpoints))
@@ -359,11 +352,10 @@ def calc_moments(
                     )
 
                 # Gaussian kernel between test points: N(a_i; a_j, 2 Gamma I)
-                kernel_between = random.multivariate_normal.pdf(
-                    testPoints[:, i],
-                    mean=testPoints[:, j],
-                    cov=2.0 * kernel_width * I_meas,
+                gd_curr = GaussianDistribution(
+                    testPoints[:, j], 2.0 * kernel_width * I_meas
                 )
+                kernel_between = gd_curr.pdf(testPoints[:, i])
 
                 # --- term 2: kernel_between * sum_l (lambda_l^2 P_l^{Gamma/2}(midpoint)) ---
                 term2 = kernel_between * sum(
@@ -374,11 +366,10 @@ def calc_moments(
                 clutter_i = clutter_pdf[i]
                 clutter_j = clutter_pdf[j]
                 midpoint = 0.5 * (testPoints[:, i] + testPoints[:, j])
-                clutter_mid_pdf = random.multivariate_normal.pdf(
-                    midpoint,
-                    mean=zeros(meas_dim),
-                    cov=clutter_cov_kernel,
+                gd_clutter = GaussianDistribution(
+                    zeros(meas_dim), clutter_cov_kernel
                 )
+                clutter_mid_pdf = gd_clutter.pdf(midpoint)
 
                 term3 = (lam_c**2) * clutter_i * clutter_j
                 term4 = mu_s[i] * lam_c * clutter_j