Minor fix in np.linalg.solve dimensions

minor edit in van loans comment section fixed minor comments minor comment edit minor comment edited
decargroup · Jun 27, 2024 · 1f626fd · 1f626fd
1 parent 08b700a
commit 1f626fd
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 5 deletions.
diff --git a/examples/ex_gsf_se2.py b/examples/ex_gsf_se2.py
@@ -52,7 +52,7 @@ def input_profile(t, u):
 
     def gsf_trial(trial_number: int) -> List[nav.GaussianResult]:
         """
-        A single Interacting Multiple Model Filter trial
+        A single Gaussian Sum Filter trial
         """
         np.random.seed(trial_number)
         state_true, input_list, meas_list = dg.generate(x0, 0, t_max, True)

diff --git a/navlie/filters.py b/navlie/filters.py
@@ -1071,7 +1071,9 @@ def correct(
             The current states and their associated weights.
         y : Measurement
             Measurement to correct the state estimate.
-
+        u : Input
+            Input measurement to be given to process model
+            
         Returns
         -------
         MixtureState

diff --git a/navlie/utils/common.py b/navlie/utils/common.py
@@ -175,7 +175,8 @@ def van_loans(
     Q_c: np.ndarray,
     dt: float,
 ) -> Tuple[np.ndarray, np.ndarray]:
-    """Van Loan's method for computing the discrete-time A and Q matrices.
+    """
+    Van Loan's method for computing the discrete-time A and Q matrices.
 
     Given a continuous-time system of the form
 
@@ -672,8 +673,14 @@ def __getitem__(self, key):
             out.nees = out.error**2 / out.covariance.flatten()
             out.dof = np.ones_like(out.stamp)
         else:
+            n_times = out.covariance.shape[0]
+            n_error = out.covariance.shape[1]
             out.nees = np.sum(
-                out.error * np.linalg.solve(out.covariance, out.error), axis=1
+                out.error
+                * np.linalg.solve(
+                    out.covariance, out.error.reshape((n_times, n_error, 1))
+                ).reshape((n_times, n_error)),
+                axis=1,
             )
             out.dof = out.error.shape[1] * np.ones_like(out.stamp)
 

diff --git a/tests/test_examples.py b/tests/test_examples.py
@@ -22,7 +22,7 @@
 examples/ex_varying_noise.py
 examples/ex_slam.py
 examples/ex_gaussian_mixture.py
-examples/ex_gsf_se3.py
+examples/ex_gsf_se2.py
 """