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()
