def test_discrete(self):
"""Test discrete time functionality"""
# Create some linear and nonlinear systems to play with
linsys = ct.StateSpace(
[[-1, 1], [0, -2]], [[0], [1]], [[1, 0]], [[0]], True)
lnios = ios.LinearIOSystem(linsys)
# Set up parameters for simulation
T, U, X0 = self.T, self.U, self.X0
# Simulate and compare to LTI output
ios_t, ios_y = ios.input_output_response(lnios, T, U, X0)
lin_t, lin_y, lin_x = ct.forced_response(linsys, T, U, X0)
np.testing.assert_array_almost_equal(ios_t, lin_t, decimal=3)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
# Test MIMO system, converted to discrete time
linsys = ct.StateSpace(self.mimo_linsys1)
linsys.dt = self.T[1] - self.T[0]
lnios = ios.LinearIOSystem(linsys)
# Set up parameters for simulation
T = self.T
U = [np.sin(T), np.cos(T)]
X0 = 0
# Simulate and compare to LTI output
ios_t, ios_y = ios.input_output_response(lnios, T, U, X0)
lin_t, lin_y, lin_x = ct.forced_response(linsys, T, U, X0)
np.testing.assert_array_almost_equal(ios_t, lin_t, decimal=3)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
def set_poles(self, poles):
poles = np.asarray(poles)
self.K_cont = np.asarray(control.place(self.A, self.B, poles))
self.K_disc = np.asarray(control.place(self.A_d, self.B_d, np.exp(poles * self.Ts)))
self.A_hat = self.A - self.B @ self.K_cont
self.sys_cont = control.StateSpace(self.A_hat, self.B, self.C, self.D, remove_useless=False)
self.A_hat_d = self.A_d - self.B_d @ self.K_disc
self.sys_disc = control.StateSpace(self.A_hat_d, self.B_d, self.C_d, self.D_d, self.Ts, remove_useless=False)
return self.K_disc
def test_reduced_order_compensator(self):
with warnings.catch_warnings():
warnings.simplefilter("ignore")
A = np.array([[0., 1., 0., 0.], [0., -25., -0.98, 0.],
[0., 0., 0., 1.], [0., 25., 10.78,
0.]]).astype('float')
B = np.array([[0.], [0.5], [0.], [-0.5]]).reshape(
(4, 1)).astype('float')
C = np.array([[1., 0., 0., 0.]]).reshape((1, 4)).astype('float')
D = np.zeros((1, 1)).astype('float')
### Controller ###
desiredPolesCtrl = np.array([
-25., -4.,
np.complex(-2., 2. * np.sqrt(3)),
np.complex(-2., -2. * np.sqrt(3))
])
G = ch6_utils.bassGura(A, B, desiredPolesCtrl)
G1 = G[0, 0].reshape((1, 1))
G2 = G[0, 1:].reshape((1, 3))
### Observer ###
desiredPolesObsv = np.roots(
np.array([1.0, 2. * 5., 2. * 5.**2, 5.**3]))
Aaa = np.array(A[0, 0]).reshape((1, 1))
Aau = np.array(A[0, 1:]).reshape((1, 3))
Aua = np.array(A[1:, 0]).reshape((3, 1))
Auu = np.array(A[1:, 1:]).reshape((3, 3))
Ba = np.array(B[0]).reshape((1, 1))
Bu = np.array(B[1:]).reshape((3, 1))
Ca = np.array([[1]]).reshape((1, 1)).astype('float')
L, Gbb, H = ch7_utils.reducedOrderObserver(Aaa, Aau, Aua, Auu, Ba,
Bu, Ca,
desiredPolesObsv)
### Compensator ###
F = Auu - L @ Aau
Ar = F - H @ G2
Br = Ar @ L + Gbb - H @ G1
Cr = G2
Dr = G1 + G2 @ L
sysD = control.StateSpace(Ar, Br, Cr, Dr)
compensatorTF = control.ss2tf(sysD)
sysplant = control.StateSpace(A, B, C, D)
openLoopTF = control.ss2tf(sysplant)
# poles are zeros of return difference
poles = control.zero(1. + compensatorTF * openLoopTF)
for p in poles:
polePresent = any([
np.isclose(np.complex(p), np.complex(pt))
for pt in np.hstack([desiredPolesCtrl, desiredPolesObsv])
])
self.assertTrue(polePresent)
def test_nonsquare_bdalg(self):
# Set up parameters for simulation
T = self.T
U2 = [np.sin(T), np.cos(T)]
U3 = [np.sin(T), np.cos(T), T]
X0 = 0
# Set up systems to be composed
linsys_2i3o = ct.StateSpace(
[[-1, 1, 0], [0, -2, 0], [0, 0, -3]], [[1, 0], [0, 1], [1, 1]],
[[1, 0, 0], [0, 1, 0], [0, 0, 1]], np.zeros((3, 2)))
iosys_2i3o = ios.LinearIOSystem(linsys_2i3o)
linsys_3i2o = ct.StateSpace(
[[-1, 1, 0], [0, -2, 0], [0, 0, -3]],
[[1, 0, 0], [0, 1, 0], [0, 0, 1]],
[[1, 0, 1], [0, 1, -1]], np.zeros((2, 3)))
iosys_3i2o = ios.LinearIOSystem(linsys_3i2o)
# Multiplication
linsys_multiply = linsys_3i2o * linsys_2i3o
iosys_multiply = iosys_3i2o * iosys_2i3o
lin_t, lin_y, lin_x = ct.forced_response(linsys_multiply, T, U2, X0)
ios_t, ios_y = ios.input_output_response(iosys_multiply, T, U2, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
linsys_multiply = linsys_2i3o * linsys_3i2o
iosys_multiply = iosys_2i3o * iosys_3i2o
lin_t, lin_y, lin_x = ct.forced_response(linsys_multiply, T, U3, X0)
ios_t, ios_y = ios.input_output_response(iosys_multiply, T, U3, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
# Right multiplication
# TODO: add real tests once conversion from other types is supported
iosys_multiply = ios.InputOutputSystem.__rmul__(iosys_3i2o, iosys_2i3o)
ios_t, ios_y = ios.input_output_response(iosys_multiply, T, U3, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
# Feedback
linsys_multiply = ct.feedback(linsys_3i2o, linsys_2i3o)
iosys_multiply = iosys_3i2o.feedback(iosys_2i3o)
lin_t, lin_y, lin_x = ct.forced_response(linsys_multiply, T, U3, X0)
ios_t, ios_y = ios.input_output_response(iosys_multiply, T, U3, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
# Mismatch should generate exception
args = (iosys_3i2o, iosys_3i2o)
self.assertRaises(ValueError, ct.series, *args)
def reachable_form(xsys):
# Check to make sure we have a SISO system
if not control.issiso(xsys):
raise control.ControlNotImplemented(
"Canonical forms for MIMO systems not yet supported")
# Create a new system, starting with a copy of the old one
zsys = control.StateSpace(xsys)
# Generate the system matrices for the desired canonical form
zsys.B = zeros(shape(xsys.B))
zsys.B[0, 0] = 1
zsys.A = zeros(shape(xsys.A))
Apoly = poly(xsys.A) # characteristic polynomial
for i in range(0, xsys.states):
zsys.A[0, i] = -Apoly[i + 1] / Apoly[0]
if (i + 1 < xsys.states): zsys.A[i + 1, i] = 1
# Compute the reachability matrices for each set of states
Wrx = control.ctrb(xsys.A, xsys.B)
Wrz = control.ctrb(zsys.A, zsys.B)
# Transformation from one form to another
Tzx = Wrz * inv(Wrx)
# Finally, compute the output matrix
zsys.C = xsys.C * inv(Tzx)
return zsys, Tzx
def plot_pzmaps(self):
"""Plots pole-zero maps of open-loop system, closed-loop system, and
observer poles.
"""
# Plot pole-zero map of open-loop system
print("Open-loop poles =", self.sysd.pole())
print("Open-loop zeroes =", self.sysd.zero())
plt.subplot(2, 2, 1)
frccnt.dpzmap(self.sysd, title="Open-loop system")
# Plot pole-zero map of closed-loop system
sys = frccnt.closed_loop_ctrl(self)
print("Closed-loop poles =", sys.pole())
print("Closed-loop zeroes =", sys.zero())
plt.subplot(2, 2, 2)
frccnt.dpzmap(sys, title="Closed-loop system")
# Plot observer poles
sys = cnt.StateSpace(self.sysd.A - self.L @ self.sysd.C, self.sysd.B,
self.sysd.C, self.sysd.D)
print("Observer poles =", sys.pole())
plt.subplot(2, 2, 3)
frccnt.plot_observer_poles(self)
plt.tight_layout()
def ApplyBalancedTruncation(SamplingInterval,
reducedOrder,
A=None,
B=None,
C=None,
D=None,
filepath=None):
r, m, p, n, A, B, C, D = AssignVars(reducedOrder, A, B, C, D, filepath)
BBT = np.matmul(B, B.transpose())
CT = C.transpose()
DiscreteSystem = ct.StateSpace(A, B, CT, D, SamplingInterval)
# We calculate the cholesky-factors of both Gramians of the system and the Gramians itself.
Rc = ct.gram(DiscreteSystem, 'cf')
Wc = np.matmul(Rc, Rc.transpose())
Ro = ct.gram(DiscreteSystem, 'of')
Wo = np.matmul(Ro.transpose(), Ro)
T, T_inv, HSV = SquareRootAlgorithm(Rc, Wc, Ro, Wo)
PlotData(HSV)
A_prime, B_prime, C_prime = SimilarityTransform(A, B, CT, T, T_inv)
A11, B1, C1 = TruncateSystemMatrices(A_prime, B_prime, C_prime, r, m, p)
return A11, B1, C1
def lqr_sys(A, B, C, D, in_q, r=1, num_q=6):
#parameter
sys = control.StateSpace(A, B, C, D)
Q = np.zeros((num_q, num_q))
R = [r]
for i in range(num_q):
Q[i, i] = in_q[i]
K, solu_R, Eig_sys = control.lqr(sys, Q, r)
kr = -1 / np.matmul(np.matmul(C, npl.inv((A - np.matmul(B, K)))), B)[0]
pole, eig_v = npl.eig(A - B * K)
pole_real = [a.real for a in pole]
minpol = -3 + min(pole_real)
repol = []
for i in range(num_q):
repol.append(minpol + i + 1)
repol = np.array(repol)
L = scis.place_poles(A.transpose(), C.transpose(), repol).gain_matrix
L = L.transpose()
upCA = np.column_stack(((A - np.matmul(B, K)), (np.matmul(B, K))))
downCA = np.column_stack((np.zeros((num_q, num_q)), (A - np.matmul(L, C))))
CA = np.row_stack((upCA, downCA))
CB = np.row_stack((B * kr, np.zeros((num_q, 1))))
CC = np.column_stack((C, np.zeros((1, num_q))))
CD = np.array([0])
return CA, CB, CC, CD
def test_double_integrator(self):
# Define a second order integrator
sys = ct.StateSpace([[-1, 1], [0, -2]], [[0], [1]], [[1, 0]], 0)
flatsys = fs.LinearFlatSystem(sys)
# Define the endpoints of a trajectory
x1 = [0, 0]; u1 = [0]; T1 = 1
x2 = [1, 0]; u2 = [0]; T2 = 2
x3 = [0, 1]; u3 = [0]; T3 = 3
x4 = [1, 1]; u4 = [1]; T4 = 4
# Define the basis set
poly = fs.PolyFamily(6)
# Plan trajectories for various combinations
for x0, u0, xf, uf, Tf in [
(x1, u1, x2, u2, T2), (x1, u1, x3, u3, T3), (x1, u1, x4, u4, T4)]:
traj = fs.point_to_point(flatsys, x0, u0, xf, uf, Tf, basis=poly)
# Verify that the trajectory computation is correct
x, u = traj.eval([0, Tf])
np.testing.assert_array_almost_equal(x0, x[:, 0])
np.testing.assert_array_almost_equal(u0, u[:, 0])
np.testing.assert_array_almost_equal(xf, x[:, 1])
np.testing.assert_array_almost_equal(uf, u[:, 1])
# Simulate the system and make sure we stay close to desired traj
T = np.linspace(0, Tf, 100)
xd, ud = traj.eval(T)
t, y, x = ct.forced_response(sys, T, ud, x0)
np.testing.assert_array_almost_equal(x, xd, decimal=3)
def test_full_order_compensator(self):
with warnings.catch_warnings():
warnings.simplefilter("ignore")
A = np.array([[0., 1., 0., 0.], [0., -25., -0.98, 0.],
[0., 0., 0., 1.], [0., 25., 10.78,
0.]]).astype('float')
B = np.array([[0.], [0.5], [0.], [-0.5]]).astype('float')
C = np.array([[1., 0., 0., 0.]]).astype('float')
D = np.zeros((1, 1)).astype('float')
desiredPolesCtrl = np.array([
-25., -4.,
np.complex(-2., 2. * np.sqrt(3)),
np.complex(-2., -2. * np.sqrt(3))
])
w0 = 5.
desiredPolesObsv = np.roots(
np.array([
1.0, 2.613 * w0, (2. + np.sqrt(2)) * w0**2, 2.613 * w0**3,
w0**4
]))
compensatorTF = fullOrderCompensator(A, B, C, D, desiredPolesCtrl,
desiredPolesObsv)
openLoopTF = control.ss2tf(control.StateSpace(A, B, C, D))
# poles are zeros of return difference
poles = control.zero(1. + compensatorTF * openLoopTF)
for p in poles:
polePresent = any([
np.isclose(np.complex(p), np.complex(pt))
for pt in np.hstack([desiredPolesCtrl, desiredPolesObsv])
])
self.assertTrue(polePresent)
def _reduce_state_space(self):
x_al = self.fsf_dict['x_al']
x_sp_al = self.fsf_dict['x_sp_al']
x_er_al = self.fsf_dict['x_er_al']
u_al = self.fsf_dict['u_al']
u_sp_al = self.fsf_dict['u_sp_al']
u_er_al = self.fsf_dict['u_er_al']
al_sys = self.fsf_dict['al_sys']
#Construct reduced state space
x_ral = x_al[7:]
x_sp_ral = x_sp_al[7:]
x_er_ral = x_er_al[7:]
u_ral = u_al[np.ix_([0,1,3,4])]
u_sp_ral = u_sp_al[np.ix_([0,1,3,4])]
u_er_ral = u_er_al[np.ix_([0,1,3,4])]
A = al_sys.A
B = al_sys.B
C = al_sys.C
D = al_sys.D
A_ral = A[np.ix_([7,8,9,10,11,12],[7,8,9,10,11,12])]
B_ral = B[np.ix_([7,8,9,10,11,12],[0,1,3,4])]
C_ral = C[np.ix_([7,8,9,10,11,12],[7,8,9,10,11,12])]
D_ral = D[np.ix_([7,8,9,10,11,12],[0,1,3,4])]
#Lateral and longitudinal state spaces
ral_sys = control.StateSpace(A_ral, B_ral, C_ral, D_ral)
ral_ctrb = control.ctrb(ral_sys.A, ral_sys.B)
ral_obsv = control.obsv(ral_sys.A, ral_sys.C)
#Update FSF dictionary
self.fsf_dict.update(x_ral = x_ral)
self.fsf_dict.update(x_sp_ral = x_sp_ral)
self.fsf_dict.update(x_er_ral = x_er_ral)
self.fsf_dict.update(u_ral = u_ral)
self.fsf_dict.update(u_sp_ral = u_sp_ral)
self.fsf_dict.update(u_er_ral = u_er_ral)
self.fsf_dict.update(ral_sys = ral_sys)
def __init__(self):
self.fast_d = 0
self.slow_d = 2
#define our matrices for the linearized model, which we have
M = 0.5 #cart mass
m = 0.2 #pedulum mass
b = 0.1 #coefficient of friction
I = 0.006 #inertia
g = 9.8 #gravity
l = 0.3 #length of the pendulum
k = I * (M + m) + M * m * l**2.0
U2 = (I + m * l**2.0) / k
U4 = -m * l / k
TF_2_3 = -(g * m**2.0 * l**2.0) / k
TF_4_3 = (g * (m + M)) / k
# TF_4_3 = (m*l*(M+m))*g/k;
self.B = [[0], [U2], [0], [U4]]
self.A = [[0, 1, 0, 0], [0, 0, TF_2_3, 0], [0, 0, 0, 1],
[0, 0, TF_4_3, 0]]
self.C = [[1, 0, 0, 0], [0, 0, 1, 0]]
self.D = [[0], [0]]
self.sys = control.StateSpace(self.A, self.B, self.C, self.D)
# R = 1
# Q = [[20, 0, 0, 0], [0, 0, 0, 0], [0, 0, 3, 0], [0, 0, 0, 0,]]
#develop our fast dynamic controller (Using pole placement)
self.K = control.place(self.A, self.B, [-5.1, -5.2, -5.3, -5.4])
self.K = np.matrix(self.K.tolist()[0])
#deevelop our slow dynamic controller (PID)
self.pid = PID(5, 0.1, 0.05)
def objective_func(v):
A = unpack_v(v, M)
system_state = x0.reshape((-1, 1)) # make state a column vector
cov = P0
meas_err_rec = [0]
state_rec = [system_state]
cov_rec = [cov]
# Convert from cont to disc
B = np.zeros((system_state.shape[0], 1))
C = H
D = np.zeros((1, 1))
system = control.StateSpace(A, B, C, D)
dis_system = control.matlab.c2d(system, 8.225e-3)
for index in range(zk.shape[1] - 1):
meas_err, system_state, cov = calc_Kalman(dis_system.A, H, Q, R,
zk[:, index],
system_state, cov)
# record and propagate
meas_err_rec.append(meas_err)
state_rec.append(system_state)
cov_rec.append(cov)
Fest = np.asarray([state[-1] for state in state_rec], dtype=float).T
sum_state_err = np.linalg.norm(Fest - F_in, 2)
sum_meas_err = np.linalg.norm(meas_err, 2)
return sum_state_err + sum_meas_err, state_rec
def full_obs(sys,poles):
"""
Full order observer of the system sys
Call:
obs = full_obs(sys,poles)
Parameters
----------
sys : System in State Space form
poles: desired observer poles
Returns
-------
obs: ss
Observer
"""
if isinstance(sys, ct.TransferFunction):
"System must be in state space form"
return
a = np.mat(sys.A)
b = np.mat(sys.B)
c = np.mat(sys.C)
d = np.mat(sys.D)
L = ct.place(a.T,c.T,poles)
L = np.mat(L).T
Ao = a-L*c
Bo = np.hstack((b-L*d,L))
n = np.shape(Ao)
m = np.shape(Bo)
Co = np.eye(n[0],n[1])
Do = np.zeros((n[0],m[1]))
obs = ct.StateSpace(Ao,Bo,Co,Do,sys.dt)
return obs
def fullOrderCompensator(A, B, C, D, controlPoles, observerPoles):
"""
combine controller and observer constructions into a compensator design
currently, supports single-input-single-output systems only
Inputs:
A (numpy matrix/array, type=real) - system state matrix
B (numpy matrix/array, type=real) - system control matrix
C (numpy matrix/array, type=real) - system measurement matrix
desiredPoles (numpy matrix/array, type=complex) - desired pole locations
D (numpy matrix/array, type=real) - direct path from control to measurement matrix
E (numpy matrix/array, type=real) - (default=None) exogeneous input matrix
controlPoles (numpy matrix/array, type=complex) - desired controller system poles
observerPoles (numpy matrix/array, type=complex) - desired observer system poles
Returns:
(control.TransferFunction) - compensator transfer function from input to output
Raises:
TypeError - if input system is not SISO
"""
if B.shape[1] > 1 or C.shape[0] > 1:
raise TypeError(
'Only single-input-single-output (SISO) systems are currently supported'
)
A = A.astype('float')
B = B.astype('float')
C = C.astype('float')
D = D.astype('float')
G = ch6_utils.bassGura(A, B, controlPoles)
K = ch7_utils.obsBassGura(A, C, observerPoles)
return control.ss2tf(control.StateSpace(A - B @ G - K @ C, K, G, D))
def __init__(self):
m = 100.0
b = 20.0
meas_noise = .001
self.vel_noise = 0.01
self.pos_noise = 0.0001
# meas_noise = 1
# self.vel_noise = 0.1
# self.pos_noise = 1
self.Q = meas_noise
self.R = np.diag([self.vel_noise,self.pos_noise])
a = np.array([[-b/m, 0],[1, 0]])
b = np.array([[1/m], [0]])
c = np.array([0, 1])
d = np.array([0])
self.dt = 0.05
sysc = control.StateSpace(a, b, c, d)
sysd = control.c2d(sysc, self.dt)
self.A = np.array(sysd.A)
self.B = np.array(sysd.B)
self.C = np.array(sysd.C)
given = sio.loadmat('hw1_soln_data.mat')
self.z = given['z']
self.xtr = given['xtr']
self.vtr = given['vtr']
def unicycle(states, inputs):
"""Returns the state-space model for a unicycle.
States: [[x], [y], [theta]]
Inputs: [[velocity], [angular velocity]]
Outputs: [[theta]]
Keyword arguments:
states -- state vector around which to linearize model
inputs -- input vector around which to linearize model
Returns:
StateSpace instance containing continuous model
"""
x = states[0, 0]
y = states[1, 0]
theta = states[2, 0]
v = inputs[0, 0]
omega = inputs[1, 0]
if abs(v) < 5e-8:
v = 5e-8
# fmt: off
A = np.array([[0, 0, -v * math.sin(theta)], [0, 0, v * math.cos(theta)],
[0, 0, 0]])
B = np.array([[math.cos(theta), 0], [math.sin(theta), 0], [0, 1]])
C = np.array([[0, 0, 1]])
D = np.array([[0, 0]])
# fmt: on
return ct.StateSpace(A, B, C, D, remove_useless=False)
def comp_form_i(sys, obs, K, Cy=[[1]]):
"""Compact form Conroller+Observer+Integral part
Only for discrete systems!!!
Call:
contr = comp_form_i(sys,obs,K [,Cy])
Parameters
----------
sys : System in State Space form
obs : Observer in State Space form
K: State feedback gains
Cy: feedback matric to choose the output for integral part
Returns
-------
contr: ss
Controller
"""
if sys.dt == None:
print('contr_form_i works only with discrete systems!')
return
Ts = sys.dt
ny = np.shape(sys.C)[0]
nu = np.shape(sys.B)[1]
nx = np.shape(sys.A)[0]
no = np.shape(obs.A)[0]
ni = np.shape(np.mat(Cy))[0]
B_obsu = np.mat(obs.B[:, 0:nu])
B_obsy = np.mat(obs.B[:, nu:nu + ny])
D_obsu = np.mat(obs.D[:, 0:nu])
D_obsy = np.mat(obs.D[:, nu:nu + ny])
k = np.mat(K)
nk = np.shape(k)[1]
Ke = k[:, nk - ni:]
K = k[:, 0:nk - ni]
X = la.inv(np.eye(nu, nu) + K * D_obsu)
a = np.mat(obs.A)
c = np.mat(obs.C)
Cy = np.mat(Cy)
tmp1 = np.hstack((a - B_obsu * X * K * c, -B_obsu * X * Ke))
tmp2 = np.hstack((np.zeros((ni, no)), np.eye(ni, ni)))
A_ctr = np.vstack((tmp1, tmp2))
tmp1 = np.hstack((np.zeros((no, ni)), -B_obsu * X * K * D_obsy + B_obsy))
tmp2 = np.hstack((np.eye(ni, ni) * Ts, -Cy * Ts))
B_ctr = np.vstack((tmp1, tmp2))
C_ctr = np.hstack((-X * K * c, -X * Ke))
D_ctr = np.hstack((np.zeros((nu, ni)), -X * K * D_obsy))
contr = ct.StateSpace(A_ctr, B_ctr, C_ctr, D_ctr, sys.dt)
return contr
def parameters(self):
self.M = 0.5
self.m = 0.2
self.b = 0.1
self.I = 0.006
self.g = 9.8
self.l = 0.3
self.p = self.I*(self.M+self.m)+self.M*self.m*self.l**2; # denominator for the A and B matrices
self.theta1 = -(self.I+self.m*self.l**2)*self.b/self.p
self.theta2 = (self.m**2*self.g*self.l**2)/self.p
self.theta3 = -(self.m*self.l*self.b)/self.p
self.theta4 = (self.m*self.g*self.l*(self.M+self.m)/self.p)
self.A = np.asarray([[0,1,0,0],[0,self.theta1,self.theta2,0],
[0,0,0,1],[0,self.theta3,self.theta4,0]])
self.B = np.asarray([[0],[(self.I+self.m*self.l**2)/self.p],
[0],[self.m*self.l/self.p]])
self.C = np.asarray([[1,0,0,0],[0,0,1,0]])
self.D = np.asarray([[0],[0]])
self.ssm = control.StateSpace(self.A, self.B, self.C, self.D)
self.ssmd = self.ssm.sample(self.ts, method='euler')
self.nx = self.A.shape[0]
self.ny = self.C.shape[0]
self.nu = self.B.shape[1]
self.x0 = np.asarray([0,0,-1,0])
def get_plant_op(self, u_h, reduce_states):
'''
Interpolate system matrices using wind speed operating point uh_op = mean(u_h)
inputs: u_h timeseries of wind speeds
'''
uh_op = np.mean(u_h)
if len(self.u_h) > 1:
f_A = sp.interpolate.interp1d(self.u_h, self.A_ops)
f_B = sp.interpolate.interp1d(self.u_h, self.B_ops)
f_C = sp.interpolate.interp1d(self.u_h, self.C_ops)
f_D = sp.interpolate.interp1d(self.u_h, self.D_ops)
f_u = sp.interpolate.interp1d(self.u_h, self.u_ops)
f_y = sp.interpolate.interp1d(self.u_h, self.y_ops)
f_x = sp.interpolate.interp1d(self.u_h, self.x_ops)
A = f_A(uh_op)
B = f_B(uh_op)
C = f_C(uh_op)
D = f_D(uh_op)
u_op = f_u(uh_op)
x_op = f_x(uh_op)
y_op = f_y(uh_op)
else:
print('WARNING: Only one linearization, at ' + str(self.u_h[0]) +
' m/s')
print('Simulation operating point is ' + str(uh_op) + 'm/s')
print('Results may not be as expected')
# Set linear system to only linearization
A = self.A_ops[:, :, 0]
B = self.B_ops[:, :, 0]
C = self.C_ops[:, :, 0]
D = self.D_ops[:, :, 0]
u_op = self.u_ops[:, 0]
y_op = self.y_ops[:, 0]
x_op = self.x_ops[:, 0]
# form state space model TODO: generalize using linear description strings
P_op = co.StateSpace(A, B, C, D)
if reduce_states:
P_op = co.minreal(P_op, tol=1e-7, verbose=False)
P_op.InputName = ['RtVAvgxh', 'BldPitch']
P_op.OutputName = ['GenSpeed', 'TwrBsMyt', 'PtfmPitch', 'NcIMUTAzs']
# operating points dict
ops = {}
ops['u'] = u_op[self.ind_fast_inps]
ops['uh'] = uh_op
ops['y'] = y_op[self.ind_fast_outs]
ops['x'] = x_op
return ops, P_op
def differential_drive(motor, num_motors, m, r, rb, J, Gl, Gr, states):
"""Returns the state-space model for a differential drive.
States: [[x], [y], [theta], [left velocity], [right velocity]]
Inputs: [[left voltage], [right voltage]]
Outputs: [[theta], [left velocity], [right velocity]]
Keyword arguments:
motor -- instance of DcBrushedMotor
num_motors -- number of motors driving the mechanism
m -- mass of robot in kg
r -- radius of wheels in meters
rb -- radius of robot in meters
J -- moment of inertia of the differential drive in kg-m^2
Gl -- gear ratio of left side of differential drive
Gr -- gear ratio of right side of differential drive
states -- state vector around which to linearize model
Returns:
StateSpace instance containing continuous model
"""
motor = fct.models.gearbox(motor, num_motors)
C1 = -(Gl ** 2) * motor.Kt / (motor.Kv * motor.R * r ** 2)
C2 = Gl * motor.Kt / (motor.R * r)
C3 = -(Gr ** 2) * motor.Kt / (motor.Kv * motor.R * r ** 2)
C4 = Gr * motor.Kt / (motor.R * r)
x = states[0, 0]
y = states[1, 0]
theta = states[2, 0]
vl = states[3, 0]
vr = states[4, 0]
v = (vr + vl) / 2.0
if abs(v) < 1e-9:
vl = 1e-9
vr = 1e-9
v = 1e-9
# fmt: off
A = np.array([[0, 0, 0, 0.5, 0.5],
[0, 0, v, 0, 0],
[0, 0, 0, -0.5 / rb, 0.5 / rb],
[0, 0, 0, (1 / m + rb**2 / J) * C1, (1 / m - rb**2 / J) * C3],
[0, 0, 0, (1 / m - rb**2 / J) * C1, (1 / m + rb**2 / J) * C3]])
B = np.array([[0, 0],
[0, 0],
[0, 0],
[(1 / m + rb**2 / J) * C2, (1 / m - rb**2 / J) * C4],
[(1 / m - rb**2 / J) * C2, (1 / m + rb**2 / J) * C4]])
C = np.array([[0, 0, 1, 0, 0],
[0, 0, 0, 1, 0],
[0, 0, 0, 0, 1]])
D = np.array([[0, 0],
[0, 0],
[0, 0]])
# fmt: on
return ct.StateSpace(A, B, C, D, remove_useless=False)
def plot_observer_poles(system):
"""Plot discrete observer poles.
Keyword arguments:
system -- a System instance
"""
sys_cl = cnt.StateSpace(system.sysd.A - system.L * system.sysd.C,
system.sysd.B, system.sysd.C, system.sysd.D)
dpzmap(sys_cl, title="Observer poles")
def state_space2(CZ0, CXq, CZadot, Cnbdot, Clr, Clda, muc, c, V, CZa, Cmadot,
KY2, CXu, CXa, CZq, CZu, CX0, Cmu, Cma, Cmq, CXde, CZde, Cmde,
CYbdot, b, mub, KX2, KXZ, KZ2, CYb, Cl, CYp, CYr, Clb, Clp,
Cnb, Cnp, Cnr, CYda, Cldr, Cnda, Cndr, CYdr):
#SYMETRIC FLIGHT CONDITIONS
CS1 = np.matrix([[-2 * muc * c / V / V, 0, 0, 0],
[0, (CZadot - 2 * muc) * c / V, 0, 0], [0, 0, -c / V, 0],
[0, Cmadot * c / V, 0, -2 * muc * KY2 * c * c / V / V]])
CS2 = np.matrix([[CXu / V, CXa, CZ0, CXq * c / V],
[CZu / V, CZa, -CX0, c / V * (CZq + 2 * muc)],
[0, 0, 0, c / V], [Cmu / V, Cma, 0, Cmq * c / V]])
CS3 = np.matrix([[CXde], [CZde], [0], [Cmde]])
A_s = -CS1**(-1) * CS2
B_s = -CS1**(-1) * CS3
C_s = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
D_s = np.matrix([[0], [0], [0], [0]])
sys_sym = control.StateSpace(A_s, B_s, C_s, D_s)
#ASYMMETRIC FLIGHT CONDITIONS
CA1 = np.matrix([[(CYbdot - 2 * mub) * b / V, 0, 0, 0],
[0, -b / 2 / V, 0, 0],
[
0, 0, -4 * mub * KX2 * b * b / 2 / V / V,
4 * mub * KXZ * b * b / 2 / V / V
],
[
Cnb * b / V, 0, 4 * mub * KXZ * b * b / 2 / V / V,
-4 * mub * KZ2 * b * b / 2 / V / V
]])
CA2 = np.matrix([[CYb, Cl, CYp * b / V / 2, (CYr - 4 * mub) * b / 2 / V],
[0, 0, b / V / 2, 0],
[Clb, 0, Clp * b / 2 / V, Clr * b / 2 / V],
[Cnb, 0, Cnp * b / 2 / V, Cnr * b / 2 / V]])
CA3 = np.matrix([[CYda, CYdr], [0, 0], [Clda, Cldr], [Cnda, Cndr]])
A_a = -CA1**(-1) * CA2
B_a = -CA1**(-1) * CA3
C_a = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
D_a = np.matrix([[0, 0], [0, 0], [0, 0], [0, 0]])
sys_asym = control.StateSpace(A_a, B_a, C_a, D_a)
return sys_sym, sys_asym
def make_model(self, v):
A = np.zeros((3, 3))
if abs(v) < 1e-9:
v = 1e-9
A[1, 2] = v
B = np.array([[1, 0], [0, 0], [0, 1]])
C = np.array([[0, 0, 1]])
D = np.array([[0, 0]])
return ct.StateSpace(A, B, C, D, remove_useless=False)
def create_uncertain_copy(nominal_dynamics: Twipr2D, std_dev_mass):
disturbed_dynamics = copy.deepcopy(nominal_dynamics)
m_a = abs(std_dev_mass * np.random.randn())
disturbed_dynamics.model.l = (disturbed_dynamics.model.l*disturbed_dynamics.model.m_b + 0.2 * m_a) / (m_a + disturbed_dynamics.model.m_b)
disturbed_dynamics.model.I_y = disturbed_dynamics.model.I_y + 0.2**2*m_a
disturbed_dynamics.model.m_b = disturbed_dynamics.model.m_b + m_a
disturbed_dynamics.A, disturbed_dynamics.B, disturbed_dynamics.C, disturbed_dynamics.D = disturbed_dynamics.linear_model()
# generate a linear continious model
disturbed_dynamics.sys_cont = control.StateSpace(disturbed_dynamics.A, disturbed_dynamics.B, disturbed_dynamics.C, disturbed_dynamics.D, remove_useless=False)
# convert the continious-time model to discrete-time
disturbed_dynamics.sys_disc = control.c2d(disturbed_dynamics.sys_cont, disturbed_dynamics.Ts)
disturbed_dynamics.A_d = np.asarray(disturbed_dynamics.sys_disc.A)
disturbed_dynamics.B_d = np.asarray(disturbed_dynamics.sys_disc.B)
disturbed_dynamics.C_d = np.asarray(disturbed_dynamics.sys_disc.C)
disturbed_dynamics.D_d = np.asarray(disturbed_dynamics.sys_disc.D)
disturbed_dynamics.A_hat_d = disturbed_dynamics.A_d - disturbed_dynamics.B_d @ disturbed_dynamics.K_disc
disturbed_dynamics.sys_disc = control.StateSpace(disturbed_dynamics.A_hat_d, disturbed_dynamics.B_d, disturbed_dynamics.C_d, disturbed_dynamics.D_d, disturbed_dynamics.Ts, remove_useless=False)
return disturbed_dynamics
def setUp(self):
# Turn off numpy matrix warnings
import warnings
warnings.simplefilter('ignore', category=PendingDeprecationWarning)
# Create a single input/single output linear system
self.siso_linsys = ct.StateSpace([[-1, 1], [0, -2]], [[0], [1]],
[[1, 0]], [[0]])
# Create a multi input/multi output linear system
self.mimo_linsys1 = ct.StateSpace([[-1, 1], [0, -2]], [[1, 0], [0, 1]],
[[1, 0], [0, 1]], np.zeros((2, 2)))
# Create a multi input/multi output linear system
self.mimo_linsys2 = ct.StateSpace([[-1, 1], [0, -2]], [[0, 1], [1, 0]],
[[1, 0], [0, 1]], np.zeros((2, 2)))
# Create simulation parameters
self.T = np.linspace(0, 10, 100)
self.U = np.sin(self.T)
self.X0 = [0, 0]
def closed_loop_ctrl(system):
"""Constructs the closed-loop system for a discrete controller.
Keyword arguments:
system -- a System instance
Returns:
StateSpace instance representing closed-loop controller.
"""
return cnt.StateSpace(system.sysd.A - system.sysd.B * system.K,
system.sysd.B * system.K,
system.sysd.C - system.sysd.D * system.K,
system.sysd.D * system.K)
def GenerateModel(Mass, Stiffness, Damping):
n = len(Mass)
M, K, G, T_u, T_w = GenerateMatricess(Mass, Stiffness, Damping)
A = np.block([[np.zeros((n, n)), np.identity(n)],
[-np.linalg.inv(M) @ K, -np.linalg.inv(M) @ G]])
B_s = np.block([[np.zeros((n, 1))], [-np.linalg.inv(M) @ T_u]])
print(np)
E = np.block([[np.zeros((n, 1))], [-T_w]])
B = np.block([B_s, E])
D = [1, 0]
C = np.block([[np.zeros((1, n)), np.ones((1, n))]])
sys = con.StateSpace(A, B, C, D)
return sys
def __init__(self, slow_d):
self.fast_d = 0
#stay still
# self.slow_d = -5 + 5*0.2
#move
self.slow_d = slow_d
self.desired_states = np.matrix([[0], [slow_d], [0], [self.fast_d]])
#define our matrices for the linearized model, which we have
M = 0.5 #cart mass
m = 0.2 #pedulum mass
b = 0.1 #coefficient of friction
I = 0.006 #inertia
g = 9.8 #gravity
l = 0.3 #length of the pendulum
k = I * (M + m) + M * m * l**2.0
U2 = (I + m * l**2.0) / k
U4 = -m * l / k
TF_2_3 = -(g * m**2.0 * l**2.0) / k
TF_4_3 = (g * (m + M)) / k
# TF_4_3 = (m*l*(M+m))*g/k;
self.B = [[0], [U2], [0], [U4]]
self.A = [[0, 1, 0, 0], [0, 0, TF_2_3, 0], [0, 0, 0, 1],
[0, 0, TF_4_3, 0]]
self.C = [[1, 0, 0, 0], [0, 0, 1, 0]]
self.D = [[0], [0]]
self.sys = control.StateSpace(self.A, self.B, self.C, self.D)
# R = 1
# Q = [[20, 0, 0, 0], [0, 0, 0, 0], [0, 0, 3, 0], [0, 0, 0, 0,]]
#develop our fast dynamic controller (Using pole placement)
self.K = control.place(self.A, self.B, [-5.1, -5.2, -5.3, -5.4])
self.K = np.matrix(self.K.tolist()[0])
print("K = ", np.matrix(self.K))
print("A = ", np.matrix(self.A))
# print("B = ",np.matrix(self.B))
# print(np.linalg.eig(np.subtract(self.A, np.matmul(self.B,self.K))))
# print(np.subtract(self.A, np.matmul(self.B,self.K)))
exit()
self.closed_loop = np.subtract(self.A, np.matmul(self.B, self.K))
self.K2 = control.place(self.closed_loop, self.B,
[-5.1, -5.2, -5.3, -5.4])
#deevelop our slow dynamic controller (PID)
self.pid = PID(2, 1, 0.0)
def sys_dict():
sdict = {}
sdict['ss'] = ct.StateSpace([[-1]], [[1]], [[1]], [[0]])
sdict['tf'] = ct.TransferFunction([1], [0.5, 1])
sdict['tfx'] = ct.TransferFunction([1, 1], [1]) # non-proper TF
sdict['frd'] = ct.frd([10 + 0j, 9 + 1j, 8 + 2j, 7 + 3j], [1, 2, 3, 4])
sdict['lio'] = ct.LinearIOSystem(ct.ss([[-1]], [[5]], [[5]], [[0]]))
sdict['ios'] = ct.NonlinearIOSystem(sdict['lio']._rhs,
sdict['lio']._out,
inputs=1,
outputs=1,
states=1)
sdict['arr'] = np.array([[2.0]])
sdict['flt'] = 3.
return sdict
