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