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))