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