def testMimoW3(self):
"MIMO plant with T weighting"
from control import augw, ss
g = ss([[-1.,-2],[-3,-4]],
[[1.,0.],[0.,1.]],
[[1.,0.],[0.,1.]],
[[1.,0.],[0.,1.]])
w3 = ss([-2],[2.],[1.],[2.])
p = augw(g,w3=w3)
self.assertEqual(4,p.outputs)
self.assertEqual(4,p.inputs)
# w->z3 should be 0
self.siso_almost_equal(0, p[0,0])
self.siso_almost_equal(0, p[0,1])
self.siso_almost_equal(0, p[1,0])
self.siso_almost_equal(0, p[1,1])
# w->v should be I
self.siso_almost_equal(1, p[2,0])
self.siso_almost_equal(0, p[2,1])
self.siso_almost_equal(0, p[3,0])
self.siso_almost_equal(1, p[3,1])
# u->z3 should be w3*g
self.siso_almost_equal(w3*g[0,0], p[0,2])
self.siso_almost_equal(w3*g[0,1], p[0,3])
self.siso_almost_equal(w3*g[1,0], p[1,2])
self.siso_almost_equal(w3*g[1,1], p[1,3])
# # u->v should be -g
self.siso_almost_equal(-g[0,0], p[2,2])
self.siso_almost_equal(-g[0,1], p[2,3])
self.siso_almost_equal(-g[1,0], p[3,2])
self.siso_almost_equal(-g[1,1], p[3,3])
def testMimoW1(self):
"""MIMO plant with S weighting"""
from control import augw, ss
g = ss([[-1., -2], [-3, -4]], [[1., 0.], [0., 1.]],
[[1., 0.], [0., 1.]], [[1., 0.], [0., 1.]])
w1 = ss([-2], [2.], [1.], [2.])
p = augw(g, w1)
self.assertEqual(4, p.outputs)
self.assertEqual(4, p.inputs)
# w->z1 should be diag(w1,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->v should be I
self.siso_almost_equal(1, p[2, 0])
self.siso_almost_equal(0, p[2, 1])
self.siso_almost_equal(0, p[3, 0])
self.siso_almost_equal(1, p[3, 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->v should be -g
self.siso_almost_equal(-g[0, 0], p[2, 2])
self.siso_almost_equal(-g[0, 1], p[2, 3])
self.siso_almost_equal(-g[1, 0], p[3, 2])
self.siso_almost_equal(-g[1, 1], p[3, 3])
def testSisoW123(self):
"SISO plant with all weights"
from control import augw, ss
g = ss([-1.],[1.],[1.],[1.])
w1 = ss([-2.],[2.],[1.],[2.])
w2 = ss([-3.],[3.],[1.],[3.])
w3 = ss([-4.],[4.],[1.],[4.])
p = augw(g,w1,w2,w3)
self.assertEqual(4,p.outputs)
self.assertEqual(2,p.inputs)
# w->z1 should be w1
self.siso_almost_equal(w1,p[0,0])
# w->z2 should be 0
self.siso_almost_equal(0,p[1,0])
# w->z3 should be 0
self.siso_almost_equal(0,p[2,0])
# w->v should be 1
self.siso_almost_equal(ss([],[],[],[1]),p[3,0])
# u->z1 should be -w1*g
self.siso_almost_equal(-w1*g,p[0,1])
# u->z2 should be w2
self.siso_almost_equal(w2,p[1,1])
# u->z3 should be w3*g
self.siso_almost_equal(w3*g,p[2,1])
# u->v should be -g
self.siso_almost_equal(-g,p[3,1])
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 testSisoW2(self):
"SISO plant with KS weighting"
from control import augw, ss
g = ss([-1.],[1.],[1.],[1.])
w2 = ss([-2],[1.],[1.],[2.])
p = augw(g,w2=w2)
self.assertEqual(2,p.outputs)
self.assertEqual(2,p.inputs)
# w->z2 should be 0
self.siso_almost_equal(ss([],[],[],0),p[0,0])
# w->v should be 1
self.siso_almost_equal(ss([],[],[],[1]),p[1,0])
# u->z2 should be w2
self.siso_almost_equal(w2,p[0,1])
# u->v should be -g
self.siso_almost_equal(-g,p[1,1])
def testSisoW1(self):
"SISO plant with S weighting"
from control import augw, ss
g = ss([-1.],[1.],[1.],[1.])
w1 = ss([-2],[2.],[1.],[2.])
p = augw(g,w1)
self.assertEqual(2,p.outputs)
self.assertEqual(2,p.inputs)
# w->z1 should be w1
self.siso_almost_equal(w1,p[0,0])
# w->v should be 1
self.siso_almost_equal(ss([],[],[],[1]),p[1,0])
# u->z1 should be -w1*g
self.siso_almost_equal(-w1*g,p[0,1])
# u->v should be -g
self.siso_almost_equal(-g,p[1,1])
def testSisoW3(self):
"""SISO plant with T weighting"""
from control import augw, ss
g = ss([-1.], [1.], [1.], [1.])
w3 = ss([-2], [1.], [1.], [2.])
p = augw(g, w3=w3)
self.assertEqual(2, p.outputs)
self.assertEqual(2, p.inputs)
# w->z3 should be 0
self.siso_almost_equal(ss([], [], [], 0), p[0, 0])
# w->v should be 1
self.siso_almost_equal(ss([], [], [], [1]), p[1, 0])
# u->z3 should be w3*g
self.siso_almost_equal(w3 * g, p[0, 1])
# u->v should be -g
self.siso_almost_equal(-g, p[1, 1])
def testSiso(self):
"mixsyn with SISO system"
from control import tf, augw, hinfsyn, mixsyn
from control import ss
# Skogestad+Postlethwaite, Multivariable Feedback Control, 1st Ed., Example 2.11
s = tf([1, 0], 1)
# plant
g = 200/(10*s+1)/(0.05*s+1)**2
# sensitivity weighting
M = 1.5
wb = 10
A = 1e-4
w1 = (s/M+wb)/(s+wb*A)
# KS weighting
w2 = tf(1, 1)
p = augw(g, w1, w2)
kref, clref, gam, rcond = hinfsyn(p, 1, 1)
ktest, cltest, info = mixsyn(g, w1, w2)
# check similar to S+P's example
np.testing.assert_allclose(gam, 1.37, atol = 1e-2)
# mixsyn is a convenience wrapper around augw and hinfsyn, so
# results will be exactly the same. Given than, use the lazy
# but fragile testing option.
np.testing.assert_allclose(ktest.A, kref.A)
np.testing.assert_allclose(ktest.B, kref.B)
np.testing.assert_allclose(ktest.C, kref.C)
np.testing.assert_allclose(ktest.D, kref.D)
np.testing.assert_allclose(cltest.A, clref.A)
np.testing.assert_allclose(cltest.B, clref.B)
np.testing.assert_allclose(cltest.C, clref.C)
np.testing.assert_allclose(cltest.D, clref.D)
np.testing.assert_allclose(gam, info[0])
np.testing.assert_allclose(rcond, info[1])
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])
# Aircraft model A = [[Y_b/u_0, -(1.0-Y_r/u_0)],[N_b, N_r]] # x1 = beta, x2 = r B = [[0.0,Y_dr/u_0],[N_da,N_dr]] #u1 = delta_a, u2 = delta_r C = [[1.0,0.0],[0.0,1.0]] # y1 = beta, y2 = r D = [[0.0,0.0],[0.0,0.0]] G = ss(A,B,C,D) #weighting function w1 = weighting(2, 2, 0.005) w1 = ss(w1) #needs to be converted for append wp = w1.append(w1) wu = ss([], [], [], np.eye(2)) #unwieghted actuator authority size of 2 #Augmented plant need to make by hand for hinf syn I = ss([], [], [], np.eye(2)) P = augw(G,wp,wu) #generalized plant P P = minreal(P) wps = tf(wp) Gs= tf(G) Is= tf(I) # correct generalized plant p11 = wps p12 = wps*Gs p21 = -Is p22 = -Gs K2 = h2syn(P, 2, 2) # H INF CONTROLLER K, CL, gam, rcond = hinfsyn(P,2,2) #generalized plant incorrect so doing mixsyn
B = [[Zde], [Mde]]
C = [0.0, 1.0]
D = 0.0
G = con.StateSpace(A, B, C, D)
# Controller synthesis
wb = 8 # Desired closed-loop bandwidth
A = 1 / 100 # Desired disturbance attenuation inside bandwidth
M = 1.5 # Desired bound on hinfnorm(S) & hinfnorm(T)
w1 = con.TransferFunction([1 / M, wb],
[1.0, wb * A]) #tracking/disturbance weight
w2 = con.TransferFunction([1.0, 0.1],
[1, 100]) #K*S (actuator authority) weight
# Augmented plant
P = con.augw(G, w1, w2) #generalized plant P
P = con.minreal(P)
# H 2 CONTROLLER
K2 = con.h2syn(P, 1, 1)
L = G * K2
Ltf = con.ss2tf(L)
So2 = 1.0 / (1.0 + Ltf)
So2 = con.minreal(So2)
To2 = G * K2 * So2
To2 = con.minreal(To2)
# H INF CONTROLLER
K, CL, gam, rcond = con.hinfsyn(P, 1, 1)
print(gam)
def longitudinal_controller(
nominal_ss: StateSpaceMatrices,
boundary_ss=Tuple[StateSpaceMatrices]) -> StateSpaceMatrices:
"""
:param nominal_ss: nomnial plant used in hinf-synthesis
:param boundary_ss: tuple of worst case plant used for gap-metric robustness requirements
returns: StateSpaceMatrices pf controller
"""
A_lon, B_lon, C_lon, D_lon = get_longitudinal_state_space(nominal_ss)
longitudinal_ss = StateSpace(A_lon, B_lon, C_lon, D_lon)
A_wcl, B_wcl, C_wcl, D_wcl = get_longitudinal_state_space(boundary_ss[0])
A_wcu, B_wcu, C_wcu, D_wcu = get_longitudinal_state_space(boundary_ss[1])
worst_case_lower_tf = ss2tf(A_wcl, B_wcl, C_wcl, D_wcl)
worst_case_upper_tf = ss2tf(A_wcu, B_wcu, C_wcu, D_wcu)
long_tf = ss2tf(longitudinal_ss)
# Initial filter design:
M = 2
w_0 = 1
A_fact = 0.001
nu_gap_l = nu_gap(long_tf, worst_case_lower_tf)
nu_gap_u = nu_gap(long_tf, worst_case_upper_tf)
omega_step = 0.1
max_freq = 13.8
while w_0 < max_freq:
w_0 = w_0 + omega_step
W_C = tf([1], [1])
W_S = tf([1 / M, w_0], [1, w_0 * A_fact])
W_T = tf([1, w_0 / M], [A_fact, w_0])
Plant = augw(longitudinal_ss, W_S, W_C, W_T)
K_tmp, CL_tmp, gam_tmp, _ = hinfsyn(Plant, 1, 1, 0.001)
controller = ss2tf(K_tmp.A, K_tmp.B, K_tmp.C, K_tmp.D)
# Stability-requirements
robust_margin = 1 / gam_tmp
stability = nu_gap_l < robust_margin and nu_gap_u < robust_margin
# Robustness-requirements
margins = check_margins(controller,
long_tf,
worst_case_upper_tf,
worst_case_lower_tf,
stab_m=0.5)
if stability and margins:
K, _, gam = K_tmp, CL_tmp, gam_tmp
else:
break
print(f"Longitudinal performance value (gamma) {gam}")
print(
f"Longitudinal controller crossover frequency (w_0) {w_0-omega_step}")
return StateSpaceMatrices(A=K.A, B=K.B, C=K.C, D=K.D)
def lateral_controller(
nominal_ss: StateSpaceMatrices,
boundary_ss=Tuple[StateSpaceMatrices]) -> StateSpaceMatrices:
"""
:param controller_filename_out: Write-to filename
:param ss_nom_filename: nominal plant filename
:param ss_wc_lower_fname: worst case lower filename
:param ss_wc_upper_fname: worst case upper filename
returns: A, B, C, D state space matrices of the synthesized controller
"""
A_lat, B_lat, C_lat, D_lat = get_lateral_state_space(nominal_ss)
lateral_statespace = StateSpace(A_lat, B_lat, C_lat, D_lat)
A_wcl, B_wcl, C_wcl, D_wcl = get_lateral_state_space(boundary_ss[0])
A_wcu, B_wcu, C_wcu, D_wcu = get_lateral_state_space(boundary_ss[1])
worst_case_lower_tf = ss2tf(A_wcl, B_wcl, C_wcl, D_wcl)
worst_case_upper_tf = ss2tf(A_wcu, B_wcu, C_wcu, D_wcu)
lateral_tf = ss2tf(A_lat, B_lat, C_lat, D_lat)
# Filter design Variables:
M = 2.0
w_0 = 1 # min_freq
A_fact = 0.0002
nu_gap_l = nu_gap(lateral_tf, worst_case_lower_tf)
nu_gap_u = nu_gap(lateral_tf, worst_case_upper_tf)
omega_step = 0.1
freq_max = 10
while w_0 < freq_max:
w_0 = w_0 + omega_step
W_S = tf([1 / M, 2 * w_0 / np.sqrt(M), w_0**2],
[1, 2 * w_0 * np.sqrt(A_fact), (w_0**2) * A_fact])
W_C = tf([1], [1])
W_T = tf([1, w_0 / M], [A_fact, w_0])
Plant = augw(lateral_statespace, W_S, W_C, W_T)
K_tmp, CL_tmp, gam_tmp, rcond_tmp = hinfsyn(Plant, 1, 1, 0.01)
controller = ss2tf(K_tmp.A, K_tmp.B, K_tmp.C, K_tmp.D)
# Stability-requirements
robust_margin = 1 / gam_tmp
stability = nu_gap_l < robust_margin and nu_gap_u < robust_margin
# Robustness-requirements
margins = check_margins(controller,
lateral_statespace,
worst_case_upper_tf,
worst_case_lower_tf,
stab_m=0.7)
if stability and margins:
K, _, gam, _ = K_tmp, CL_tmp, gam_tmp, rcond_tmp
else:
break
print(f"Lateral performance value (gamma) {gam}")
print(f"Latereal controller crossover frequency (w_0) {w_0-omega_step}")
return StateSpaceMatrices(K.A, K.B, K.C, K.D)
import numpy import control m = 1. k = 0.4 zeta = 0.01 b = numpy.sqrt(k / m) * 2 * m * zeta #G = control.tf(1,[m, 0., 0.]) G = control.tf([1.], [m, b, k]) f_bandwidth = 100.0 w_bandwidth = 2 * numpy.pi * f_bandwidth A = 0.01 M = 2 w1 = control.tf([1 / M, w_bandwidth], [1.0, A * w_bandwidth]) w2 = control.tf([1.], [1.]) w3 = control.tf([1.0, w_bandwidth / M], [A, w_bandwidth]) control.bode((G, w1, w3)) P = control.augw(G, w1=w1, w2=None, w3=w3) ## K, CL, gam, rcond = control.hinfsyn(P, 1, 1) #K, CL, gam, rcond = control.h2syn(P,1,1) C = control.ss2tf(K) S = 1 / (1 + C * G) T = G * C / (1 + C * G) control.bode((S, 1 / w1))
