adding minor changes to most files, and some modifications to IMM filter.

Author: vweirens

src/gncpy/control/elqr.py

@@ -832,6 +832,7 @@ def calculate_control(
             print("Starting ELQR optimization loop...")
 
         for ii in range(self.max_iters):
+            print(f"Loop Iteration : {ii}")
             # forward pass
             traj = self.forward_pass(
                 ii,
@@ -844,12 +845,13 @@ def calculate_control(
                 inv_state_args,
                 inv_ctrl_args,
             )
-
+            print("Quadratizing final cost")
             # quadratize final cost
             traj = self.quadratize_final_cost(
                 ii, num_timesteps, traj, time_vec, cost_args
             )
 
+            print("Starting Backward Pass")
             # backward pass
             traj = self.backward_pass(
                 ii,

src/gncpy/dynamics/basic/clohessy_wiltshire_orbit.py

@@ -29,7 +29,7 @@ def __init__(self, **kwargs):
         else:
             self.__model = cpp_bindings.ClohessyWiltshire(0.01, 0.0)
         if "control_model" in kwargs and kwargs["control_model"] is not None:
-            self.__model.set_control_model(kwargs("control_model"))
+            self.__model.set_control_model(kwargs["control_model"])
 
     @property
     def allow_cpp(self):

src/gncpy/dynamics/basic/clohessy_wiltshire_orbit2d.py

@@ -30,13 +30,13 @@ def __init__(self, mean_motion=None, **kwargs):
         super().__init__(**kwargs)
         self.__controlParams = cpp_control.ControlParams()
         self.__stateTransParams = cpp_bindings.StateTransParams()
-        
+
         if self.mean_motion is not None:
             self.__model = cpp_bindings.ClohessyWiltshire2D(0.01, mean_motion)
         else:
             self.__model = cpp_bindings.ClohessyWiltshire2D(0.01, 0.0)
         if "control_model" in kwargs and kwargs["control_model"] is not None:
-            self.__model.set_control_model(kwargs("control_model"))
+            self.__model.set_control_model(kwargs["control_model"])
 
     @property
     def allow_cpp(self):

src/gncpy/dynamics/basic/double_integrator.py

@@ -20,7 +20,7 @@ def __init__(self, **kwargs):
         self.__stateTransParams = cpp_bindings.StateTransParams()
         self.__model = cpp_bindings.DoubleIntegrator(0.1)
         if "control_model" in kwargs and kwargs["control_model"] is not None:
-            self.__model.set_control_model(kwargs("control_model"))
+            self.__model.set_control_model(kwargs["control_model"])
 
     @property
     def allow_cpp(self):

src/gncpy/filters/extended_kalman_filter.py

@@ -10,6 +10,14 @@
 from gncpy.filters.kalman_filter import KalmanFilter
 
 
+# def _cont_dyn(self, t, x, *args):
+#         """Used in integrator if an ode list is specified."""
+#         out = np.zeros(x.shape)
+
+#         for ii, f in enumerate(self._ode_lst):
+#             out[ii] = f(t, x, *args)
+#         return out
+
 class ExtendedKalmanFilter(KalmanFilter):
     """Implementation of a continuous-discrete time Extended Kalman Filter.
 
@@ -127,8 +135,10 @@ def set_state_model(self, dyn_obj=None, ode_lst=None):
             msg = "Invalid state model specified. Check arguments"
             raise RuntimeError(msg)
 
+    # @staticmethod
     def _cont_dyn(self, t, x, *args):
         """Used in integrator if an ode list is specified."""
+
         out = np.zeros(x.shape)
 
         for ii, f in enumerate(self._ode_lst):

src/gncpy/filters/interacting_multiple_model.py

@@ -211,15 +211,16 @@ def correct(self, timestep, meas, *args, **kwargs):
                 self.cov_list[ii] = filt.cov.copy()
             new_weight_list[ii] = meas_fit_prob_list[ii] * self.filt_weights[ii]
         out_meas_fit_prob = np.sum(new_weight_list)
-        if np.sum(new_weight_list) == 0:
-            new_weight_list = new_weight_list * 0
-        else:
+        if np.sum(new_weight_list) != 0:
             new_weight_list = new_weight_list / np.sum(new_weight_list)
-        self.filt_weights = new_weight_list
+            self.filt_weights = new_weight_list
+        else:
+            self.filt_weights = np.array([1/len(new_weight_list) for x in range(1, len(new_weight_list))])
 
         out_state = np.zeros(self.mean_list[0].shape)
         for ii in range(len(self.in_filt_list)):
-            out_state += new_weight_list[ii] * self.mean_list[ii]
+            out_state += self.filt_weights[ii] * self.mean_list[ii]
+            # out_state += new_weight_list[ii] * self.mean_list[ii]
 
         self.cur_out_state = out_state
         return (out_state, out_meas_fit_prob)

test/unit/test_dynamics.py

@@ -449,13 +449,13 @@ def test_clohessy_wiltshire_control():
 
         plt.close("all")
 
-    test_double_integrator_mat()
-    test_double_integrator_prop()
-    test_double_integrator_control()
+    # test_double_integrator_mat()
+    # test_double_integrator_prop()
+    # test_double_integrator_control()
 
-    test_clohessy_wiltshire2d_mat()
-    test_clohessy_wiltshire2d_prop()
-    test_clohessy_wiltshire2d_control()
+    # test_clohessy_wiltshire2d_mat()
+    # test_clohessy_wiltshire2d_prop()
+    # test_clohessy_wiltshire2d_control()
 
     test_clohessy_wiltshire_mat()
     test_clohessy_wiltshire_prop()