def loadBlocks(self, s):
for i in Workspace.blocks:
if i.type == 'input':
i.input = np.zeros(len(s['t']))
s['inputs'][i.id] = i
elif i.type == "function":
i.y = np.zeros(len(s['t']))
i.y.fill(np.nan)
s['functions'][i.id] = i
elif i.type == "sum":
i.y = np.zeros(len(s['t']))
i.y.fill(np.nan)
s['sums'][i.id] = i
elif i.type == 'system':
if i.code['type'] == "TF":
if i.code['sub_type'] == "continuous":
i.ss = c.ssdata(
c.c2d(
c.ss(
c.tf(json.loads(i.code['self'][0]),
json.loads(i.code['self'][1]))),
s['T']))
else:
i.ss = c.ssdata(
c.ss(
c.tf(json.loads(i.code['self'][0]),
json.loads(i.code['self'][1]), s['T'])))
elif i.code['type'] == "SS":
if i.code['sub_type'] == "continuous":
i.ss = c.ssdata(
c.c2d(
c.ss(json.loads(i.code['self'][0]),
json.loads(i.code['self'][1]),
json.loads(i.code['self'][2]),
json.loads(i.code['self'][3])), s['T']))
else:
i.ss = c.ssdata(
c.ss(json.loads(i.code['self'][0]),
json.loads(i.code['self'][1]),
json.loads(i.code['self'][2]),
json.loads(i.code['self'][3]), s['T']))
else:
Dialog.alert(
"Alerta",
["Um dos blocos não está configurado como TF ou SS"])
lt = len(s['t'])
lA = len(i.ss[0])
i.x = np.zeros((lt, lA, 1))
i.y = np.zeros(lt)
i.y.fill(np.nan)
s['systems'][i.id] = i
return s
def c2d_modified(sys, Ts):
""" A
"""
if isinstance(sys, float) or isinstance(sys, int):
return sys
else:
return co.c2d(sys, Ts)
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 main():
m, b, k = map(float, sys.argv[1:4])
nu, ny = map(int, sys.argv[4:])
# m = 1.0
# b = 3.0
# k = 5.0
# ny = 3
# nu = 3
tf_c = co.tf([1], [m, b, k])
tf_d = co.c2d(tf_c, 0.008)
num = tf_d.num[0][0].tolist()
den = tf_d.den[0][0].tolist()
# balance num and den
num = [0] * (len(den) - len(num)) + num
T = len(den)
L = np.zeros((T, nu, ny))
MB2 = np.zeros((T, nu, nu))
for i in range(T):
L[i, :] = np.eye(nu) * num[i]
if i != 0:
MB2[i, :] = np.eye(nu) * den[i]
admittance3D = {
'L': L.flatten().tolist(),
'MB2': MB2.flatten().tolist(),
'T': T,
'nu': nu,
'ny': ny
}
string = yaml.dump({'admittance3D': admittance3D})
print(string)
def generate_and_print(profile_name, m, b, k, Ts=0.008):
"""Generate an admittance controller from the given setting and print.
Note that the first input is reference force, which is always
zero. The second input is force measured with sensor.
"""
print("""
Printing admittance controller
------------------------------
> name: {:}
> m:={:.1f}, b:={:.1f}, k:={:.1f}
> discretization time step: Ts:={:.3f}
""".format(profile_name, m, b, k, Ts))
s = co.tf([1, 0], [1])
admittance_model = 1.0 / (m * s**2 + b * s + k)
admittance_model_discrete = co.c2d(admittance_model, Ts)
controller = Ss.tf_blocks([[0, admittance_model_discrete]])
print(controller)
print_controller(
profile_name,
controller,
scale_output=1,
DATA_DIR=
'~/catkin_ws/src/infinite_interaction/config/teaching_experiment')
def plant(dT=0.008):
""" Discrete-time transfer function.
Diagram:
+--------+ v
+----->| R(s) |-------->
| +--------+
| +--------+
+<-----| K(s) |<------- f
u +--------+ m
Transfer functions:
[v] = [0 R(s)] [f]
[m] [1 0 ] [u]
Signals:
v: output velocity
m: measured force
f: acting force
u: position command
"""
s = co.tf([1, 0], [1])
# NOTE: This mapping is from commanding position to velocity
R = (-s + 55.56) / (s + 55.56) / (0.0437 * s + 1) * s
P = Ss.tf_blocks([[0, R], [1, 0]])
Pss = Ss.tf2ss(P, minreal=True)
Pdss = co.c2d(Pss, dT)
return Pdss
def __init__(self, dt, period, z, Q=1.0e+8, H=1.0):
self.dt = dt
self.period = period
G = 9.8
A = np.matrix([[0.0, 1.0, 0.0], [0.0, 0.0, 1.0], [0.0, 0.0, 0.0]])
B = np.matrix([[0.0], [0.0], [1.0]])
C = np.matrix([[1.0, 0.0, -z / G]])
D = 0
sys = control.matlab.ss(A, B, C, D)
sys_d = control.c2d(sys, dt)
self.A_d, self.B_d, self.C_d, D_d = control.matlab.ssdata(sys_d)
E_d = np.matrix([[dt], [1.0], [0.0]])
Zero = np.matrix([[0.0], [0.0], [0.0]])
Phai = np.block([[1.0, -self.C_d * self.A_d], [Zero, self.A_d]])
G = np.block([[-self.C_d * self.B_d], [self.B_d]])
GR = np.block([[1.0], [Zero]])
Gd = np.block([[-self.C_d * E_d], [E_d]])
Qm = np.zeros((4, 4))
Qm[0][0] = Q
P = control.dare(Phai, G, Qm, H)[0]
self.F = -np.linalg.inv(H + G.transpose() * P * G) * G.transpose(
) * P * Phai
xi = (np.eye(4) - G * np.linalg.inv(H + G.transpose() * P * G) *
G.transpose() * P) * Phai
self.f = []
self.xp, self.yp = np.matrix([[0.0], [0.0], [0.0]]), np.matrix([[0.0],
[0.0],
[0.0]])
self.ux, self.uy = 0.0, 0.0
for i in range(0, round(period / dt)):
self.f += [
-np.linalg.inv(H + G.transpose() * P * G) * G.transpose() *
np.linalg.matrix_power(xi.transpose(), i - 1) * P * GR
]
def test_damp(self):
# Test the continuous time case.
zeta = 0.1
wn = 42
p = -wn * zeta + 1j * wn * np.sqrt(1 - zeta**2)
sys = tf(1, [1, 2 * zeta * wn, wn**2])
expected = ([wn, wn], [zeta, zeta], [p, p.conjugate()])
np.testing.assert_allclose(sys.damp(), expected)
np.testing.assert_allclose(damp(sys), expected)
# Also test the discrete time case.
dt = 0.001
sys_dt = c2d(sys, dt, method='matched')
p_zplane = np.exp(p * dt)
expected_dt = ([wn, wn], [zeta,
zeta], [p_zplane,
p_zplane.conjugate()])
np.testing.assert_almost_equal(sys_dt.damp(), expected_dt)
np.testing.assert_almost_equal(damp(sys_dt), expected_dt)
#also check that for a discrete system with a negative real pole the damp function can extract wn and zeta.
p2_zplane = -0.2
sys_dt2 = tf(1, [1, -p2_zplane], dt)
wn2, zeta2, p2 = sys_dt2.damp()
p2_splane = -wn2 * zeta2 + 1j * wn2 * np.sqrt(1 - zeta2**2)
p2_zplane = np.exp(p2_splane * dt)
np.testing.assert_almost_equal(p2, p2_zplane)
def __init__(self, num, den, Ts):
self.Ts = Ts
sys = tf(num, den)
sys_d = c2d(sys, Ts, method='tustin')
self.den_d = sys_d.den[0][0]
self.num_d = sys_d.num[0][0]
self.prev_filt_output = np.zeros(len(self.num_d)-1)
self.prev_filt_input = np.zeros(len(self.den_d))
def __init__(self, H, dt):
sys = control.tf2ss(control.c2d(H, dt))
self.x = np.zeros((sys.A.shape[0], 1))
self.A = sys.A
self.B = sys.B
self.C = sys.C
self.D = sys.D
self.dt = sys.dt
def Admittance(m=1.0, b=1.0, k=1.0, Ts=0.008):
""" Classical Admittance Controller
"""
s = co.tf([1, 0], [1])
admittance_model = 1.0 / (m * s**2 + b * s + k)
admittance_model_discrete = co.c2d(admittance_model, Ts)
controller = Ss.tf_blocks([[0, admittance_model_discrete]])
return controller
def test_step_response_return(self, tsystem):
"""Verify continuous and discrete time use same return conventions."""
sysc = tsystem.sys
sysd = c2d(sysc, 1) # discrete time system
Tvec = np.linspace(0, 10, 11) # make sure to use integer times 0..10
Tc, youtc = step_response(sysc, Tvec, input=0)
Td, youtd = step_response(sysd, Tvec, input=0)
np.testing.assert_array_equal(Tc.shape, Td.shape)
np.testing.assert_array_equal(youtc.shape, youtd.shape)
def PI_v1(Kp=1e-3, Ki=20e-3):
""" integral controller
Parameters selected for stiff environment with stifness = 10e3.
"""
s = co.tf([1, 0], [1])
K_ctime_single = Kp + Ki / s
K_dtime_single = co.c2d(K_ctime_single, 0.008)
PI_v1 = -Ss.tf_blocks([[K_dtime_single, -K_dtime_single]])
return PI_v1
def _construct_rotor_speed_filter(self):
cutoff_frequency = 5 # cut-off frequency
low_pass_transfer_function = ct.tf([0, cutoff_frequency],
[1, cutoff_frequency])
low_pass_state_space = ct.tf2ss(low_pass_transfer_function)
low_pass_state_space_discrete = ct.c2d(low_pass_state_space,
self._sampling_time, 'tustin')
beta_num = 0.0195 # notch parameter
beta_den = 0.125 # notch parameter
notch_frequency = 2 * np.pi * 0.62
notch_transfer_function = ct.tf(
[1, 2 * beta_num * notch_frequency, notch_frequency**2],
[1, 2 * beta_den * notch_frequency, notch_frequency**2])
notch_state_space = ct.tf2ss(notch_transfer_function)
notch_state_space_discrete = ct.c2d(notch_state_space,
self._sampling_time, 'tustin')
combined_filter_transfer_function = low_pass_state_space_discrete * notch_state_space_discrete
return combined_filter_transfer_function
def main():
Ptf_design = plant(Hdelay=0.05, Hgain=50)
Pss_design = Ss.tf2ss(Ptf_design, minreal=True)
Pssd_design = co.c2d(Pss_design, dT)
# synthesize controller
freqs_bnd_T = [1e-2, 2.3, 7.3, 25, 357]
mag_bnd_T = [-3, -3, -3, -10, -40]
# freqs_bnd_yn = [1e-2, 3.0, 30, 80, 255] # rad
# mag_bnd_yn = [-10, -10, -20, -74, -100] # db
freqs_bnd_yn = [1e-2, 3.0, 20, 50, 255] # rad
mag_bnd_yn = [-20, -20, -20, -94, -130] # db
Asls, internal_data = SLS_synthesis_p1(Pssd_design, 256, regularization=1,
freqs_bnd_T=freqs_bnd_T, mag_bnd_T=mag_bnd_T,
freqs_bnd_yn=freqs_bnd_yn, mag_bnd_yn=mag_bnd_yn,
m=1.5, b=24, k=60, T_delay=7)
# # test/analysis
Ptf_test = plant(Hgain=50, Hdelay=0.05)
Pssd_test = co.c2d(Ss.tf2ss(Ptf_test, minreal=True), dT)
if Asls is not None:
analysis(Pssd_test, Asls,
internal_data=internal_data, Tr=1.0, controller_name='SLS',
freqs_bnd_T=freqs_bnd_T, mag_bnd_T=mag_bnd_T, freqs_bnd_yn=freqs_bnd_yn,
mag_bnd_yn=mag_bnd_yn, m=1.5, b=24, k=60)
analysis(Pssd_test, OldControllers.A1, Tr=1.0, controller_name='admittance',
freqs_bnd_T=freqs_bnd_T, mag_bnd_T=mag_bnd_T,
freqs_bnd_yn=freqs_bnd_yn, mag_bnd_yn=mag_bnd_yn,
m=1.5, b=28, k=65)
analysis(Pssd_test, OldControllers.Ac14, Tr=1.0, controller_name='Hinf',
freqs_bnd_T=freqs_bnd_T, mag_bnd_T=mag_bnd_T,
freqs_bnd_yn=freqs_bnd_yn, mag_bnd_yn=mag_bnd_yn,
m=1.5, b=28, k=65)
# to print controller
Ss.SLS.print_controller(internal_data['L'], internal_data['MB2'])
import IPython
if IPython.get_ipython() is None:
IPython.embed()
class OldControllers:
""" Controllers that I designed sometime ago using different methods.
"""
A1 = co.c2d(co.tf([1], [1, 6, 5]), dT)
Alp = co.c2d(1.0 / 5 / (0.3 * s + 1) / (0.5 * s + 1)
* (0.032 * s + 1) * (0.0021 * s + 1), dT)
Ainf = co.tf([0, 0.000234247581937, -
0.001519129809213, 0.004117723048895, -
0.005971926120458, 0.004887991561889, -
0.002140996863836, 0.000392090600785],
[1.0, - 7.692092610286219, 25.432665781511435, -
46.858394653166798, 51.963159172009036, -
34.686122493591625, 12.905678916635351, -
2.064894113111176], dT)
Ac14 = co.tf([0, 0.000780043976221, -
0.001879759125377, 0.001518988906942, -
0.000411374804692, 0],
[1.0, -
2.915259796026691, 2.835076504158775, -
0.919776846642308, 0.000000000000039, 0], dT)
def plant(self, request):
plants = {
'syscont':
TransferFunction(1, [1, 3, 0]),
'sysdisc1':
c2d(TransferFunction(1, [1, 3, 0]), .1),
'syscont221':
StateSpace([[-.3, 0], [1, 0]], [[
-1,
], [
.1,
]], [0, -.3], 0)
}
return plants[request.param]
def control_rocket():
s = control.tf([1, 0], [0, 1])
H = 10 * (s / 100 + 1) * (s / 100 + 1) / s * (70 / (s + 70))
Hd = control.tf2ss(control.c2d(H, 0.004))
x = ca.SX.sym('x', 2)
u = ca.SX.sym('u', 1)
x1 = ca.mtimes(Hd.A, x) + ca.mtimes(Hd.B, u)
y = ca.mtimes(Hd.C, x) + ca.mtimes(Hd.D, u)
f_control1 = ca.Function('control_elv', [x, u], [x1, y], ['x', 'u'],
['x1', 'y'])
# f_control2 = ca.Function('control_h',[])
return f_control1
def control_equations(jit=True):
#define symbolic inputs and outputs
x_pitch_ctrl = ca.SX.sym('x_pitch_ctrl', 2)
u_pitch_cmd = ca.SX.sym('u_pitch_cmd', 1)
x_rocket_state = ca.SX.sym('x_rocket_state', 14)
y_elev_cmd = ca.SX.sym('y_elev_cmd', 1)
x1_pitch_ctrl = ca.SX.sym('x1_pitch_ctrl', 2)
p = ca.SX.sym(
'p', 16
) #Pass in all vehicle and environment metrics in case we need to use them
#define intermediate variables
rho = p[13] #density
#Below comments label according to general SS form x_dot = Ax + bu, y = Cx + du
#Get actual pitch using modified rodrigues parameters
r_nb = x_rocket_state[3:7]
euler = so3.Euler.from_mrp(r_nb) # roll, pitch, yaw
x_actual_pitch = euler[1]
#u for our SS pitch controller
pitch_error = (u_pitch_cmd - x_actual_pitch)
#Convert control tf to controllable canonical form with A,B,C,D matrices
s = control.tf([1, 0], [0, 1])
H = 10 * (s / 100 + 1) * (s / 100 + 1) / s * (70 / (s + 70))
Hd = control.tf2ss(control.c2d(H, 0.004))
#Compute discrete analog of x_dot and y
x1_pitch_ctrl = ca.mtimes(Hd.A, x_pitch_ctrl) + ca.mtimes(
Hd.B, pitch_error)
y_elev_cmd = ca.mtimes(Hd.C, x_pitch_ctrl) + ca.mtimes(Hd.D, pitch_error)
pitch_control = ca.Function(
'pitch_control', [x_pitch_ctrl, u_pitch_cmd, x_rocket_state, p],
[x1_pitch_ctrl, y_elev_cmd],
['x_pitch_ctrl', 'u_pitch_cmd', 'x_rocket_state', 'p'],
['x1_pitch_ctrl', 'y_elev_cmd'])
return {
'pitch_control': pitch_control,
'x_pitch_ctrl': x_pitch_ctrl,
'u_pitch_cmd': u_pitch_cmd,
'x_rocket_state': x_rocket_state,
'x1_pitch_ctrl': x1_pitch_ctrl,
'y_elev_cmd': y_elev_cmd
}
def test_damp(self):
# Test the continuous time case.
zeta = 0.1
wn = 42
p = -wn * zeta + 1j * wn * np.sqrt(1 - zeta**2)
sys = tf(1, [1, 2 * zeta * wn, wn**2])
expected = ([wn, wn], [zeta, zeta], [p, p.conjugate()])
np.testing.assert_equal(sys.damp(), expected)
np.testing.assert_equal(damp(sys), expected)
# Also test the discrete time case.
dt = 0.001
sys_dt = c2d(sys, dt, method='matched')
p_zplane = np.exp(p*dt)
expected_dt = ([wn, wn], [zeta, zeta],
[p_zplane, p_zplane.conjugate()])
np.testing.assert_almost_equal(sys_dt.damp(), expected_dt)
np.testing.assert_almost_equal(damp(sys_dt), expected_dt)
def test_damp(self):
# Test the continuous time case.
zeta = 0.1
wn = 42
p = -wn * zeta + 1j * wn * np.sqrt(1 - zeta**2)
sys = tf(1, [1, 2 * zeta * wn, wn**2])
expected = ([wn, wn], [zeta, zeta], [p, p.conjugate()])
np.testing.assert_equal(sys.damp(), expected)
np.testing.assert_equal(damp(sys), expected)
# Also test the discrete time case.
dt = 0.001
sys_dt = c2d(sys, dt, method='matched')
p_zplane = np.exp(p*dt)
expected_dt = ([wn, wn], [zeta, zeta],
[p_zplane, p_zplane.conjugate()])
np.testing.assert_almost_equal(sys_dt.damp(), expected_dt)
np.testing.assert_almost_equal(damp(sys_dt), expected_dt)
def main():
# constants
dT = 0.008
# setup
Ptf_design = plant(Hdelay=0.05, Hgain=50)
Pss_design = Ss.tf2ss(Ptf_design, minreal=True)
Pssd_design = co.c2d(Pss_design, dT)
atomic_pool = load_atomic_pool(Pssd_design, T=10)
# frequencies bounds
freqs_bnd_T = [1e-2, 2.3, 7.3, 25, 357]
mag_bnd_T = [-3, -3, -3, -10, -40]
freqs_bnd_yn = [1e-2, 3.0, 20, 50, 255] # rad
mag_bnd_yn = [-20, -20, -20, -94, -130] # db
m = 1.5
b = 28
k = 65
atomic_sls, internal_data = SLS_synthesis2(
atomic_pool, m, b, k, freqs_bnd_yn, mag_bnd_yn, freqs_bnd_T, mag_bnd_T)
# just for fun, analyze a controller
freqs_bnd_T = [1e-2, 2.3, 7.3, 25, 357]
mag_bnd_T = [-3, -3, -3, -10, -40]
# freqs_bnd_yn = [1e-2, 3.0, 30, 80, 255] # rad
# mag_bnd_yn = [-10, -10, -20, -74, -100] # db
freqs_bnd_yn = [1e-2, 3.0, 20, 50, 255] # rad
mag_bnd_yn = [-20, -20, -20, -94, -130] # db
analysis(
Pssd_design,
# atomic_pool['c4'].get_executable_controller(),
atomic_sls.get_executable_controller(),
Tr=1.0,
controller_name='admittance',
freqs_bnd_T=freqs_bnd_T,
mag_bnd_T=mag_bnd_T,
freqs_bnd_yn=freqs_bnd_yn,
mag_bnd_yn=mag_bnd_yn,
m=1.5,
b=28,
k=65
)
import IPython
if IPython.get_ipython() is None:
IPython.embed()
def PID2(Kp=1, Ki=0.0, Kd=0, b=1, c=1, Tf=0, dt=None):
# Inputs: ['ref', 'sens']
# Outputs: ['cmd']
sysR = control.tf2ss(
control.tf([Kp * b * Tf + Kd * c, Kp * b + Ki * Tf, Ki], [Tf, 1, 0]))
sysY = control.tf2ss(
control.tf([Kp * Tf + Kd, Kp + Ki * Tf, Ki], [Tf, 1, 0]))
sys = control.append(sysR, sysY)
sys.C = sys.C[0, :] - sys.C[1, :]
sys.D = sys.D[0, :] - sys.D[1, :]
sys.outputs = 1
if dt is not None:
sys = control.c2d(sys, dt)
return sys
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 test_sample_system_prewarp(self, tsys, plantname):
"""bilinear approximation with prewarping test"""
wwarp = 50
Ts = 0.025
# test state space version
plant = getattr(tsys, plantname)
plant_fr = plant(wwarp * 1j)
plant_d_warped = plant.sample(Ts, 'bilinear', prewarp_frequency=wwarp)
dt = plant_d_warped.dt
plant_d_fr = plant_d_warped(np.exp(wwarp * 1.j * dt))
np.testing.assert_array_almost_equal(plant_fr, plant_d_fr)
plant_d_warped = sample_system(plant, Ts, 'bilinear',
prewarp_frequency=wwarp)
plant_d_fr = plant_d_warped(np.exp(wwarp * 1.j * dt))
np.testing.assert_array_almost_equal(plant_fr, plant_d_fr)
plant_d_warped = c2d(plant, Ts, 'bilinear', prewarp_frequency=wwarp)
plant_d_fr = plant_d_warped(np.exp(wwarp * 1.j * dt))
np.testing.assert_array_almost_equal(plant_fr, plant_d_fr)
def PID2Exc(Kp=1, Ki=0, Kd=0, b=1, c=1, Tf=0, dt=None):
# Inputs: ['ref', 'sens', 'exc']
# Outputs: ['cmd', 'ff', 'fb', 'exc']
sysR = control.tf2ss(
control.tf([Kp * b * Tf + Kd * c, Kp * b + Ki * Tf, Ki], [Tf, 1, 0]))
sysY = control.tf2ss(
control.tf([Kp * Tf + Kd, Kp + Ki * Tf, Ki], [Tf, 1, 0]))
sysX = control.tf2ss(control.tf(1, 1)) # Excitation Input
sys = control.append(sysR, sysY, sysX)
sys.C = np.concatenate((sys.C[0, :] - sys.C[1, :] + sys.C[2, :], sys.C))
sys.D = np.concatenate((sys.D[0, :] - sys.D[1, :] + sys.D[2, :], sys.D))
sys.outputs = 4
if dt is not None:
sys = control.c2d(sys, dt)
return sys
def testMarkovResults(self, k, m, n):
#
# Test over a range of parameters
#
# k = order of the system
# m = number of Markov parameters
# n = size of the data vector
#
# Values *should* match exactly for n = m, otherewise you get a
# close match but errors due to the assumption that C A^k B =
# 0 for k > m-2 (see modelsimp.py).
#
# Generate stable continuous time system
Hc = rss(k, 1, 1)
# Choose sampling time based on fastest time constant / 10
w, _ = np.linalg.eig(Hc.A)
Ts = np.min(-np.real(w)) / 10.
# Convert to a discrete time system via sampling
Hd = c2d(Hc, Ts, 'zoh')
# Compute the Markov parameters from state space
Mtrue = np.hstack([Hd.D] + [
Hd.C @ np.linalg.matrix_power(Hd.A, i) @ Hd.B
for i in range(m-1)])
# Generate input/output data
T = np.array(range(n)) * Ts
U = np.cos(T) + np.sin(T/np.pi)
_, Y = forced_response(Hd, T, U, squeeze=True)
Mcomp = markov(Y, U, m)
# Compare to results from markov()
# experimentally determined probability to get non matching results
# with rtot=1e-6 and atol=1e-8 due to numerical errors
# for k=5, m=n=10: 0.015 %
np.testing.assert_allclose(Mtrue, Mcomp, rtol=1e-6, atol=1e-8)
def testMarkovResults(self):
#
# Test over a range of parameters
#
# k = order of the system
# m = number of Markov parameters
# n = size of the data vector
#
# Values should match exactly for n = m, otherewise you get a
# close match but errors due to the assumption that C A^k B =
# 0 for k > m-2 (see modelsimp.py).
#
for k, m, n in \
((2, 2, 2), (2, 5, 5), (5, 2, 2), (5, 5, 5), (5, 10, 10)):
# Generate stable continuous time system
Hc = control.rss(k, 1, 1)
# Choose sampling time based on fastest time constant / 10
w, _ = np.linalg.eig(Hc.A)
Ts = np.min(-np.real(w)) / 10.
# Convert to a discrete time system via sampling
Hd = control.c2d(Hc, Ts, 'zoh')
# Compute the Markov parameters from state space
Mtrue = np.hstack([Hd.D] + [np.dot(
Hd.C, np.dot(np.linalg.matrix_power(Hd.A, i),
Hd.B)) for i in range(m-1)])
# Generate input/output data
T = np.array(range(n)) * Ts
U = np.cos(T) + np.sin(T/np.pi)
_, Y, _ = control.forced_response(Hd, T, U, squeeze=True)
Mcomp = markov(Y, U, m, transpose=False)
# Compare to results from markov()
np.testing.assert_array_almost_equal(Mtrue, Mcomp)
def setup_KF(self):
# Derive the optimal controller (LQR).
# This sets self.A, self.B, self.Q, self.R and self.gain
super().setup_model()
# Construct continuous-time linear model
Gcss = control.ss(self.A, self.B, self.C, self.D)
# Convert to discrete-time model
Gdss = control.c2d(Gcss, self.env.tau)
A = Gdss.A
B = Gdss.B
C = Gdss.C
D = Gdss.D
# Choose KF parameters: process noise covariance
# matrix and output measurement noise covariance matrix
Q = 0.1**2 * np.eye(4)
R = 0.001**2 * np.eye(2)
# Calculate discrete-time Kalman filter gain and state
# estimation error covariance matrix (P)
X, L, G = control.dare(A.T, C.T, Q, R)
P = X.T
self.kf_gain = G.T
self.kf_params = {
'A': A,
'B': B,
'C': C,
'D': D,
'Q': Q,
'R': R,
'P': P,
'L': L
}
breakpoint()
def __init__(self, model, Ts, x0=None):
self.Ts = Ts
self.model = model
self.n = 6
self.p = 2
self.q = 1
if not (x0 is None):
self.x0 = x0
else:
self.x0 = np.zeros(self.n)
self.state = self.x0
self.state_names = ['x', 'x_dot', 'theta', 'theta_dot', 'psi', 'psi_dot']
self.K_cont = np.zeros((1, self.n)) # continous-time state controller
self.K_disc = np.zeros((1, self.n)) # discrete-time state controller
# get the linear continious model matrixes
self.A, self.B, self.C, self.D = self.linear_model()
# generate a linear continious model
self.sys_cont = control.StateSpace(self.A, self.B, self.C, self.D, remove_useless=False)
# convert the continious-time model to discrete-time
self.sys_disc = control.c2d(self.sys_cont, self.Ts)
self.A_d = np.asarray(self.sys_disc.A)
self.B_d = np.asarray(self.sys_disc.B)
self.C_d = np.asarray(self.sys_disc.C)
self.D_d = np.asarray(self.sys_disc.D)
self.A_hat = self.A
self.A_hat_d = self.A_d
self.ref_ctrl_is_set = False
self.ref_ctrl_1 = lib_control.PidController(0, 0, 0, self.Ts)
self.ref_ctrl_2 = lib_control.PidController(0, 0, 0, self.Ts)
self.ref_ctrl_1_state_num = 0
self.ref_ctrl_2_state_num = 0
minu = np.matrix([-v10_nmp, -v20_nmp]).T
maxz = np.matrix([
10 - h10_nmp / 2 - 0.1, 10 - h20_nmp / 2 - 0.1, 10 - h30_nmp / 2 - 0.1,
10 - h40_nmp / 2 - 0.1
]).T # Limits on controlled outputs
minz = np.matrix([-h10_nmp / 2, -h20_nmp / 2, -h30_nmp / 2, -h40_nmp / 2]).T
# maxz = np.matrix([10-h10_nmp/2-0.1, 10-h20_nmp/2-0.1]).T # Limits on controlled outputs
# minz = np.matrix([-h10_nmp/2, -h20_nmp/2]).T
# # MPC parameters
Np = 30 # Prediction horizon
Nu = 10
# Hu = 10 # Horizon for varying input signal
sysc = control.ss(A_nmp, B_nmp, C_nmp, D_nmp)
sysd = control.c2d(sysc, h)
A = sysd.A
B = sysd.B
Cy = sysd.C
Cz = Cy
Cc = 0.5 * np.matrix(np.eye(4))
(nx, nu) = B.shape
ny = Cy.shape[0]
nz = Cz.shape[0]
# Initial Values
x0 = np.matrix([[0.0], [0.0], [0.0], [0.0]])
u0 = np.matrix([[0.0], [0.0]])
y0 = Cy * x0
x0_hat = np.matrix([[0.0], [0.0], [0.0], [0.0]])
