def normalized_place(wc, zc): # Get the dynamics and input matrices, for later use A, B = lateral_normalized.A, lateral_normalized.B # Compute the eigenvalues from the characteristic polynomial eigs = np.roots([1, 2 * zc * wc, wc**2]) # Compute the feedback gain using eigenvalue placement K = ct.place_varga(A, B, eigs) # Create a new system representing the closed loop response clsys = ct.StateSpace(A - B @ K, B, lateral_normalized.C, 0) # Compute the feedforward gain based on the zero frequency gain of the closed loop kf = np.real(1 / clsys(0)) # Scale the input by the feedforward gain clsys *= kf # Return gains and closed loop system dynamics return K, kf, clsys
# Note that "rho" is always positive, but "kappa" has the same sign as omega. if vv['kappa'] is None: sdot = 0.0 odot = vv['direction'] * vv['d_vhat_dt'] / pp.rhohat else: sdot = vv['d_vhat_dt'] / (1.0 + pp.rhohat * abs(vv['kappa'])) odot = vv['kappa'] * sdot s0 = s0 if s0 else 0.001 A = np.array( ((-qx, 0, 0, 0, 0, -omega0 * s0), (omega0, 0, s0, 0, 0, -qx * s0), (0, 0, -qo, 0, 0, 0), (1, 0, 0, 0, 0, 0), (0, 1, 0, 0, 0, 0), (0, 0, 1, 0, 0, 0))) print(gstr({'A': A, 'B': B})) #Kr, S, E = control.lqr(A, B, Q, R) Kr = control.place_varga(A, B, np_poles) # debug ''' Kr = np.array([ [.191, 3.32, -.445, .592, .806, -1.53], [.15, -2.65, .433, .806, -5.92, 1.37] ]) Kr = np.matrix([ [-1.67580, 0.75032, 0.17497, 0.85075, 0.26784, -1.03952], [-1.72772, -0.63308, -0.15645, 1.35021, -0.20051, 0.87364] ]) ''' print(gstr({'Kr': Kr})) print(gstr({'eps * Kr': np.squeeze(eps) * np.asarray(Kr)})) lrs = -Kr * eps print({'lrs': np.squeeze(lrs)})