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)
