def ConnectName(sysList, connectNames, inKeep, outKeep): sys = [] for s in sysList: if sys == []: sys = s else: sys = control.append(sys, control.ss(s)) inNames = [] outNames = [] for s in sysList: inNames += s.InputName outNames += s.OutputName Q = [[inNames.index(s) + 1, outNames.index(s) + 1] for s in connectNames] inputv = [inNames.index(s) + 1 for s in inKeep] outputv = [outNames.index(s) + 1 for s in outKeep] sysOut = control.connect(sys, Q, inputv, outputv) sysOut.InputName = inKeep sysOut.OutputName = outKeep return sysOut
def test_connect(): """ Demonstrate the control library's connect function. """ system = get_generic_second_order() system2 = get_generic_second_order() big_system = control.append(system, system2) print(system) print(big_system)
def testMimoW123(self): """MIMO plant with all weights""" from control import augw, ss, append, minreal g = ss([[-1., -2], [-3, -4]], [[1., 0.], [0., 1.]], [[1., 0.], [0., 1.]], [[1., 0.], [0., 1.]]) # this should be expaned to w1*I w1 = ss([-2.], [2.], [1.], [2.]) # diagonal weighting w2 = append(ss([-3.], [3.], [1.], [3.]), ss([-4.], [4.], [1.], [4.])) # full weighting w3 = ss([[-4., -5], [-6, -7]], [[2., 3.], [5., 7.]], [[11., 13.], [17., 19.]], [[23., 29.], [31., 37.]]) p = augw(g, w1, w2, w3) self.assertEqual(8, p.outputs) self.assertEqual(4, p.inputs) # w->z1 should be w1 self.siso_almost_equal(w1, p[0, 0]) self.siso_almost_equal(0, p[0, 1]) self.siso_almost_equal(0, p[1, 0]) self.siso_almost_equal(w1, p[1, 1]) # w->z2 should be 0 self.siso_almost_equal(0, p[2, 0]) self.siso_almost_equal(0, p[2, 1]) self.siso_almost_equal(0, p[3, 0]) self.siso_almost_equal(0, p[3, 1]) # w->z3 should be 0 self.siso_almost_equal(0, p[4, 0]) self.siso_almost_equal(0, p[4, 1]) self.siso_almost_equal(0, p[5, 0]) self.siso_almost_equal(0, p[5, 1]) # w->v should be I self.siso_almost_equal(1, p[6, 0]) self.siso_almost_equal(0, p[6, 1]) self.siso_almost_equal(0, p[7, 0]) self.siso_almost_equal(1, p[7, 1]) # u->z1 should be -w1*g self.siso_almost_equal(-w1 * g[0, 0], p[0, 2]) self.siso_almost_equal(-w1 * g[0, 1], p[0, 3]) self.siso_almost_equal(-w1 * g[1, 0], p[1, 2]) self.siso_almost_equal(-w1 * g[1, 1], p[1, 3]) # u->z2 should be w2 self.siso_almost_equal(w2[0, 0], p[2, 2]) self.siso_almost_equal(w2[0, 1], p[2, 3]) self.siso_almost_equal(w2[1, 0], p[3, 2]) self.siso_almost_equal(w2[1, 1], p[3, 3]) # u->z3 should be w3*g w3g = w3 * g self.siso_almost_equal(w3g[0, 0], minreal(p[4, 2])) self.siso_almost_equal(w3g[0, 1], minreal(p[4, 3])) self.siso_almost_equal(w3g[1, 0], minreal(p[5, 2])) self.siso_almost_equal(w3g[1, 1], minreal(p[5, 3])) # u->v should be -g self.siso_almost_equal(-g[0, 0], p[6, 2]) self.siso_almost_equal(-g[0, 1], p[6, 3]) self.siso_almost_equal(-g[1, 0], p[7, 2]) self.siso_almost_equal(-g[1, 1], p[7, 3])
def SensorModel(bw, delay=(0, 1)): # Inputs: ['meas', 'dist'] # Outputs: ['sens'] sysNom = control.tf2ss(control.tf(1, [1 / bw, 1])) delayPade = control.pade(delay[0], n=delay[1]) sysDelay = control.tf2ss(control.tf(delayPade[0], delayPade[1])) sysDist = control.tf2ss(control.tf(1, 1)) sys = control.append(sysDelay * sysNom, sysDist) sys.C = sys.C[0, :] + sys.C[1, :] sys.D = sys.D[0, :] + sys.D[1, :] sys.outputs = 1 return sys
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 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 testMimoW123(self): "MIMO plant with all weights" from control import augw, ss, append g = ss([[-1.,-2],[-3,-4]], [[1.,0.],[0.,1.]], [[1.,0.],[0.,1.]], [[1.,0.],[0.,1.]]) # this should be expaned to w1*I w1 = ss([-2.],[2.],[1.],[2.]) # diagonal weighting w2 = append(ss([-3.],[3.],[1.],[3.]), ss([-4.],[4.],[1.],[4.])) # full weighting w3 = ss([[-4.,-5],[-6,-7]], [[2.,3.],[5.,7.]], [[11.,13.],[17.,19.]], [[23.,29.],[31.,37.]]) p = augw(g,w1,w2,w3) self.assertEqual(8,p.outputs) self.assertEqual(4,p.inputs) # w->z1 should be w1 self.siso_almost_equal(w1, p[0,0]) self.siso_almost_equal(0, p[0,1]) self.siso_almost_equal(0, p[1,0]) self.siso_almost_equal(w1, p[1,1]) # w->z2 should be 0 self.siso_almost_equal(0, p[2,0]) self.siso_almost_equal(0, p[2,1]) self.siso_almost_equal(0, p[3,0]) self.siso_almost_equal(0, p[3,1]) # w->z3 should be 0 self.siso_almost_equal(0, p[4,0]) self.siso_almost_equal(0, p[4,1]) self.siso_almost_equal(0, p[5,0]) self.siso_almost_equal(0, p[5,1]) # w->v should be I self.siso_almost_equal(1, p[6,0]) self.siso_almost_equal(0, p[6,1]) self.siso_almost_equal(0, p[7,0]) self.siso_almost_equal(1, p[7,1]) # u->z1 should be -w1*g self.siso_almost_equal(-w1*g[0,0], p[0,2]) self.siso_almost_equal(-w1*g[0,1], p[0,3]) self.siso_almost_equal(-w1*g[1,0], p[1,2]) self.siso_almost_equal(-w1*g[1,1], p[1,3]) # u->z2 should be w2 self.siso_almost_equal(w2[0,0], p[2,2]) self.siso_almost_equal(w2[0,1], p[2,3]) self.siso_almost_equal(w2[1,0], p[3,2]) self.siso_almost_equal(w2[1,1], p[3,3]) # u->z3 should be w3*g w3g = w3*g; self.siso_almost_equal(w3g[0,0], p[4,2]) self.siso_almost_equal(w3g[0,1], p[4,3]) self.siso_almost_equal(w3g[1,0], p[5,2]) self.siso_almost_equal(w3g[1,1], p[5,3]) # u->v should be -g self.siso_almost_equal(-g[0,0], p[6,2]) self.siso_almost_equal(-g[0,1], p[6,3]) self.siso_almost_equal(-g[1,0], p[7,2]) self.siso_almost_equal(-g[1,1], p[7,3])
def test_linfnorm_ct_mimo(self, ct_siso): siso, refgpeak, reffpeak = ct_siso sys = ct.append(siso, siso) gpeak, fpeak = linfnorm(sys) np.testing.assert_allclose(gpeak, refgpeak) np.testing.assert_allclose(fpeak, reffpeak)
sysAlt.OutputName = ['refAltRate'] sysPhi = PID2(0.100, Ki=0.000, Kd=0.000, b=1, c=0, Tf=tFrameRate_s) sysPhi.InputName = ['refPhi', 'sensPhi'] sysPhi.OutputName = ['refP'] sysTheta = PID2(0.100, Ki=0.000, Kd=0.000, b=1, c=0, Tf=tFrameRate_s) sysTheta.InputName = ['refTheta', 'sensTheta'] sysTheta.OutputName = ['refQ'] sysPsi = PID2(0.100, Ki=0.020, Kd=0.000, b=1, c=0, Tf=tFrameRate_s) sysPsi.InputName = ['refPsi', 'sensPsi'] sysPsi.OutputName = ['refR'] # Append Attitude systems sysAtt = control.append(sysAlt, sysPhi, sysTheta, sysPsi) sysAtt.InputName = sysAlt.InputName + sysPhi.InputName + sysTheta.InputName + sysPsi.InputName sysAtt.OutputName = sysAlt.OutputName + sysPhi.OutputName + sysTheta.OutputName + sysPsi.OutputName # SCAS Controller Models sysAltRate = PID2(0.200, Ki=0.050, Kd=0.010, b=1, c=0, Tf=tFrameRate_s) sysAltRate.InputName = ['refAltRate', 'sensAltRate'] sysAltRate.OutputName = ['cmdHeave'] sysP = PID2(0.150, Ki=0.050, Kd=0.030, b=1, c=1, Tf=tFrameRate_s) sysP.InputName = ['refP', 'sensP'] sysP.OutputName = ['cmdP'] sysQ = PID2(0.150, Ki=0.050, Kd=0.030, b=1, c=0, Tf=tFrameRate_s) sysQ.InputName = ['refQ', 'sensQ'] sysQ.OutputName = ['cmdQ']
# -*- coding: utf-8 -*- """ @author:XuMing([email protected]) @description: """ import control control.append('a') print('a')
# SCAS Controller Models sysPhi = PID2(0.64, Ki=0.20, Kd=0.07, b=1, c=0, Tf=tFrameRate_s) sysPhi.InputName = ['refPhi', 'sensPhi'] sysPhi.OutputName = ['cmdP'] sysTheta = PID2(0.90, Ki=0.30, Kd=0.08, b=1, c=0, Tf=tFrameRate_s) sysTheta.InputName = ['refTheta', 'sensTheta'] sysTheta.OutputName = ['cmdQ'] tauYaw = 5.72 sysYaw = control.tf2ss(control.tf([0.03, 0.0], [1.0, tauYaw])) sysYaw.InputName = ['sensR'] sysYaw.OutputName = ['cmdR'] # Append systems sysScas = control.append(sysPhi, sysTheta, sysYaw) sysScas.InputName = sysPhi.InputName + sysTheta.InputName + sysYaw.InputName sysScas.OutputName = sysPhi.OutputName + sysTheta.OutputName + sysYaw.OutputName #sysScas = control.c2d(sysScas, tFrameRate_s) # Mixer ctrlEff = np.array([[0.00000, -0.14180, -1.33413, -0.56340, 0.56340, 1.33413], [-2.2716, 0.00000, 0.06000, 0.05800, 0.05800, 0.06000], [0.00000, -1.59190, 0.00000, 0.00000, 0.00000, 0.00000]]) mixSurf = np.linalg.pinv(ctrlEff) mixSurf[abs(mixSurf) / np.max(abs(mixSurf)) < 0.05] = 0.0 #%% ## Load Sim
def connect_ml(mods, inputs, outputs): ''' Connects models like the matlab function does inputs: mods - list of ss models, should have InputName and OutputName fields inputs - list of inputs of connected system outputs - list of ouptuts of connected system output: sys - connected ss model ''' # Input checking - all timesteps should be the same as the first one if mods[0].dt is None: mods[0].dt = 0 dt_main = mods[0].dt for mod in mods: # timesteps if mod.dt is None: mod.dt = 0 elif mod.dt != dt_main: print('Some systems have different dts') if not hasattr(mod, 'InputName'): print('WARNING: missing InputName') if not hasattr(mod, 'OutputName'): print('WARNING: missing OutputName') # append sequentially sys = co.ss([], [], [], []) # init empty system sys.dt = 0 for mod in mods: sys = co.append(sys, mod) # Set of input/output names InNames = [] for mod in mods: if isinstance(mod.InputName, list): for InName in mod.InputName: InNames.append(InName) else: InNames.append(mod.InputName) OutNames = [] for mod in mods: if isinstance(mod.OutputName, list): for OutName in mod.OutputName: OutNames.append(OutName) else: OutNames.append(mod.OutputName) # init interconnection matrix Q = np.zeros((len(InNames), 2), dtype=int) # assign input list into sequential indices Q[:, 0] = np.arange(1, len(InNames) + 1) # assign output list to input indices for input_name in InNames: if input_name in OutNames: Q[InNames.index(input_name), 1] = OutNames.index(input_name) + 1 # Input/Output indices, TODO: better error catching here inV = [InNames.index(in_v_name) + 1 for in_v_name in inputs] outV = [OutNames.index(out_v_name) + 1 for out_v_name in outputs] # Connect...finally! # Q = np.array(([1,0,],[2,5],[3,1])).tolist() sys = co.connect(sys, Q, inV, outV) sys.InputName = inputs sys.OutputName = outputs return sys
#R=1e10 DPlv=0.01 # Load variation (pu) #%% Transfer functions s_gov=ctr.tf(1,[ Tg, 1]) s_pmov=ctr.tf(1,[Tch, 1]) s_gen=ctr.tf(1,[M, D]) s_droop=ctr.tf(1,[R]) s_aux=ctr.tf(1,1) s_aux1=ctr.tf(1,1) #s_cont=ctr.tf(1,[Tg, 1]) #%% Block diagram reduction syst=ctr.append(ctr.tf2ss(s_gov),ctr.tf2ss(s_pmov),ctr.tf2ss(s_gen),ctr.tf2ss(s_droop), ctr.tf2ss(s_aux),ctr.tf2ss(s_aux1)) q=[[1,-4, 0], [2, 1, 0], [3, 6, 0], [4, 3, 0], [5, 0, 0], [6, 2, -5] ] inp=[1,5] out=[3,2,6] syslc=ctr.connect(syst,q,inp,out) #%% Simulation t=np.arange(0,40,0.05) Lr= np.zeros(t.size,) # Load reference DPl= DPlv*np.ones(t.size,) # Load variations #DPl= np.zeros(t.size,) # Load variations
ssPart = control.StateSpace(A, BF, C_part, np.array( [0, 0, 0, 0, 0, Vn])) # build big state space system... with single output # Kalman estimator # Kf, P, E = control.lqe(A, Vd, C, Vd, Vn) # design Kalman filter Kf, _, _ = control.lqr( np.transpose(A), np.expand_dims(C_part, axis=1), Vd, Vn) # alternatively, possible to design using "LQR" code Kf = np.transpose(Kf) ssKF = control.StateSpace( A - Kf * C_part, np.concatenate([B, Kf], axis=1), np.eye(4), 0 * np.concatenate([B, Kf], axis=1)) # Kalman filter estimator # Combine sysPart and sysKF ssPartKF = control.connect(control.append(ssPart, ssKF), [[8, 1]], [1, 2, 3, 4, 5, 6, 7], [1, 2, 3, 4, 5]) sysPartKF = control.LinearIOSystem(ssPartKF, inputs=('u', 'vd0', 'vd1', 'vd2', 'vd3', 'vn', 'uKF'), outputs=('x_n', 'x_h', 'xdot_h', 'theta_h', 'thetadot_h'), name='sysPartKF') def compareKalmanAndSys(): # test kalman estimator & compare non-linear system and linear system T = np.arange(tspan[0], tspan[1], dt) uDIST = np.random.multivariate_normal(np.zeros(4), Vd, size=T.shape[0]) uDIST = np.transpose(uDIST) uNOISE = np.random.normal(0.0, Vn, size=(1, T.shape[0]))
sysRud.OutputName = ['posRud'] sysAilL = Systems.ActuatorModel(servoBW_rps, servoDelay) sysAilL.InputName = ['cmdAilL'] sysAilL.OutputName = ['posAilL'] sysAilR = Systems.ActuatorModel(servoBW_rps, servoDelay) sysAilR.InputName = ['cmdAilR'] sysAilR.OutputName = ['posAilR'] sysFlapL = Systems.ActuatorModel(servoBW_rps, servoDelay) sysFlapL.InputName = ['cmdFlapL'] sysFlapL.OutputName = ['posFlapL'] sysFlapR = Systems.ActuatorModel(servoBW_rps, servoDelay) sysFlapR.InputName = ['cmdFlapR'] sysFlapR.OutputName = ['posFlapR'] # Combine Actuators sysAct = control.append(sysMotor, sysElev, sysRud, sysAilL, sysAilR, sysFlapL, sysFlapR) sysAct.InputName = sysMotor.InputName + sysElev.InputName + sysRud.InputName + sysAilL.InputName + sysAilR.InputName + sysFlapL.InputName + sysFlapR.InputName sysAct.OutputName = sysMotor.OutputName + sysElev.OutputName + sysRud.OutputName + sysAilL.OutputName + sysAilR.OutputName + sysFlapL.OutputName + sysFlapR.OutputName #%% Sensor models sensorBW_rps = 1 / dt * hz2rps sensorAirBW_rps = 2 * hz2rps sensorDelay_s = 1 * dt # sensorDelay_s = sensorDelay_s + 4*dt; # Artificial added Delay sensorDelay = (sensorDelay_s, ordPade) sysSensPhi = Systems.SensorModel(sensorBW_rps, sensorDelay) sysSensPhi.InputName = ['phi', 'phiDist'] sysSensPhi.OutputName = ['sensPhi'] sysSensTheta = Systems.SensorModel(sensorBW_rps, sensorDelay)