def testModredUnstable(self, matarrayin):
"""Check if an error is thrown when an unstable system is given"""
A = matarrayin(
[[4.5418, 3.3999, 5.0342, 4.3808],
[0.3890, 0.3599, 0.4195, 0.1760],
[-4.2117, -3.2395, -4.6760, -4.2180],
[0.0052, 0.0429, 0.0155, 0.2743]])
B = matarrayin([[1.0, 1.0], [2.0, 2.0], [3.0, 3.0], [4.0, 4.0]])
C = matarrayin([[1.0, 2.0, 3.0, 4.0], [1.0, 2.0, 3.0, 4.0]])
D = matarrayin([[0.0, 0.0], [0.0, 0.0]])
sys = StateSpace(A, B, C, D)
np.testing.assert_raises(ValueError, modred, sys, [2, 3])
def long_modes(aircraft, x_0, u_0):
"""longitudinal mode calculations."""
j = aircraft['weight']['inertia']
m = aircraft['weight']['weight'] / g # [slug]
x_ss = [0, 2, 4, 7]
a, b, c, d = nonlinear_eom_to_ss(aircraft, x_ss, [1], x_0, u_0, m, j)
sys = StateSpace(a, b, c, d)
wn, zeta, poles = damp(sys)
wn_sp = unique(max(wn))
zeta_sp = unique(zeta[wn == wn_sp])
wn_ph = unique(min(wn))
zeta_ph = unique(zeta[wn == wn_ph])
return wn_sp, zeta_sp, wn_ph, zeta_ph
def siso_ss2(self, siso_ss1):
"""System siso_ss2 with D=0"""
ss1 = siso_ss1.sys
T = TSys(StateSpace(ss1.A, ss1.B, ss1.C, 0, 0))
T.t = siso_ss1.t
T.ystep = siso_ss1.ystep - 9
T.initial = siso_ss1.yinitial - 9
T.yimpulse = np.array([
86., 70.1808, 57.3753, 46.9975, 38.5766, 31.7344, 26.1668, 21.6292,
17.9245, 14.8945
])
return T
def testAddition(self, tsys):
# State space addition
sys = tsys.siso_ss1 + tsys.siso_ss1d
sys = tsys.siso_ss1 + tsys.siso_ss1c
sys = tsys.siso_ss1c + tsys.siso_ss1
sys = tsys.siso_ss1d + tsys.siso_ss1
sys = tsys.siso_ss1c + tsys.siso_ss1c
sys = tsys.siso_ss1d + tsys.siso_ss1d
sys = tsys.siso_ss3d + tsys.siso_ss3d
sys = tsys.siso_ss1d + tsys.siso_ss3d
with pytest.raises(ValueError):
StateSpace.__add__(tsys.mimo_ss1c, tsys.mimo_ss1d)
with pytest.raises(ValueError):
StateSpace.__add__(tsys.mimo_ss1d, tsys.mimo_ss2d)
# Transfer function addition
sys = tsys.siso_tf1 + tsys.siso_tf1d
sys = tsys.siso_tf1 + tsys.siso_tf1c
sys = tsys.siso_tf1c + tsys.siso_tf1
sys = tsys.siso_tf1d + tsys.siso_tf1
sys = tsys.siso_tf1c + tsys.siso_tf1c
sys = tsys.siso_tf1d + tsys.siso_tf1d
sys = tsys.siso_tf2d + tsys.siso_tf2d
sys = tsys.siso_tf1d + tsys.siso_tf3d
with pytest.raises(ValueError):
TransferFunction.__add__(tsys.siso_tf1c, tsys.siso_tf1d)
with pytest.raises(ValueError):
TransferFunction.__add__(tsys.siso_tf1d, tsys.siso_tf2d)
# State space + transfer function
sys = tsys.siso_ss1c + tsys.siso_tf1c
sys = tsys.siso_tf1c + tsys.siso_ss1c
sys = tsys.siso_ss1d + tsys.siso_tf1d
sys = tsys.siso_tf1d + tsys.siso_ss1d
with pytest.raises(ValueError):
TransferFunction.__add__(tsys.siso_tf1c, tsys.siso_ss1d)
def test_lsim_double_integrator(self, u, x0, xtrue):
"""Test forced response of double integrator"""
# Note: scipy.signal.lsim fails if A is not invertible
A = np.array([[0., 1.], [0., 0.]])
B = np.array([[0.], [1.]])
C = np.array([[1., 0.]])
D = 0.
sys = StateSpace(A, B, C, D)
t = np.linspace(0, 1, 10)
_t, yout, xout = forced_response(sys, t, u, x0, return_x=True)
np.testing.assert_array_almost_equal(xout, xtrue, decimal=6)
ytrue = np.squeeze(np.asarray(C.dot(xtrue)))
np.testing.assert_array_almost_equal(yout, ytrue, decimal=6)
def test_syn_ss_sol_simulate(self):
"""Verifies that dyn_ss_sol mathes a simulation"""
for roll in np.linspace(math.radians(-20), math.radians(20), num=11):
for u in np.linspace(1, 30, num=10):
A, B = create_dyn_state_matrices(u, self.VM)
# Convert to discrete time system
ss = StateSpace(A, B, np.eye(2), np.zeros((2, 2)))
ss = ss.sample(0.01)
for sa in np.linspace(math.radians(-20), math.radians(20), num=11):
inp = np.array([[sa], [roll]])
# Simulate for 1 second
x1 = np.zeros((2, 1))
for _ in range(100):
x1 = ss.A @ x1 + ss.B @ inp
# Compute steady state solution directly
x2 = dyn_ss_sol(sa, u, roll, self.VM)
np.testing.assert_almost_equal(x1, x2, decimal=3)
def testHSVD(self, matarrayout, matarrayin):
A = matarrayin([[1., -2.], [3., -4.]])
B = matarrayin([[5.], [7.]])
C = matarrayin([[6., 8.]])
D = matarrayin([[9.]])
sys = StateSpace(A, B, C, D)
hsv = hsvd(sys)
hsvtrue = np.array([24.42686, 0.5731395]) # from MATLAB
np.testing.assert_array_almost_equal(hsv, hsvtrue)
# test for correct return type: ALWAYS return ndarray, even when
# use_numpy_matrix(True) was used
assert isinstance(hsv, np.ndarray)
assert not isinstance(hsv, np.matrix)
def testMultiplication(self, tsys):
# State space multiplication
sys = tsys.siso_ss1 * tsys.siso_ss1d
sys = tsys.siso_ss1 * tsys.siso_ss1c
sys = tsys.siso_ss1c * tsys.siso_ss1
sys = tsys.siso_ss1d * tsys.siso_ss1
sys = tsys.siso_ss1c * tsys.siso_ss1c
sys = tsys.siso_ss1d * tsys.siso_ss1d
sys = tsys.siso_ss1d * tsys.siso_ss3d
with pytest.raises(ValueError):
StateSpace.__mul__(tsys.mimo_ss1c, tsys.mimo_ss1d)
with pytest.raises(ValueError):
StateSpace.__mul__(tsys.mimo_ss1d, tsys.mimo_ss2d)
# Transfer function multiplication
sys = tsys.siso_tf1 * tsys.siso_tf1d
sys = tsys.siso_tf1 * tsys.siso_tf1c
sys = tsys.siso_tf1c * tsys.siso_tf1
sys = tsys.siso_tf1d * tsys.siso_tf1
sys = tsys.siso_tf1c * tsys.siso_tf1c
sys = tsys.siso_tf1d * tsys.siso_tf1d
sys = tsys.siso_tf1d * tsys.siso_tf3d
with pytest.raises(ValueError):
TransferFunction.__mul__(tsys.siso_tf1c, tsys.siso_tf1d)
with pytest.raises(ValueError):
TransferFunction.__mul__(tsys.siso_tf1d, tsys.siso_tf2d)
# State space * transfer function
sys = tsys.siso_ss1c * tsys.siso_tf1c
sys = tsys.siso_tf1c * tsys.siso_ss1c
sys = tsys.siso_ss1d * tsys.siso_tf1d
sys = tsys.siso_tf1d * tsys.siso_ss1d
with pytest.raises(ValueError):
TransferFunction.__mul__(tsys.siso_tf1c,
tsys.siso_ss1d)
def mimo_ss2(self, siso_ss2):
# Create MIMO system, contains ``siso_ss2`` twice
A = np.zeros((4, 4))
A[:2, :2] = siso_ss2.sys.A
A[2:, 2:] = siso_ss2.sys.A
B = np.zeros((4, 2))
B[:2, :1] = siso_ss2.sys.B
B[2:, 1:] = siso_ss2.sys.B
C = np.zeros((2, 4))
C[:1, :2] = siso_ss2.sys.C
C[1:, 2:] = siso_ss2.sys.C
D = np.zeros((2, 2))
T = copy(siso_ss2)
T.sys = StateSpace(A, B, C, D, 0)
return T
def siso_ss1(self):
A = np.array([[1., -2.], [3., -4.]])
B = np.array([[5.], [7.]])
C = np.array([[6., 8.]])
D = np.array([[9.]])
T = TSys(StateSpace(A, B, C, D, 0))
T.t = np.linspace(0, 1, 10)
T.ystep = np.array([9., 17.6457, 24.7072, 30.4855, 35.2234,
39.1165, 42.3227, 44.9694, 47.1599, 48.9776])
T.yinitial = np.array([11., 8.1494, 5.9361, 4.2258, 2.9118,
1.9092, 1.1508, 0.5833, 0.1645, -0.1391])
return T
def build_flying_wing_actuator_system(elevon_time_constant: float,
motor_time_constat: float) -> LinearIOSystem:
A_flying_wing, B_flying_wing, C_flying_wing, D_flying_wing = get_MIMO_state_space(
elevon_time_constant, motor_time_constat)
transform_matrix = flying_wing2ctrl_input_matrix()
inv_transform_matrix = np.linalg.inv(transform_matrix)
A = transform_matrix.dot(A_flying_wing).dot(inv_transform_matrix)
B = transform_matrix.dot(B_flying_wing).dot(inv_transform_matrix)
C = transform_matrix.dot(C_flying_wing).dot(inv_transform_matrix)
D = transform_matrix.dot(D_flying_wing).dot(inv_transform_matrix)
lin_sys = StateSpace(A, B, C, D)
inputs = ('elevator_deflection_command', 'aileron_deflection_command',
'rudder_deflection_command', 'throttle_command')
states = ('elevator_deflection', 'aileron_deflection', 'rudder_deflection', 'throttle')
outputs = ('elevator_deflection', 'aileron_deflection', 'rudder_deflection', 'throttle')
name = 'actuator_model'
return LinearIOSystem(lin_sys, inputs=inputs, outputs=outputs, states=states, name=name)
def tsys(self):
"""Create some systems for testing"""
class Tsys:
pass
T = Tsys()
# Single input, single output continuous and discrete time systems
sys = rss(3, 1, 1)
T.siso_ss1 = StateSpace(sys.A, sys.B, sys.C, sys.D, None)
T.siso_ss1c = StateSpace(sys.A, sys.B, sys.C, sys.D, 0.0)
T.siso_ss1d = StateSpace(sys.A, sys.B, sys.C, sys.D, 0.1)
T.siso_ss2d = StateSpace(sys.A, sys.B, sys.C, sys.D, 0.2)
T.siso_ss3d = StateSpace(sys.A, sys.B, sys.C, sys.D, True)
# Two input, two output continuous time system
A = [[-3., 4., 2.], [-1., -3., 0.], [2., 5., 3.]]
B = [[1., 4.], [-3., -3.], [-2., 1.]]
C = [[4., 2., -3.], [1., 4., 3.]]
D = [[-2., 4.], [0., 1.]]
T.mimo_ss1 = StateSpace(A, B, C, D, None)
T.mimo_ss1c = StateSpace(A, B, C, D, 0)
# Two input, two output discrete time system
T.mimo_ss1d = StateSpace(A, B, C, D, 0.1)
# Same system, but with a different sampling time
T.mimo_ss2d = StateSpace(A, B, C, D, 0.2)
# Single input, single output continuus and discrete transfer function
T.siso_tf1 = TransferFunction([1, 1], [1, 2, 1], None)
T.siso_tf1c = TransferFunction([1, 1], [1, 2, 1], 0)
T.siso_tf1d = TransferFunction([1, 1], [1, 2, 1], 0.1)
T.siso_tf2d = TransferFunction([1, 1], [1, 2, 1], 0.2)
T.siso_tf3d = TransferFunction([1, 1], [1, 2, 1], True)
return T
def latdir_modes(aircraft, x_0, u_0):
"""calculate lateral-directional modal parameters."""
j = aircraft['weight']['inertia']
m = aircraft['weight']['weight'] / g # slug
# x = [u v w phi theta psi p q r p_n p_e h]
x_ss = [1, 3, 6, 8]
a, b, c, d = nonlinear_eom_to_ss(aircraft, x_ss, [0, 2], x_0, u_0, m, j)
sys = StateSpace(a, b, c, d)
wn, zeta, poles = damp(sys)
wn_dr = unique(max(wn[abs(zeta) != 1])) # [rad/s]
zeta_dr = unique(max(zeta[wn == wn_dr])) # []
re = real(poles)
t = unique(re[abs(zeta) == 1])
if len(t) == 2:
t_r = -1 / min(t)
t_s = -1 / max(t)
else:
t_r = float("nan")
t_s = float("nan")
return wn_dr, zeta_dr, t_r, t_s
def testBalredTruncate(self, matarrayin):
# controlable canonical realization computed in matlab for the transfer
# function:
# num = [1 11 45 32], den = [1 15 60 200 60]
A = matarrayin([[-15., -7.5, -6.25, -1.875], [8., 0., 0., 0.],
[0., 4., 0., 0.], [0., 0., 1., 0.]])
B = matarrayin([[2.], [0.], [0.], [0.]])
C = matarrayin([[0.5, 0.6875, 0.7031, 0.5]])
D = matarrayin([[0.]])
sys = StateSpace(A, B, C, D)
orders = 2
rsys = balred(sys, orders, method='truncate')
Artrue = np.array([[-1.958, -1.194], [-1.194, -0.8344]])
Brtrue = np.array([[0.9057], [0.4068]])
Crtrue = np.array([[0.9057, 0.4068]])
Drtrue = np.array([[0.]])
np.testing.assert_array_almost_equal(rsys.A, Artrue, decimal=2)
np.testing.assert_array_almost_equal(rsys.B, Brtrue, decimal=4)
np.testing.assert_array_almost_equal(rsys.C, Crtrue, decimal=4)
np.testing.assert_array_almost_equal(rsys.D, Drtrue, decimal=4)
def testModredTruncate(self, matarrayin):
#balanced realization computed in matlab for the transfer function:
# num = [1 11 45 32], den = [1 15 60 200 60]
A = matarrayin([[-1.958, -1.194, 1.824, -1.464],
[-1.194, -0.8344, 2.563, -1.351],
[-1.824, -2.563, -1.124, 2.704],
[-1.464, -1.351, -2.704, -11.08]])
B = matarrayin([[-0.9057], [-0.4068], [-0.3263], [-0.3474]])
C = matarrayin([[-0.9057, -0.4068, 0.3263, -0.3474]])
D = matarrayin([[0.]])
sys = StateSpace(A, B, C, D)
rsys = modred(sys, [2, 3], 'truncate')
Artrue = np.array([[-1.958, -1.194], [-1.194, -0.8344]])
Brtrue = np.array([[-0.9057], [-0.4068]])
Crtrue = np.array([[-0.9057, -0.4068]])
Drtrue = np.array([[0.]])
np.testing.assert_array_almost_equal(rsys.A, Artrue)
np.testing.assert_array_almost_equal(rsys.B, Brtrue)
np.testing.assert_array_almost_equal(rsys.C, Crtrue)
np.testing.assert_array_almost_equal(rsys.D, Drtrue)
def linstep(mod: str, step_var: int, out: str, show: bool, tmax: float, nt: int, step_size: float):
"""
Calculates the step response of a linear model
"""
with open(mod, 'r') as infile:
data = json.load(infile)
ss = StateSpace(data['A'], data['B'], data['C'], data['D'])
t = np.linspace(0.0, tmax, nt)
u = np.zeros((4, nt))
u[step_var, :] = step_size
T, yout = forced_response(ss, T=t, U=u)
if out != "":
all_data = np.vstack((T, yout))
np.savetxt(out, all_data.T, delimiter=",")
print(f"Step result response written to {out}")
if show:
plot_respons(T, yout)
plt.show()
def testBalredMatchDC(self, matarrayin):
# controlable canonical realization computed in matlab for the transfer
# function:
# num = [1 11 45 32], den = [1 15 60 200 60]
A = matarrayin([[-15., -7.5, -6.25, -1.875], [8., 0., 0., 0.],
[0., 4., 0., 0.], [0., 0., 1., 0.]])
B = matarrayin([[2.], [0.], [0.], [0.]])
C = matarrayin([[0.5, 0.6875, 0.7031, 0.5]])
D = matarrayin([[0.]])
sys = StateSpace(A, B, C, D)
orders = 2
rsys = balred(sys, orders, method='matchdc')
Artrue = np.array([[-4.43094773, -4.55232904],
[-4.55232904, -5.36195206]])
Brtrue = np.array([[1.36235673], [1.03114388]])
Crtrue = np.array([[1.36235673, 1.03114388]])
Drtrue = np.array([[-0.08383902]])
np.testing.assert_array_almost_equal(rsys.A, Artrue, decimal=2)
np.testing.assert_array_almost_equal(rsys.B, Brtrue, decimal=4)
np.testing.assert_array_almost_equal(rsys.C, Crtrue, decimal=4)
np.testing.assert_array_almost_equal(rsys.D, Drtrue, decimal=4)
def test_convert_to_transfer_function(self):
"""Test for correct state space to transfer function conversion."""
A = [[1., -2.], [-3., 4.]]
B = [[6., 5.], [4., 3.]]
C = [[1., -2.], [3., -4.], [5., -6.]]
D = [[1., 0.], [0., 1.], [1., 0.]]
sys = StateSpace(A, B, C, D)
tfsys = _convert_to_transfer_function(sys)
num = [[np.array([1., -7., 10.]), np.array([-1., 10.])],
[np.array([2., -8.]), np.array([1., -2., -8.])],
[np.array([1., 1., -30.]), np.array([7., -22.])]]
den = [[np.array([1., -5., -2.]) for _ in range(sys.ninputs)]
for _ in range(sys.noutputs)]
for i in range(sys.noutputs):
for j in range(sys.ninputs):
np.testing.assert_array_almost_equal(tfsys.num[i][j],
num[i][j])
np.testing.assert_array_almost_equal(tfsys.den[i][j],
den[i][j])
def comp_form(sys,obs,K):
"""Compact form Conroller+Observer
Call:
contr=comp_form(sys,obs,K)
Parameters
----------
sys : System in State Space form
obs : Observer in State Space form
K: State feedback gains
Returns
-------
contr: ss
Controller
"""
nx=shape(sys.A)[0]
ny=shape(sys.C)[0]
nu=shape(sys.B)[1]
no=shape(obs.A)[0]
Bu=mat(obs.B[:,0:nu])
By=mat(obs.B[:,nu:])
Du=mat(obs.D[:,0:nu])
Dy=mat(obs.D[:,nu:])
X=inv(eye(nu,nu)+K*Du)
Ac = mat(obs.A)-Bu*X*K*mat(obs.C);
Bc = hstack((Bu*X,By-Bu*X*K*Dy))
Cc = -X*K*mat(obs.C);
Dc = hstack((X,-X*K*Dy))
contr = StateSpace(Ac,Bc,Cc,Dc,sys.dt)
return contr
def to_ss(self, parameters):
return StateSpace(*self.matrices(parameters))
Tc = 0.0021
dt = 0.01#tempo de discretizacao para o motor real
#Velocidade Real
df = pd.read_csv(str(V)+'V.csv',sep=';')
#modelo do sistema
A = np.array([[-R/L,-K/L],[K/J,-b/J]])
B = np.array([[1/L , 0],[0 ,-1/J]])
C = np.array([0,1])
D = np.array([0,0])
sys = StateSpace(A,B,C,D)
#discretizando o modelo
dsys = sys.sample(dt)
#Setando os dados que serao usados
Samples = len(df)
tempo = np.arange(0,(Samples)*dt,dt)
medida = []
#converte Dataframe para um lista de matrizes
for i in range(0,Samples):
medida.append(np.array([[df.iloc[i]['Corrente']],[df.iloc[i]['Velocidade']]]))
P = np.array([[1,0],[0,1]])
policy_sess = tf.Session()
K.set_session(policy_sess)
NUM_LAYERS = 4 # number of layers of the state space
MAX_TRIALS = 100 # maximum number of models generated
MAX_EPOCHS = 8 # maximum number of epochs to train
CHILD_BATCHSIZE = 128 # batchsize of the child models
EXPLORATION = 0.9 # high exploration for the first 1000 steps
REGULARIZATION = 1e-3 # regularization strength
CONTROLLER_CELLS = 64 # number of cells in RNN controller
EMBEDDING_DIM = 20 # dimension of the embeddings for each state
ACCURACY_BETA = 0.85 # beta value for the moving average of the accuracy
CLIP_REWARDS = 0.0 # clip rewards in the [-0.05, 0.05] range
# construct a state space
state_space = StateSpace()
# add states
state_space.add_state(name='neurons', values=range(32, 512, 32))
state_space.print_state_space()
def train(dataset1, dataset2, initial_state, if_restore):
total_reward = 0.0
with policy_sess.as_default():
# create the Controller and build the internal policy network
controller = Controller(policy_sess,
NUM_LAYERS,
state_space,
reg_param=REGULARIZATION,
exploration=EXPLORATION,
controller_cells=CONTROLLER_CELLS,
def __call__(self, variant: int, codeddata: str, _globals: dict):
"""
Check whether a given answer is within tolerance.
This checks the variable defined in the constructor against
a reference value in the codeddata.
Parameters
----------
variant : int
Variant of the answer.
codeddata : str
Encoded answer array.
Returns
-------
testname : str
Short name for the test
score : float
Normalized score, 0 or 1
result : str
Text describing the result
studentanswer : str
Student's answer, as interpreted
modelanswer : str
Reference answer
"""
dec = json.JSONDecoder()
Aref, Bref, Cref, Dref = map(
np.array,
dec.decode(cmpr.decompress(b64decode(codeddata.encode('ascii')))
.decode('utf-8'))[variant])
sys_ref = StateSpace(Aref, Bref, Cref, Dref)
# checking function
def within_tolerance(xr, x):
return np.allclose(xr, x, self.d_rel, self.d_abs)
fails = 0
value = self._extractValue(_globals)
try:
A = np.array(value["A"])
B = np.array(value["B"])
C = np.array(value["C"])
D = np.array(value["D"])
dt = value["dt"]
value = StateSpace(A, B, C, D, dt)
except Exception:
return self._return(0.0, "1 is not a state-space system",
_globals[self.var], sys_ref)
report = []
try:
if A.shape != Aref.shape:
value = minreal(value)
A = value.A
B = value.B
C = value.C
if A.shape == Aref.shape:
fails += round(self.threshold / 2)
report.append('system is not minimal realization')
if A.shape != Aref.shape:
fails += self.threshold
report.append('incorrect system order')
if B.shape[1] != Bref.shape[1]:
fails += self.threshold
report.append('incorrect number of inputs')
if C.shape[0] != Cref.shape[0]:
fails += self.threshold
report.append('incorrect number of outputs')
if fails > self.threshold:
return self._return(0.0, "\n".join(report),
value, sys_ref)
# d matrix
ndfail = Dref.size - \
np.sum(np.isclose(Dref, D, self.d_rel, self.d_abs))
if ndfail:
report.append('{ndfail} incorrect elements in D matrix'
''.format(ndfail=ndfail))
fails += ndfail
# orders/sizes correct, can now check transfers
zpk_ref = ss_to_zpk(Aref, Bref, Cref)
zpk_val = ss_to_zpk(A, B, C)
for im in range(len(zpk_ref)):
for ip in range(len(zpk_ref[0])):
ok, r = zpk_ref[im][ip].check(zpk_val[im][ip], im, ip,
within_tolerance)
if not ok:
fails += 1
report.extend(r)
except Exception:
return self._return(0.0, "not a valid state-space system",
value, sys_ref)
score = max(round(
(self.threshold + 1 - fails) / (self.threshold + 1), 3), 0.0)
return self._return(
score, ((not report) and "answer is correct") or "\n".join(report),
value, sys_ref)
def tsystem(self, request):
"""Define some test systems"""
"""continuous"""
A = np.array([[1., -2.], [3., -4.]])
B = np.array([[5.], [7.]])
C = np.array([[6., 8.]])
D = np.array([[9.]])
siso_ss1 = TSys(StateSpace(A, B, C, D, 0))
siso_ss1.t = np.linspace(0, 1, 10)
siso_ss1.ystep = np.array([9., 17.6457, 24.7072, 30.4855, 35.2234,
39.1165, 42.3227, 44.9694, 47.1599,
48.9776])
siso_ss1.X0 = np.array([[.5], [1.]])
siso_ss1.yinitial = np.array([11., 8.1494, 5.9361, 4.2258, 2.9118,
1.9092, 1.1508, 0.5833, 0.1645, -0.1391])
ss1 = siso_ss1.sys
"""D=0, continuous"""
siso_ss2 = TSys(StateSpace(ss1.A, ss1.B, ss1.C, 0, 0))
siso_ss2.t = siso_ss1.t
siso_ss2.ystep = siso_ss1.ystep - 9
siso_ss2.initial = siso_ss1.yinitial - 9
siso_ss2.yimpulse = np.array([86., 70.1808, 57.3753, 46.9975, 38.5766,
31.7344, 26.1668, 21.6292, 17.9245,
14.8945])
"""System with unspecified timebase"""
siso_ss2_dtnone = TSys(StateSpace(ss1.A, ss1.B, ss1.C, 0, None))
siso_ss2_dtnone.t = np.arange(0, 10, 1.)
siso_ss2_dtnone.ystep = np.array([0., 86., -72., 230., -360., 806.,
-1512., 3110., -6120., 12326.])
siso_tf1 = TSys(TransferFunction([1], [1, 2, 1], 0))
siso_tf2 = copy(siso_ss1)
siso_tf2.sys = ss2tf(siso_ss1.sys)
"""MIMO system, contains ``siso_ss1`` twice"""
mimo_ss1 = copy(siso_ss1)
A = np.zeros((4, 4))
A[:2, :2] = siso_ss1.sys.A
A[2:, 2:] = siso_ss1.sys.A
B = np.zeros((4, 2))
B[:2, :1] = siso_ss1.sys.B
B[2:, 1:] = siso_ss1.sys.B
C = np.zeros((2, 4))
C[:1, :2] = siso_ss1.sys.C
C[1:, 2:] = siso_ss1.sys.C
D = np.zeros((2, 2))
D[:1, :1] = siso_ss1.sys.D
D[1:, 1:] = siso_ss1.sys.D
mimo_ss1.sys = StateSpace(A, B, C, D)
"""MIMO system, contains ``siso_ss2`` twice"""
mimo_ss2 = copy(siso_ss2)
A = np.zeros((4, 4))
A[:2, :2] = siso_ss2.sys.A
A[2:, 2:] = siso_ss2.sys.A
B = np.zeros((4, 2))
B[:2, :1] = siso_ss2.sys.B
B[2:, 1:] = siso_ss2.sys.B
C = np.zeros((2, 4))
C[:1, :2] = siso_ss2.sys.C
C[1:, 2:] = siso_ss2.sys.C
D = np.zeros((2, 2))
mimo_ss2.sys = StateSpace(A, B, C, D, 0)
"""discrete"""
siso_dtf0 = TSys(TransferFunction([1.], [1., 0.], 1.))
siso_dtf0.t = np.arange(4)
siso_dtf0.yimpulse = [0., 1., 0., 0.]
siso_dtf1 = TSys(TransferFunction([1], [1, 1, 0.25], True))
siso_dtf1.t = np.arange(0, 5, 1)
siso_dtf1.ystep = np.array([0. , 0. , 1. , 0. , 0.75])
siso_dtf2 = TSys(TransferFunction([1], [1, 1, 0.25], 0.2))
siso_dtf2.t = np.arange(0, 5, 0.2)
siso_dtf2.ystep = np.array(
[0. , 0. , 1. , 0. , 0.75 , 0.25 ,
0.5625, 0.375 , 0.4844, 0.4219, 0.457 , 0.4375,
0.4482, 0.4424, 0.4456, 0.4438, 0.4448, 0.4443,
0.4445, 0.4444, 0.4445, 0.4444, 0.4445, 0.4444,
0.4444])
"""Time step which leads to rounding errors for time vector length"""
num = [-0.10966442, 0.12431949]
den = [1., -1.86789511, 0.88255018]
dt = 0.12493963338370018
siso_dtf3 = TSys(TransferFunction(num, den, dt))
siso_dtf3.t = np.linspace(0, 9*dt, 10)
siso_dtf3.ystep = np.array(
[ 0. , -0.1097, -0.1902, -0.2438, -0.2729,
-0.2799, -0.2674, -0.2377, -0.1934, -0.1368])
"""dtf1 converted statically, because Slycot and Scipy produce
different realizations, wich means different initial condtions,"""
siso_dss1 = copy(siso_dtf1)
siso_dss1.sys = StateSpace([[-1., -0.25],
[ 1., 0.]],
[[1.],
[0.]],
[[0., 1.]],
[[0.]],
True)
siso_dss1.X0 = [0.5, 1.]
siso_dss1.yinitial = np.array([1., 0.5, -0.75, 0.625, -0.4375])
siso_dss2 = copy(siso_dtf2)
siso_dss2.sys = tf2ss(siso_dtf2.sys)
mimo_dss1 = TSys(StateSpace(ss1.A, ss1.B, ss1.C, ss1.D, True))
mimo_dss1.t = np.arange(0, 5, 0.2)
mimo_dss2 = copy(mimo_ss1)
mimo_dss2.sys = c2d(mimo_ss1.sys, mimo_ss1.t[1]-mimo_ss1.t[0])
mimo_tf2 = copy(mimo_ss2)
tf_ = ss2tf(siso_ss2.sys)
mimo_tf2.sys = TransferFunction(
[[tf_.num[0][0], [0]], [[0], tf_.num[0][0]]],
[[tf_.den[0][0], [1]], [[1], tf_.den[0][0]]],
0)
mimo_dtf1 = copy(siso_dtf1)
tf_ = siso_dtf1.sys
mimo_dtf1.sys = TransferFunction(
[[tf_.num[0][0], [0]], [[0], tf_.num[0][0]]],
[[tf_.den[0][0], [1]], [[1], tf_.den[0][0]]],
True)
# for pole cancellation tests
pole_cancellation = TSys(TransferFunction(
[1.067e+05, 5.791e+04],
[10.67, 1.067e+05, 5.791e+04]))
no_pole_cancellation = TSys(TransferFunction(
[1.881e+06],
[188.1, 1.881e+06]))
# System Type 1 - Step response not stationary: G(s)=1/s(s+1)
siso_tf_type1 = TSys(TransferFunction(1, [1, 1, 0]))
siso_tf_type1.step_info = {
'RiseTime': np.NaN,
'SettlingTime': np.NaN,
'SettlingMin': np.NaN,
'SettlingMax': np.NaN,
'Overshoot': np.NaN,
'Undershoot': np.NaN,
'Peak': np.Inf,
'PeakTime': np.Inf,
'SteadyStateValue': np.NaN}
# SISO under shoot response and positive final value
# G(s)=(-s+1)/(s²+s+1)
siso_tf_kpos = TSys(TransferFunction([-1, 1], [1, 1, 1]))
siso_tf_kpos.step_info = {
'RiseTime': 1.242,
'SettlingTime': 9.110,
'SettlingMin': 0.90,
'SettlingMax': 1.208,
'Overshoot': 20.840,
'Undershoot': 28.0,
'Peak': 1.208,
'PeakTime': 4.282,
'SteadyStateValue': 1.0}
# SISO under shoot response and negative final value
# k=-1 G(s)=-(-s+1)/(s²+s+1)
siso_tf_kneg = TSys(TransferFunction([1, -1], [1, 1, 1]))
siso_tf_kneg.step_info = {
'RiseTime': 1.242,
'SettlingTime': 9.110,
'SettlingMin': -1.208,
'SettlingMax': -0.90,
'Overshoot': 20.840,
'Undershoot': 28.0,
'Peak': 1.208,
'PeakTime': 4.282,
'SteadyStateValue': -1.0}
siso_tf_asymptotic_from_neg1 = TSys(TransferFunction([-1, 1], [1, 1]))
siso_tf_asymptotic_from_neg1.step_info = {
'RiseTime': 2.197,
'SettlingTime': 4.605,
'SettlingMin': 0.9,
'SettlingMax': 1.0,
'Overshoot': 0,
'Undershoot': 100.0,
'Peak': 1.0,
'PeakTime': 0.0,
'SteadyStateValue': 1.0}
siso_tf_asymptotic_from_neg1.kwargs = {
'step_info': {'T': np.arange(0, 5, 1e-3)}}
# example from matlab online help
# https://www.mathworks.com/help/control/ref/stepinfo.html
siso_tf_step_matlab = TSys(TransferFunction([1, 5, 5],
[1, 1.65, 5, 6.5, 2]))
siso_tf_step_matlab.step_info = {
'RiseTime': 3.8456,
'SettlingTime': 27.9762,
'SettlingMin': 2.0689,
'SettlingMax': 2.6873,
'Overshoot': 7.4915,
'Undershoot': 0,
'Peak': 2.6873,
'PeakTime': 8.0530,
'SteadyStateValue': 2.5}
A = [[0.68, -0.34],
[0.34, 0.68]]
B = [[0.18, -0.05],
[0.04, 0.11]]
C = [[0, -1.53],
[-1.12, -1.10]]
D = [[0, 0],
[0.06, -0.37]]
mimo_ss_step_matlab = TSys(StateSpace(A, B, C, D, 0.2))
mimo_ss_step_matlab.kwargs['step_info'] = {'T': 4.6}
mimo_ss_step_matlab.step_info = [[
{'RiseTime': 0.6000,
'SettlingTime': 3.0000,
'SettlingMin': -0.5999,
'SettlingMax': -0.4689,
'Overshoot': 15.5072,
'Undershoot': 0.,
'Peak': 0.5999,
'PeakTime': 1.4000,
'SteadyStateValue': -0.5193},
{'RiseTime': 0.,
'SettlingTime': 3.6000,
'SettlingMin': -0.2797,
'SettlingMax': -0.1043,
'Overshoot': 118.9918,
'Undershoot': 0,
'Peak': 0.2797,
'PeakTime': .6000,
'SteadyStateValue': -0.1277}],
[{'RiseTime': 0.4000,
'SettlingTime': 2.8000,
'SettlingMin': -0.6724,
'SettlingMax': -0.5188,
'Overshoot': 24.6476,
'Undershoot': 11.1224,
'Peak': 0.6724,
'PeakTime': 1,
'SteadyStateValue': -0.5394},
{'RiseTime': 0.0000, # (*)
'SettlingTime': 3.4000,
'SettlingMin': -0.4350, # (*)
'SettlingMax': -0.1485,
'Overshoot': 132.0170,
'Undershoot': 0.,
'Peak': 0.4350,
'PeakTime': .2,
'SteadyStateValue': -0.1875}]]
# (*): MATLAB gives 0.4 for RiseTime and -0.1034 for
# SettlingMin, but it is unclear what 10% and 90% of
# the steady state response mean, when the step for
# this channel does not start a 0.
siso_ss_step_matlab = copy(mimo_ss_step_matlab)
siso_ss_step_matlab.sys = siso_ss_step_matlab.sys[1, 0]
siso_ss_step_matlab.step_info = siso_ss_step_matlab.step_info[1][0]
Ta = [[siso_tf_kpos, siso_tf_kneg, siso_tf_step_matlab],
[siso_tf_step_matlab, siso_tf_kpos, siso_tf_kneg]]
mimo_tf_step_info = TSys(TransferFunction(
[[Ti.sys.num[0][0] for Ti in Tr] for Tr in Ta],
[[Ti.sys.den[0][0] for Ti in Tr] for Tr in Ta]))
mimo_tf_step_info.step_info = [[Ti.step_info for Ti in Tr]
for Tr in Ta]
# enforce enough sample points for all channels (they have different
# characteristics)
mimo_tf_step_info.kwargs['step_info'] = {'T_num': 2000}
systems = locals()
if isinstance(request.param, str):
return systems[request.param]
else:
return [systems[sys] for sys in request.param]
def red_obs(sys,T,poles):
"""Reduced order observer of the system sys
Call:
obs=red_obs(sys,T,poles)
Parameters
----------
sys : System in State Space form
T: Complement matrix
poles: desired observer poles
Returns
-------
obs: ss
Reduced order Observer
"""
if isinstance(sys, TransferFunction):
"System must be in state space form"
return
a=mat(sys.A)
b=mat(sys.B)
c=mat(sys.C)
d=mat(sys.D)
T=mat(T)
P=mat(vstack((c,T)))
invP=inv(P)
AA=P*a*invP
ny=shape(c)[0]
nx=shape(a)[0]
nu=shape(b)[1]
A11=AA[0:ny,0:ny]
A12=AA[0:ny,ny:nx]
A21=AA[ny:nx,0:ny]
A22=AA[ny:nx,ny:nx]
L1=place(A22.T,A12.T,poles)
L1=mat(L1).T
nn=nx-ny
tmp1=mat(hstack((-L1,eye(nn,nn))))
tmp2=mat(vstack((zeros((ny,nn)),eye(nn,nn))))
Ar=tmp1*P*a*invP*tmp2
tmp3=vstack((eye(ny,ny),L1))
tmp3=mat(hstack((P*b,P*a*invP*tmp3)))
tmp4=hstack((eye(nu,nu),zeros((nu,ny))))
tmp5=hstack((-d,eye(ny,ny)))
tmp4=mat(vstack((tmp4,tmp5)))
Br=tmp1*tmp3*tmp4
Cr=invP*tmp2
tmp5=hstack((zeros((ny,nu)),eye(ny,ny)))
tmp6=hstack((zeros((nn,nu)),L1))
tmp5=mat(vstack((tmp5,tmp6)))
Dr=invP*tmp5*tmp4
obs=StateSpace(Ar,Br,Cr,Dr,sys.dt)
return obs
# Buat objek dari kelas sistem pegas massa
pegasMassa = sistemPegasMassa(m, c, k)
# Keluarkan matriks dinamik dari sistem tersebut
A = pegasMassa.A
print('Matriks dinamika sistem adalah: \n', A, '\n')
# Kemudian buat representasi State Space dari sistem pegas-massa tersebut.
# Buat matriks B dan D berisi 0, dan C = matriks identitas.
B = np.zeros(np.shape(A))
C = np.identity(np.ndim(A))
D = np.zeros(np.shape(B))
# Representasi state space nya adalah sebagai berikut
mySys = StateSpace(A, B, C, D)
#print('Model linier sistem adalah: \n', mySys)
# Hitung eigenvalue dari sistem pegas massa tersebut
poles = mySys.pole()
print('Poles sistem adalah: \n', poles, '\n')
# Nyatakan apakah sistem stabil atau tidak. Apabila nilai poles nya bukan bilangan complex
# sistem tidak stabil
if str(type(poles[1])) == "<class 'numpy.float64'>":
print('===> sistem tidak stabil \n')
else:
print('===> sistem stabil \n')
# Visualisasikan pula lokasi poles dengan memetakannya di ruang kompleks C.
# Seluruh poles dari sistem yang stabil berada di sebelah kanan sumbu
Tc = 0.0021
dt = 0.01 #tempo de discretizacao para o motor real
#Velocidade Real
df = pd.read_csv('file.csv')
velocidadeReal = df['Velocidade'].tolist()
#modelo do sistema
A = np.array([[-R / L, -K / L], [K / J, -b / J]])
B = np.array([[1 / L, 0], [0, -1 / J]])
C = np.array([0, 1])
D = np.array([0, 0])
sys = StateSpace(A, B, C, D)
#discretizando o modelo
dsys = sys.sample(dt)
#Setando os dados que serao usados
Samples = len(velocidadeReal)
tempo = np.arange(0, (Samples + 1) * dt, dt)
P = np.array([[0.1389, 0], [0, 0.0389]])
#Sendo Q a covariancia do ruido do preocesso. O que representa a incerteza do modelo
Q = np.array([[0.1, 0], [0, 0.1]])
#Sendo R ruido na medicao
def d2c(sys,method='zoh'):
"""Continous to discrete conversion with ZOH method
Call:
sysc=d2c(sys,method='log')
Parameters
----------
sys : System in statespace or Tf form
method: 'zoh' or 'tustin'
Returns
-------
sysc: continous system ss or tf
"""
flag = 0
if isinstance(sys, TransferFunction):
sys=tf2ss(sys)
flag=1
a=sys.A
b=sys.B
c=sys.C
d=sys.D
Ts=sys.dt
n=shape(a)[0]
nb=shape(b)[1]
nc=shape(c)[0]
tol=1e-12
if method=='zoh':
if n==1:
if b[0,0]==1:
A=0
B=b/sys.dt
C=c
D=d
else:
tmp1=hstack((a,b))
tmp2=hstack((zeros((nb,n)),eye(nb)))
tmp=vstack((tmp1,tmp2))
s=logm(tmp)
s=s/Ts
if norm(imag(s),inf) > sqrt(sp.finfo(float).eps):
print('Warning: accuracy may be poor')
s=real(s)
A=s[0:n,0:n]
B=s[0:n,n:n+nb]
C=c
D=d
elif method=='foh':
a=mat(a)
b=mat(b)
c=mat(c)
d=mat(d)
Id = mat(eye(n))
A = logm(a)/Ts
A = real(around(A,12))
Amat = mat(A)
B = (a-Id)**(-2)*Amat**2*b*Ts
B = real(around(B,12))
Bmat = mat(B)
C = c
D = d - C*(Amat**(-2)/Ts*(a-Id)-Amat**(-1))*Bmat
D = real(around(D,12))
elif method=='tustin':
a=mat(a)
b=mat(b)
c=mat(c)
d=mat(d)
poles=eigvals(a)
if any(abs(poles-1)<200*sp.finfo(float).eps):
print('d2c: some poles very close to one. May get bad results.')
I=mat(eye(n,n))
tk = 2 / sqrt (Ts)
A = (2/Ts)*(a-I)*inv(a+I)
iab = inv(I+a)*b
B = tk*iab
C = tk*(c*inv(I+a))
D = d- (c*iab)
else:
print('Method not supported')
return
sysc=StateSpace(A,B,C,D)
if flag==1:
sysc=ss2tf(sysc)
return sysc
B1_11 = 0 B1_12 = 1 / CFLY / RON / 2 B1_21 = -1 / CLOAD B1_22 = 1 / CLOAD / RON / 2 B1 = np.array([[B1_11, B1_12], [B1_21, B1_22]]) C1_11 = 0 C1_12 = 1 C1 = np.array([C1_11, C1_12]) D1_11 = 0 D1_12 = 0 D1 = np.array([D1_11, D1_12]) phase_1 = StateSpace(A1, B1, C1, D1) # ====================================================================================================================== # State Space Matrices Phase 2 # ====================================================================================================================== A2_11 = -1 / CFLY / RON / 2 A2_12 = 1 / CFLY / RON / 2 A2_21 = 1 / CLOAD / RON / 2 A2_22 = -1 / CLOAD / RON / 2 A2 = np.array([[A2_11, A2_12], [A2_21, A2_22]]) B2_11 = 0 B2_12 = 0 B2_21 = -1 / CLOAD B2_22 = 0 B2 = np.array([[B2_11, B2_12], [B2_21, B2_22]])
def mimo_dss1(self, mimo_ss1):
ss1 = mimo_ss1.sys
T = TSys(
StateSpace(ss1.A, ss1.B, ss1.C, ss1.D, True))
T.t = np.arange(0, 5, 0.2)
return T
