Minor update to order in simulation code

botprof · Feb 23, 2022 · 0055f6e · 0055f6e
1 parent f161c4e
commit 0055f6e
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 12 deletions.
diff --git a/control_approx_linearization.py b/control_approx_linearization.py
@@ -65,6 +65,9 @@
 
 for k in range(1, N):
 
+    # Simulate the differential drive vehicle motion
+    x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T)
+
     # Compute the approximate linearization
     A = np.array(
         [
@@ -83,9 +86,6 @@
     u_unicycle = -K.gain_matrix @ (x[:, k - 1] - x_d[:, k - 1]) + u_d[:, k - 1]
     u[:, k] = vehicle.uni2diff(u_unicycle)
 
-    # Simulate the differential drive vehicle motion
-    x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T)
-
 # %%
 # MAKE PLOTS
 

diff --git a/dynamic_extension_tracking.py b/dynamic_extension_tracking.py
@@ -99,6 +99,15 @@
 
 for k in range(1, N):
 
+    # Simulate the vehicle motion
+    x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T)
+
+    # Update the extended system states
+    xi[0, k] = x[0, k]
+    xi[1, k] = x[1, k]
+    xi[2, k] = u_unicycle[0] * np.cos(x[2, k])
+    xi[3, k] = u_unicycle[0] * np.sin(x[2, k])
+
     # Compute the extended linear system input control signals
     eta = K.gain_matrix @ (xi_d[:, k - 1] - xi[:, k - 1]) + ddz_d[:, k - 1]
 
@@ -116,15 +125,6 @@
     # Convert unicycle inputs to differential drive wheel speeds
     u[:, k] = vehicle.uni2diff(u_unicycle)
 
-    # Simulate the vehicle motion
-    x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T)
-
-    # Update the extended system states
-    xi[0, k] = x[0, k]
-    xi[1, k] = x[1, k]
-    xi[2, k] = u_unicycle[0] * np.cos(x[2, k])
-    xi[3, k] = u_unicycle[0] * np.sin(x[2, k])
-
 # %%
 # MAKE PLOTS