def testSystemInitialization(self, tsys):
# Check to make sure systems are discrete time with proper variables
assert tsys.siso_ss1.dt is None
assert tsys.siso_ss1c.dt == 0
assert tsys.siso_ss1d.dt == 0.1
assert tsys.siso_ss2d.dt == 0.2
assert tsys.siso_ss3d.dt is True
assert tsys.mimo_ss1c.dt == 0
assert tsys.mimo_ss1d.dt == 0.1
assert tsys.mimo_ss2d.dt == 0.2
assert tsys.siso_tf1.dt is None
assert tsys.siso_tf1c.dt == 0
assert tsys.siso_tf1d.dt == 0.1
assert tsys.siso_tf2d.dt == 0.2
assert tsys.siso_tf3d.dt is True
# keyword argument check
# dynamic systems
assert TransferFunction(1, [1, 1], dt=0.1).dt == 0.1
assert TransferFunction(1, [1, 1], 0.1).dt == 0.1
assert StateSpace(1,1,1,1, dt=0.1).dt == 0.1
assert StateSpace(1,1,1,1, 0.1).dt == 0.1
# static gain system, dt argument should still override default dt
assert TransferFunction(1, [1,], dt=0.1).dt == 0.1
assert TransferFunction(1, [1,], 0.1).dt == 0.1
assert StateSpace(0,0,1,1, dt=0.1).dt == 0.1
assert StateSpace(0,0,1,1, 0.1).dt == 0.1
def myfunc(variant: int):
"""Generate test systems. """
A = np.array((0, 0, 0, 1, 0, -0.9215, 0, 1, -0.738)).reshape((3, 3))
B = np.array((1 + 0.1 * variant, 0, 0)).reshape((3, 1))
C = np.array((0, 0.151, -0.6732)).reshape((1, 3))
D = np.zeros((1, 1))
sys1 = StateSpace(A, B, C, D)
sys2 = tf2ss(ss2tf(sys1))
As, Z = schur(A)
Bs = Z.T @ B
Cs = C @ Z
Ds = D
# print(Bs)
sys3 = StateSpace(As, Bs, Cs, Ds)
Ds[0, 0] = 0.3
sys4 = StateSpace(As, Bs, Cs, Ds)
Ds[0, 0] = 0
Ab = np.zeros((4, 4))
Ab[:3, :3] = A
Bb = np.zeros((4, 1))
Bb[:3, :] = B
Cb = np.zeros((1, 4))
Cb[:, :3] = C
sys5 = StateSpace(Ab, Bb, Cb, D)
# sys5.A = Ab
# sys5.B = Bb
# sys5.C = Cb
return locals()
def combine(systems):
""" systems: 2D array of systems to combine """
rrows = []
for srow in systems:
s1 = srow[0]
if not isinstance(s1, StateSpace):
s1 = _convertToStateSpace(s1)
for s2 in srow[1:]:
if not isinstance(s2, StateSpace):
s2 = _convertToStateSpace(s2)
if s1.dt != s2.dt:
raise ValueError("Systems must have the same time step")
n = s1.states + s2.states
m = s1.inputs + s2.inputs
p = s1.outputs
if s2.outputs != p:
raise ValueError('inconsistent systems')
A = np.zeros((n, n))
B = np.zeros((n, m))
C = np.zeros((p, n))
D = np.zeros((p, m))
A[:s1.states, :s1.states] = s1.A
A[s1.states:, s1.states:] = s2.A
B[:s1.states, :s1.inputs] = s1.B
B[s1.states:, s1.inputs:] = s2.B
C[:, :s1.states] = s1.C
C[:, s1.states:] = s2.C
D[:, :s1.inputs] = s1.D
D[:, s1.inputs:] = s2.D
s1 = StateSpace(A, B, C, D, s1.dt)
rrows.append(s1)
r1 = rrows[0]
for r2 in rrows[1:]:
if r1.dt != r2.dt:
raise ValueError("Systems must have the same time step")
n = r1.states + r2.states
m = r1.inputs
if r2.inputs != m:
raise ValueError('inconsistent systems')
p = r1.outputs + r2.outputs
A = np.zeros((n, n))
B = np.zeros((n, m))
C = np.zeros((p, n))
D = np.zeros((p, m))
A[:r1.states, :r1.states] = r1.A
A[r1.states:, r1.states:] = r2.A
B[:r1.states, :] = r1.B
B[r1.states:, :] = r2.B
C[:r1.outputs, :r1.states] = r1.C
C[r1.outputs:, r1.states:] = r2.C
D[:r1.outputs, :] = r1.D
D[r1.outputs:, :] = r2.D
r1 = StateSpace(A, B, C, D, r1.dt)
return r1
def setUp(self):
"""Set up a SISO and MIMO system to test operations on."""
# Single input, single output continuous and discrete time systems
sys = matlab.rss(3, 1, 1)
self.siso_ss1 = StateSpace(sys.A, sys.B, sys.C, sys.D)
self.siso_ss1c = StateSpace(sys.A, sys.B, sys.C, sys.D, 0.0)
self.siso_ss1d = StateSpace(sys.A, sys.B, sys.C, sys.D, 0.1)
self.siso_ss2d = StateSpace(sys.A, sys.B, sys.C, sys.D, 0.2)
self.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.]]
self.mimo_ss1 = StateSpace(A, B, C, D)
self.mimo_ss1c = StateSpace(A, B, C, D, 0)
# Two input, two output discrete time system
self.mimo_ss1d = StateSpace(A, B, C, D, 0.1)
# Same system, but with a different sampling time
self.mimo_ss2d = StateSpace(A, B, C, D, 0.2)
# Single input, single output continuus and discrete transfer function
self.siso_tf1 = TransferFunction([1, 1], [1, 2, 1])
self.siso_tf1c = TransferFunction([1, 1], [1, 2, 1], 0)
self.siso_tf1d = TransferFunction([1, 1], [1, 2, 1], 0.1)
self.siso_tf2d = TransferFunction([1, 1], [1, 2, 1], 0.2)
self.siso_tf3d = TransferFunction([1, 1], [1, 2, 1], True)
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 test_sample_ss(self, tsys):
# double integrators, two different ways
sys1 = StateSpace([[0.,1.],[0.,0.]], [[0.],[1.]], [[1.,0.]], 0.)
sys2 = StateSpace([[0.,0.],[1.,0.]], [[1.],[0.]], [[0.,1.]], 0.)
I = np.eye(2)
for sys in (sys1, sys2):
for h in (0.1, 0.5, 1, 2):
Ad = I + h * sys.A
Bd = h * sys.B + 0.5 * h**2 * sys.A @ sys.B
sysd = sample_system(sys, h, method='zoh')
np.testing.assert_array_almost_equal(sysd.A, Ad)
np.testing.assert_array_almost_equal(sysd.B, Bd)
np.testing.assert_array_almost_equal(sysd.C, sys.C)
np.testing.assert_array_almost_equal(sysd.D, sys.D)
assert sysd.dt == h
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 = np.mat(sys.A)
b = np.mat(sys.B)
c = np.mat(sys.C)
d = np.mat(sys.D)
T = np.mat(T)
P = np.mat(np.vstack((c, T)))
invP = np.inv(P)
AA = P * a * invP
ny = np.shape(c)[0]
nx = np.shape(a)[0]
nu = np.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 = np.mat(L1).T
nn = nx - ny
tmp1 = np.mat(np.hstack((-L1, np.eye(nn, nn))))
tmp2 = np.mat(np.vstack((np.zeros((ny, nn)), np.eye(nn, nn))))
Ar = tmp1 * P * a * invP * tmp2
tmp3 = np.vstack((np.eye(ny, ny), L1))
tmp3 = np.mat(np.hstack((P * b, P * a * invP * tmp3)))
tmp4 = np.hstack((np.eye(nu, nu), np.zeros((nu, ny))))
tmp5 = np.hstack((-d, np.eye(ny, ny)))
tmp4 = np.mat(np.vstack((tmp4, tmp5)))
Br = tmp1 * tmp3 * tmp4
Cr = invP * tmp2
tmp5 = np.hstack((np.zeros((ny, nu)), np.eye(ny, ny)))
tmp6 = np.hstack((np.zeros((nn, nu)), L1))
tmp5 = np.mat(np.vstack((tmp5, tmp6)))
Dr = invP * tmp5 * tmp4
obs = StateSpace(Ar, Br, Cr, Dr, sys.dt)
return obs
def testCopyConstructor(self):
for sys in (self.siso_ss1, self.siso_ss1c, self.siso_ss1d):
newsys = StateSpace(sys)
self.assertEqual(sys.dt, newsys.dt)
for sys in (self.siso_tf1, self.siso_tf1c, self.siso_tf1d):
newsys = TransferFunction(sys)
self.assertEqual(sys.dt, newsys.dt)
def full_obs(sys,poles):
"""Full order observer of the system sys
Call:
obs=full_obs(sys,poles)
Parameters
----------
sys : System in State Space form
poles: desired observer poles
Returns
-------
obs: ss
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)
L=place(a.T,c.T,poles)
L=mat(L).T
Ao=a-L*c
Bo=hstack((b-L*d,L))
n=shape(Ao)
m=shape(Bo)
Co=eye(n[0],n[1])
Do=zeros((n[0],m[1]))
obs=StateSpace(Ao,Bo,Co,Do,sys.dt)
return obs
def comp_form_i(sys,obs,K,Cy=[[1]]):
"""Compact form Conroller+Observer+Integral part
Only for discrete systems!!!
Call:
contr=comp_form_i(sys,obs,K [,Cy])
Parameters
----------
sys : System in State Space form
obs : Observer in State Space form
K: State feedback gains
Cy: feedback matric to choose the output for integral part
Returns
-------
contr: ss
Controller
"""
if sys.dt==None:
print('contr_form_i works only with discrete systems!')
return
Ts = sys.dt
ny=shape(sys.C)[0]
nu=shape(sys.B)[1]
nx=shape(sys.A)[0]
no=shape(obs.A)[0]
ni=shape(mat(Cy))[0]
B_obsu = mat(obs.B[:,0:nu])
B_obsy = mat(obs.B[:,nu:nu+ny])
D_obsu = mat(obs.D[:,0:nu])
D_obsy = mat(obs.D[:,nu:nu+ny])
k=mat(K)
nk=shape(k)[1]
Ke=k[:,nk-ni:]
K=k[:,0:nk-ni]
X = inv(eye(nu,nu)+K*D_obsu);
a=mat(obs.A)
c=mat(obs.C)
Cy=mat(Cy)
tmp1=hstack((a-B_obsu*X*K*c,-B_obsu*X*Ke))
tmp2=hstack((zeros((ni,no)),eye(ni,ni)))
A_ctr=vstack((tmp1,tmp2))
tmp1=hstack((zeros((no,ni)),-B_obsu*X*K*D_obsy+B_obsy))
tmp2=hstack((eye(ni,ni)*Ts,-Cy*Ts))
B_ctr=vstack((tmp1,tmp2))
C_ctr=hstack((-X*K*c,-X*Ke))
D_ctr=hstack((zeros((nu,ni)),-X*K*D_obsy))
contr=StateSpace(A_ctr,B_ctr,C_ctr,D_ctr,sys.dt)
return contr
def tsys():
"""Set up a system to test operations on."""
A = [[-3., 4., 2.], [-1., -3., 0.], [2., 5., 3.]]
B = [[1.], [-3.], [-2.]]
C = [[4., 2., -3.]]
D = [[0.]]
return StateSpace(A, B, C, D)
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 testCopyConstructor(self, tsys):
for sys in (tsys.siso_ss1, tsys.siso_ss1c, tsys.siso_ss1d):
newsys = StateSpace(sys)
assert sys.dt == newsys.dt
for sys in (tsys.siso_tf1, tsys.siso_tf1c, tsys.siso_tf1d):
newsys = TransferFunction(sys)
assert sys.dt == newsys.dt
def setup(self, cfg):
self.cfg = cfg.sys
self.dt = self.cfg.params.dt
# Angle limit set to 2 * theta_threshold_radians so failing observation is still within bounds
self.seed(seed=cfg.random_seed)
a = np.mat(self.cfg.params.A)
b = np.mat(self.cfg.params.B)
c = np.mat(self.cfg.params.C)
d = np.mat(self.cfg.params.D)
self.dx = np.shape(a)[1]
self.du = np.shape(b)[1]
self.dy = np.shape(c)[0]
low_a = np.ones((self.du)) * 10
high_a = np.ones((self.du)) * 10
self.action_space = self.action_space = spaces.Box(low=low_a,
high=high_a,
dtype=np.float32)
low_obs = -np.ones((self.dx)) * np.inf
high_obs = np.ones((self.dx)) * np.inf
self.observation_space = spaces.Box(low_obs,
high_obs,
dtype=np.float32)
self.sys = StateSpace(a, b, c, d, self.dt)
self.reset()
self.setup_ran = True
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 mimo_ss_step_matlab(self):
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]]
T = TSys(StateSpace(A, B, C, D, 0.2))
T.kwargs['step_info'] = {'T': 4.6}
T.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.1034,
'SettlingMax': -0.1485,
'Overshoot': 132.0170,
'Undershoot': 79.222, # 0. in MATLAB
'Peak': 0.4350,
'PeakTime': .2,
'SteadyStateValue': -0.1875
}
]
]
# (*): MATLAB gives 0.4 here, 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 for
# 0 initial conditions
return T
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 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 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 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 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 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 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 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 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 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
