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 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 __init__(self, A, B, C, Qr, Rr, Qe, Re):
self.A = A
self.B = B
self.C = C
self.Qr = Qr
self.Rr = Rr
self.Qe = Qe
self.Re = Re
self.nx = A.shape[0]
self.nu = B.shape[1]
self.ny = C.shape[0]
###########################################
# Infinite-horizon discrete-time LQR design
###########################################
ctrb_rank = np.linalg.matrix_rank(control.ctrb(A, B))
# if ctrb_rank != self.nx:
# raise ValueError('The pair A, B must be controllable (rank of controllability matrix is %d instead of %d).' % (ctrb_rank, self.nx))
if not is_psd(Qr):
raise ValueError('Qr must be positive semi-definite.')
if not is_pd(Rr):
raise ValueError('Rr must be positive definite.')
# Solve Discrete Algebraic Riccati Equation
Pr = sp.linalg.solve_discrete_are(A, B, Qr, Rr)
# Feedback gain
self.Kr = -np.linalg.inv(Rr + B.conj().T @ Pr @ B) @ (
B.conj().T @ Pr @ A)
def dlqr(sys, Q, R):
"""Solves for the optimal discrete-time LQR controller.
x(n+1) = A * x(n) + B * u(n)
J = sum(0, inf, x.T * Q * x + u.T * R * u)
Keyword arguments:
A -- numpy.array(states x states), The A matrix.
B -- numpy.array(inputs x states), The B matrix.
Q -- numpy.array(states x states), The state cost matrix.
R -- numpy.array(inputs x inputs), The control effort cost matrix.
Returns:
numpy.array(states x inputs), K
"""
m = sys.A.shape[0]
controllability_rank = np.linalg.matrix_rank(ct.ctrb(sys.A, sys.B))
if controllability_rank != m:
print(
"Warning: Controllability of %d != %d, uncontrollable state"
% (controllability_rank, m)
)
# P = A.T * P * A - (A.T * P * B) * np.linalg.inv(R + B.T * P * B) *
# (B.T * P.T * A) + Q
P = sp.linalg.solve_discrete_are(a=sys.A, b=sys.B, q=Q, r=R)
F = np.linalg.inv(R + sys.B.T * P * sys.B) * sys.B.T * P * sys.A
return F
def lqr(sys, Q, R):
"""Solves for the optimal linear-quadratic regulator (LQR).
For a continuous system:
xdot = A * x + B * u
J = int(0, inf, x.T * Q * x + u.T * R * u)
For a discrete system:
x(n+1) = A * x(n) + B * u(n)
J = sum(0, inf, x.T * Q * x + u.T * R * u)
Keyword arguments:
A -- numpy.array(states x states), The A matrix.
B -- numpy.array(inputs x states), The B matrix.
Q -- numpy.array(states x states), The state cost matrix.
R -- numpy.array(inputs x inputs), The control effort cost matrix.
Returns:
numpy.array(states x inputs), K
"""
m = sys.A.shape[0]
controllability_rank = np.linalg.matrix_rank(cnt.ctrb(sys.A, sys.B))
if controllability_rank != m:
print(
"Warning: Controllability of %d != %d, uncontrollable state"
% (controllability_rank, m)
)
if sys.dt == 0.0:
P = sp.linalg.solve_continuous_are(a=sys.A, b=sys.B, q=Q, r=R)
return np.linalg.inv(R) @ sys.B.T @ P
else:
P = sp.linalg.solve_discrete_are(a=sys.A, b=sys.B, q=Q, r=R)
return np.linalg.inv(R + sys.B.T @ P @ sys.B) @ sys.B.T @ P @ sys.A
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 test_point_to_point(self):
# Machine precision for floats.
eps = np.finfo(float).eps
for states in range(1, self.maxStates):
# Start with a random system
linsys = matlab.rss(states, 1, 1)
# Make sure the system is not degenerate
Cmat = ctrl.ctrb(linsys.A, linsys.B)
if (np.linalg.matrix_rank(Cmat) != states):
if (self.debug):
print(" skipping (not reachable)")
continue
if (self.debug): print(linsys)
# Create a flat system representation
flatsys = tg.LinearFlatSystem(linsys)
# Generate several different initial and final conditions
for i in range(self.numTests):
x0 = np.random.rand(linsys.states)
xf = np.random.rand(linsys.states)
Tf = np.random.randn()
# Generate a trajectory from start to stop
systraj = tg.point_to_point(flatsys, x0, xf, Tf)
xd, ud = systraj.eval((0, Tf))
np.testing.assert_array_almost_equal(x0, xd[0, :], decimal=4)
np.testing.assert_array_almost_equal(xf, xd[1, :], decimal=4)
def test_point_to_point(self):
# Machine precision for floats.
eps = np.finfo(float).eps
for states in range(1, self.maxStates):
# Start with a random system
linsys = matlab.rss(states, 1, 1)
# Make sure the system is not degenerate
Cmat = ctrl.ctrb(linsys.A, linsys.B)
if (np.linalg.matrix_rank(Cmat) != states):
if (self.debug):
print(" skipping (not reachable)")
continue
if (self.debug): print(linsys)
# Create a flat system representation
flatsys = tg.LinearFlatSystem(linsys)
# Generate several different initial and final conditions
for i in range(self.numTests):
x0 = np.random.rand(linsys.states)
xf = np.random.rand(linsys.states)
Tf = np.random.randn()
# Generate a trajectory from start to stop
systraj = tg.point_to_point(flatsys, x0, xf, Tf)
xd, ud = systraj.eval((0,Tf))
np.testing.assert_array_almost_equal(x0, xd[0,:], decimal=4)
np.testing.assert_array_almost_equal(xf, xd[1,:], decimal=4)
def _check_if_controllable(self):
C = ctrb(self.A, self.Bu)
print('num. unstable = ', np.sum(np.abs(np.linalg.eig(self.A)[0]) > 1))
rank = np.linalg.matrix_rank(C)
# assert rank == self.A.shape[0]
print('ctrb rank = ', rank)
print('required rank = ', self.A.shape[0])
def regulator_with_observer(sys,
poles=None,
po=None,
ts=None,
extra_poles=[],
ck=[]):
final_poles = []
if poles is not None:
final_poles = poles
if po is not None and ts is not None:
log_po = np.log(100 / po)
psi = log_po / np.sqrt(np.pi**2 + log_po**2)
wn = 4 / psi / ts
s1 = -psi * wn + 1j * wn * np.sqrt(1 - psi**2)
s2 = -psi * wn - 1j * wn * np.sqrt(1 - psi**2)
final_poles.append(s1)
final_poles.append(s2)
for p in extra_poles:
final_poles.append(p)
rank = np.linalg.matrix_rank(ct.ctrb(sys.A, sys.B))
total_poles = len(final_poles)
if total_poles < rank:
raise BaseException(
f"you have {total_poles} but you need {rank} to implement a FSFB")
k = ct.acker(sys.A, sys.B, final_poles)
mo = ct.obsv(sys.A, sys.C)
obs_rank = np.linalg.matrix_rank(mo)
print("obs_rank=", obs_rank)
final_poles_obs = 5 * final_poles
L = ct.acker(sys.A.T, sys.C.T, final_poles_obs)
L = L.T
print("")
print("L=")
print(L)
pa = np.concatenate([sys.A, -sys.B * k], axis=1)
pb = np.concatenate([L * sys.C, sys.A - sys.B * k - L * sys.C], axis=1)
alc = np.concatenate([pa, pb], axis=0)
blc = np.concatenate(
[np.zeros((sys.A.shape[0], 1)),
np.zeros((sys.A.shape[0], 1))])
clc = np.concatenate([sys.C, -sys.D * k], axis=1)
dlc = np.array([[0]])
return ct.ss(alc, blc, clc, dlc)
def get_linear_quadratic_regulator(linear_model, q=numpy.identity(3),
r=numpy.identity(1)):
ctrb_matrix = control.ctrb(linear_model.A, linear_model.B)
if ctrb_matrix.shape[0] != linear_model.A.shape[0]:
raise ValueError("System is not controllable!")
k_matrix, s_matrix, e_matrix = control.lqr(linear_model.A, linear_model.B,
q, r)
return k_matrix, s_matrix, e_matrix
def __isControllable(self, th=None):
if th is None:
th = self.__th
F = np.array(self.__F(th))
C = np.array(self.__C(th))
n = self.__n
ctrb_matrix = control.ctrb(F, C)
rank = np.linalg.matrix_rank(ctrb_matrix)
return rank == n
def full_observer(system, poles_o, poles_s, debug=False) -> ObserverResult:
"""implementation of the complete observer for an autonomous system
:param system : tuple (a, b, c) of system matrices
:param poles_o: tuple of complex poles/eigenvalues of the observer
dynamics
:param poles_s: tuple of complex poles/eigenvalues of the closed
system
:param debug: output control for debugging in unittest(False:normal
output,True: output local variables and normal output)
:return: dataclass ObserverResult (controller function, feedback matrix, observer_gain, and pre-filter)
"""
# systemically relevant matrices
a = system[0]
b = system[1]
c = system[2]
d = system[3]
# ignore the PendingDeprecationWarning for built-in packet control
warnings.filterwarnings('ignore', category=PendingDeprecationWarning)
ctr_matrix = ctr.ctrb(a, b) # controllabilty matrix
assert np.linalg.det(ctr_matrix) != 0, 'this system is not controllable'
# State feedback
f_t = ctr.acker(a, b, poles_s)
# pre-filter
a1 = a - b * f_t
v = (-1 * (c * a1**(-1) * b)**(-1)) # pre-filter
obs_matrix = ctr.obsv(a, c) # observability matrix
assert np.linalg.det(obs_matrix) != 0, 'this system is unobservable'
# calculate observer gain
l_v = ctr.acker(a.T, c.T, poles_o).T
# controller function
# in this case controller function is: -1 * feedback * states + pre-filter * reference output
# disturbance value ist not considered
# t = sp.Symbol('t')
# sys_input = -1 * (f_t * sys_state)[0]
# input_func = sp.lambdify((sys_state, t), sys_input, modules='numpy')
# this method returns controller function, feedback matrix, observer gain and pre-filter
result = ObserverResult(f_t, l_v, v, None)
# return internal variables for unittest
if debug:
c_locals = Container(fetch_locals=True)
result.debug = c_locals
return result
def __init__(self):
self.values = None
self.boostPercent = 0.0 #boost Percentage 0 - 100
self.pitchPercent = 0 #pitch percentage
self.lastz = None #the tick priors value of position
self.lastvz = None #the tick priors value of velocity
self.lastError = 0.0 #prior error
self.errorzI = 0.0 #summation of values for integral of error
self.errorzICOUNTER = 1.0
#Rocket League physics (using unreal units (uu))
self.g = 650 #uu/s^2
self.Dp = -2.798194258050845 #Drag coeff for pitch
self.Tp = 12.14599781908070
T_r = -36.07956616966136
# torque coefficient for roll
T_p = -12.14599781908070
# torque coefficient for pitch
T_y = 8.91962804287785
# torque coefficient for yaw
D_r = -4.47166302201591
# drag coefficient for roll
D_p = -2.798194258050845
# drag coefficient for pitch
D_y = -1.886491900437232
# drag coefficient for yaw
self.I = 1
self.m = 180 #mass of the car arbitrary units
#State Space matrix coefficients
self.A = np.matrix([[0, 1], [0, 0]])
self.B = np.matrix([[0], [self.Tp]])
self.C = np.matrix([[1, 0], [0, 1]])
self.D = np.matrix([[0], [0]])
self.system = control.ss(self.A, self.B, self.C, self.D, None)
self.controllability = control.ctrb(self.A, self.B)
print("ctrb:", self.controllability)
#print(self.B)
#print(control.ctrb(self.A, self.B))
self.poles = np.array([-1000, -5])
print('poles: ', self.poles)
#self.eigen= control.pole(self.system)
self.eigen = self.system.pole()
print("Eigen: ", self.eigen)
self.K = control.place(self.A, self.B, self.poles)
print("\nK: ", self.K)
def design_controller_observer(self):
if self.in_low_gear:
q_vel = 1.0
else:
q_vel = 0.95
q = [q_vel, q_vel]
r = [12.0, 12.0]
self.design_lqr(q, r)
self.design_two_state_feedforward()
q_vel = 1.0
r_vel = 0.01
self.design_kalman_filter([q_vel, q_vel], [r_vel, r_vel])
print("ctrb cond =", np.linalg.cond(ct.ctrb(self.sysd.A, self.sysd.B)))
def feed_forward(self, A, B, C, D, K):
"""creates the feed forward matrix F to compute
control inputs using a desired state"""
F = np.linalg.pinv(-C * np.linalg.inv(A - B * K) * B)
if np.linalg.matrix_rank(control.ctrb(A - B * K, B * F)) < A.shape[0]:
print "System is not fully controllable"
else:
print "System is fully controllable"
if np.linalg.matrix_rank(control.obsv(A - B * K,
C - D * K)) < A.shape[0]:
print "System is not fully observable"
else:
print "System is fully observable"
return F
def lqr(*args, **kwargs):
"""Solves for the optimal linear-quadratic regulator (LQR).
For a continuous system:
.. math:: xdot = A * x + B * u
.. math:: J = \int_0^\infty (x^T Q x + u^T R u + 2 x^T N u) dt
For a discrete system:
.. math:: x(n + 1) = A x(n) + B u(n)
.. math:: J = \sum_0^\infty (x^T Q x + u^T R u + 2 x^T N u) \Delta T
Keyword arguments:
sys -- StateSpace object representing a linear system.
Q -- numpy.array(states x states), state cost matrix.
R -- numpy.array(inputs x inputs), control effort cost matrix.
N -- numpy.array(states x inputs), cross weight matrix.
Returns:
K -- numpy.array(states x inputs), controller gain matrix.
"""
sys = args[0]
Q = args[1]
R = args[2]
if len(args) == 4:
N = args[3]
else:
N = np.zeros((sys.A.shape[0], sys.B.shape[1]))
m = sys.A.shape[0]
controllability_rank = np.linalg.matrix_rank(ct.ctrb(sys.A, sys.B))
if controllability_rank != m:
print(
"Warning: Controllability of %d != %d, uncontrollable state"
% (controllability_rank, m)
)
if sys.dt == None:
P = sp.linalg.solve_continuous_are(a=sys.A, b=sys.B, q=Q, r=R, s=N)
return np.linalg.solve(R, sys.B.T @ P + N.T)
else:
P = sp.linalg.solve_discrete_are(a=sys.A, b=sys.B, q=Q, r=R, s=N)
return np.linalg.solve(R + sys.B.T @ P @ sys.B, sys.B.T @ P @ sys.A + N.T)
def control_eq(self):
self.equilibrium_point = hstack(( 0, pi / 2 * ones(len(self.q) - 1), zeros(len(self.u)) ))
equilibrium_dict = dict(zip(self.q + self.u, self.equilibrium_point))
parameter_dict = dict(zip(self.parameters, self.parameter_vals))
# symbolically linearize about arbitrary equilibrium
self.linear_state_matrix, self.linear_input_matrix, inputs = self.kane.linearize()
# sub in the equilibrium point and the parameters
self.f_A_lin = self.linear_state_matrix.subs(parameter_dict).subs(equilibrium_dict)
self.f_B_lin = self.linear_input_matrix.subs(parameter_dict).subs(equilibrium_dict)
m_mat = self.kane.mass_matrix_full.subs(parameter_dict).subs(equilibrium_dict)
# compute A and B
from numpy import matrix
A = matrix(m_mat.inv() * self.f_A_lin)
B = matrix(m_mat.inv() * self.f_B_lin)
assert matrix_rank(control.ctrb(A, B)) == A.shape[0]
self.K, self.X, self.E = control.lqr(A, B, ones(A.shape), 1);
def regulator(sys, poles=None, po=None, ts=None, extra_poles=[], ck=[]):
final_poles = []
if poles is not None:
final_poles = poles
if po is not None and ts is not None:
log_po = np.log(100 / po)
psi = log_po / np.sqrt(np.pi**2 + log_po**2)
wn = 4 / psi / ts
s1 = -psi * wn + 1j * wn * np.sqrt(1 - psi**2)
s2 = -psi * wn - 1j * wn * np.sqrt(1 - psi**2)
final_poles.append(s1)
final_poles.append(s2)
for p in extra_poles:
final_poles.append(p)
rank = np.linalg.matrix_rank(ct.ctrb(sys.A, sys.B))
total_poles = len(final_poles)
if total_poles < rank:
raise BaseException(
f"you have {total_poles} but you need {rank} to implement a FSFB")
kk = ct.acker(sys.A, sys.B, final_poles)
ak = sys.A - sys.B * kk
bk = np.zeros((sys.A.shape[0], 1))
default = np.zeros((1, sys.A.shape[0]))
default[0] = 1
ck2 = np.array(ck) if ck else default
dk = np.array([[0]])
return ct.ss(ak, bk, ck2, dk)
def __init__(self, *args, **params):
# using dict.get(key, default) to set defaults
self.b = params.get('motor_damping_constant', 1)
self.k_motor = params.get('motor_force_constant', 1)
self.m = params.get('chassis_mass', 1)
self.width = params.get('track_width', 1)
# calculate dynamics
# x_dot = Ax + Bu
# y = Cx + Du
b = self.b
m = self.m
w = self.width
# A matrix
self.A = np.matrix([[0, 1, 0, 0], [0, -b / m, 0, 0], [0, 0, 0, 1],
[0, 0, 0, -b * w / m]])
# B matrix
kdm = self.k_motor / m
r = self.width / 2.0
self.B = np.matrix([[0, 0], [kdm, kdm], [0, 0], [kdm * r, -kdm * r]])
# C matrix
self.C = np.matrix([[1, 0, 0, 0], [0, 0, 1, 0]])
# D matrix
# D = 0
self.D = np.zeros((self.C.shape[0], self.B.shape[1]))
# reality check on feasibility of system
self.ctrb = control.ctrb(self.A, self.B)
assert int(np.linalg.matrix_rank(self.ctrb)) == int(
self.ctrb.shape[0]), "System is not controllable!"
#construct a continuous time system
self.sys_ct = control.ss(self.A, self.B, self.C, self.D)
# S**2 + alpha1*S + alpha0
th_alpha1 = 2.0 * th_zeta * th_wn
th_alpha0 = th_wn**2
# Desired Poles
des_char_poly_lat = np.convolve(
np.convolve([1, s_alpha1, s_alpha0], [1, p_alpha1, p_alpha0]),
np.poly(integrator_pole_lat))
des_char_poly_lon = np.convolve([1, th_alpha1, th_alpha0],
np.poly(integrator_pole_lon))
des_poles_lat = np.roots(des_char_poly_lat)
des_poles_lon = np.roots(des_char_poly_lon)
# Latitudinal Controllability Matrix
if np.linalg.matrix_rank(cnt.ctrb(A1_lat, B1_lat)) != 5:
print("The Latitudinal system is not controllable")
else:
K1_lat = cnt.acker(A1_lat, B1_lat, des_poles_lat)
K_lat = K1_lat[0, 0:4]
ki_lat = K1_lat[0, 4]
# Longitudinal Controllability Matrix
if np.linalg.matrix_rank(cnt.ctrb(A1_lon, B1_lon)) != 3:
print("The Longitudinal system is not controllable")
else:
K1_lon = cnt.acker(A1_lon, B1_lon, des_poles_lon)
K_lon = K1_lon[0, 0:2]
ki_lon = K1_lon[0, 2]
UNCERTAINTY_PARAMETERS = False
def testConvert(self):
"""Test state space to transfer function conversion."""
verbose = self.debug
from control.statesp import _mimo2siso
#print __doc__
# Machine precision for floats.
eps = np.finfo(float).eps
for states in range(1, self.maxStates):
for inputs in range(1, self.maxIO):
for outputs in range(1, self.maxIO):
# start with a random SS system and transform to TF then
# back to SS, check that the matrices are the same.
ssOriginal = matlab.rss(states, outputs, inputs)
if (verbose):
self.printSys(ssOriginal, 1)
# Make sure the system is not degenerate
Cmat = control.ctrb(ssOriginal.A, ssOriginal.B)
if (np.linalg.matrix_rank(Cmat) != states):
if (verbose):
print(" skipping (not reachable)")
continue
Omat = control.obsv(ssOriginal.A, ssOriginal.C)
if (np.linalg.matrix_rank(Omat) != states):
if (verbose):
print(" skipping (not observable)")
continue
tfOriginal = matlab.tf(ssOriginal)
if (verbose):
self.printSys(tfOriginal, 2)
ssTransformed = matlab.ss(tfOriginal)
if (verbose):
self.printSys(ssTransformed, 3)
tfTransformed = matlab.tf(ssTransformed)
if (verbose):
self.printSys(tfTransformed, 4)
# Check to see if the state space systems have same dim
if (ssOriginal.states != ssTransformed.states):
print("WARNING: state space dimension mismatch: " + \
"%d versus %d" % \
(ssOriginal.states, ssTransformed.states))
# Now make sure the frequency responses match
# Since bode() only handles SISO, go through each I/O pair
# For phase, take sine and cosine to avoid +/- 360 offset
for inputNum in range(inputs):
for outputNum in range(outputs):
if (verbose):
print("Checking input %d, output %d" \
% (inputNum, outputNum))
ssorig_mag, ssorig_phase, ssorig_omega = \
control.bode(_mimo2siso(ssOriginal, \
inputNum, outputNum), \
deg=False, Plot=False)
ssorig_real = ssorig_mag * np.cos(ssorig_phase)
ssorig_imag = ssorig_mag * np.sin(ssorig_phase)
#
# Make sure TF has same frequency response
#
num = tfOriginal.num[outputNum][inputNum]
den = tfOriginal.den[outputNum][inputNum]
tforig = control.tf(num, den)
tforig_mag, tforig_phase, tforig_omega = \
control.bode(tforig, ssorig_omega, \
deg=False, Plot=False)
tforig_real = tforig_mag * np.cos(tforig_phase)
tforig_imag = tforig_mag * np.sin(tforig_phase)
np.testing.assert_array_almost_equal( \
ssorig_real, tforig_real)
np.testing.assert_array_almost_equal( \
ssorig_imag, tforig_imag)
#
# Make sure xform'd SS has same frequency response
#
ssxfrm_mag, ssxfrm_phase, ssxfrm_omega = \
control.bode(_mimo2siso(ssTransformed, \
inputNum, outputNum), \
ssorig_omega, \
deg=False, Plot=False)
ssxfrm_real = ssxfrm_mag * np.cos(ssxfrm_phase)
ssxfrm_imag = ssxfrm_mag * np.sin(ssxfrm_phase)
np.testing.assert_array_almost_equal( \
ssorig_real, ssxfrm_real)
np.testing.assert_array_almost_equal( \
ssorig_imag, ssxfrm_imag)
#
# Make sure xform'd TF has same frequency response
#
num = tfTransformed.num[outputNum][inputNum]
den = tfTransformed.den[outputNum][inputNum]
tfxfrm = control.tf(num, den)
tfxfrm_mag, tfxfrm_phase, tfxfrm_omega = \
control.bode(tfxfrm, ssorig_omega, \
deg=False, Plot=False)
tfxfrm_real = tfxfrm_mag * np.cos(tfxfrm_phase)
tfxfrm_imag = tfxfrm_mag * np.sin(tfxfrm_phase)
np.testing.assert_array_almost_equal( \
ssorig_real, tfxfrm_real)
np.testing.assert_array_almost_equal( \
ssorig_imag, tfxfrm_imag)
def check_controllability(A, B, n):
Cmatrix = control.ctrb(A, B)
rankCmatrix = np.linalg.matrix_rank(Cmatrix)
print('controllability matrix rank is {}, ns={}, nz={}'.format(
rankCmatrix, n, A.shape[0]))
return rankCmatrix
ylim(0., 2.)
grid()
if ii == 0:
title(
'Predicted state evolution of different models with open loop control'
)
xlabel('Time (sec)')
legend(fontsize=10, loc='best')
savefig(open_filename, format='pdf', dpi=2400)
show()
#close()
print('in {:.2f}s'.format(time.process_time() - t0))
print('in {:.2f}s'.format(time.process_time() - t0))
Cmatrix = control.ctrb(A=edmd_model.A, B=edmd_model.B)
print('edmd controllability matrix rank is {}, ns={}, nz={}'.format(
np.linalg.matrix_rank(Cmatrix), n, edmd_model.A.shape[0]))
print(n_lift_edmd)
#%%
#!============================================== EVALUATE PERFORMANCE -- CLOSED LOOP =============================================
t0 = time.process_time()
print('Evaluate Performance with closed loop trajectory tracking...', end=" ")
# Set up trajectory and controller for prediction task:
x_0 = array([2., 0.25, 0., 0.])
mpc_controller.eval(x_0, 0)
q_d_pred = mpc_controller.parse_result()
x_0 = q_d_pred[:, 0]
t_pred = t_d.squeeze()
Cout = np.array([[0.0, 1.0, 0.0, 0.0]])
A1 = np.array([[0.0, 0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0, 0.0],
[-P.k / P.Js, P.k / P.Js, -P.b / P.Js, P.b / P.Js, 0.0],
[P.k / P.Jp, -P.k / P.Jp, P.b / P.Jp, -P.b / P.Jp, 0.0],
[0.0, -1.0, 0.0, 0.0, 0.0]])
B1 = np.array([[0.0], [0.0], [1.0 / P.Js], [0.0], [0.0]])
# gain calculation
des_char_poly = np.convolve(
np.convolve([1, 2 * zeta_phi * wn_phi, wn_phi**2],
[1, 2 * zeta_th * wn_th, wn_th**2]), np.poly(integrator_pole))
des_poles = np.roots(des_char_poly)
# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 5:
print("The system is not controllable")
else:
K1 = cnt.acker(A1, B1, des_poles)
K = np.matrix([K1.item(0), K1.item(1), K1.item(2), K1.item(3)])
ki = K1.item(4)
# computer observer gains
des_obs_char_poly = np.convolve([1, 2 * zeta_phi * wn_phi_obs, wn_phi_obs**2],
[1, 2 * zeta_th * wn_th_obs, wn_th_obs**2])
des_obs_poles = np.roots(des_obs_char_poly)
# Compute the gains if the system is observable
if np.linalg.matrix_rank(cnt.ctrb(A.T, C.T)) != 4:
print("The system is not observable")
else:
def __init__(self):
# get parameters
try:
param_namespace = '/whirlybird'
self.param = rospy.get_param(param_namespace)
except KeyError:
rospy.logfatal('Parameters not set in ~/whirlybird namespace')
rospy.signal_shutdown('Parameters not set')
g = self.param['g']
l1 = self.param['l1']
l2 = self.param['l2']
m1 = self.param['m1']
m2 = self.param['m2']
d = self.param['d']
h = self.param['h']
r = self.param['r']
Jx = self.param['Jx']
Jy = self.param['Jy']
Jz = self.param['Jz']
km = self.param['km']
Jm = m1*l1**2 + m2*l2**2
self.Fe = (m1*l1 - m2*l2)*g/l1 #Note this is not the correct value for Fe, you will have to find that yourself
# Controller
self.controller = 'SS'
if self.controller == 'SS':
self.A_lat = [[0,0,1,0,0],[0,0,0,1,0],[0,0,0,0,0],[l1*self.Fe/(Jm+Jz),0,0,0,0],[0,-1,0,0,0]]
self.B_lat = [[0],[0],[1/Jx],[0],[0]]
self.C_lat = [[1,0,0,0],[0,1,0,0]]
if 5 != np.linalg.matrix_rank(control.ctrb(self.A_lat,self.B_lat)):
print 'Not controlable!!!!!!!!!!!!!!!!!!!!!!!!!!!!!'
self.A_lon = [[0,1,0],[(m1*l1-m2*l2)*g*np.sin(0)/(Jm + Jy),0,0],[-1,0,0]]
self.B_lon = [[0],[l1/(Jm + Jy)],[0]]
self.C_lon = [[1,0]]
if 3 != np.linalg.matrix_rank(control.ctrb(self.A_lon,self.B_lon)):
print 'Not controlable!!!!!!!!!!!!!!!!!!!!!!!!!!!!!'
# Pitch Gains
t_r_theta = 1.4
zeta_theta = 0.707
w_n_theta = 2.2/t_r_theta
p_i_lon = -5#-w_n_theta/2.0
theta_poles = np.roots([1,2*zeta_theta*w_n_theta,w_n_theta**2])
poles_lon = []
for pole in theta_poles:
poles_lon.append(pole)
poles_lon.append(p_i_lon)
K = control.place(self.A_lon,self.B_lon,poles_lon)
self.kr_lon = K[0,2]
self.K_lon = K[0,0:2]
#self.kr_lon = -1/(np.matmul(self.C_lon,np.matmul(np.linalg.inv(np.subtract(self.A_lon,np.matmul(self.B_lon,self.K_lon))),self.B_lon)))
# Yaw Gains
t_r_phi = 0.3
zeta_phi = 0.707
M = 5
t_r_psi = M*t_r_phi
zeta_psi = 0.707
w_n_phi = 2.2/t_r_phi
w_n_psi = 2.2/t_r_psi
p_i_lat = -5#-w_n_psi/2.0
phi_poles = np.roots([1,2*zeta_phi*w_n_phi,w_n_phi**2])
psi_poles = np.roots([1,2*zeta_psi*w_n_psi,w_n_psi**2])
poles_lat = []
for pole in phi_poles:
poles_lat.append(pole)
for pole in psi_poles:
poles_lat.append(pole)
poles_lat.append(p_i_lat)
K = control.place(self.A_lat,self.B_lat,poles_lat)
self.kr_lat = K[0,4]
self.K_lat = K[0,0:4]
#self.kr_lat = -1/(np.matmul(self.C_lat[1],np.matmul(np.linalg.inv(np.subtract(self.A_lat,np.matmul(self.B_lat,self.K_lat))),self.B_lat)))
# Dirty Derivative gains
self.sigma_theta = 0.05
self.sigma_phi = 0.05
self.sigma_psi = 0.05
# Initialize
self.thetao = 0
self.phio = 0
self.psio = 0
self.theta_doto = 0
self.phi_doto = 0
self.psi_doto = 0
self.Int_theta = 0.0
self.error_theta = 0.0
self.Int_psi = 0.0
self.error_psi = 0.0
self.psi_vlim = 0.05
self.theta_vlim = 0.05
self.theta_r = 0;
self.psi_r = 0;
elif self.controller == 'PID':
# Roll Gains
t_r_phi = 0.3
zeta_phi = 0.707
self.sigma_phi = 0.05
self.phi_r = 0.0
self.P_phi_ = (2.2/t_r_phi)**2*Jx
self.I_phi_ = 0.0
self.D_phi_ = 2*zeta_phi*(2.2/t_r_phi)*Jx
self.prev_phi = 0.0
self.Int_phi = 0.0
self.Dir_phi = 0.0
self.error_phi = 0.0
self.phi_vlim = 0.05
# Pitch Gains
b_theta = l1/(m1*l1**2 + m2*l2**2 + Jy)
t_r_theta = 1.0
zeta_theta = 0.707
self.sigma_theta = 0.05
self.theta_r = 0.0
self.P_theta_ = (2.2/t_r_theta)**2/(b_theta)
self.I_theta_ = 0.3
self.D_theta_ = 2*zeta_theta*(2.2/t_r_theta)/(b_theta)
self.prev_theta = 0.0
self.Int_theta = 0.0
self.Dir_theta = 0.0
self.error_theta = 0.0
self.theta_vlim = 0.05
# self.theta_ddot_old = [0.0];
# Yaw Gains
M = 10
t_r_psi = M*t_r_phi
zeta_psi = 0.707
Fe = (m1*l1 - m2*l2)*g/l1
b_psi = l1*Fe/(m1*l1**2 + m2*l2 **2 + Jz)
self.sigma_psi = 0.05
self.psi_r = 0.0
self.P_psi_ = (2.2/t_r_psi)**2/(b_psi)
self.I_psi_ = 0.01
self.D_psi_ = 2*zeta_psi*(2.2/t_r_psi)/(b_psi)
self.prev_psi = 0.0
self.Int_psi = 0.0
self.Dir_psi = 0.0
self.error_psi = 0.0
self.psi_vlim = 0.05
self.prev_time = rospy.Time.now()
self.sat_low = 0.0
self.sat_high = 0.7
self.command_sub_ = rospy.Subscriber('whirlybird', Whirlybird, self.whirlybirdCallback, queue_size=5)
self.psi_r_sub_ = rospy.Subscriber('psi_r', Float32, self.psiRCallback, queue_size=5)
self.theta_r_sub_ = rospy.Subscriber('theta_r', Float32, self.thetaRCallback, queue_size=5)
self.command_pub_ = rospy.Publisher('command', Command, queue_size=5)
while not rospy.is_shutdown():
# wait for new messages and call the callback when they arrive
rospy.spin()
Q = np.eye(4,4)
R = np.eye(2,2)
#S1 = sp.linalg.solve_continuous_are(A, B, Q, R)
#K1 = np.linalg.inv(R).dot(B.T).dot(S1)
#E1 = np.linalg.eigvals(A-B.dot(K1))
#print("S1 =\n", S1)
#print("K1 =\n", K1)
#print("E1 =\n", E1)
S2, E2, K2 = ct.care(Ap, Bp, Q, R)
#print("\nS2 =\n", S2)
#print("K2 =\n", K2)
#print("E2 =\n", E2)
print "ctrb = "+ str(np.linalg.matrix_rank(ct.ctrb(Ap,Bp)))
print "obsv = " + str(np.linalg.matrix_rank(ct.obsv(Ap,Cp)))
#K3, S3, E3 = ct.matlab.lqr(Ap, Bp, Q, R)
#print("\nS3 =\n", S3)
#print("K3 =\n", K3)
#print("E3 =\n", E3)
ss_P = ct.ss(Ap,Bp,Cp,Dp)
print ss_P
t = np.linspace(0, 3, 1000)
stepy = ct.step(ss_P,t)
#plt.plot(t,stepy)
#ct.impulse(ss_P)
ct.nyquist(ss_P)
#bode plot only implemented for SISO system.omg
#ct.bode(ss_P)
def dare(F, G1, G2, H):
"""Solves the discrete-time algebraic Riccati equation
0 = F ^ T * X * F
- X - F ^ T * X * G1 * (G2 + G1 ^ T * X * G1) ^ -1 * G1 ^ T * X * F + H
Under the assumption that X ^ -1 exists, this equation is equivalent to
0 = F ^ T * (X ^ -1 + G1 * G2 ^ -1 * G1 ^ T) ^ -1 * F - X + H
Parameters
==========
Inputs are real matrices:
F : n x n
G1 : n x m
G2 : m x m, symmetric, positive definite
H : n x n, symmetric, positive semi-definite
Assumptions
===========
(F, G1) is a stabilizable pair
(C, F) is a detectable pair (where C is full rank factorization of H, i.e.,
C ^ T * C = H and rank(C) = rank(H).
F is invertible
Returns
=======
Unique nonnegative definite solution of discrete Algrebraic Ricatti
equation.
Notes
=====
This is an implementation of the Schur method for solving algebraic Riccati
eqautions as described in dx.doi.org/10.1109/TAC.1979.1102178
"""
# Verify that F is non-singular
u, s, v = la.svd(F)
assert(np.all(s > 0.0))
# Verify that (F, G1) controllable
C = ctrb(F, G1)
u, s, v = la.svd(C)
assert(np.all(s > 0.0))
# Verify that (H**.5, F) is observable
O = obsv(H**.5, F)
u, s, v = la.svd(O)
assert(np.all(s > 0.0))
n = F.shape[0]
m = G2.shape[0]
G = np.dot(G1, np.dot(inv(G2), G1.T))
Finv = inv(F)
Finvt = Finv.T
# Form symplectic matrix
Z = empty((2*n, 2*n))
Z[:n, :n] = F + np.dot(G, np.dot(Finvt, H))
Z[:n, n:] = -np.dot(G, Finvt)
Z[n:, :n] = -np.dot(Finvt, H)
Z[n:, n:] = Finvt
S, U, sdim = schur(Z, sort='iuc')
# Verify that the n eigenvalues of the upper left block stable
assert(sdim == n)
U11 = U[:n, :n]
U21 = U[n:, :n]
return solve(U[:n, :n].T, U[n:, :n].T).T
# xdot = A*x + B*u
# y = C*x
A = np.array([[0.0, 1.0], [-P.k / P.m, -P.b / P.m]])
B = np.array([[0.0], [1 / P.m]])
C = np.array([[1.0, 0.0]])
# form augmented system
A1 = np.array([[0.0, 1.0, 0.0], [-P.k / P.m, -P.b / P.m, 0.0],
[-1.0, 0.0, 0.0]])
B1 = np.array([[0.0], [1 / P.m], [0.0]])
# gain calculation
wn = 2.2 / tr # natural frequency
des_char_poly = np.convolve([1, 2 * zeta * wn, wn**2],
np.poly(integrator_pole))
des_poles = np.roots(des_char_poly)
# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 3:
print("The system is not controllable")
else:
K1 = cnt.acker(A1, B1, des_poles)
K = np.array([[K1.item(0), K1.item(1)]])
ki = K1.item(2)
print('K: ', K)
print('ki: ', ki)
B = np.matrix([[0], [1 / (P.mc + 2 * P.mr)]])
C = np.matrix([[1.0, 0.0]])
# Augmented Longitudinal Parameters
A1 = np.matrix([[0.0, 1.0, 0.0], [0.0, 0.0, 0.0], [-1.0, 0.0, 0.0]])
B1 = np.matrix([[0], [1 / (P.mc + 2 * P.mr)], [0.0]])
# gain calculation
des_char_poly = np.convolve([1, 2 * zeta_h * wn_h, wn_h**2],
np.poly([h_integrator_pole]))
des_poles = np.roots(des_char_poly)
# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 3:
print("The longitudinal system is not controllable")
else:
K1 = cnt.acker(A1, B1, des_poles)
K = np.matrix([K1.item(0), K1.item(1)])
ki = K1.item(2)
# ----------------------------------------- OBSERVER GAINS -----------------------------------------
# compute longitudinal observer gains
des_obs_char_poly = [1, 2 * zeta_h * wn_h_obs, wn_h_obs**2]
des_obs_poles = np.roots(des_obs_char_poly)
# Compute the gains if the system is observable
if np.linalg.matrix_rank(cnt.ctrb(A.T, C.T)) != 2:
print("The longitudinal observer system is not observable")
else:
def train_net(u_all_training,y_all_training,mean_diff_nocovar,optimizer,u_control_all_training=None,valid_error_thres=1e-2,test_error_thres=1e-2,max_iters=100000,step_size_val=0.01):
iter = 0;
samplerate = 5000;
good_start = 1;
valid_error = 100.0;
test_error = 100.0;
training_error_history_nocovar = [];
validation_error_history_nocovar = [];
test_error_history_nocovar = [];
training_error_history_withcovar = [];
validation_error_history_withcovar = [];
test_error_history_withcovar = [];
covar_actual = compute_covarmat(u_all_training,y_all_training);
covar_model_history = [];
covar_diff_history = [];
while (((test_error>test_error_thres) or (valid_error > valid_error_thres)) and iter < max_iters):
iter+=1;
all_ind = set(np.arange(0,len(u_all_training)));
select_ind = np.random.randint(0,len(u_all_training),size=batchsize);
valid_ind = list(all_ind -set(select_ind))[0:batchsize];
select_ind_test = list(all_ind - set(valid_ind) - set(select_ind))[0:batchsize];
u_batch =[];
u_control_batch = [];
y_batch = [];
u_valid = [];
u_control_valid = [];
y_valid = [];
u_test_train = [];
u_control_train = [];
y_test_train= [];
u_control_test_train = [];
for j in range(0,len(select_ind)):
u_batch.append(u_all_training[select_ind[j]]);
y_batch.append(y_all_training[select_ind[j]]);
if with_control:
u_control_batch.append(u_control_all_training[select_ind[j]]);
for k in range(0,len(valid_ind)):
u_valid.append(u_all_training[valid_ind[k]]);
y_valid.append(y_all_training[valid_ind[k]]);
if with_control:
u_control_valid.append(u_control_all_training[valid_ind[k]]);
for k in range(0,len(select_ind_test)):
u_test_train.append(u_all_training[select_ind_test[k]]);
y_test_train.append(y_all_training[select_ind_test[k]]);
if with_control:
u_control_test_train.append(u_control_all_training[select_ind_test[k]]);
if with_control:
optimizer.run(feed_dict={yp_feed:u_batch,yf_feed:y_batch,u_control:u_control_batch,step_size:step_size_val});
valid_error = mean_diff_nocovar.eval(feed_dict={yp_feed:u_valid,yf_feed:y_valid,u_control:u_control_valid});
test_error = mean_diff_nocovar.eval(feed_dict={yp_feed:u_test_train,yf_feed:y_test_train,u_control:u_control_test_train});
else:
optimizer.run(feed_dict={yp_feed:u_batch,yf_feed:y_batch,step_size:step_size_val});
valid_error = mean_diff_nocovar.eval(feed_dict={yp_feed:u_valid,yf_feed:y_valid});
test_error = mean_diff_nocovar.eval(feed_dict={yp_feed:u_test_train,yf_feed:y_test_train});
if iter%samplerate==0:
if with_control:
training_error_history_nocovar.append(mean_diff_nocovar.eval(feed_dict={yp_feed:u_batch,yf_feed:y_batch,u_control:u_control_batch}));
validation_error_history_nocovar.append(mean_diff_nocovar.eval(feed_dict={yp_feed:u_valid,yf_feed:y_valid,u_control:u_control_valid}));
test_error_history_nocovar.append(mean_diff_nocovar.eval(feed_dict={yp_feed:u_test_train,yf_feed:y_test_train,u_control:u_control_test_train}));
else:
training_error_history_nocovar.append(mean_diff_nocovar.eval(feed_dict={yp_feed:u_batch,yf_feed:y_batch}));
validation_error_history_nocovar.append(mean_diff_nocovar.eval(feed_dict={yp_feed:u_valid,yf_feed:y_valid}));
test_error_history_nocovar.append(mean_diff_nocovar.eval(feed_dict={yp_feed:u_test_train,yf_feed:y_test_train}));
if (iter%10==0) or (iter==1):
plt.close();
if plot_deep_basis:
fig_hand = expose_deep_basis(psiypz_list,num_bas_obs,deep_dict_size,iter,yp_feed);
fig_hand = quick_nstep_predict(Y_p_old,u_control_all_training,with_control,num_bas_obs,iter);
if with_control:
print ("step %d , validation error %g"%(iter, mean_diff_nocovar.eval(feed_dict={yp_feed:u_valid,yf_feed:y_valid,u_control:u_control_valid})));
print ("step %d , test error %g"%(iter, mean_diff_nocovar.eval(feed_dict={yp_feed:u_test_train,yf_feed:y_test_train,u_control:u_control_test_train})));
#print ( test_synthesis(sess.run(Kx).T,sess.run(Ku).T ))
else:
print ("step %d , validation error %g"%(iter, mean_diff_nocovar.eval(feed_dict={yp_feed:u_valid,yf_feed:y_valid})));
print ("step %d , test error %g"%(iter, mean_diff_nocovar.eval(feed_dict={yp_feed:u_test_train,yf_feed:y_test_train})));
if ((iter>20000) and iter%10) :
valid_gradient = np.gradient(np.asarray(validation_error_history_nocovar[np.int(iter/samplerate*3/10):]) );
mu_gradient = np.mean(valid_gradient);
if ((iter <1000) and (mu_gradient >= 5e-1)): # eventually update this to be 1/10th the mean of batch data, or mean of all data handed as input param to func
good_start = 0; # if after 10,000 iterations validation error is still above 1e0, initialization was poor.
print("Terminating model refinement loop with gradient:") + repr(mu_gradient) + ", validation error after " + repr(iter) + " epochs: " + repr(valid_error);
iter = max_iters; # terminate while loop and return histories
if iter > 10000 and with_control:
At = sess.run(Kx).T;
Bt = sess.run(Ku).T;
ctrb_rank = np.linalg.matrix_rank(control.ctrb(At,Bt));
if debug_splash:
print(repr(ctrb_rank) + " : " + repr(control.ctrb(At,Bt).shape[0]));
if ctrb_rank == control.ctrb(At,Bt).shape[0] and test_error_history_nocovar[-1] <1e-5:
iter=max_iters;
all_histories = [training_error_history_nocovar, validation_error_history_nocovar,test_error_history_nocovar, \
training_error_history_withcovar,validation_error_history_withcovar,test_error_history_withcovar,covar_actual,covar_diff_history,covar_model_history];
plt.close();
x = np.arange(0,len(validation_error_history_nocovar),1);
plt.plot(x,training_error_history_nocovar,label='train. err.');
plt.plot(x,validation_error_history_nocovar,label='valid. err.');
plt.plot(x,test_error_history_nocovar,label='test err.');
#plt.gca().set_yscale('log');
plt.savefig('all_error_history.pdf');
plt.close();
return all_histories,good_start;
def main():
r = 0.05
L = 0.235
w_ref_l = 3
w_ref_r = w_ref_l
v_bar = r / 2 * (w_ref_l + w_ref_r)
A = np.array([[v_bar, -r / L, r / L], [0, -0.1, 0], [0, 0, -0.1]])
print(np.linalg.inv(A))
B = np.array([[0, 0], [1, 0], [0, 1]])
Q = control.ctrb(A, B) # reachability matrix
C = np.eye(3)
D = np.zeros((3, 2))
# F, G, H, M, dt = sig.cont2discrete((A, B, C, D), .01)
# print(F)
Q = np.array([[100, 0, 0], [0, 1, 0], [0, 0, 1]])
r_lqr = 0.01
R = r_lqr * np.eye(2)
K, S, E = controlpy.synthesis.controller_lqr(A, B, Q, R)
# K, S, E = controlpy.synthesis.controller_lqr(F, G, Q, R)
print(K)
state_space = control.StateSpace(A - B @ K, B, C, D)
# state_space = control.StateSpace(F - G @ K, G, H, M)
Z = C @ np.linalg.inv(-A + B @ K) @ B # Intermediate term
# Z = H @ np.linalg.inv(-F + G @ K) @ G # Intermediate term
F = np.linalg.inv(Z.T @ Z) @ Z.T
ref = np.array([[0], [3], [3]])
Fr = F @ ref
t = np.arange(0, 6, .01)
Fr_t = Fr * np.ones((2, len(t)))
T, y_out, x_out = control.forced_response(state_space,
t,
Fr_t,
X0=[.2, 3, 3]) # .2, 3, 3
fig1, ax1 = plt.subplots()
ax1.plot(T, y_out[0], T, y_out[1], T, y_out[2])
T, y_out, x_out = control.forced_response(state_space,
t,
Fr_t,
X0=[0, 0, 0]) # .2, 3, 3
fig2, ax2 = plt.subplots()
ax2.plot(T, y_out[0], T, y_out[1], T, y_out[2])
T, y_out, x_out = control.forced_response(state_space,
t,
Fr_t,
X0=[-.2, 3, 3]) # .2, 3, 3
fig3, ax3 = plt.subplots()
ax3.plot(T, y_out[0], T, y_out[1], T, y_out[2])
plt.show()
# tuning parameters
tr = 0.4
zeta = 0.707
# State Space Equations
# xdot = A*x + B*u
# y = C*x
A = np.array([[0.0, 1.0], [0.0, -1.0 * P.b / P.m / (P.ell**2)]])
B = np.array([[0.0], [3.0 / P.m / (P.ell**2)]])
C = np.array([[1.0, 0.0]])
# gain calculation
wn = 2.2 / tr # natural frequency
des_char_poly = [1, 2 * zeta * wn, wn**2]
des_poles = np.roots(des_char_poly)
# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A, B)) != 2:
print("The system is not controllable")
else:
#.A just turns K matrix into a numpy array
K = (cnt.acker(A, B, des_poles)).A
kr = -1.0 / (C @ np.linalg.inv(A - B @ K) @ B)
print('K: ', K)
print('kr: ', kr)
# MÉTODO 2 xa[:, k] = np.array([[dx[:, k]],
# [ y[k] ]])
Aa = np.array([[A[0, 0], A[0, 1], 0], [A[1, 0], A[1, 1], 0],
[np.dot(C, A)[0], np.dot(C, A)[1], 1]])
Ba = np.array([[B[0, 0], B[0, 1]], [B[1, 0], B[1, 1]],
[np.dot(C, B)[0], np.dot(C, B)[1]]])
Ca = np.array([0, 0, 1])
Da = 0
# %% Controllability and Observability analysis
Co = control.ctrb(Aa, Ba)
if matrix_rank(Co) == len(Aa):
print('The augmented system is controllable.')
Ob = control.obsv(Aa, Ca)
if matrix_rank(Ob) == len(Aa):
print('The augmented system is observable.')
# %% LQR controller
# y | Delta*x1 | Delta*x2
Qlqr = np.diag([1, 100, 1])
Rlqr = 1 * np.eye(2)
# K, X, eigVals = dlqr(Aa, Ba, Qlqr, Rlqr)
# Recursive Riccati Difference Equation (RDE)
C_h = np.array([1.0, 0.0])
# gain calculation
wn_th = 2.2 / tr_theta # natural frequency for angle
wn_z = 2.2 / tr_z # natural frequency for latral
wn_h = 2.2 / tr_h # natural frequency for longitudinal
#-----------------------------latral-----------------------------------------
des_char_poly_z = np.convolve([1, 2 * zeta_z * wn_z, wn_z**2],
[1, 2 * zeta_th * wn_th, wn_th**2])
des_poles_z = np.roots(des_char_poly_z)
# Compute the gains if the system is controllable for latral
if np.linalg.matrix_rank(cnt.ctrb(A_z, B_z)) != 4:
print("The system is not controllable")
else:
K_z = cnt.acker(A_z, B_z, des_poles_z)
Cr_z = np.array([[1.0, 0.0, 0.0, 0.0]])
kr_z = -1.0 / (Cr_z * np.linalg.inv(A_z - B_z @ K_z) @ B_z)
print('K_z: ', K_z)
print('kr_z: ', kr_z)
#----------------------------longi----------------------------------------
des_char_poly_h = [1, 2 * zeta_h * wn_h, wn_h**2]
des_poles_h = np.roots(des_char_poly_h)
# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A_h, B_h)) != 2:
def sparse(self, gamma, niter):
zero_thres = 1.e-6
print("gamma = %e | number of modes = " % (gamma))
# Define and solve the sparsity-promoting optimization problem
# Weighted L1 norm is updated iteratively
alpha = cp.Variable(self.q)
weights = np.ones(self.q)
for i in range(niter):
objective_sparse = cp.Minimize(
cp.quad_form(alpha, self.P) -
2. * cp.real(self.d.conj().T @ alpha) + self.s +
gamma * cp.pnorm(np.diag(weights) @ alpha, p=1))
prob_sparse = cp.Problem(objective_sparse)
sol_sparse = prob_sparse.solve(verbose=False, solver=cp.SCS)
alpha_sp = alpha.value # Sparse solution
if alpha_sp is None:
alpha_sp = np.ones(self.q)
# Update weights
weights = 1.0 / (np.abs(alpha_sp) + np.finfo(float).eps)
nonzero = np.abs(alpha_sp) > zero_thres # Nonzero modes
print(" %d of %d" %
(np.sum(nonzero), self.q))
J_sp = np.real(alpha_sp.conj().T @ self.P @ alpha_sp -
2 * np.real(self.d.conj().T @ alpha_sp) +
self.s) # Square error
nx = np.sum(
nonzero
) # Number of nonzero modes - order of the sparse/reduced model
Ez = np.eye(self.q)[:, ~nonzero]
# Define and solve the refinement optimization problem
#alpha = cp.Variable(self.q)
objective_refine = cp.Minimize(
cp.quad_form(alpha, self.P) -
2. * cp.real(self.d.conj().T @ alpha) + self.s)
if np.sum(~nonzero):
constraint_refine = [Ez.T @ alpha == 0]
prob_refine = cp.Problem(objective_refine, constraint_refine)
else:
prob_refine = cp.Problem(objective_refine)
sol_refine = prob_refine.solve()
alpha_ref = alpha.value
J_ref = np.real(alpha_ref.conj().T @ self.P @ alpha_ref -
2 * np.real(self.d.conj().T @ alpha_ref) +
self.s) # Square error
P_loss = 100 * np.sqrt(J_ref / self.s)
# Truncate modes
E = np.eye(self.q)[:, nonzero]
Lambda_bar = E.T @ self.Lambda @ E
Beta_bar = E.T @ self.Gamma
Phi_bar = self.Phi @ np.diag(alpha_ref) @ E
# Save data
stats = {}
stats["nx"] = nx
stats["alpha_sp"] = alpha_sp
stats["alpha_ref"] = alpha_ref
stats["z_0"] = (np.linalg.pinv(self.Phi) @ self.Y0[:, 0])[nonzero]
stats["E"] = E
stats["J_sp"] = J_sp
stats["J_ref"] = J_ref
stats["P_loss"] = P_loss
if nx != 0:
print("Rank of controllability matrix: %d of %d" %
(np.linalg.matrix_rank(control.ctrb(Lambda_bar,
Beta_bar)), nx))
# Convert system from complex modal form to real block-modal form
lambda_bar = np.diag(Lambda_bar)
A = np.zeros((nx, nx))
B = np.zeros((nx, self.nu))
Theta = np.zeros((self.ny, nx))
i = 0
while i < nx:
if np.isreal(lambda_bar[i]):
A[i, i] = lambda_bar[i]
B[i] = Beta_bar[i]
Theta[:, i] = Phi_bar[:, i]
i += 1
elif i == nx - 1:
# In case only one of the conjugate pairs is truncated - this rarely happens
A[i, i] = np.real(lambda_bar[i])
B[i] = np.real(Beta_bar[i])
Theta[:, i] = np.real(Phi_bar[:, i])
i += 1
elif np.isreal(np.isreal(lambda_bar[i] + lambda_bar[i + 1])):
A[i:i + 2, i:i + 2] = np.array(
[[np.real(lambda_bar[i]),
np.imag(lambda_bar[i])],
[-np.imag(lambda_bar[i]),
np.real(lambda_bar[i])]])
B[i] = np.real(Beta_bar[i])
B[i + 1] = -np.imag(Beta_bar[i])
Theta[:, i] = 2 * np.real(Phi_bar[:, i])
Theta[:, i + 1] = 2 * np.imag(Phi_bar[:, i])
i += 2
else:
raise ValueError(
"Eigenvalues are not grouped in conjugate pairs")
#return StateSpace(Lambda_bar, Beta_bar, Phi_bar), lambda_bar, stats
return StateSpace(A, B, Theta), lambda_bar, stats
