fixed control.forced_response error

Author: forgi86

.idea/efficient-calibration-embedded-MPC.iml

@@ -74,17 +74,17 @@ def kalman_design_simple(A, B, C, D, Qn, Rn, type='predictor'):
      \hat y_{k}   = Cx_k + Du_k
     """
 
-    P,W,K, = control.dare(np.transpose(A), np.transpose(C), Qn, Rn)
+    P, W, K, = control.dare(np.transpose(A), np.transpose(C), Qn, Rn)
 #    L = np.transpose(K) # Kalman gain
 
     if type == 'filter':
-        L = P@np.transpose(C) @ sp.linalg.basic.inv(C@P@np.transpose(C)+Rn)
+        L = P @ np.transpose(C) @ sp.linalg.basic.inv(C @ P @ np.transpose(C) + Rn)
     elif type == 'predictor':
-        L = A@P@np.transpose(C) @ sp.linalg.basic.inv(C@P@np.transpose(C)+Rn)
+        L = A @ P @ np.transpose(C) @ sp.linalg.basic.inv(C @ P @np.transpose(C) + Rn)
     else:
         raise ValueError("Unknown Kalman design type. Specify either filter or predictor!")
 
-    return L,P,W
+    return L, P, W
 
 
 class LinearStateEstimator:
@@ -102,7 +102,7 @@ def __init__(self, x0, A, B, C, D, L=None):
         self.nu = __second_dim__(B) # number of controlled inputs
         self.ny = __first_dim__(C)
 
-    def out_y(self,u):
+    def out_y(self, u):
         return self.y
 
     def predict(self, u):
@@ -131,7 +131,7 @@ def sim(self, u_seq, x=None):
         for i in range(Np):
             u_tmp = u_seq[i]
             y[i,:] = self.C @ x_tmp + self.D @ u_tmp
-            x_tmp = self.A @x_tmp + self.B @ u_tmp
+            x_tmp = self.A @ x_tmp + self.B @ u_tmp
 
         #y[Np] = self.C @ x_tmp + self.D @ u_tmp # not really true for D. Here it is 0 anyways
         return y

.idea/misc.xml

@@ -142,7 +142,7 @@ def simulate_pendulum_MPC(sim_options):
     N_sim_d = N_sim_d + N_skip
     e = np.random.randn(N_sim_d)
     te = np.arange(N_sim_d) * Ts_fast
-    _, d, _ = control.forced_response(Hu, te, e)
+    _, d = control.forced_response(Hu, te, e, return_x=False)
     d_fast = d[N_skip:]
     #td = np.arange(len(d)) * Ts_fast
 
@@ -214,7 +214,7 @@ def simulate_pendulum_MPC(sim_options):
         EMERGENCY_STOP = True
 
     x0_est = x0
-    KF = LinearStateEstimator(x0_est, Ad, Bd, Cd, Dd,L)
+    KF = LinearStateEstimator(x0_est, Ad, Bd, Cd, Dd, L)
 
     # Simulate in closed loop
     len_sim = get_parameter(sim_options, 'len_sim')  # simulation length (s)