From 1f626fd87f9e2c15115fc47aff4877ac0e795d77 Mon Sep 17 00:00:00 2001
From: Mitchell Cohen <mitchell.cohen3@mail.mcgill.ca>
Date: Wed, 26 Jun 2024 09:24:32 -0400
Subject: [PATCH] Minor fix in np.linalg.solve dimensions

minor edit in van loans comment section

fixed minor comments

minor comment edit

minor comment edited
---
 examples/ex_gsf_se2.py |  2 +-
 navlie/filters.py      |  4 +++-
 navlie/utils/common.py | 11 +++++++++--
 tests/test_examples.py |  2 +-
 4 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/examples/ex_gsf_se2.py b/examples/ex_gsf_se2.py
index a343836..e6fda7f 100644
--- 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
index a22cbd0..879f17e 100644
--- 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
index 81a1ca9..1bbc394 100644
--- 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
index bb9589d..90fa84a 100644
--- 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
 """