def getControlOutput(self, d_est, phi_est, d_ref, phi_ref, v_ref, t_delay, dt_last): #Exception for dt_last=0 if (dt_last == 0): dt_last = 0.0001 #Update state v_ref = self.gain * v_ref x = np.array([[d_est], [phi_est], [self.d_int]]) #Adapt State Space to current time step a = np.array([[1., v_ref * dt_last, 0], [0, 1., 0], [-dt_last, -0.5 * v_ref * dt_last * dt_last, 1]]) b = np.array([[0.5 * v_ref * dt_last * dt_last], [dt_last], [-v_ref * dt_last * dt_last * dt_last / 6]]) #Solve ricatti equation (x_ric, l, g) = control.dare(a, b, self.q, self.r) #feed trough velocity v_out = v_ref #Change sign on on K_I (because Ricatti equation returns [K_d, K_phi, -K_I]) g[[0], [2]] = -g[[0], [2]] #Calculate new input omega_out = -g * x * self.gain #Update integral term self.d_int = self.d_int + d_est * dt_last return (v_out, omega_out)
def dlqe(A, G, C, Q, R): """ Kalmann filter [K,X] = dlqe(A, G, C, Q, R) """ # Get the weighting matrices (converting to matrices, if needed) Q = np.array(Q, ndmin=2, dtype=float); R = np.array(R, ndmin=2, dtype=float); A = np.mat(A) C = np.mat(C) G = np.mat(G) #Solve the riccati equation (X,L,G) = ct.dare(A.T, C.T, G*Q*G.T, R) # Now compute the return value K = X*C.T /(C*X*C.T+R) Z = X - K*C*X Z = (Z+Z.T)/2 return K, X
def gen_preview_control_parameter(Zc, T, t_preview, Qe, R): ####Zc is the height of CoM ####T sampling time ####t_preview is the preview time #### Qe and R is the weight g = 9.8 A_d = np.array([[1, T, T * T / 2], [0, 1, T], [0, 0, 1]]) B_d = np.array([[pow(T, 3) / 6], [pow(T, 2) / 2], [T]]) C_d = np.array([1, 0, -Zc / g]) D_d = np.array([0]) ####A,B,C matrix for LQI control A_tilde = np.array([[1, 1, T, T * T / 2 - Zc / g], [0, 1, T, T * T / 2], [0, 0, 1, T], [0, 0, 0, 1]]) B_tilde = np.array([[pow(T, 3) / 6 - Zc * T / g], [pow(T, 3) / 6], [T * T / 2], [T]]) C_tilde = np.array([1, 0, 0, 0]) Q = np.zeros((4, 4)) Q[0, 0] = Qe I_tilde = np.array([[1], [0], [0], [0]]) [P, DC1, DC2] = control.dare(A_tilde, B_tilde, Q, R) Gi = 1 / (R + np.dot(B_tilde.T, np.dot(P, B_tilde))) * np.dot( B_tilde.T, np.dot(P, I_tilde)) F_tilde = np.vstack((np.dot(C_d, A_d), A_d)) Gx = 1 / (R + np.dot(B_tilde.T, np.dot(P, B_tilde))) * np.dot( B_tilde.T, np.dot(P, F_tilde)) K = np.hstack((Gi, Gx)) Gd = np.zeros((1, 1 + int(t_preview / T))) Gd[0, 0] = -Gi ##Orignial formula in the paper is to complicated, it can be reduced into equation below Ac_tilde = A_tilde - np.dot(B_tilde, K) X_tilde = -np.dot(Ac_tilde.T, np.dot(P, I_tilde)) for i in range(1, 1 + int(t_preview / T)): Gd[0, i] = pow(R + np.dot(B_tilde.T, np.dot(P, B_tilde)), -1) * np.dot( B_tilde.T, X_tilde) X_tilde = np.dot(Ac_tilde.T, X_tilde) return A_d, B_d, C_d, Gi, Gx, Gd
def setup_KF(self): # Derive the optimal controller (LQR). # This sets self.A, self.B, self.Q, self.R and self.gain super().setup_model() # Construct continuous-time linear model Gcss = control.ss(self.A, self.B, self.C, self.D) # Convert to discrete-time model Gdss = control.c2d(Gcss, self.env.tau) A = Gdss.A B = Gdss.B C = Gdss.C D = Gdss.D # Choose KF parameters: process noise covariance # matrix and output measurement noise covariance matrix Q = 0.1**2 * np.eye(4) R = 0.001**2 * np.eye(2) # Calculate discrete-time Kalman filter gain and state # estimation error covariance matrix (P) X, L, G = control.dare(A.T, C.T, Q, R) P = X.T self.kf_gain = G.T self.kf_params = { 'A': A, 'B': B, 'C': C, 'D': D, 'Q': Q, 'R': R, 'P': P, 'L': L } breakpoint()
def kalman_design_simple(A, B, C, D, Qn, Rn, type='filter'): """ Simplified design a Kalman predictor or a Kalman filter for the discrete-time system x_{k+1} = Ax_{k} + Bu_{k} + Iw_{k} y_{k} = Cx_{k} + Du_{k} + I v_{k} with known inputs u and stochastic disturbances v, w. In particular, v and w are zero mean, white Gaussian noise sources with E[vv'] = Qn, E[ww'] = Rn, E[wv'] = 0 The Kalman filter has structure \hat x_{k+1|k+1} = A\hat x_{k|k} + Bu_{k} + L[y_{k+1} - C(A \hat x{k|k} + Bu_{k})] \hat y_{k|k} = Cx_{k|k} where L is the Kalman filter gain The Kalman predictor has structure \hat x_{k+1|k} = Ax_{k|k-1} + Bu_{k} + L[y_{k} - C\hat x{k|k-1}] \hat y_{k|k-1} = Cx_{k|k-1} where L is the Kalman predictor gain """ 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) elif type == 'predictor': 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
def find_minimizer(A, B, Q, R): """Returns a tuple of things from solving a Riccatti equation, the third entry in the tuple is the optimal policy.""" return control.dare(A, B, Q, R)
def compute_optimal_controller(A, B, Q, R): return control.dare(A, B, Q, R)[2]
def __init__(self, dt=1. / 240., Tsup_time=0.5, Tdl_time=0.1, CoMheight=0.45, g=9.8, previewStepNum=240, stride=0.1, initialTargetZMP=np.array([0., 0.]), initialFootPrint=np.array([[[0., 0.065], [0., -0.065]]]), R=np.matrix([1.]), Q=np.matrix([[7000, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])): self._RIGHT_LEG = 1 self._LEFT_LEG = 0 self.dt = dt self.previewStepNum = previewStepNum self.A = np.matrix([[1, dt, (dt**2) / 2], [0, 1, dt], [0, 0, 1]]) self.B = np.matrix([(dt**3) / 6, (dt**2) / 2, dt]).T self.C = np.matrix([1, 0, -CoMheight / g]) self.CoMheight = CoMheight self.G = np.vstack((-self.C * self.B, self.B)) self.Gr = np.matrix([1., 0., 0., 0.]).T #state vector self.x = np.matrix(np.zeros(3)).T self.y = np.matrix(np.zeros(3)).T self.footPrints = np.array([[[0., 0.065], [0., -0.065]], [[0., 0.065], [0., -0.065]], [[0., 0.065], [0., -0.065]]]) self.Tsup = int(Tsup_time / dt) self.Tdl = int(Tdl_time / dt) self.px_ref = np.full((self.Tsup + self.Tdl) * 3, initialTargetZMP[0]) self.py_ref = np.full((self.Tsup + self.Tdl) * 3, initialTargetZMP[1]) self.px = np.array([0.0]) #zmp self.py = np.array([0.0]) self.phi = np.hstack( (np.matrix([1, 0, 0, 0]).T, np.vstack((-self.C * self.A, self.A)))) P, _, _ = control.dare(self.phi, self.G, Q, R) zai = (np.eye(4) - self.G * la.inv(R + self.G.T * P * self.G) * self.G.T * P) * self.phi self.Fr = np.array([]) for j in range(1, previewStepNum + 1): self.Fr = np.append( self.Fr, -la.inv(R + self.G.T * P * self.G) * self.G.T * ((zai.T)**(j - 1)) * P * self.Gr) self.F = -la.inv(R + self.G.T * P * self.G) * self.G.T * P * self.phi self.px_ref_log = self.px_ref[:(self.Tsup + self.Tdl) * 2] self.py_ref_log = self.py_ref[:(self.Tsup + self.Tdl) * 2] self.xdu = 0 self.ydu = 0 self.xu = 0 self.yu = 0 self.dx = np.matrix(np.zeros(3)).T self.dy = np.matrix(np.zeros(3)).T self.swingLeg = self._RIGHT_LEG self.supportLeg = self._LEFT_LEG self.targetZMPold = np.array([initialTargetZMP]) self.currentFootStep = 0
def __init__(self, dT, mpc_horizon, curr_pos, robot_size, lb_state, ub_state, lb_control, ub_control, Q, R, relative_measurement_noise_cov, maxComm_distance, obs, animate, multi_agent): # initialize Optistack class self.opti = casadi.Opti() # dt = discretized time difference self.dT = dT # mpc_horizon = number of time steps for the mpc to look ahead self.N = mpc_horizon # robot_size = input a radius value, where the corresponding circle represents the size of the robot self.robot_size = robot_size # if the uav is to be animated along the same graph as the ugv, then put multi_agent to true self.multi_agent = multi_agent # lower_bound_state = numpy array corresponding to the lower limit of the robot states, e.g. # lb_state = np.array([[-20], [-20], [-pi], dtype=float), the same for the upper limit (ub). Similar symbolic # representation for the controls (lb_control and ub_control) as well self.lb_state = lb_state self.ub_state = ub_state self.lb_control = lb_control self.ub_control = ub_control # initialize obstacles self.obs = obs # distance to obstacle to be used as constraints self.max_obs_distance = 10 # Q and R diagonal matrices, used for the MPC objective function, Q is 3x3, R is 4x4 (first 2 diagonals # represent the cost on linear and angular velocity, the next 2 diagonals represent cost on state slack, # and terminal slack respectively. The P diagonal matrix represents the cost on the terminal constraint. self.relative_measurement_noise_cov = relative_measurement_noise_cov # initialize the maximum distance that robot 1 and 2 are allowed to have for cross communication self.maxComm_distance = maxComm_distance # initialize cross diagonal system noise covariance matrix self.P12 = np.array([[0, 0], [0, 0]]) # bool variable to indicate whether the robot has made first contact with the uav self.first_contact = False self.Q = Q self.R = R # initialize cost on slack self.slack_cost = 10000 # initialize discretized state matrices A and B A1 = 0.7969 A2 = 0.02247 A3 = -1.7976 A4 = 0.9767 B1 = 0.01166 B2 = 0.9921 g = 9.81 KT = 0.91 m = .001 #originally 1.42 self.A = np.array( [[1, self.dT, (g * self.dT**2) / 2, 0, 0, 0, 0, 0, 0, 0], [0, 1, g * self.dT, 0, 0, 0, 0, 0, 0, 0], [0, 0, A1, A2, 0, 0, 0, 0, 0, 0], [0, 0, A3, A4, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 1, self.dT, (g * self.dT**2) / 2, 0, 0, 0], [0, 0, 0, 0, 0, 1, g * self.dT, 0, 0, 0], [0, 0, 0, 0, 0, 0, A1, A2, 0, 0], [0, 0, 0, 0, 0, 0, A3, A4, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, self.dT], [0, 0, 0, 0, 0, 0, 0, 0, 0, 1]]) self.B = np.array([[0, 0, 0], [0, 0, 0], [B1, 0, 0], [B2, 0, 0], [0, 0, 0], [0, 0, 0], [0, B1, 0], [0, B2, 0], [0, 0, (-KT * self.dT**2) / (m * 2)], [0, 0, (-KT * self.dT) / m]]) self.P, _, _ = control.dare(self.A, self.B, self.Q, self.R) # TODO: adding the C matrix does not work, potentially a problem with equations #self.C = np.array([[0],[0],[0],[0],[0],[0],[0],[0],[0.0031], [0.2453]]) # Current covariance of robot self.r1_cov_curr = np.array([[0.01 + self.robot_size, 0], [0, 0.01 + self.robot_size]]) # initialize robot's current position self.curr_pos = curr_pos # initialize measurement noise (in our calculation, measurement noise is set by the user and is gaussian, # zero-mean). It largely represents the noise due to communication transmitters, or other sensor devices. It # is assumed to be a 3x3 matrix (x, y, and theta) for both robots self.relative_measurement_noise_cov = relative_measurement_noise_cov self.initVariables() # initialize parameters for animation if animate: if not self.multi_agent: plt.ion() fig = plt.figure() fig.canvas.mpl_connect( 'key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) self.ax = fig.add_subplot(111, projection='3d') self.ax = Axes3D(fig) u = np.linspace(0, 2 * np.pi, 100) v = np.linspace(0, np.pi, 100) self.x_fig = np.outer(self.robot_size * np.cos(u), np.sin(v)) self.y_fig = np.outer(self.robot_size * np.sin(u), np.sin(v)) self.z_fig = np.outer(self.robot_size * np.ones(np.size(u)), np.cos(v))
Q = numpy.array([ [weighting['state']['arm_angle'], 0, 0, 0], [0, weighting['state']['pendulum_angle'], 0, 0], [0, 0, weighting['state']['arm_angle_velocity'], 0], [0, 0, 0, weighting['state']['pendulum_angle_velocity']] ]) # weight for control input R = weighting['input'] # discretization Qd = Q * T Rd = R * T # solve Riccati equation P, L, F = control.dare(Ad, Bd, Qd, Rd) # P:solution of Riccati equation # L:eigen value of closed loop system # F:linear state Fb gain vector # =============================================================== # Furuta pendulum simulator # state vector # x = (x1,x2,x3,x4) # = (theta_a, phi_p, dot_theta_a, dot_phi_p) # Non-lienar equation of motion # M(x2)ddot_[x3 \\ x4] = - D[x3 \\ x4] + Bv + h(x2,x3,x4)
def __init__(self, dT, mpc_horizon, curr_pos, robot_size, lb_state, ub_state, lb_control, ub_control, Q, R, angle_noise_r1, angle_noise_r2, relative_measurement_noise_cov, maxComm_distance, obs, animate): # initialize Optistack class self.opti = casadi.Opti() # dt = discretized time difference self.dT = dT # mpc_horizon = number of time steps for the mpc to look ahead self.N = mpc_horizon # robot_size = input a radius value, where the corresponding circle represents the size of the robot self.robot_size = robot_size # lower_bound_state = numpy array corresponding to the lower limit of the robot states, e.g. # lb_state = np.array([[-20], [-20], [-pi], dtype=float), the same for the upper limit (ub). Similar symbolic # representation for the controls (lb_control and ub_control) as well self.lb_state = lb_state self.ub_state = ub_state self.lb_control = lb_control self.ub_control = ub_control # Q and R diagonal matrices, used for the MPC objective function, Q is 3x3, R is 4x4 (first 2 diagonals # represent the cost on linear and angular velocity, the next 2 diagonals represent cost on state slack, # and terminal slack respectively. The P diagonal matrix represents the cost on the terminal constraint. self.Q = Q self.R_dare = R self.R = np.array([[R[0,0], 0], [0, R[2,2]]]) # initialize discretized state matrices A and B (note, A is constant, but B will change as it is a function of # state theta) self.A = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) self.B = np.array([[self.dT, 0, 0], [0, self.dT, 0], [0, 0, self.dT]]) # initialize the P matrix, which is the cost matrix that defines the optimal state feedback controller self.P, _, _ = control.dare(self.A, self.B, self.Q, self.R_dare) # initalize cost on slack self.slack_cost = 1000 # initialize measurement noise (in our calculation, measurement noise is set by the user and is gaussian, # zero-mean). It largely represents the noise due to communication transmitters, or other sensor devices. It # is assumed to be a 3x3 matrix (x, y, and theta) for both robots self.relative_measurement_noise_cov = relative_measurement_noise_cov # we assume that there is constant noise in angle (while x and y are dynamically updated) - should be a variance # value self.angle_noise_r1 = angle_noise_r1 self.angle_noise_r2 = angle_noise_r2 # initialize the maximum distance that robot 1 and 2 are allowed to have for cross communication self.maxComm_distance = maxComm_distance # distance to obstacle to be used as constraints self.max_obs_distance = 20 # initialize obstacles self.obs = obs # initialize robot's current position self.curr_pos = curr_pos # self.change_goal_point(goal_pos) # initialize the current positional uncertainty (and add the robot size to it) # TODO: this is a temporary fix for testing self.r1_cov_curr = np.array([[0.1 + self.robot_size, 0], [0, 0.1 + self.robot_size]]) # initialize cross diagonal system noise covariance matrix self.P12 = np.array([[0, 0], [0, 0]]) # bool variable to indicate whether the robot has made first contact with the uav self.first_contact = False # initialize state, control, and slack variables self.initVariables() # initialize states for DQN (relative distance between robot-goal, and robot-obstacles num_obs_const = 0 for i in range(1, len(self.obs)+1): num_obs_const += self.obs[i]['polygon_type'] self.dqn_states = np.zeros((num_obs_const,)) # initialize parameters for animation if animate: plt.ion() fig = plt.figure() fig.canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) self.ax = fig.add_subplot(111, projection='3d') self.ax = Axes3D(fig) u = np.linspace(0, 2 * np.pi, 100) v = np.linspace(0, np.pi, 100) self.x_fig = np.outer(self.robot_size * np.cos(u), np.sin(v)) self.y_fig = np.outer(self.robot_size * np.sin(u), np.sin(v)) self.z_fig = np.outer(self.robot_size * np.ones(np.size(u)), np.cos(v))
def get_KL(ss, Q, R): A, B, C, Bw, W, V = ss K = control.dare(A, B, Q, R)[2] L = control.dare(A.T, C.T, Bw @ Bw.T * W, V)[2] return torch.tensor(K), torch.tensor(L)
m = np.shape(B)[1] # Discretizing the continuous-time system C = np.zeros((n, n)) D = np.zeros((n, m)) dt = 0.05 sys1 = control.StateSpace(A, B, C, D) sysd = sys1.sample(dt) A = np.asarray(sysd.A) B = np.asarray(sysd.B) # LQR control for the original system Q = np.eye(np.shape(A)[0]) R = np.eye(np.shape(B)[1]) (P1, L1, K1) = control.dare(A, B, Q, R) # Adding dA Ur, _, _ = la.linalg.svd(B, 0) iterator = 0 while 1: np.random.seed(iterator) dA = np.zeros((4, 4)) dA[0:4, 0:4] = np.random.normal(0, .05, (4, 4)) tilde_A = (np.eye(n) - Ur @ Ur.T) @ (A + dA) if np.max(np.abs(la.eigvals(tilde_A))) < 1.: break iterator += 1 A = A + dA
def Trajectory_generator(A, B, K_old, x0, noise, alpha, number_of_iteration, control_mode): ''' generate the trajectory and upper-bound based on DGR paper and compare it to DeePC paper ''' # System dimensions n = np.shape(A)[0] m = np.shape(B)[1] mean_w = 0 ############# Implementing DeePC for comparison ################### # Parameters for DeePC algorithm (for comparison) if control_mode == 0 or control_mode == 2: T_ini = 1 # 8 N_deepc = 4 # 30 else: T_ini = 1 N_deepc = 5 # L_deepc = T_ini + N_deepc T_deepc = (m+1)*(T_ini+N_deepc + n) # solver parameters q = 80 r = 0.000000001 lam_sigma = 800000 lam_g = 1 X_deepc = [x0] U_deepc = [] # offline data data_u = [] data_x = [x0] # offline data generation for t in range(T_deepc): data_u.append(-K_old@data_x[-1] + np.array(np.random.normal(mean_w, 2, size=(m, 1)))) data_x.append(A @ data_x[-1] + B @ data_u[-1]) print('DeePC started') # DeePC implementation for t in range(number_of_iteration): if noise: if control_mode == 0 or control_mode == 2: w = np.array(np.random.normal(mean_w, 0.001, size=(n, 1))) else: w = np.array(np.random.normal(mean_w, 0.01, size=(n, 1))) else: w = np.zeros((n, 1)) if t <= T_ini: U_deepc.append(np.array(np.random.normal(mean_w, 0.1, size=(m, 1)))) X_deepc.append(A @ X_deepc[-1] + B @ U_deepc[-1] + w) else: if la.norm(X_deepc[-1]) > 1000: print('DeePC is blown up at') print(t) break try: u_N_deepc = deepc(U_deepc, X_deepc[:-1], data_u, data_x[:-1], T_ini, N_deepc, q, r, lam_sigma, lam_g) except: print('Deepc is stopped at iteration') print(t) break U_deepc.append(u_N_deepc) X_deepc.append(A@X_deepc[-1] + B @ U_deepc[-1] + w) print('DeePC ended') ###################### Algorithm 1 implementation #################### # Parameters for Algorithm 1 X = [x0] Y = [] K = [np.zeros((m, n))] u = [] G_alpha = la.pinv(alpha * np.eye(m) + B.T @ B) @ B.T X_old = [x0] X_new = [x0] print('DGR started') for t in range(number_of_iteration): if noise: if control_mode == 0 or control_mode == 2: w = np.array(np.random.normal(mean_w, 0.001, size=(n, 1))) else: w = np.array(np.random.normal(mean_w, 0.01, size=(n, 1))) else: w = np.zeros((n, 1)) u.append(-K[-1] @ X[-1]) X.append(A @ X[-1] + B @ u[-1] + w) Y.append(X[-1] - B @ u[-1] ) K.append(G_alpha @ np.hstack(Y) @ la.pinv(np.hstack(X[:-1]))) if t < T_deepc: X_new.append(X[-1]) elif t == T_deepc: xposition = t Q = np.eye(np.shape(A)[0]) if control_mode == 0 or control_mode ==2: R = 0.0000001*np.eye(np.shape(B)[1]) else: R = 0.0000001*np.eye(np.shape(B)[1]) A_hat = np.hstack(Y) @ la.pinv(np.hstack(X[:-1])) (_,_,K1) = control.dare(A_hat, B, Q, R) X_new.append((A-B@K1) @ X_new[-1] + w) else: X_new.append((A-B@K1) @ X_new[-1] + w) if la.norm(X_old[-1]) < 200: X_old.append((A - B @ K_old) @ X_old[-1] + w) print('DGR ended') # Find upper-bound based on Lemma 2 z = [X[0]] z_bar = [X[0] / la.norm(X[0])] w_bar = [X[0] / la.norm(X[0]) - z_bar[0]] for t in range(1, number_of_iteration+1): X_subspace = np.hstack(X[:t]) l = len(X_subspace) Ux, _ , _ = la.svd(X_subspace, 0) z.append((np.eye(l) - Ux @ Ux.T) @ X[t]) if la.norm(z[-1]) > 10e-12: z_bar.append(z[-1] / la.norm(z[-1])) w_bar.append(X[t]/la.norm(X[t]) - z_bar[-1]) else: z_bar.append(np.zeros((n, 1))) w_bar.append(X[t] / la.norm(X[t])) Ur, _, _ = la.linalg.svd(B, 0) tilde_A = (np.eye(n) - Ur @ Ur.T) @ A tilde_B = Ur @ Ur.T @ A a_t = [la.norm(A)] for t in range(1, number_of_iteration + 1): a_t.append(la.norm(la.matrix_power(tilde_A, t) @ A, 2)) L = [0] UB = [la.norm(X[0])] L.append(la.norm(A @ z_bar[0])) UB.append(L[-1] * la.norm(X[0])) Delta = B @ (la.pinv(B)-G_alpha) @ A for t in range(2, number_of_iteration + 1): sum_bl = 0 for r in range(1, t): sum_bl += np.sqrt( la.norm( la.matrix_power(tilde_A, t-1-r) @ tilde_B @ z_bar[r])**2 + la.norm( la.matrix_power(tilde_A, t-1-r) @ Delta @ w_bar[r])**2 ) * L[r] L.append( a_t[t-1] + sum_bl ) UB.append( L[-1] * la.norm(X[0]) ) # ###################### DPC for a system after DGR is done ############### # online data generation for a system with DGR in the closed loop data_u = [] data_x = [x0] # Parameters for DGR Y = [] u_dgr = [] K_dgr = [np.zeros((m, n))] G_alpha = la.pinv(alpha * np.eye(m) + B.T @ B) @ B.T for t in range(T_deepc): if noise: if control_mode == 0 or control_mode == 2: w = np.array(np.random.normal(mean_w, 0.001, size=(n, 1))) else: w = np.array(np.random.normal(mean_w, 0.01, size=(n, 1))) else: w = np.zeros((n, 1)) data_u.append(np.array(np.random.normal(mean_w, 1, size=(m, 1)))) u_dgr.append(-K_dgr[-1] @ data_x[-1] + data_u[-1]) data_x.append(A @ data_x[-1] + B@u_dgr[-1] + w) Y.append(data_x[-1] - B @ u_dgr[-1]) K_dgr.append(G_alpha @ np.hstack(Y) @ la.pinv(np.hstack(data_x[:-1]))) print('DGR+DeePC started') # DeePC implementation X_deepc_dgr = [] U_deepc_dgr = [] X_deepc_dgr[0:T_deepc] = data_x U_deepc_dgr[0:T_deepc] = data_u # solver parameters if control_mode == 0 or control_mode == 2: q = 400 r = 0.05 lam_sigma = 10000 lam_g = 1 elif control_mode == 1 or control_mode == 3: q = 400 r = 0.05 lam_sigma = 10000 lam_g = 1 for t in range(T_deepc, number_of_iteration): if noise: if control_mode == 0 or control_mode == 2: w = np.array(np.random.normal(mean_w, 0.001, size=(n, 1))) else: w = np.array(np.random.normal(mean_w, 0.01, size=(n, 1))) else: w = np.zeros((n, 1)) if la.norm(X_deepc_dgr[-1]) > 1000: print('DeePC is blown up at') print(t) break try: u_N_deepc_dgr = deepc(U_deepc_dgr, X_deepc_dgr[:-1], data_u, data_x[:-1], T_ini, N_deepc, q, r, lam_sigma, lam_g) except: print('DeePC is stopped at iteration') print(t) break U_deepc_dgr.append(u_N_deepc_dgr) X_deepc_dgr.append(A @ X_deepc_dgr[-1] + B @(-K_dgr[-1]@ X_deepc_dgr[-1] + U_deepc_dgr[-1]) + w) print('DGR+DeePC ended') return X, X_old, UB, X_new, xposition, K, X_deepc, X_deepc_dgr
# ]) B = tau * np.array([[0], [1 / M], [0], [b / (M * L)]]) Q = np.eye(4) # state cost, 4x4 identity matrix R = 0.0001 # control cost def f(x, u): return A @ x + B @ u #%% Traditional LQR # lq = dlqr(A, B, Q, R) # C = lq[0] #%% Solve riccati equation soln = dare(A * np.sqrt(gamma), B * np.sqrt(gamma), Q, R) P = soln[0] C = np.linalg.inv(R + gamma * B.T @ P @ B) @ (gamma * B.T @ P @ A) #%% Construct snapshots of u from random agent and initial states x0 x0 = np.array([[-1], [0], [np.pi], [0]]) perturbation = np.array([[0], [0], [np.random.normal(0, 0.05)], [0]]) x = x0 + perturbation N = 10000 action_range = 50 state_range = 10 U = np.random.rand(1, N) * action_range * np.random.choice(np.array([-1, 1]), size=(1, N)) X0 = np.random.rand(4, N) * state_range * np.random.choice(np.array([-1, 1]),
def Trajectory_generator(A, B, K_old, x0, noise, alpha): ''' generate the trajectory and upper-bound based on DGR ''' # System dimensions n = np.shape(A)[0] m = np.shape(B)[1] # Parameters for Algorithm 1 X = [x0] Y = [] K = [np.zeros((m, n))] u = [] G_alpha = la.pinv(alpha * np.eye(m) + B.T @ B) @ B.T X_old = [x0] X_new = [x0] number_of_iteration = 40 * n mean_w = 0 # Algorithm 1 implementation for t in range(number_of_iteration): if noise: w = np.array(np.random.normal(mean_w, 0.01, size=(n, 1))) else: w = np.zeros((n, 1)) u.append(-K[-1] @ X[-1]) X.append(A @ X[-1] + B @ u[-1] + w) Y.append(X[-1] - B @ u[-1]) A_hat = np.hstack(Y) @ la.pinv(np.hstack(X[:-1])) K.append(G_alpha @ A_hat) if t < 30: X_new.append(X[-1]) elif t == 30: xposition = t Y_ = np.hstack(Y) X_ = np.hstack(X[:-1]) A1_hat = Y_[0:4, :] @ la.pinv(X_[0:4, :]) A2_hat = Y_[4:8, :] @ la.pinv(X_[4:8, :]) A_hat = 0 * A_hat A_hat[0:4, 0:4] = A1_hat[0:4, 0:4] A_hat[4:8, 4:8] = A2_hat[0:4, 0:4] Q = np.eye(np.shape(A)[0]) R = np.eye(np.shape(B)[1]) (P1, L1, K1) = control.dare(A_hat, B, Q, R) X_new.append((A - B @ K1) @ X_new[-1] + w) else: X_new.append((A - B @ K1) @ X_new[-1] + w) X_old.append((A - B @ K_old) @ X_old[-1] + w) # Find upper-bound based on Lemma 2 z = [X[0]] z_bar = [X[0] / la.norm(X[0])] w_bar = [X[0] / la.norm(X[0]) - z_bar[0]] for t in range(1, number_of_iteration + 1): X_subspace = np.hstack(X[:t]) l = len(X_subspace) Ux, _, _ = la.svd(X_subspace, 0) z.append((np.eye(l) - Ux @ Ux.T) @ X[t]) if la.norm(z[-1]) > 10e-12: z_bar.append(z[-1] / la.norm(z[-1])) w_bar.append(X[t] / la.norm(X[t]) - z_bar[-1]) else: z_bar.append(np.zeros((n, 1))) w_bar.append(X[t] / la.norm(X[t])) Ur, _, _ = la.linalg.svd(B, 0) tilde_A = (np.eye(n) - Ur @ Ur.T) @ A tilde_B = Ur @ Ur.T @ A a_t = [la.norm(A)] for t in range(1, number_of_iteration + 1): a_t.append(la.norm(la.matrix_power(tilde_A, t) @ A, 2)) L = [0] UB = [la.norm(X[0])] L.append(la.norm(A @ z_bar[0])) UB.append(L[-1] * la.norm(X[0])) Delta = B @ (la.pinv(B) - G_alpha) @ A for t in range(2, number_of_iteration + 1): sum_bl = 0 for r in range(1, t): sum_bl += np.sqrt( la.norm( la.matrix_power(tilde_A, t - 1 - r) @ tilde_B @ z_bar[r])**2 + la. norm(la.matrix_power(tilde_A, t - 1 - r) @ Delta @ w_bar[r])**2 ) * L[r] L.append(a_t[t - 1] + sum_bl) UB.append(L[-1] * la.norm(X[0])) return X, X_old, UB, X_new, xposition
def run_robot(T, A, B, Q, R, noise, lam, mode, ini_lambda): # Initialize _myopic_x = np.zeros((T, np.shape(A)[0])) _optimal_x = np.zeros((T, np.shape(A)[0])) _online_x = np.zeros((T, np.shape(A)[0])) w = np.zeros((T, np.shape(A)[0])) estimated_w = np.zeros((T, np.shape(A)[0])) W = 0 Z = 0 Y = 0 X = 0 epsilon = 0 P, _, _ = control.dare(A, B, Q, R) D = _get_D(B, P, R) H = _get_H(B, D) F = _get_F(A, P, H) myopic_ALG = 0 online_ALG = 0 OPT = 0 for t in range(T): # Generate perturbations w = generate_w(mode, A, T) estimated_w = w + noise # Compute norms inner_epsilon = 0 inner_W = 0 inner_Z = 0 for s in range(t, T): inner_epsilon += np.linalg.norm(matrix_power( F, s - t), 2) * np.linalg.norm(P, 2) * np.linalg.norm(noise[s]) inner_W += np.linalg.norm(matrix_power( F, s - t), 2) * np.linalg.norm(P, 2) * np.linalg.norm( estimated_w[s]) inner_Z += np.linalg.norm(matrix_power( F, s - t), 2) * np.linalg.norm(P, 2) * np.linalg.norm(w[s]) epsilon += inner_epsilon**2 W += inner_W**2 Z += inner_Z**2 Y += inner_epsilon * inner_Z X += inner_Z * inner_W for t in range(T): # Update actions _myopic_E = np.matmul(P, np.matmul(A, _myopic_x[t])) _online_E = np.matmul(P, np.matmul(A, _online_x[t])) _optimal_E = np.matmul(P, np.matmul(A, _optimal_x[t])) _myopic_G = 0 _optimal_G = 0 for s in range(t, T): _myopic_G += np.matmul( np.linalg.matrix_power(np.transpose(F), s - t), np.matmul(P, estimated_w[s])) _optimal_G += np.matmul( np.linalg.matrix_power(np.transpose(F), s - t), np.matmul(P, w[s])) # Myopic algorithm _myopic_u = -np.matmul(D, _myopic_E) - lam * np.matmul(D, _myopic_G) # Online algorithm (time-varying lambda) _FTL_lam = _find_lam(t, w, estimated_w, P, F, H, ini_lambda) _online_u = -np.matmul(D, _online_E) - _FTL_lam * np.matmul( D, _myopic_G) # Omniscient algorithm _optimal_u = -np.matmul(D, _optimal_E) - np.matmul(D, _optimal_G) # Update states if t < T - 1: _myopic_x[t + 1] = np.matmul(A, _myopic_x[t]) + np.matmul( B, _myopic_u) + w[t] _online_x[t + 1] = np.matmul(A, _online_x[t]) + np.matmul( B, _online_u) + w[t] _optimal_x[t + 1] = np.matmul(A, _optimal_x[t]) + np.matmul( B, _optimal_u) + w[t] # Update costs if t < T - 1: myopic_ALG += np.matmul(np.transpose( _myopic_x[t]), np.matmul(Q, _myopic_x[t])) + np.matmul( np.transpose(_myopic_u), np.matmul(R, _myopic_u)) online_ALG += np.matmul(np.transpose( _online_x[t]), np.matmul(Q, _online_x[t])) + np.matmul( np.transpose(_online_u), np.matmul(R, _online_u)) OPT += np.matmul(np.transpose( _optimal_x[t]), np.matmul(Q, _optimal_x[t])) + np.matmul( np.transpose(_optimal_u), np.matmul(R, _optimal_u)) else: online_ALG += np.matmul(np.transpose(_online_x[t]), np.matmul(P, _online_x[t])) myopic_ALG += np.matmul(np.transpose(_myopic_x[t]), np.matmul(P, _myopic_x[t])) OPT += np.matmul(np.transpose(_optimal_x[t]), np.matmul(P, _optimal_x[t])) print("Online Cost is") print(online_ALG) print("Myopic Cost is") print(myopic_ALG) print("Optimal Cost is") print(OPT) return epsilon, X, Y, W, Z, myopic_ALG, online_ALG, OPT
threshold = 0.01 T = 100 A = np.zeros((2, 2)) B = r * np.array([[1 / 2, 1 / 2], [-1 / n, 1 / n]]) C = np.eye(2) D = 0 sysc = ctrl.ss(A, B, C, D) sysd = ctrl.sample_system(sysc, dt) Ad, Bd, Cd, Dd = sysd.A, sysd.B, sysd.C, sysd.D Q = np.array([[1, 0], [0, 100]]) R = 25 * np.eye(2) K = ctrl.dare(Ad, Bd, Q, R)[-1] x0 = np.array([0, 0]).T start_loc = np.array([0, 0]) end_loc = np.array([5, 5]) def angle(vec): return np.arctan2(vec[1], vec[0]) def velocity(psi, u): return r * np.array([[np.cos(psi) / 2, np.cos(psi) / 2], [np.sin(psi) / 2, np.sin(psi) / 2]]) @ u
C = np.transpose(B) found = False while not found: A_random = np.random.rand(state_dim, state_dim) ctrb_mat = control.ctrb(A_random, B) obsv_mat = control.obsv(A_random, C) rank_ctrb = np.linalg.matrix_rank(ctrb_mat) rank_obsv = np.linalg.matrix_rank(obsv_mat) if rank_ctrb == state_dim and rank_obsv == state_dim: found = True Qstate = 10 * np.eye(state_dim) Qinput = 1000 * np.eye(input_dim) (_, L, Klqr) = control.dare(A_random, B, Qstate, Qinput) for eigval in L: assert (np.abs(eigval) < 1) A = A_random - B @ Klqr # or just use the standard integrator chain elif args.dynamics == "integrator_chain": dt = 0.05 A = np.eye(state_dim) for i in range(state_dim - 1): A[i, i + 1] = dt B = np.zeros((state_dim, 1)) B[-1, 0] = 1 C = np.transpose(B)
def Trajectory_generator(A, B, K_old, x0, noise, alpha): X = [] # Define state array u = [] # Define control array Y = [] K = [] P = [] Q_ = [] xi = [] n = np.shape(A)[0] m = np.shape(B)[1] G_alpha = la.pinv(alpha * np.eye(m) + B.T @ B) @ B.T X.append(x0) X_old_control = [x0] X_newController = [x0] K = [np.zeros((m, n))] number_of_iteration = 40 * n mean_w = 0 for _ in range(number_of_iteration): if noise: w = np.array(np.random.normal(mean_w, 0.01, size=(n, 1))) else: w = np.zeros((n, 1)) u.append(-K[-1] @ X[-1]) X.append(A @ X[-1] + B @ u[-1] + w) Y.append(X[-1] - B @ u[-1]) A_hat = np.hstack(Y) @ la.pinv(np.hstack(X[:-1])) if _ == 0: P.append((X[0] @ X[0].T) / (la.norm(X[0])**2)) Q_.append((X[1] @ X[0].T) / (la.norm(X[0])**2)) K.append(G_alpha @ Q_[-1]) else: xi.append(X[-1] - P[-1] @ X[-1]) Q_.append(Q_[-1] + (Y[-1] - Q_[-1] @ X[-1]) @ la.pinv(xi[-1])) P.append(P[-1] - xi[-1] @ la.pinv(xi[-1])) K.append(G_alpha @ Q_[-1]) if _ < 30: X_newController.append(X[-1]) elif _ == 30: xposition = _ Y_ = np.hstack(Y) X_ = np.hstack(X[:-1]) A1_hat = Y_[0:4, :] @ la.pinv(X_[0:4, :]) A2_hat = Y_[4:8, :] @ la.pinv(X_[4:8, :]) A_hat = 0 * A_hat A_hat[0:4, 0:4] = A1_hat[0:4, 0:4] A_hat[4:8, 4:8] = A2_hat[0:4, 0:4] Q = np.eye(np.shape(A)[0]) R = np.eye(np.shape(B)[1]) (P1, L1, K1) = control.dare(A_hat, B, Q, R) X_newController.append((A - B @ K1) @ X_newController[-1] + w) else: X_newController.append((A - B @ K1) @ X_newController[-1] + w) X_old_control.append((A - B @ K_old) @ X_old_control[-1] + w) ############ Find z_t ############### z = [X[0]] z_bar = [X[0] / la.norm(X[0])] w_bar = [X[0] / la.norm(X[0]) - z_bar[0]] for t in range(1, number_of_iteration + 1): X_subspace = np.hstack(X[:t]) l = len(X_subspace) Ux, _, _ = la.svd(X_subspace, 0) z.append((np.eye(l) - Ux @ Ux.T) @ X[t]) if la.norm(z[-1]) > 10e-12: z_bar.append(z[-1] / la.norm(z[-1])) w_bar.append(X[t] / la.norm(X[t]) - z_bar[-1]) else: z_bar.append(np.zeros((n, 1))) w_bar.append(X[t] / la.norm(X[t])) ####################################### ''' ###### Upper bound ###### ''' Ur, _, _ = la.linalg.svd(B, 0) tilde_A = (np.eye(n) - Ur @ Ur.T) @ A tilde_B = Ur @ Ur.T @ A a_t = [la.norm(A)] for t in range(1, number_of_iteration + 1): a_t.append(la.norm(la.matrix_power(tilde_A, t) @ A, 2)) L = [0] UB = [la.norm(X[0])] L.append(la.norm(A @ z_bar[0])) UB.append(L[-1] * la.norm(X[0])) Delta = B @ (la.pinv(B) - G_alpha) @ A for t in range(2, number_of_iteration + 1): sum_bl = 0 for r in range(1, t): sum_bl += np.sqrt( la.norm( la.matrix_power(tilde_A, t - 1 - r) @ tilde_B @ z_bar[r])**2 + la. norm(la.matrix_power(tilde_A, t - 1 - r) @ Delta @ w_bar[r])**2 ) * L[r] L.append(a_t[t - 1] + sum_bl) UB.append(L[-1] * la.norm(X[0])) # xposition=0 return X, X_old_control, UB, X_newController, xposition
def __init__(self, A, B): self.state_dim = A.shape[0] self.action_dim = B.shape[1] Q = np.eye(self.state_dim) R = np.eye(self.action_dim) [self.P,L,self.K] = control.dare(A,B,Q,R)
G2.A = np.array([[1, 0, .1, 0], [0, 1, 0, .1], [0, 0, 1, 0], [0, 0, 0, 1]]) G2.B = np.array([[0, 0], [0, 0], [1, 0], [0, 1]]) G2.C = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) G2.D = np.array([[0, 0], [0, 0]]) x01 = np.array([[0], [0], [0], [0]]) # Initial state WP1 = np.array([[5], [0], [0], [0]]) # Waypoint x02 = np.array([[-5], [0], [0], [0]]) # Initial state WP2 = np.array([[2], [0], [0], [0]]) # Waypoint w1 = mpc.Weight() # Empty weights class w1.Q = np.array([[5, 0, 0, 0], [0, 5, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) # State weight matrix Q w1.R = np.array([[1, 0], [0, 1]]) # Control input weight matrix R w1.P = control.dare(G1.A, G1.B, w1.Q, w1.R)[0] # Final state matrix P, based off DARE w2 = mpc.Weight() # Empty weights class w2.Q = np.array([[5, 0, 0, 0], [0, 5, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) # State weight matrix Q w2.R = np.array([[1, 0], [0, 1]]) # Control input weight matrix R w2.P = control.dare(G2.A, G2.B, w2.Q, w2.R)[0] # Final state matrix P, based off DARE predMat1 = mpc.generate_prediction_matrices(G1, N) # Prediction matrices P and S costMat1 = mpc.generate_cost_matrices( predMat1, w1, N) # Cost matrices Q, inverse Q (Qi) and c predMat2 = mpc.generate_prediction_matrices(G2, N) # Prediction matrices P and S
def dlqr(self, A, B, Q, R, state): X, L, K = ct.dare(A, B, Q, R) control = -np.matmul(K, state) return control
def compute_steadystate_cov(A, C, state_noise_cov, meas_noise_cov): (X, _, _) = control.dare(A, C.T, state_noise_cov, meas_noise_cov) return X
#%% Definitions G = mpc.LTI() # State-space system G.A = np.array([[1, 0, .1, 0], [0, 1, 0, .1], [0, 0, 1, 0], [0, 0, 0, 1]]) G.B = np.array([[0, 0], [0, 0], [1, 0], [0, 1]]) G.C = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) G.D = np.array([[0, 0], [0, 0]]) x0 = np.array([[0], [0], [0], [0]]) # Initial state WP = np.array([[5], [0], [0], [0]]) # Waypoint w = mpc.Weight() # Empty weights class w.Q = np.array([[5, 0, 0, 0], [0, 5, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) # State weight matrix Q w.R = np.array([[1, 0], [0, 1]]) # Control input weight matrix R w.P = control.dare(G.A, G.B, w.Q, w.R)[0] # Final state matrix P, based off DARE predMat = mpc.generate_prediction_matrices(G, N) # Prediction matrices P and S costMat = mpc.generate_cost_matrices( predMat, w, N) # Cost matrices Q, inverse Q (Qi) and c vMax = 5 # Bound on velocity xBound = 5 # Bound on position Ac = np.array([[0, 1, 0, 0], [0, -1, 0, 0], [0, 0, 1, 0], [0, 0, -1, 0], [0, 0, 0, 1], [0, 0, 0, -1]]) # Constraint matrix A (Ax <= b) bc = np.array([[xBound], [xBound], [vMax], [vMax], [vMax], [vMax]]) # Constraint vector b #%% MPC simulation x = np.zeros((4, T + 1)) # States
#!/usr/bin/env python3 import numpy as np import control # pylint: disable=import-error dt = 0.01 A = np.array([[0., 1.], [-0.78823806, 1.78060701]]) B = np.array([[-2.23399437e-05], [7.58330763e-08]]) C = np.array([[1., 0.]]) # Kalman tuning Q = np.diag([1, 1]) R = np.atleast_2d(1e5) (_, _, L) = control.dare(A.T, C.T, Q, R) L = L.T # LQR tuning Q = np.diag([2e5, 1e-5]) R = np.atleast_2d(1) (_, _, K) = control.dare(A, B, Q, R) A_cl = (A - B.dot(K)) sys = control.ss(A_cl, B, C, 0, dt) dc_gain = control.dcgain(sys) print(("self.A = np." + A.__repr__()).replace('\n', '')) print(("self.B = np." + B.__repr__()).replace('\n', '')) print(("self.C = np." + C.__repr__()).replace('\n', '')) print(("self.K = np." + K.__repr__()).replace('\n', '')) print(("self.L = np." + L.__repr__()).replace('\n', '')) print("self.dc_gain = " + str(dc_gain))
#=========================================================== #weighting matrix Q Q = numpy.array( [ [1, 0, 0, 0], [0, 1, 0 ,0], [0, 0, 10, 0], [0, 0, 0, 10] ] ) #weighting scalar R R = 1000.0 #calculate the weighting values for the discrete system Qd = Q * T Rd = R * T #solve the Discrete-time Algebraic Riccati Equation (DARE) #P: solution of the Riccati equation #L: eigenvalues of the closed loop #G: optimal feedback gain P, L, G = control.dare(Ax, Bx, Qd, Rd) print("**********************************") print("*Solution of the Riccati equation*") print("**********************************") #calculate Gain: Gain = (B^TPB+R)^-1B^TPA tran_Bx = Bx.transpose() BTPB = numpy.dot( numpy.dot(tran_Bx, P), Bx ) temp = 1.0/(BTPB[0][0] + Rd) Gain = - temp * numpy.dot( numpy.dot(tran_Bx, P), Ax ) print("Gain (calculated)") print(Gain) print("") print("Gain (obtained from congtrol.dare)") print(-G)
K_1[i - 1] = np.matmul( np.matmul( np.transpose(A), K_1[i] - np.matmul( np.matmul(K_1[i], B), np.linalg.inv( np.matmul(np.matmul(np.transpose(B), K_1[i]), B) + R), np.matmul(np.transpose(B), K_1[i]))), A) + Q L_1 = np.empty([N, n, n]) for i in range(0, N): L_1[i] = -np.matmul( np.linalg.inv( np.matmul(np.matmul(np.transpose(B), K_1[i + 1]), B) + R), np.matmul(np.matmul(np.transpose(B), K_1[i + 1]), A)) K_2, G, E = control.dare(A=A, B=B, Q=Q, R=R) L_2 = -np.matmul( np.linalg.inv(np.matmul(np.matmul(np.transpose(B), K_2), B) + R), np.matmul(np.matmul(np.transpose(B), K_2), A)) for i in range(0, N): # Values normal x_1[i + 1] = np.matmul(A + np.matmul(B, L_1[i]), x_1[i]) + w[i] # Values much larger x_2[i + 1] = np.matmul(A + np.matmul(B, L_2), x_2[i]) + w[i] # plot plot_fig5 = plt.figure(5) plt.subplot(211) plt.title('Behavior of the system optimal control vs steady-state control')
def dlqr(*args, **keywords): """Linear quadratic regulator design for discrete systems Usage ===== [K, S, E] = dlqr(A, B, Q, R, [N]) [K, S, E] = dlqr(sys, Q, R, [N]) The dlqr() function computes the optimal state feedback controller that minimizes the quadratic cost J = \sum_0^\infty x' Q x + u' R u + 2 x' N u Inputs ------ A, B: 2-d arrays with dynamics and input matrices sys: linear I/O system Q, R: 2-d array with state and input weight matrices N: optional 2-d array with cross weight matrix Outputs ------- K: 2-d array with state feedback gains S: 2-d array with solution to Riccati equation E: 1-d array with eigenvalues of the closed loop system """ # # Process the arguments and figure out what inputs we received # # Get the system description if (len(args) < 3): raise ControlArgument("not enough input arguments") elif (issys(args[0])): # We were passed a system as the first argument; extract A and B A = array(args[0].A, ndmin=2, dtype=float); B = array(args[0].B, ndmin=2, dtype=float); index = 1; if args[0].dt==None: print('dlqr works only for discrete systems!') return else: # Arguments should be A and B matrices A = array(args[0], ndmin=2, dtype=float); B = array(args[1], ndmin=2, dtype=float); index = 2; # Get the weighting matrices (converting to matrices, if needed) Q = array(args[index], ndmin=2, dtype=float); R = array(args[index+1], ndmin=2, dtype=float); if (len(args) > index + 2): N = array(args[index+2], ndmin=2, dtype=float); Nflag = 1; else: N = zeros((Q.shape[0], R.shape[1])); Nflag = 0; # Check dimensions for consistency nstates = B.shape[0]; ninputs = B.shape[1]; if (A.shape[0] != nstates or A.shape[1] != nstates): raise ControlDimension("inconsistent system dimensions") elif (Q.shape[0] != nstates or Q.shape[1] != nstates or R.shape[0] != ninputs or R.shape[1] != ninputs or N.shape[0] != nstates or N.shape[1] != ninputs): raise ControlDimension("incorrect weighting matrix dimensions") if Nflag==1: Ao=A-B*inv(R)*N.T Qo=Q-N*inv(R)*N.T else: Ao=A Qo=Q #Solve the riccati equation (X,L,G) = dare(Ao,B,Qo,R) # X = bb_dare(Ao,B,Qo,R) # Now compute the return value Phi=mat(A) H=mat(B) K=inv(H.T*X*H+R)*(H.T*X*Phi+N.T) L=eig(Phi-H*K) return K,X,L
def run(self): # publish message every 1/x second rate = rospy.Rate(15) car_cmd_msg = WheelsCmdStamped() tnew = time.time() while not rospy.is_shutdown(): #computing dt for I-part of controller told = tnew tnew = time.time() dt = tnew - told #state feedback: statevector x = [d;phi] #K places poles at -1 and -2 Ts = dt #computing integral state self.dint += self.dist * dt ''' #discrete state space matrices (without integral d state) A = np.matrix([[1,self.vref*Ts],[0,1]]) B = np.matrix([[0.5*Ts*Ts*self.vref],[Ts]]) #weighting matrices Q = np.matrix([[2.5,0],[0,1]]) R = np.matrix([[0.1]]) ''' #discrete state space matrices (with integral d state) A = np.matrix([[1, self.vref * Ts, 0], [0, 1, 0], [Ts, 0.5 * self.vref * Ts * Ts, 1]]) B = np.matrix([[0.5 * self.vref * Ts * Ts], [Ts], [self.vref * Ts * Ts * Ts / 6.0]]) Q = np.matrix([[4.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 0.5]]) R = np.matrix([[0.1]]) #solving dare (X, L, G) = control.dare(A, B, Q, R) #extracting K-matrix-components k1 = G[0, 0] k2 = -G[0, 1] k3 = G[0, 2] # u = -K*x (state feedback) #self.omega = -k1*self.dist - k2*self.phi self.omega = -k1 * self.dist - k2 * self.phi - k3 * self.dint self.omega = self.omega #def. motor commands that will be published car_cmd_msg.header.stamp = rospy.get_rostime() car_cmd_msg.vel_left = self.vref + self.L * self.omega car_cmd_msg.vel_right = self.vref - self.L * self.omega self.pub_wheels_cmd.publish(car_cmd_msg) #printing messages to verify that program is working correctly #i.ei if dist and tist are always zero, then there is probably no data from the lan_pose message1 = k1 message2 = k2 message3 = self.omega rospy.loginfo('k1: %s' % message1) rospy.loginfo('k2: %s' % message2) rospy.loginfo('omega: %s' % message3) rate.sleep()