fix bug initialization moving frame reg

trajectory-invariants · Aug 23, 2024 · 77cd1da · 77cd1da
1 parent 87c2bf5
commit 77cd1da
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 3 deletions.
diff --git a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position_mf.py b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position_mf.py
@@ -215,7 +215,7 @@ def calculate_invariants(self, measured_positions, stepsize, use_previous_soluti
             else:  
 
                 # Initialize states and controls using measurements
-                self.values_variables = initialization.initialize_VI_pos2(measured_positions)
+                self.values_variables = initialization.initialize_VI_pos2(measured_positions,stepsize)
                 self.first_time = False
 
         # Solve the optimization problem for the given measurements starting from previous solution

diff --git a/invariants_py/invariants_handler.py b/invariants_py/invariants_handler.py
@@ -35,8 +35,8 @@ def reconstruct_trajectory(invariants, position_init=np.zeros((3,1)), movingfram
     
     Parameters:
     - invariants (numpy array of shape (N,3)): Array of vector invariants.
-    - p_init (numpy array of shape (3,1), optional): Initial position. Defaults to a 3x1 zero array.
-    - mf_init (numpy array of shape (3,3), optional): Initial frame matrix. Defaults to a 3x3 identity matrix.
+    - position_init (numpy array of shape (3,1), optional): Initial position. Defaults to a 3x1 zero array.
+    - movingframe_init (numpy array of shape (3,3), optional): Initial frame matrix. Defaults to a 3x3 identity matrix.
     
     Returns:
     - positions (numpy array of shape (N,3)): Array of reconstructed positions.