def sum(SS1, SS2, negative=False):
'''
Given 2 systems or gain matrices (or a combination of the two) having the
same amount of input/output, the function returns a gain or state space
model summing the two. Namely, given:
u -> SS1 -> y1
u -> SS2 -> y2
we obtain:
u -> SStot -> y1+y2 if negative=False
'''
type_dlti = scsig.ltisys.StateSpaceDiscrete
if isinstance(SS1, np.ndarray) and isinstance(SS2, np.ndarray):
SStot = SS1 + SS2
elif isinstance(SS1, np.ndarray) and isinstance(SS2, type_dlti):
Kmat = SS1
A = SS2.A
B = SS2.B
C = SS2.C
D = SS2.D + Kmat
SStot = scsig.StateSpace(A, B, C, D, dt=SS2.dt)
elif isinstance(SS1, type_dlti) and isinstance(SS2, np.ndarray):
Kmat = SS2
A = SS1.A
B = SS1.B
C = SS1.C
D = SS1.D + Kmat
SStot = scsig.StateSpace(A, B, C, D, dt=SS2.dt)
elif isinstance(SS1, type_dlti) and isinstance(SS2, type_dlti):
assert np.abs(1.-SS1.dt/SS2.dt)<1e-13,\
'State-space models must have the same time-step'
Nin01, Nout01 = SS1.inputs, SS1.outputs
Nin02, Nout02 = SS2.inputs, SS2.outputs
Nx01, Nx02 = SS1.A.shape[0], SS2.A.shape[0]
A = np.block([[SS1.A, np.zeros((Nx01, Nx02))],
[np.zeros((Nx02, Nx01)), SS2.A]])
B = np.block([[
SS1.B,
], [SS2.B]])
C = np.block([SS1.C, SS2.C])
D = SS1.D + SS2.D
SStot = scsig.StateSpace(A, B, C, D, dt=SS1.dt)
else:
raise NameError(
'Input types not recognised in any implemented option!')
return SStot
def ss_concatenate(sys1, sys2, F1=None, H1=None):
""" Concatenate two systems in series.
w ---- u1 ------ y1 ---- u2 ------ y2
---> | F1 | ---> | sys1 | ---> | H1 | ---> | sys2 | --->
---- ------ ---- ------
Args:
sys1 (StateSpace): The first state space system
sys2 (StateSpace): The second state space system
F1 (matrix, optional): The first system's input selection matrix.
Defaults to the identity matrix
H1 (matrix, optional): The first system's output selection matrix.
Defaults to the identity matrix
Returns:
StateSpace: The series system where
[ x1 ]
x = [----], u = w, y = y2
[ x2 ]
"""
A1 = sys1.A
B1 = sys1.B
C1 = sys1.C
D1 = sys1.D
A2 = sys2.A
B2 = sys2.B
C2 = sys2.C
D2 = sys2.D
(n1, p1) = B1.shape
(q1, n1) = C1.shape
(n2, p2) = B2.shape
(q2, n2) = C2.shape
if F1 is None:
F1 = np.matrix(np.eye(p1))
if F2 is None:
F2 = np.matrix(np.eye(p2))
if H1 is None:
H1 = np.matrix(np.eye(q1))
if H2 is None:
H2 = np.matrix(np.eye(q2))
A_top_row = np.concatenate((A1, np.zeros((n1, n2))), axis=1)
A_bot_row = np.concatenate(B2*H1*C1, A2), axis=1)
A = np.concatenate((A_top_row, A_bot_row), axis=0)
B = np.concatenate((B1*F1, B2*H1*D1*F1), axis=0)
C = np.concatenate((D2*H1*C1, C2), axis=1)
D = np.matrix(D2*H1*D1*F1)
return signal.StateSpace(A, B, C, D)
def discrete_ss(self, dt): ''' Convert the continuous system to a discrete system ''' # system = signal.cont2discrete((self.A, self.B, self.C, self.D), dt) self.sys_d = signal.StateSpace(self.A, self.B, self.C, self.D).to_discrete(dt) return self.sys_d
def _generate_system(self):
"""
Prepared by Arpan Mukherjee
:return: state-space representation of the 1-D Shallow Water Model
"""
return signal.StateSpace(self.A, self.B, self.C, self.D, dt=self.dt)
def _generate_system_clusters(self, clust_matrix):
"""
Prepared by Arpan Mukherjee
:param clust_matrix:
:return: list of shallow water equation objects for specific length of cluster
"""
A = self.A
B = self.B
C = self.C
D = self.D
number_cluster = clust_matrix.shape[0]
_state_space_clust = []
for k in range(number_cluster):
cluster = clust_matrix[k, :]
cl = cluster.astype(bool)
A_cl = A[cl][:, cl]
B_cl = B[cl]
C_cl = C[cl][:, cl]
D_cl = D[cl]
_state_space_clust.append(signal.StateSpace(A_cl, B_cl, C_cl, D_cl, dt=self.dt))
return _state_space_clust
def linearize(self):
"""Return the linearized system.
Returns
-------
a : ndarray
The state matrix.
b : ndarray
The action matrix.
"""
gravity = self.gravity
length = self.length
friction = self.friction
inertia = self.inertia
A = np.array([[0, 1], [gravity / length, -friction / inertia]],
dtype=config.np_dtype)
B = np.array([[0], [1 / inertia]], dtype=config.np_dtype)
if self.normalization is not None:
Tx, Tu = map(np.diag, self.normalization)
Tx_inv, Tu_inv = map(np.diag, self.inv_norm)
A = np.linalg.multi_dot((Tx_inv, A, Tx))
B = np.linalg.multi_dot((Tx_inv, B, Tu))
sys = signal.StateSpace(A, B, np.eye(2), np.zeros((2, 1)))
sysd = sys.to_discrete(self.dt)
return sysd.A, sysd.B
def system_cluster(self, cluster_matrix):
A = self._ss.A
B = self._ss.B
C = self._ss.C
D = self._ss.D
dt = self._ss.dt
number_cluster = cluster_matrix.shape[0]
_state_space_clust = []
for k in range(number_cluster):
cluster = cluster_matrix[k, :]
cl = cluster.astype(bool)
A_cl = A[cl][:, cl]
B_cl = B[cl]
C_cl = C[cl][:, cl]
D_cl = D[cl]
_state_space_clust.append(signal.StateSpace(A_cl, B_cl, C_cl, D_cl, dt=dt))
return _state_space_clust
def linearize(self):
"""Return the linearized system.
Returns
-------
a : ndarray
The state matrix.
b : ndarray
The action matrix.
"""
k = self.k
mass = self.mass
c = self.c
m = self.m
A = np.array([[0, 1], [k / mass, -c / m]], dtype=config.np_dtype)
B = np.array([[0], [1 / (m * mass**2)]], dtype=config.np_dtype)
if self.normalization is not None:
Tx, Tu = map(np.diag, self.normalization)
Tx_inv, Tu_inv = map(np.diag, self.inv_norm)
A = np.linalg.multi_dot((Tx_inv, A, Tx))
B = np.linalg.multi_dot((Tx_inv, B, Tu))
sys = signal.StateSpace(A, B, np.eye(2), np.zeros((2, 1)))
sysd = sys.to_discrete(self.dt)
return sysd.A, sysd.B
def createStateSpace(order, stateLength, t, omg, zeta, gamma_d, gamma_omg):
Ac = np.zeros([stateLength, stateLength], dtype=float)
Bc = np.zeros([stateLength, 1], dtype=float)
Cc = np.zeros([1, stateLength], dtype=float)
Dc = 0
#This is to populate State matrices
# -Python takes the LHS which forms a 2x2 matrix, matches it to the RHS which, to python looks like a
# 2x2 matrix and assigns each element accordingly. Process follows suite for Bc matrix
for k in range(order):
i = (1 + k) * 2
k = k + 1 #handles the one off error
Ac[i - 2:i, i - 2:i] = [[0, 1], [float(-(k * omg)**2), 0]]
Bc[i - 2:i] = [[0], [(k * omg)**2]]
Cc[0][i - 1] = (2 * zeta) / (k * omg)
#Assigns edge values
Bc[len(Bc) - 1][0] = gamma_d
Cc[0][len(Cc[0]) - 1] = 1
#uses the "scipy" library and converts the continuous matrixes into a SS
# -Then takes the continuous system and decritizes it (NOTE: method for c2d may not match model)
dt = float(t[1] - t[0])
contSystem = signal.StateSpace(Ac, Bc, Cc, Dc)
discSystem = contSystem.to_discrete(dt)
#Posible solution if .to_discrete system does not work.
# -function for computing matrix exponenetal (Future implimentation)
# -might need to maintain stability
#discSystem = signal.cont2discrete(contSystem,dt,'impulse')
return discSystem
def get_state_space(self):
"""Get a state space representation of this filter
Returns:
ss: a scipy.signal state space representation of the filter
"""
zs, ps, k = self.get_zpk(Hz=False)
return sig.StateSpace(*sig.zpk2ss(assertArr(zs), assertArr(ps), k))
def ss_common_in(sys1, sys2, F1=None):
""" Connection of two systems with a common input
------ y1
|---> | sys1 | --->
| ------
w ---- u |
---> | F1 | ---|
---- |
| ------ y2
|---> | sys2 | --->
------
Args:
sys1 (StateSpace): The first state space system
sys2 (StateSpace): The second state space system
F1 (matrix, optional): Each system's input selection matrix.
Defaults to the identity matrix
Returns:
StateSpace: The common input system where
[ x1 ] [ y1 ]
x = [----], u = w, y = [----]
[ x2 ] [ y2 ]
"""
A1 = sys1.A
B1 = sys1.B
C1 = sys1.C
D1 = sys1.D
A2 = sys2.A
B2 = sys2.B
C2 = sys2.C
D2 = sys2.D
(n1, p1) = B1.shape
(q1, n1) = C1.shape
(n2, p2) = B2.shape
(q2, n2) = C2.shape
if F1 is None:
F1 = np.matrix(np.eye(p1))
A_top_row = np.concatenate((A1, np.zeros((n1, n2))), axis=1)
A_bot_row = np.concatenate((np.zeros((n2, n1)), A2), axis=1)
A = np.concatenate((A_top_row, A_bot_row), axis=0)
B = np.concatenate((B1*F1, B2*F1), axis=0)
C_top_row = np.concatenate((C1, np.zeros((q1, n2))), axis=1)
C_bot_row = np.concatenate((np.zeros((q2, n1)), sys), axis=1)
C = np.concatenate((C_top_row, C_bot_row), axis=0)
D = np.concatenate((D1*F1, D2*F1), axis=0)
return signal.StateSpace(A, B, C, D)
def continous_time_step_response(A, B, C, D, plot=True):
ss = sig.StateSpace(A, B, C, D)
t, ys = sig.step(system=ss)
if plot:
for i in range(ys.shape[1]):
plt.plot(t, ys[:, i])
plt.title("Response Y" + str(i))
plt.show()
return (t, ys)
def plot_w1w2(Ra,La,Kg,K1,Km,N1,N2,J2,c,br,grid):
# Pembuatan model transfer function
A= [[(-Ra/La),(-Kg/La)],[Km/((N2/N1)**2 * J2),(-c+br)]]
B= [[(K1/La)],[0]]
C= [[0,1],[0,(N2/N1)]]
D= [[0],[0]]
sys1=signal.StateSpace(A,B,C,D)
t1,y1=signal.step(sys1)
plt.title("Plot $\\omega_1$ dan $\\omega_2$")
plt.plot(t1,y1)
def MotorControl():
J = .01 # kgm**2
b = .1 # N.m.s
Ke = .01 # V/rad/sec
Kt = .01 # N.m/Amp
R = 1 # Electrical resistance
L = .5 # Henries
K = Kt # or Ke, Ke == Kt here
# State Space Model
A = np.matrix([[-b / J, K / J], [-K / L, -R / L]])
B = np.matrix([[0], [1 / L]])
C = np.matrix([1, 0])
D = np.matrix([0])
dt = .025 # Discrete Sample Time
ss = sig.StateSpace(A, B, C, D)
ssd = sig.cont2discrete((A, B, C, D), dt)
Ad = ssd[0]
Bd = ssd[1]
Cd = ssd[2]
evc, evecc = np.linalg.eig(A)
evd, evecd = np.linalg.eig(Ad)
# Create Bode
#w, mag, phase = sig.bode(ss)
#plot_SISO(w,mag, "Freq Response Mag vs w")
#plot_SISO(w, phase, "Freq Response Phase vs w")
#discrete_time_frequency_repsonse()
# Transfer Function
tf = ss.to_tf()
print(tf)
t, y = sig.step(ss)
td, yd = sig.dstep(ssd)
yd = np.reshape(yd, -1)
#plot_SISO(t,y)
#plot_SISO(td, yd)
#pole_zeros(A, B, C, plot=True)
#pole_zeros(Ad, Bd, Cd, plot=True)
T = 10
N = T / dt
times = np.arange(0, T, dt)
U = np.ones(int(N))
x0 = np.matrix([0, 0]).T
xr = np.matrix([2.5, 0])
kp = 15
kd = 4
ki = 100
Q = Cd.T * Cd
R = np.matrix([.0005])
discrete_time_response(Ad, Bd, Cd, U, x0, dt, plot=True)
discrete_time_frequency_repsonse(Ad, Bd, Cd, 20, dt)
continous_time_frequency_repsonse(A, B, C)
discrete_pid_response((Ad, Bd, Cd), x0, xr, times, kp, ki, kd)
sim_dlqr(Ad, Bd, Cd, Q, R, x0, xr, dt, T)
x = 9
def fullModel(IsPfc, angles, Rc, time):
Is2Bpol2 = buildIs2BpolB(isttok_mag_2)
RIc = isttok_mag['RM'] + isttok_mag['Rcopper'] * np.cos(anglesIc)
ZIc = isttok_mag['Rcopper'] * np.sin(anglesIc)
A2, Bs2, C2, Ds2 = buildLtiModelB(RIc, ZIc, isttok_mag_2, Rc)
Ic2Bpol = buildIc2Bpol(RIc, ZIc)
magSys2 = signal.StateSpace(A2, Bs2, C2, Ds2)
bPolIs2 = np.matmul(Is2Bpol2, IsPfc)
tout2, Ic2, x2 = signal.lsim(magSys2, IsPfc.T, time)
bPolIc2 = np.matmul(Ic2Bpol, Ic2.T)
bPolTot2 = (bPolIs2.T + bPolIc2.T) * 50 * 49e-6
return bPolTot2, (RIc, ZIc)
def mass_spring_damper():
m = .25
k = 10 # n/m
b = .2 # Nm/s
A = np.matrix([[0, 1], [-k / m, -b / m]])
B = np.matrix([0, 1 / m]).T
C = np.matrix([1, 0])
D = np.matrix([0])
dt = .025
ss = sig.StateSpace(A, B, C, D)
ssd = sig.cont2discrete((A, B, C, D), dt)
ctrb_rank = np.linalg.matrix_rank(ctrb(A, B))
def data_gen():
parser = argparse.ArgumentParser(
description='Generate Training and Testing Samples')
parser.add_argument('-n', action="store", dest="ts1", type=int)
parser.add_argument('-t', action="store", dest="ts2", type=int)
trainingsample = parser.parse_args().ts1
testingsample = parser.parse_args().ts2
# Simulate two mass spring system
A = np.array([[0, 0, 1, 0], [0, 0, 0, 1], [-2, 1, 0, 0], [1, -2, 0, 0]])
B = np.array([[0, 0], [0, 0], [1, 0], [0, 1]])
C = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])
D = np.array([[0, 0], [0, 0]])
sys = sg.StateSpace(A, B, C, D).to_discrete(0.1) #sample period 0.1s
print "Two Mass Spring System:\n", sys, '\n'
x = np.array([[10], [20], [0], [0]], dtype=np.float64) #initial state
ls = []
for i in range(trainingsample):
mu, sigma = 0, 0.25 # mean and standard deviation
#s = np.random.normal(mu, sigma, 2)# generate noise for the training
#x[0:2,0]+=s
f = 2 * (np.random.rand(2, 1) - 0.5) #random force to the mass
x = sys.A.dot(x) + sys.B.dot(f)
y = sys.C.dot(x) + sys.D.dot(f)
ls.append([f, y])
#store the training data in a file for future analysis
with open(r'./traindata.log', 'wb') as afile:
pickle.dump(ls, afile)
#reload object from the file
"""
with open(r'./td', 'rb') as _load_file:
new_d = pickle.load(_load_file)
"""
ls = []
for i in range(testingsample):
f = 2 * (np.random.rand(2, 1) - 0.5) #random force to the mass
x = sys.A.dot(x) + sys.B.dot(f)
y = sys.C.dot(x) + sys.D.dot(f)
ls.append([f, y])
#store the training data in a file for future analysis
with open(r'./testdata.log', 'wb') as afile:
pickle.dump(ls, afile)
def InvertedPendulum():
x0 = 0
dx0 = 0
theta0 = np.pi
dtheta0 = 0
M = .5
m = 0.2
b = 0.1
I = 0.006
g = 9.8
l = 0.3
F = symbols('F')
x, theta, dx, dtheta = symbols('x, theta, dx, dtheta')
X = Matrix([x, dx, theta, dtheta])
U = Matrix([F])
RHS = Matrix([[M + m, -m * l * cos(theta)],
[m * l * cos(theta), I + m * l**2]])
LHS1 = RHS.inv() * Matrix([
-b * x + m * l * dtheta**2 * sin(theta), -m * l * g * sin(theta)
]) #Dynamics
LHS2 = RHS.inv() * Matrix([F, 0]) #Input Force
A = LHS1.jacobian(X).subs([(x, x0), (theta, theta0), (dx, dx0),
(dtheta, dtheta0)])
B = LHS2.jacobian(U).subs([(x, x0), (theta, theta0), (dx, dx0),
(dtheta, dtheta0)])
A = np.reshape(
np.matrix(np.vstack(
(np.matrix([0, 1, 0, 0]), A[0, :], np.matrix([0, 0, 0,
1]), A[1, :])),
dtype=np.float), (4, 4))
B = np.matrix([0, B[0, 0], 0, B[1, 0]], dtype=np.float).T
C = np.matrix([[1, 0, 0, 0], [0, 0, 1, 0]])
D = np.matrix([[0], [0]])
ss = sig.StateSpace(A, B, C, D)
dt = .025
ssd = sig.cont2discrete((A, B, C, D), dt)
tc, yc = sig.impulse(ss)
td, yd = sig.dimpulse(ssd)
plt.plot(td, [y[0, 0] for y in yd])
plt.show()
plt.plot(td, [y[1, 0] for y in yd])
plt.show()
f00 = 9
def cruise_control():
m = 1000
b = 50
u = 500
A = np.matrix([[0, .99], [0, -b / m]])
B = np.matrix([[0], [u / m]])
C = np.matrix(np.eye(2))
D = np.matrix([[0], [0]])
evals, evecs = np.linalg.eig(A)
pole_zeros(A, B, C[1, :])
ss = sig.StateSpace(A, B, C, D)
continous_time_impulse_response(A, B, C, D, plot=True)
continous_time_step_response(A, B, C, D, plot=True)
#continous_time_frequency_repsonse(A,B,C, plot = True)
Gc = gram_ctrb(A, B)
U = continous_time_least_norm_input(A, B, np.matrix([0, 10]), 5, .025)
#Go = gram_obsv(A,C)
foo = 0
def discrete_model(env, measurements):
# linearized continuous model
J = 1 / 12 * (env.masspole * 2)**2
beta = env.masspole * env.masscart * env.length**2 + J * (env.masspole +
env.masscart)
ac = -(env.masspole**2 * env.length**2 * env.gravity) / beta
bc = (J + env.masspole * env.length**2) / beta
cc = env.masspole * env.length * env.gravity * (env.masscart +
env.masspole) / beta
dc = -env.masspole * env.length / beta
A_c = np.array([[0, 1, 0, 0], [0, 0, ac, 0], [0, 0, 0, 1], [0, 0, cc, 0]])
B_c = np.array([[0], [bc], [0], [dc]])
# C_c = np.array([[1,env.tau,0,0],
# [0,1,0,0],
# [0,0,0,1]])
# D_c = np.array([[0],
# [0],
# [0]])
# C_c = np.array([[1,0,0,0]])
# D_c = np.array([[0]])
if measurements is 2:
C_c = np.array([[1, 0, 0, 0], [0, 0, 0, 1]])
D_c = np.array([[0], [0]])
elif measurements is 1:
C_c = np.array([[1, 0, 0, 0]])
D_c = np.array([[0]])
# discrete linearized model
sys = signal.StateSpace(A_c, B_c, C_c, D_c)
discrete_sys = sys.to_discrete(env.tau)
A = discrete_sys.A
B = discrete_sys.B
C = discrete_sys.C
D = discrete_sys.D
# print(A)
# print(B)
# print(C)
# while True:
# pass
return A, B, C, D
def cavity_ss(RoverQ, Qg, Q0, Qprobe, bw, Kg_r, Kg_i, Ib,
foffset):
Rg = RoverQ * Qg
Kdrive = 2 * np.sqrt(Rg)
Ql = 1.0 / (1.0/Qg + 1.0/Q0 + 1.0/Qprobe)
K_beam = RoverQ * Ql
w_d = 2 * np.pi * foffset
a = Kdrive * bw
b = K_beam * bw
c = bw
A = -c
B = a
C = 1.0
D = 0
sys = signal.StateSpace(A,B,C,D)
t,y = signal.step(sys)
return t, y
def SerialPlotIdentify(signals):
''' Remove voltage from output vector. '''
labels = signals['labels']
data = signals['data']
speedidx = labels.index('Speed')
curridx = labels.index('Current')
voltidx = labels.index('Voltage')
yvec = np.array([data[:, speedidx], data[:, curridx]]).T
yvec = yvec[1:, :]
xvec = np.array([data[:, speedidx], data[:, curridx], data[:, voltidx]]).T
xvec = xvec[:-1, :]
logging.debug('XVec dims: {}'.format(xvec.shape))
logging.debug('YVec dims: {}'.format(yvec.shape))
params, res, rank, s = np.linalg.lstsq(xvec, yvec, rcond=None)
logging.debug('Params: {}'.format(params))
logging.debug('Params Dims: {}'.format(params.shape))
yhat = xvec.dot(params)
amtx = params[:yhat.shape[1], :].T
bmtx = params[-1, :].reshape(1, 2)
cmtx = np.eye(amtx.shape[0])
dmtx = np.zeros([2, 1])
logging.debug('A-Mat Shape: {}'.format(amtx.shape))
logging.debug('B-Mat Shape: {}'.format(bmtx.T.shape))
logging.debug('C-Mat Shape: {}'.format(cmtx.shape))
logging.debug('D-Mat Shape: {}'.format(dmtx.shape))
sys = signal.StateSpace(amtx, bmtx.T, cmtx, dmtx, dt=T_SAMPLE)
u = data[:, voltidx]
t = np.arange(len(u)) * T_SAMPLE
x0 = xvec[0, :-1]
yhat = np.squeeze(signal.dlsim(sys, u, x0=x0)[-1])
ylabels = [labels[speedidx], labels[curridx]]
for i in range(yhat.shape[1]):
plt.subplot(yhat.shape[1], 1, i + 1)
plt.plot(yvec[:, i])
plt.plot(yhat[:, i], 'r--', LineWidth=0.9)
plt.title(ylabels[i])
plt.legend(['Actual', 'Estimate'])
plt.grid()
plt.show()
def linearize(self):
"""Return the linearized system.
Returns
-------
a : ndarray
The state matrix.
b : ndarray
The action matrix.
"""
gravity = self.gravity
length = self.length
friction = self.friction
inertia = self.inertia
a = np.array([[0, 1], [gravity / length, -friction / inertia]])
b = np.array([[0], [1 / inertia]])
sys = signal.StateSpace(a, b, np.eye(2), np.zeros((2, 1)))
sysd = sys.to_discrete(self.step_size)
return LinearSystem(sysd.A, sysd.B)
def test_unstable_system(self):
T = 15
Ts = 1e-1
A = [[1.178, 0.001, 0.511, -0.403], [-0.051, 0.661, -0.011, 0.061],
[0.076, 0.335, 0.560, 0.382], [0., 0.335, 0.089, 0.849]]
B = [[0.004, -0.087], [0.467, 0.001], [0.213, -0.235], [0.213, -0.016]]
A = np.array(A)
B = np.array(B)
n, m = B.shape
sys = scipysig.StateSpace(A,
B,
np.identity(n),
np.zeros((n, m)),
dt=Ts)
u = np.random.randn(T + 1, m)
_, _, x = scipysig.dlsim(sys, u)
Qs = [np.identity(n), 0 * np.identity(n)]
Rs = [np.identity(m), 0 * np.identity(m)]
for Q in Qs:
for R in Rs:
if np.all(Q == Qs[1]):
with self.assertRaises(ValueError):
K, V = optimal_control(x, u, Q, R)
else:
# Solve optimal control problem with Riccati equation
P = dare(A, B, Q, R)
trueK = -np.linalg.inv(B.T @ P @ B + R) @ B.T @ P @ A
# Solve using Willems' lemma
K, V = optimal_control(x, u, Q, R)
# Compare results
self.assertTrue(np.linalg.norm(K - trueK) < 1e-4)
def discretize_state_space(self, dt):
"""
Calculate discrete state space matrices
Input:
- dt: timestep length
Output:
- discrete state space matrices A, B, C, D
"""
# model constants
lf = self.lf
lr = self.lr
Caf = self.Caf
Car = self.Car
Iz = self.Iz
m = self.m
vx = self.vx
self.A = np.array([
[0, 1, 0, 0],
[
0, -2 * (Caf + Car) / (m * vx), 2 * (Caf + Car) / m,
2 * (Car * lr - Caf * lf) / (m * vx)
],
[0, 0, 0, 1],
[
0, -2 * (lf * Caf - lr * Car) / (Iz * vx),
2 * (Caf * lf - Car * lr) / Iz,
-2 * (lf**2 * Caf + lr**2 * Car) / (Iz * vx)
],
])
Gcont = signal.StateSpace(self.A, self.B, self.C, self.D)
Gdisc = Gcont.to_discrete(dt)
return Gdisc.A, Gdisc.B, Gdisc.C, Gdisc.D
Bm[1, 1] = alpha2.value[0] / c5
Cm[0, 2] = 1
Cm[1, 3] = 1
# state space simulation
ss = GEKKO()
x, y, u = ss.state_space(Am, Bm, Cm, D=None)
u[0].value = data['Q1'].values
u[1].value = data['Q2'].values
ss.time = data['Time'].values
ss.options.IMODE = 7
ss.solve(disp=False)
# state space simulation with scipy
sys = signal.StateSpace(Am, Bm, Cm, Dm)
tsys = data['Time'].values
Qsys = np.vstack((data['Q1'].values, data['Q2'].values))
Qsys = Qsys.T
tsys, ysys, xsys = signal.lsim(sys, Qsys, tsys)
sae = 0.0
for i in range(len(data)):
sae += np.abs(data['T1'][i] - TC1.value[i])
sae += np.abs(data['T2'][i] - TC2.value[i])
print('SAE Energy Balance: ' + str(sae))
# Create plot
plt.figure(figsize=(10, 7))
ax = plt.subplot(2, 1, 1)
ax.grid()
def control_update(self):
traj = self.traj
vehicle = self.vehicle
lr = vehicle.lr
lf = vehicle.lf
Ca = vehicle.Ca
Iz = vehicle.Iz
f = vehicle.f
m = vehicle.m
g = vehicle.g
delT = 0.05
#reading current vehicle states
X = vehicle.state.X
Y = vehicle.state.Y
xdot = vehicle.state.xd
ydot = vehicle.state.yd
phi = vehicle.state.phi
phidot = vehicle.state.phid
delta = vehicle.state.delta
vx = xdot
# ---------------|Lateral Controller|-------------------------
dist, index1 = closest_node(X, Y, traj)
n = 180
index = index1 + n
if (index >= 8203):
index = 8202
X_new = traj[index][0]
Y_new = traj[index][1]
K_p = 2500
K_i = 0.0001
K_d = 1.5
dist1 = math.sqrt((Y_new - Y)**2 + (X_new - X)**2)
v_d = dist1 / ((n - 80) * delT)
v_d = 8
v = math.sqrt(xdot**2 + ydot**2)
# e22 = wrap2pi(phi - np.arctan2(Y_new-Y, X_new-X))
# curv=self.curv
# ------------|Lateral Controller|---------------------------------
A = np.array([[0, 1, 0, 0],
[
0, -(4 * Ca) / (m * xdot), (4 * Ca) / m,
(-(2 * Ca * lf) + (2 * Ca * lr)) / (m * xdot)
], [0, 0, 0, 1],
[
0, (-(2 * Ca * lf) + (2 * Ca * lr)) / (Iz * xdot),
((2 * Ca * lf) - (2 * Ca * lr)) / (Iz),
(-(2 * Ca * lf * lf) - (2 * Ca * lr * lr)) /
(Iz * xdot)
]])
# print(np.shape(A))
B = np.array([[0], [(2 * Ca) / m], [0], [(2 * Ca * lf) / Iz]])
C = np.identity(4)
D = [[0], [0], [0], [0]]
syscont = signal.StateSpace(A, B, C, D)
sysdisc = syscont.to_discrete(delT)
Ad = sysdisc.A
Bd = sysdisc.B
R = np.zeros((1, 1))
R[0][0] = 0.06
Q = 0.8 * np.identity(4)
S = np.matrix(scipy.linalg.solve_discrete_are(Ad, Bd, Q, R))
r = wrap2pi(phi - np.arctan2(Y_new - Y, X_new - X))
e21 = 0.1
e22 = ydot + (vx * r)
e23 = r
e24 = phidot
e = np.matrix([e21, e22, e23, e24])
#print(len(e), len(e[0]))
K = np.matrix(scipy.linalg.inv((Bd.T @ S @ Bd) + R) @ (Bd.T @ S @ Ad))
#W= np.identity(4)
V = (np.identity(4)) * 500
#V=np.eye(4)
W = [[1, 0, 0, 0], [0, 0.5, 0, 0], [0, 0, 0.5, 0], [0, 0, 0, 0.05]]
#V=np.matrix([1,0.5,0.5,0.05])
xk_km = (Ad) @ (self.xkm) + (Bd) * (self.Z)
Pk_km = ((Ad) @ (self.Pkm)) @ (Ad.T) + W
yk = e.T
Lk = ((Pk_km) @ (C.T)) @ (np.linalg.inv(((C) @ (Pk_km) @ (C.T) + V)))
xk_k = xk_km + Lk @ (yk - C @ xk_km)
Pk_k = (np.identity(4) - (Lk @ C)) @ Pk_km
deltades = float(-K @ (xk_k))
deltad = ((deltades - delta)) / 0.05
# deltad = float(-K*(e))
#delta = np.matmul(K, xk_k)
#deltad = -delta[0][0]
#deltad = float(deltad)
#--------|Longitudinal Controller|------------------------------
e11 = (v_d - v)
F = K_p * (e11) + K_i * (self.e + e11) + K_d * (e11 - self.e111)
self.e += e11
self.e111 = e11
# -----------------------------------------------------------------
# Communicating the control commands with the BuggySimulator
controlinp = vehicle.command(F, deltad)
self.Z = deltades
self.xkm = xk_k
self.Pkm = Pk_k
#print(e)
# F: Force
# deltad: desired rate of steering command
return controlinp
def join(SS1, SS2):
'''
Join two state-spaces or gain matrices such that, given:
u1 -> SS1 -> y1
u2 -> SS2 -> y2
we obtain:
u -> SStot -> y
with u=(u1,u2)^T and y=(y1,y2)^T.
The output SStot is either a gain matrix or a state-space system according
to the input SS1 and SS2
'''
type_dlti = scsig.ltisys.StateSpaceDiscrete
if isinstance(SS1, np.ndarray) and isinstance(SS2, np.ndarray):
Nin01, Nin02 = SS1.shape[1], SS2.shape[1]
Nout01, Nout02 = SS1.shape[0], SS2.shape[0]
SStot = np.block([[SS1, np.zeros((Nout01, Nin02))],
[np.zeros((Nout02, Nin01)), SS2]])
elif isinstance(SS1, np.ndarray) and isinstance(SS2, type_dlti):
Nin01, Nout01 = SS1.shape[1], SS1.shape[0]
Nin02, Nout02 = SS2.inputs, SS2.outputs
Nx02 = SS2.A.shape[0]
A = SS2.A
B = np.block([np.zeros((Nx02, Nin01)), SS2.B])
C = np.block([[np.zeros((Nout01, Nx02))], [SS2.C]])
D = np.block([[SS1, np.zeros((Nout01, Nin02))],
[np.zeros((Nout02, Nin01)), SS2.D]])
SStot = scsig.StateSpace(A, B, C, D, dt=SS2.dt)
elif isinstance(SS1, type_dlti) and isinstance(SS2, np.ndarray):
Nin01, Nout01 = SS1.inputs, SS1.outputs
Nin02, Nout02 = SS2.shape[1], SS2.shape[0]
Nx01 = SS1.A.shape[0]
A = SS1.A
B = np.block([SS1.B, np.zeros((Nx01, Nin02))])
C = np.block([[SS1.C], [np.zeros((Nout02, Nx01))]])
D = np.block([[SS1.D, np.zeros((Nout01, Nin02))],
[np.zeros((Nout02, Nin01)), SS2]])
SStot = scsig.StateSpace(A, B, C, D, dt=SS1.dt)
elif isinstance(SS1, type_dlti) and isinstance(SS2, type_dlti):
assert SS1.dt == SS2.dt, 'State-space models must have the same time-step'
Nin01, Nout01 = SS1.inputs, SS1.outputs
Nin02, Nout02 = SS2.inputs, SS2.outputs
Nx01, Nx02 = SS1.A.shape[0], SS2.A.shape[0]
A = np.block([[SS1.A, np.zeros((Nx01, Nx02))],
[np.zeros((Nx02, Nx01)), SS2.A]])
B = np.block([[SS1.B, np.zeros((Nx01, Nin02))],
[np.zeros((Nx02, Nin01)), SS2.B]])
C = np.block([[SS1.C, np.zeros((Nout01, Nx02))],
[np.zeros((Nout02, Nx01)), SS2.C]])
D = np.block([[SS1.D, np.zeros((Nout01, Nin02))],
[np.zeros((Nout02, Nin01)), SS2.D]])
SStot = scsig.StateSpace(A, B, C, D, dt=SS1.dt)
else:
raise NameError(
'Input types not recognised in any implemented option!')
return SStot
# check parallel connector
Nout = 2
Nin01, Nin02 = 2, 3
Nst01, Nst02 = 4, 2
# build random systems
fac = 0.1
A01, A02 = fac * np.random.rand(Nst01, Nst01), fac * np.random.rand(
Nst02, Nst02)
B01, B02 = np.random.rand(Nst01, Nin01), np.random.rand(Nst02, Nin02)
C01, C02 = np.random.rand(Nout, Nst01), np.random.rand(Nout, Nst02)
D01, D02 = np.random.rand(Nout, Nin01), np.random.rand(Nout, Nin02)
dt = 0.1
SS01 = scsig.StateSpace(A01, B01, C01, D01, dt=dt)
SS02 = scsig.StateSpace(A02, B02, C02, D02, dt=dt)
# simulate
NT = 11
U01, U02 = np.random.rand(NT, Nin01), np.random.rand(NT, Nin02)
# reference
Y01, X01 = simulate(SS01, U01)
Y02, X02 = simulate(SS02, U02)
Yref = Y01 + Y02
# parallel
SStot = parallel(SS01, SS02)
Utot = np.block([U01, U02])
Ytot, Xtot = simulate(SStot, Utot)
if x0[i] > xg[i]:
u[i] = 0.0
return u
c = 0.0001 #room transfer coef
cu = 0.02 #heating on coef
A = [[-c, c], [c, -c]]
B = [[cu, 0.0], [0.0, cu]]
C = A
D = B
x0 = [50.0, 10.0]
xg = [50.0, 200.0]
sys1 = signal.StateSpace(A, B, C, D)
simTime = 60 * 60 * 1
controlStep = 15 * 60
c = greedyController(x0, xg)
tall = []
uall = np.array([]).reshape((0, len(x0)))
xall = np.array([]).reshape((0, len(x0)))
for ti in range(0, simTime, 1):
t = np.linspace(ti, ti + 1, 10)
if ti % controlStep == 0:
c = greedyController(x0, xg)
u = np.transpose([np.ones(len(t)) * c[0], np.ones(len(t)) * c[1]])
t1, y1, x1 = signal.lsim(sys1, u, t, x0)
x0 = x1[len(x1) - 3]
tall = np.concatenate((tall, t1), axis=0)
