def IMstabilization_general(freqs, dim_Y, dim_U, cG1, B1, IMstabmargin,
IMstabmethod):
'''
Stabilization of the internal model pair (cG1,B1) using either LQR or pole placement.
Parameters
----------
freqs : (, N) array_like
The (real) frequencies (w_k)_{k=0}^q of the reference and disturbance signals.
dim_Y, dim_U : integer
Dimensions of the output space and input space, respectively.
cG1 : (N1, M1) array_like
The internal model for the frequencies 'freqs' and dim_Y
B1 : (N2, M2) array_like
The input matrix B1 of the internal model.
IMstabmargin : float
The desired stability margin for the internal model.
IMstabmethod : string
Stabilization of the internal model using either 'LQR' or 'poleplacement'.
Returns
-------
K : (M1, N1) array_like
The the stabilizing feedback K for the internal model so that cG1+B1*K is Hurwitz.
Raises
------
IMstabmethodexception : Exception
Thrown in case the internal model stabilization method is not 'LQR' or 'poleplacement'.
'''
if IMstabmethod == 'LQR':
q = np.size(freqs)
dim_Z = 2 * q * dim_Y - (freqs[0] == 0) * dim_Y
return -lqr(cG1 + IMstabmargin * np.eye(cG1.shape[0], cG1.shape[1]),
B1, 100 * np.eye(dim_Z), 0.001 * np.eye(dim_U))[0]
elif IMstabmethod == 'poleplacement':
# Necessary to get the same behaviour as in Matlab for
# np.linspace(-1.1*IMstabmargin, -1*IMstabmargin, dim_Y)
# in the case dim_Y = 1
temp = (-np.linspace(IMstabmargin, 1.1 * IMstabmargin, dim_Y))[::-1]
if freqs[0] == 0:
t1 = np.dot(np.ones((2 * np.size(freqs) - 1, 1)),
np.atleast_2d(temp))
t2 = 1j * np.dot(
np.atleast_2d(np.concatenate(
(freqs[::-1], -freqs[1:]))).T, np.ones((1, dim_Y)))
target_eigs = (t1 + t2).flatten()
return -place_poles(cG1, B1, target_eigs).gain_matrix
else:
t1 = np.dot(np.ones((2 * np.size(freqs), 1)), np.atleast_2d(temp))
t2 = 1j * np.dot(
np.atleast_2d(np.concatenate(
(freqs[::-1], -freqs))).T, np.ones((1, dim_Y)))
target_eigs = (t1 + t2).flatten()
return -place_poles(cG1, B1, target_eigs).gain_matrix
else:
raise Exception(
'Invalid IMstabmethod, choose either \'LQR\' or \'poleplacement\'')
def stability_measure(input):
zpoles = [
complex(input[0], 0),
complex(input[1], input[2]),
complex(input[1], -input[2])
]
for zpole in zpoles:
if abs(zpole) > 1:
return 100
# TODO: for now we check for duplicates and don't allow them. When FBG is done,
# then use that
if len(zpoles) != len(set(zpoles)):
return 100
fsf = signal.place_poles(phi, gamma, zpoles)
L = fsf.gain_matrix
ugms = control_eval.upper_gain_margin(phi, gamma, L, output_dB=False)
lgms = control_eval.lower_gain_margin(phi, gamma, L, output_dB=False)
phms = control_eval.phase_margin(phi, gamma, L)
(t, u, x) = control_sim.sim_regsf(phi, gamma, L, T, x0, Ts * 3)
st = control_eval.settling_time(t, x)
st_diff = abs(st - Ts)
gm_max = 1000
phm_max = 120
st_max = Ts
ugm_percent = 10 / gm_max
lgm_percent = 0 / gm_max
phm_percent = 1 / phm_max
st_percent = 4 / Ts
result = 0
for i in range(0, len(ugms)):
result += lgms[i] * lgm_percent - ugms[i] * ugm_percent - phms[
0] * phm_percent + st_diff * st_percent
return result
def test_K_gains(self):
'''
Check the gain matrix
'''
#Values for the test
os = 30
ts = 3
#Actual value
zeta = (-sp.log(os / 100)) / sp.sqrt(sp.pi**2 + sp.log(os / 100)**2)
wn = 4 / (ts * zeta)
P = sp.array([
-zeta * wn + 1j * wn * sp.sqrt(1 - zeta**2),
-zeta * wn - 1j * wn * sp.sqrt(1 - zeta**2)
])
A = sp.array([[0, 1], [0, -2]])
B = sp.array([[0], [1]])
test_K = signal.place_poles(A, B, P).gain_matrix[0]
#Getting output from file
K = optomization.for_the_gui(os, ts)
print(test_K, K)
for i in range(len(test_K)):
self.assertAlmostEqual(K[i], test_K[i], places=14)
def calculate_lateral_gains(sim, state, vehicle, desired_poles,
target_speed):
# Only calculate gains if the target_speed is updated.
# TODO: Replace this w/ an isclose(...) check
if state.target_speed == target_speed:
return
state.target_speed = target_speed
# Vehicle params
half_vehicle_len = vehicle.length / 2
vehicle_mass, vehicle_inertia_z = vehicle.chassis.mass_and_inertia
road_stiffness = sim.road_stiffness
# Linearization of lateral dynamics
if target_speed > 0:
state_matrix = np.array([
[0, target_speed, 0, target_speed],
[0, 0, 1, 0],
[
0,
0,
-(2 * road_stiffness * (half_vehicle_len**2)) /
(target_speed * vehicle_inertia_z),
0,
],
[
0,
0,
-1,
-2 * road_stiffness / (vehicle_mass * target_speed),
],
])
input_matrix = np.array([
[0],
[0],
[half_vehicle_len * road_stiffness / vehicle_inertia_z],
[road_stiffness / (vehicle_mass * target_speed)],
])
fsf1 = signal.place_poles(state_matrix,
input_matrix,
desired_poles,
method="KNV0")
# 0.01 and 0.015 denote the max and min gains for heading controller
# This is done to ensure that the linearization error will not affect
# the stability of the controller.
state.heading_error_gain = np.clip(fsf1.gain_matrix[0][1], 0.02,
0.04)
# 3.4 and 4.1 denote the max and min gains for lateral error controller
# As for heading, this is done to ensure that the linearization error
# will not affect the stability and performance of the controller.
state.lateral_error_gain = np.clip(fsf1.gain_matrix[0][0], 3.4,
4.1)
else:
# 0.01 and 0.36 are initial values for heading and lateral gains
# This is only done to ensure that the vehicle starts to move for
# the first time step where speed=0
state.heading_error_gain = 0.01
state.lateral_error_gain = 0.36
def plotfig(P, x_var, y_var):
for i in P:
name.append(i)
K = signal.place_poles(A,B,np.array(i)).gain_matrix
runrk4(K)
plt.plot(x_var, y_var)
global x_label
global y_label
if np.array_equal(x_var, t_arr):
x_label = "Time [s]"
elif np.array_equal(x_var, cart_pos):
x_label = "Position [m]"
elif np.array_equal(x_var, pend_ang):
x_label = "Angle [rads]"
elif np.array_equal(x_var, cart_vel):
x_label = "Velocity [m/s]"
elif np.array_equal(x_var, pend_vel):
x_label = "Angular Velocity [rads/s]"
if np.array_equal(y_var, t_arr):
y_label = "Time [s]"
elif np.array_equal(y_var, cart_pos):
y_label = "Position [m]"
elif np.array_equal(y_var, pend_ang):
y_label = "Angle [rads]"
elif np.array_equal(y_var, cart_vel):
y_label = "Velocity [m/s]"
elif np.array_equal(y_var, pend_vel):
y_label = "Angular Velocity [rads/s]"
def lqr_sys(A, B, C, D, in_q, r=1, num_q=6):
#parameter
sys = control.StateSpace(A, B, C, D)
Q = np.zeros((num_q, num_q))
R = [r]
for i in range(num_q):
Q[i, i] = in_q[i]
K, solu_R, Eig_sys = control.lqr(sys, Q, r)
kr = -1 / np.matmul(np.matmul(C, npl.inv((A - np.matmul(B, K)))), B)[0]
pole, eig_v = npl.eig(A - B * K)
pole_real = [a.real for a in pole]
minpol = -3 + min(pole_real)
repol = []
for i in range(num_q):
repol.append(minpol + i + 1)
repol = np.array(repol)
L = scis.place_poles(A.transpose(), C.transpose(), repol).gain_matrix
L = L.transpose()
upCA = np.column_stack(((A - np.matmul(B, K)), (np.matmul(B, K))))
downCA = np.column_stack((np.zeros((num_q, num_q)), (A - np.matmul(L, C))))
CA = np.row_stack((upCA, downCA))
CB = np.row_stack((B * kr, np.zeros((num_q, 1))))
CC = np.column_stack((C, np.zeros((1, num_q))))
CD = np.array([0])
return CA, CB, CC, CD
def _check(self, A, B, P, **kwargs):
"""
Perform the most common tests on the poles computed by place_poles
and return the Bunch object for further specific tests
"""
fsf = place_poles(A, B, P, **kwargs)
expected, _ = np.linalg.eig(A - np.dot(B, fsf.gain_matrix))
_assert_poles_close(expected,fsf.requested_poles)
_assert_poles_close(expected,fsf.computed_poles)
_assert_poles_close(P,fsf.requested_poles)
return fsf
def _check(self, A, B, P, **kwargs):
"""
Perform the most common tests on the poles computed by place_poles
and return the Bunch object for further specific tests
"""
fsf = place_poles(A, B, P, **kwargs)
expected, _ = np.linalg.eig(A - np.dot(B, fsf.gain_matrix))
_assert_poles_close(expected, fsf.requested_poles)
_assert_poles_close(expected, fsf.computed_poles)
_assert_poles_close(P, fsf.requested_poles)
return fsf
def invpend():
plt.close("all")
fig = plt.figure("Pendule Inverse")
ax = fig.add_subplot(1, 1, 1)
l = 1
M = 5
g = 9.81
m = 1
dt = 0.1
A = array([[0, 0, 1, 0],
[0, 0, 0, 1],
[0, m * g / M, 0, 0],
[0, (M + m) * g / (l * M), 0, 0]
])
B = array([[0],
[0],
[1 / M],
[1 / (l * M) ]
])
poles = [-2, -2.1, -2.2, -2.3]
K = (signal.place_poles(A, B, poles)).gain_matrix
E = array([[1, 0, 0, 0]])
C = array([[1, 0, 0, 0],
[0, 1, 0, 0]
])
At = A.T
Ct = C.T
# L = ((signal.place_poles(At, Ct, poles)).gain_matrix).T
h = -inv(E @ inv(A - B @ K) @ B)
x = array([[0],
[0.1],
[0],
[0]
])
xhat = array([[0],
[0],
[0],
[0]
])
Gx = eye(4)
Galpha = dt * 0.001 * eye(4)
Gbeta = (0.01**2) * eye(2)
for i in arange(0, 10, dt):
w = 2
u = -K @ xhat + h * w
y = C @ x + 0.01 * np.random.randn(2, 1)
# xhat = xhat + dt * (A @ xhat + B @ u - L @ (C @ xhat - y))
xhat, Gx = kalman(xhat, Gx, dt * B @ u, y, Galpha, Gbeta, eye(4) + dt * A, C)
x = x + dt * f(x, u, l=l, M=M, m=m)
draw(x, w, l=l)
def place_poles(A, B, poles):
"""
Wrapper of the scipy.signal.place_poles
Calculates the K matrix of a system with desired poles
Requires:
System object
Array (desired poles)
Returns:
Numpy array: K matrix with the placed poles
"""
K = spsg.place_poles(A, B, poles)
return K.gain_matrix
def compute_integrator_gains(n, p1, dp, dt=None):
''' Compute the feedback gains to get the specified poles of the closed-loop dynamics
n: order of the integrator
p1: the first pole
dp: the distance between consecutive poles
dt: time step, if system is discrete time, None otherwise
'''
des_poles = np.array([p1 + i * dp for i in range(n)])
if (dt is not None):
(H, A, B) = compute_integrator_dynamics_dt(matlib.zeros((1, n)), dt)
des_poles = np.exp(des_poles * dt)
else:
(H, A, B) = compute_integrator_dynamics(matlib.zeros((1, n)))
res = place_poles(A, B, des_poles)
des_gains = res.gain_matrix.squeeze()
return des_gains
def optimize_eigenvalues(e_start, e_stop, iterations, value=cart_pos):
start = time.time()
P_random = random_eigenvalues(e_start, e_stop, iterations)
S_list = []
for i in P_random:
K = signal.place_poles(A, B, np.array(i)).gain_matrix
runrk4(K)
S = 0
# print(i)
for j in value:
S += abs(j) * t_step
S_list.append(S)
# print(S)
# print(P_random[S_list.index(min(S_list))])
print("the best option is {:g}".format(S_list.index(min(S_list))))
print("the best eigenvalues are", P_random[S_list.index(min(S_list))])
print(time.time() - start)
return P_random[S_list.index(min(S_list))]
def optimize_eigenvalues(e_start,e_stop,iterations, variable=cart_pos):
# start=time.time()
P_random = random_eigenvalues(e_start,e_stop,iterations)
# print(time.time()-start)
S_list=[]
for i in P_random:
K = signal.place_poles(A,B,np.array(i)).gain_matrix
# start_append=time.time()
runrk4(K)
# print(time.time()-start_append)
S=np.sum(np.abs(variable)*t_step)
S_list.append(S)
# print(time.time()-start)
# print(P_random[S_list.index(min(S_list))])
# print("the best option is {:g}".format(S_list.index(min(S_list))))
print("the best eigenvalues are", P_random[S_list.index(min(S_list))])
# print("total time", time.time()-start)
return np.array(P_random[S_list.index(min(S_list))])
def plotfig(P):
for i in P:
name.append(i)
K = signal.place_poles(A, B, np.array(i)).gain_matrix
runrk4(K)
plt.subplot(2, 1, 1)
plt.xlabel("Time [s]", fontsize=14)
plt.ylabel("Position [m]", fontsize=14)
plt.plot(t_arr, cart_pos)
plt.ylim(0, 0.9) #Position
plt.xlim(0, 5)
plt.legend(name, loc="upper right")
plt.xlim(left=0)
plt.subplot(2, 1, 2)
plt.xlabel("Time [s]", fontsize=14)
plt.ylabel("Angle [rad]", fontsize=14)
plt.plot(t_arr, pend_ang)
plt.ylim(-0.15, 0.15) #Angle
plt.xlim(0, 5)
plt.legend(name, loc="upper right")
def for_the_gui(Mp, ts):
"""Calculates the system gains to place the poles at
the desired locations. This is the only function
that the GUI will need to call and give to the user.
The matrices are assumed to be for a motor with a
transfer function of a/(s(s+b))."""
a = 1
b = 2
"""The matrices are for state-space form Ax + Bu = x_dot, where A is the"
state coefficient matrix, and B is the input coefficient matrix. a and b are
arbitrary values for the motor dynamics"""
A = sp.array([[0, 1], [0, -b]])
B = sp.array([[0], [a]])
zeta, wn = sys_vals(Mp, ts)
poles = place_poles(zeta, wn)
"""The scipy function that creates
controller gains for the system"""
k = signal.place_poles(A, B, poles).gain_matrix[0]
return k
def pole_place_ctrl_gains(pars):
"""Calculate the gains for the pole placement controller """
# Assemble matrix 'A'
A = numpy.zeros((3, 3))
A[0, 2] = pars['vOrb']
A[1, 2] = -pars['gOrb']
A[2, 0] = -3. * pars['gOrb'] / pars['rOrb']
A[2, 1] = 2. / pars['rOrb'] + pars['gOrb'] / (pars['vOrb']**2)
# Assemble matrix 'B'
B = numpy.zeros((3, 2))
B[1, 0] = pars['gOrb']
B[2, 1] = pars['gOrb']
P = pars['poles']
fsf = place_poles(A, B, P)
return fsf.gain_matrix
def pole_place(x,z,u,d,v,xdot,gn,y_hat_k,Pk_1,Qk,Rk,xss,dis,zss,uss,delta):
nx=int(x.dim()[0])
nz=int(z.dim()[0])
nu=int(u.dim()[0])
nv=int(v.dim()[0])
A1=Function('A1',[x,z,u,d],[jacobian(vertcat(xdot,v),vertcat(x,v))])
B1=Function('B1',[x,z,u,d],[jacobian(vertcat(xdot,v),z)])
C1=Function('C1',[x,z,u,d],[jacobian(gn,vertcat(x,v))])
D1=Function('D1',[x,z,u,d],[jacobian(gn,z)])
Alow1=[core.mtimes(-NP.linalg.inv(D1(xss,zss,uss,4)),C1(xss,zss,uss,4),A1(xss,zss,uss,4))]
Alow2=[core.mtimes(-NP.linalg.inv(D1(xss,zss,uss,4)),C1(xss,zss,uss,4),B1(xss,zss,uss,4))]
Aaug=vertcat(horzcat(A1(xss,zss,uss,4), B1(xss,zss,uss,4)),horzcat(Alow1[0], Alow2[0]))
theta=linalg.expm(Aaug.full()*delta)
gamma=vertcat(NP.eye(nx+nv),core.mtimes(linalg.inv(D1(xss,zss,uss,4)),C1(xss,zss,uss,4))).full()
Hk=NP.array([(0, 0, 1, 0, 1 ,0 ,0, 0, 0, 0),
(0, 0, 0, 1, 0, 0, 0 ,0, 0, 0),
(0, 0, 0, 0, 0, 1, 0 ,1, 0, 0)])
Pk_k_1=core.mtimes(theta,Pk_1,theta.T)+core.mtimes(gamma,Qk,gamma.T)
Sk=core.mtimes(Hk,Pk_k_1,Hk.T)+Rk
Kk=core.mtimes(Pk_k_1,Hk.T,NP.linalg.inv(Sk))
aug_k=vertcat(xss,dis,zss)+core.mtimes(Kk,y_hat_k)
Pk=core.mtimes(NP.eye(nx+nz+nv)-core.mtimes(Kk,Hk),Pk_k_1)
xss=aug_k[0:nx].full().flatten().tolist()
dis=aug_k[nx:nx+nv].full().flatten()
zss=aug_k[nx+nv:].full().flatten().tolist()
print(NP.linalg.eigvals(theta-core.mtimes(Kk,Hk)))
print(theta)
Kl=signal.place_poles(theta.T,Hk.T,NP.array([0.1, 0.3, 0.15, 0.22, 0.3, 0.36, 0.45, 0.5, 0.6, 0.7]))
return Kl
def place_dynamics_poles(
A: np.ndarray,
B: np.ndarray,
abs_low: float = 0.0,
abs_high: float = 1.0,
rng: RNG = None,
):
"""Compute a solution that re-places the eigenvalues of linear dynamics."""
# pylint:disable=invalid-name
poles = sample_eigvals(A.shape[-1], abs_low, abs_high, size=(), rng=rng)
with warnings.catch_warnings():
warnings.filterwarnings(
"ignore",
"Convergence was not reached after maxiter iterations.*",
UserWarning,
module="scipy.signal",
)
for exp in itertools.count():
result = place_poles(A, B, poles, maxiter=2**exp)
abs_poles = np.abs(result.computed_poles)
if np.all(np.logical_and(abs_low < abs_poles,
abs_poles < abs_high)):
break
return result
def compute(R, L, Km, b, J, P):
print os.getcwd()
A = numpy.matrix([[-b / J, Km / J], [-Km / L, -R / L]])
B = numpy.matrix([[0, 1 / J], [1 / L, 0]])
C = numpy.matrix([1.0, 0])
D = numpy.matrix([0.0, 0.0])
Aa = numpy.concatenate((numpy.column_stack((A, numpy.zeros(
(2, 1)))), numpy.column_stack((-C, 0))))
Ba = numpy.concatenate((B, numpy.zeros((1, 2))))
K = signal.place_poles(Aa, Ba, P)
K = K.gain_matrix
Acont = Aa - Ba * K
Bcont = numpy.array([0, 0, 1.0])
Bcont.shape = (3, 1)
Ccont = [1.0, 0, 0]
dcm_cont = ss(Acont, Bcont, Ccont, 0)
T, yout = control.step_response(dcm_cont, T=None)
fig = Figure()
FigureCanvas(fig)
ax = fig.add_subplot(1, 1, 1)
ax.plot(T.T, yout.T)
#if not os.path.isdir('static'):
#os.mkdir('static')
#else:
for filename in glob.glob(
os.path.join('C:\Users\Alessandra\Desktop\dcmotor\dcmotor\static',
'*.png')):
os.remove(filename)
plotfile = os.path.join(
'C:\Users\Alessandra\Desktop\dcmotor\dcmotor\static',
str(time.time()) + '.png')
fig.savefig(plotfile)
return plotfile
def test_errors(self):
# Test input mistakes from user
A = np.array([0, 7, 0, 0, 0, 0, 0, 7 / 3., 0, 0, 0, 0, 0, 0, 0,
0]).reshape(4, 4)
B = np.array([0, 0, 0, 0, 1, 0, 0, 1]).reshape(4, 2)
#should fail as the method keyword is invalid
assert_raises(ValueError,
place_poles,
A,
B, (-2.1, -2.2, -2.3, -2.4),
method="foo")
#should fail as poles are not 1D array
assert_raises(ValueError, place_poles, A, B,
np.array((-2.1, -2.2, -2.3, -2.4)).reshape(4, 1))
#should fail as A is not a 2D array
assert_raises(ValueError, place_poles, A[:, :, np.newaxis], B,
(-2.1, -2.2, -2.3, -2.4))
#should fail as B is not a 2D array
assert_raises(ValueError, place_poles, A, B[:, :, np.newaxis],
(-2.1, -2.2, -2.3, -2.4))
#should fail as there are too many poles
assert_raises(ValueError, place_poles, A, B,
(-2.1, -2.2, -2.3, -2.4, -3))
#should fail as there are not enough poles
assert_raises(ValueError, place_poles, A, B, (-2.1, -2.2, -2.3))
#should fail as the rtol is greater than 1
assert_raises(ValueError,
place_poles,
A,
B, (-2.1, -2.2, -2.3, -2.4),
rtol=42)
#should fail as maxiter is smaller than 1
assert_raises(ValueError,
place_poles,
A,
B, (-2.1, -2.2, -2.3, -2.4),
maxiter=-42)
# should fail as rank(B) is two
assert_raises(ValueError, place_poles, A, B, (-2, -2, -2, -2))
#unctrollable system
assert_raises(ValueError, place_poles, np.ones((4, 4)), np.ones(
(4, 2)), (1, 2, 3, 4))
# Should not raise ValueError as the poles can be placed but should
# raise a warning as the convergence is not reached
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter("always")
fsf = place_poles(A, B, (-1, -2, -3, -4), rtol=1e-16, maxiter=42)
assert_(len(w) == 1)
assert_(issubclass(w[-1].category, UserWarning))
assert_("Convergence was not reached after maxiter iterations" in
str(w[-1].message))
assert_equal(fsf.nb_iter, 42)
# should fail as a complex misses its conjugate
assert_raises(ValueError, place_poles, A, B,
(-2 + 1j, -2 - 1j, -2 + 3j, -2))
# should fail as A is not square
assert_raises(ValueError, place_poles, A[:, :3], B, (-2, -3, -4, -5))
# should fail as B has not the same number of lines as A
assert_raises(ValueError, place_poles, A, B[:3, :], (-2, -3, -4, -5))
# should fail as KNV0 does not support complex poles
assert_raises(ValueError,
place_poles,
A,
B, (-2 + 1j, -2 - 1j, -2 + 3j, -2 - 3j),
method="KNV0")
# poles_req = np.array([-0.01+0.1j, 0.04, -0.01-0.1j, -0.01+0.1j,\
# -0.01-0.1j, -0.01+0.1j, -0.01-0.1j, -0.01+0.1j, -0.01-0.1j])
# poles_req = np.array([-0.03+0.1j, 0.115, -0.03-0.1j, -0.01+0.1j,\
# -0.008-0.1j, -0.008+0.1j, -0.01-0.1j, -0.01+0.1j, -0.01-0.1j])
# poles_req = np.array([-0.01+0.1j, 0.03, -0.01-0.1j, -0.01+0.115j,\
# -0.01-0.115j, -0.01+0.1j, -0.01-0.1j, -0.008+0.1j, -0.008-0.1j])
# best so far
poles_req = np.array([
-0.0085 + 0.01j, -0.0085 - 0.01j, -0.0085 - 0.01j, -0.0085 + 0.01j,
-0.0085 - 0.01j, -0.0085 + 0.01j, -0.0085 + 0.01j, 0.025,
-0.0085 - 0.01j
])
fsf = signal.place_poles(A, B, A_eigenvals, rtol=1e-4, maxiter=50)
K = fsf.gain_matrix
new_eigenvals, new_V = linalg.eig((A - np.matmul(B, K)))
# x_0 = np.zeros(A.shape[0])
# 0.01745 is degree in radians. this is psi here
# x_0 = np.array([0., 0., 0., 0., 0., 0., 0., 0., 0.01745])
x_0 = np.array([0., 0., 0., 0., 0., 0., 0.01745, 0.01745, 0.01745])
# x_0 = np.array([0., 0., 0., 0., 0., 0., 0., 0.01745, 0.])
newA = A - np.matmul(B, K)
newB = np.matmul(B, K)
newC = np.identity(newA.shape[0])
newD = np.zeros(newB.shape)
def test_complex(self):
# Test complex pole placement on a linearized car model, taken from L.
# Jaulin, Automatique pour la robotique, Cours et Exercices, iSTE
# editions p 184/185
A = np.array([0,7,0,0,0,0,0,7/3.,0,0,0,0,0,0,0,0]).reshape(4,4)
B = np.array([0,0,0,0,1,0,0,1]).reshape(4,2)
# Test complex poles on YT
P = np.array([-3, -1, -2-1j, -2+1j])
self._check(A, B, P)
# Try to reach the specific case in _YT_complex where two singular
# values are almost equal. This is to improve code coverage but I
# have no way to be sure this code is really reached
P = [0-1e-6j,0+1e-6j,-10,10]
self._check(A, B, P, maxiter=1000)
# Try to reach the specific case in _YT_complex where the rank two
# update yields two null vectors. This test was found via Monte Carlo.
A = np.array(
[-2148,-2902, -2267, -598, -1722, -1829, -165, -283, -2546,
-167, -754, -2285, -543, -1700, -584, -2978, -925, -1300,
-1583, -984, -386, -2650, -764, -897, -517, -1598, 2, -1709,
-291, -338, -153, -1804, -1106, -1168, -867, -2297]
).reshape(6,6)
B = np.array(
[-108, -374, -524, -1285, -1232, -161, -1204, -672, -637,
-15, -483, -23, -931, -780, -1245, -1129, -1290, -1502,
-952, -1374, -62, -964, -930, -939, -792, -756, -1437,
-491, -1543, -686]
).reshape(6,5)
P = [-25.-29.j, -25.+29.j, 31.-42.j, 31.+42.j, 33.-41.j, 33.+41.j]
self._check(A, B, P)
# Use a lot of poles to go through all cases for update_order
# in _YT_loop
big_A = np.ones((11,11))-np.eye(11)
big_B = np.ones((11,10))-np.diag([1]*10,1)[:,1:]
big_A[:6,:6] = A
big_B[:6,:5] = B
P = [-10,-20,-30,40,50,60,70,-20-5j,-20+5j,5+3j,5-3j]
self._check(big_A, big_B, P)
#check with only complex poles and only real poles
P = [-10,-20,-30,-40,-50,-60,-70,-80,-90,-100]
self._check(big_A[:-1,:-1], big_B[:-1,:-1], P)
P = [-10+10j,-20+20j,-30+30j,-40+40j,-50+50j,
-10-10j,-20-20j,-30-30j,-40-40j,-50-50j]
self._check(big_A[:-1,:-1], big_B[:-1,:-1], P)
# need a 5x5 array to ensure YT handles properly when there
# is only one real pole and several complex
A = np.array([0,7,0,0,0,0,0,7/3.,0,0,0,0,0,0,0,0,
0,0,0,5,0,0,0,0,9]).reshape(5,5)
B = np.array([0,0,0,0,1,0,0,1,2,3]).reshape(5,2)
P = np.array([-2, -3+1j, -3-1j, -1+1j, -1-1j])
place_poles(A, B, P)
# same test with an odd number of real poles > 1
# this is another specific case of YT
P = np.array([-2, -3, -4, -1+1j, -1-1j])
self._check(A, B, P)
m = 15.0
Ac = np.asarray([[0, 1],
[-k/m, -c/m]])
Bc = np.asarray([[0],
[1/m]])
Cc = np.asarray([[1, 0]])
Dc = np.asarray([0])
freq = 200
dt = 1. / freq
F, B, _, _, _ = signal.cont2discrete((Ac, Bc, Cc, Dc), dt)
H = np.asarray([[1, 0],
[0, 0]])
K = signal.place_poles(Ac, Bc, [-2.0, -3.0]).gain_matrix
Q = np.asarray([[0.01, 0.01],
[0.01, 0.01]])
R = np.asarray([[0.05, 0.05],
[0.05, 0.05]])
def run_controller_no_observer(y, prev_y):
x_hat = np.copy(y)
x_hat[1] = (y[0] - prev_y[0]) / dt
u = np.dot(-K, x_hat)
return u
prev_x_hat = [[10], [0]]
prev_u = 0
[0 ],
[2*Ca*lf/Iz]])
C = np.eye(4)
D = np.zeros((4,1))
# AB = np.matmul(A,B)
# A2B = np.matmul(A,AB)
# A3B = np.matmul(A,A2B)
# P = np.concatenate((B,AB),axis=1)
# P = np.concatenate((P,A2B),axis=1)
# P = np.concatenate((P,A3B),axis=1)
poles1 = np.array([-1, -2, -12, -3])
poles2 = np.array([-1, -2, -12, -3])/10
# poles = np.array([-1, -2, -4, -5])
K1 = signal.place_poles(A, B, poles1, method='YT')
K2 = signal.place_poles(A, B, poles2, method='YT')
print(K1.gain_matrix)
print(K2.gain_matrix)
# for i in range(0,4):
# print(K.computed_poles[i])
# K = control.place(A, B, poles)
# print(K.gain_matrix)
# print(np.shape(K))
# test_array = np.array([0,0,0,0])
Q = C.T.dot(C)
R = np.eye(n2)
X = np.matrix(lin.solve_continuous_are(A, B, Q, R))
K = -np.matrix(lin.inv(R)*(B.T*X)).getA()
# observability
ob = obsv(A, C)
print "observability =", ob
# controllability
cb = ctrb(A, B)
print "controllability =", cb
# get the observer gain, using pole placement
p = np.array([-13, -12, -11, -10])
fsf1 = sgn.place_poles(A.T, C.T, p)
L = fsf1.gain_matrix.T
x = x0; xhat = 0*x0
h = 0.01; ts = 1000;
X = np.array([[],[],[],[]])
Y = np.array([[],[]])
Xhat = X; t = []
for k in range(ts):
t.append(k*h)
u = K.dot(xhat)
y = C.dot(x)
yhat = C.dot(xhat)
X = np.hstack((X, x))
Xhat = np.hstack((Xhat, xhat))
def place(A, B, poles):
return place_poles(A, B, poles).gain_matrix
poles_open_loop = np.array([k for k in A.eigenvals().keys()], dtype=complex)
print(f"open-loop poles:\n{poles_open_loop}")
# ------------------------------------------------------------------------------
# q1.c
print("\n#", "-" * 50)
print("q1 c) state feedback loop")
A = np.asarray(A).astype(np.float64)
print(f"A:\n{A}")
B = np.asarray(B.subs({J1: _J1, ki: _ki})).astype(np.float64)
print(f"B:\n{B}")
poles = [-2, -1, -1 + 1j, -1 - 1j]
fsf = signal.place_poles(A, B, poles, method='YT')
K = fsf.gain_matrix
print(f"K:\n{K}")
# visualizing poles
# t = np.linspace(0, 2*np.pi, 401)
# plt.plot(np.cos(t), np.sin(t), 'k--') # unit circle
# plt.plot(poles_open_loop.real, poles_open_loop.imag, 'rx',
# label='open_loop eig')
# plt.plot(fsf.computed_poles.real, fsf.computed_poles.imag, 'bx',
# label='closed_loop eig')
# plt.axis([-2.1, 1.1, -1.1, 1.1])
# plt.grid()
# plt.legend()
# plt.show()
# plt.close()
def test_complex(self):
# Test complex pole placement on a linearized car model, taken from L.
# Jaulin, Automatique pour la robotique, Cours et Exercices, iSTE
# editions p 184/185
A = np.array([0, 7, 0, 0, 0, 0, 0, 7 / 3., 0, 0, 0, 0, 0, 0, 0,
0]).reshape(4, 4)
B = np.array([0, 0, 0, 0, 1, 0, 0, 1]).reshape(4, 2)
# Test complex poles on YT
P = np.array([-3, -1, -2 - 1j, -2 + 1j])
self._check(A, B, P)
# Try to reach the specific case in _YT_complex where two singular
# values are almost equal. This is to improve code coverage but I
# have no way to be sure this code is really reached
P = [0 - 1e-6j, 0 + 1e-6j, -10, 10]
self._check(A, B, P, maxiter=1000)
# Try to reach the specific case in _YT_complex where the rank two
# update yields two null vectors. This test was found via Monte Carlo.
A = np.array([
-2148, -2902, -2267, -598, -1722, -1829, -165, -283, -2546, -167,
-754, -2285, -543, -1700, -584, -2978, -925, -1300, -1583, -984,
-386, -2650, -764, -897, -517, -1598, 2, -1709, -291, -338, -153,
-1804, -1106, -1168, -867, -2297
]).reshape(6, 6)
B = np.array([
-108, -374, -524, -1285, -1232, -161, -1204, -672, -637, -15, -483,
-23, -931, -780, -1245, -1129, -1290, -1502, -952, -1374, -62,
-964, -930, -939, -792, -756, -1437, -491, -1543, -686
]).reshape(6, 5)
P = [
-25. - 29.j, -25. + 29.j, 31. - 42.j, 31. + 42.j, 33. - 41.j,
33. + 41.j
]
self._check(A, B, P)
# Use a lot of poles to go through all cases for update_order
# in _YT_loop
big_A = np.ones((11, 11)) - np.eye(11)
big_B = np.ones((11, 10)) - np.diag([1] * 10, 1)[:, 1:]
big_A[:6, :6] = A
big_B[:6, :5] = B
P = [-10, -20, -30, 40, 50, 60, 70, -20 - 5j, -20 + 5j, 5 + 3j, 5 - 3j]
self._check(big_A, big_B, P)
#check with only complex poles and only real poles
P = [-10, -20, -30, -40, -50, -60, -70, -80, -90, -100]
self._check(big_A[:-1, :-1], big_B[:-1, :-1], P)
P = [
-10 + 10j, -20 + 20j, -30 + 30j, -40 + 40j, -50 + 50j, -10 - 10j,
-20 - 20j, -30 - 30j, -40 - 40j, -50 - 50j
]
self._check(big_A[:-1, :-1], big_B[:-1, :-1], P)
# need a 5x5 array to ensure YT handles properly when there
# is only one real pole and several complex
A = np.array([
0, 7, 0, 0, 0, 0, 0, 7 / 3., 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5, 0,
0, 0, 0, 9
]).reshape(5, 5)
B = np.array([0, 0, 0, 0, 1, 0, 0, 1, 2, 3]).reshape(5, 2)
P = np.array([-2, -3 + 1j, -3 - 1j, -1 + 1j, -1 - 1j])
place_poles(A, B, P)
# same test with an odd number of real poles > 1
# this is another specific case of YT
P = np.array([-2, -3, -4, -1 + 1j, -1 - 1j])
self._check(A, B, P)
from scipy import signal
import matplotlib.pyplot as plt
A = np.array([[1.380, -0.2077, 6.715, -5.676], [-0.5814, -4.290, 0, 0.6750],
[1.067, 4.273, -6.654, 5.893], [0.0480, 4.273, 1.343, -2.104]])
B = np.array([[0, 5.679], [1.136, 1.136], [
0,
0,
], [-3.146, 0]])
P = np.array([-0.2, -0.5, -5.0566, -8.6659])
# Now compute K with KNV method 0, with the default YT method and with the YT
# method while forcing 100 iterations of the algorithm and print some results
# after each call.
fsf1 = signal.place_poles(A, B, P, method='KNV0')
fsf1.gain_matrix
# array([[ 0.20071427, -0.96665799, 0.24066128, -0.10279785],
# [ 0.50587268, 0.57779091, 0.51795763, -0.41991442]])
fsf2 = signal.place_poles(A, B, P) # uses YT method
fsf2.computed_poles
# array([-8.6659, -5.0566, -0.5 , -0.2 ])
fsf3 = signal.place_poles(A, B, P, rtol=-1, maxiter=100)
fsf3.X
# array([[ 0.52072442+0.j, -0.08409372+0.j, -0.56847937+0.j, 0.74823657+0.j],
# [-0.04977751+0.j, -0.80872954+0.j, 0.13566234+0.j, -0.29322906+0.j],
# [-0.82266932+0.j, -0.19168026+0.j, -0.56348322+0.j, -0.43815060+0.j],
# [ 0.22267347+0.j, 0.54967577+0.j, -0.58387806+0.j, -0.40271926+0.j]])
def place(A, B, P):
if size(nonzero(imag(P))) != 0:
fsf = place_poles(A, B, P)
else:
fsf = place_poles(A, B, P, method='KNV0')
return fsf.gain_matrix
def place(A, B, p):
"""Place closed loop eigenvalues
K = place(A, B, p)
Parameters
----------
A : 2D array_like
Dynamics matrix
B : 2D array_like
Input matrix
p : 1D array_like
Desired eigenvalue locations
Returns
-------
K : 2D array (or matrix)
Gain such that A - B K has eigenvalues given in p
Notes
-----
Algorithm
This is a wrapper function for :func:`scipy.signal.place_poles`, which
implements the T**s and Yang algorithm [1]_. It will handle SISO,
MISO, and MIMO systems. If you want more control over the algorithm,
use :func:`scipy.signal.place_poles` directly.
Limitations
The algorithm will not place poles at the same location more
than rank(B) times.
The return type for 2D arrays depends on the default class set for
state space operations. See :func:`~control.use_numpy_matrix`.
References
----------
.. [1] A.L. T**s and Y. Yang, "Globally convergent algorithms for robust
pole assignment by state feedback, IEEE Transactions on Automatic
Control, Vol. 41, pp. 1432-1452, 1996.
Examples
--------
>>> A = [[-1, -1], [0, 1]]
>>> B = [[0], [1]]
>>> K = place(A, B, [-2, -5])
See Also
--------
place_varga, acker
Notes
-----
The return type for 2D arrays depends on the default class set for
state space operations. See :func:`~control.use_numpy_matrix`.
"""
from scipy.signal import place_poles
# Convert the system inputs to NumPy arrays
A_mat = np.array(A)
B_mat = np.array(B)
if (A_mat.shape[0] != A_mat.shape[1]):
raise ControlDimension("A must be a square matrix")
if (A_mat.shape[0] != B_mat.shape[0]):
err_str = "The number of rows of A must equal the number of rows in B"
raise ControlDimension(err_str)
# Convert desired poles to numpy array
placed_eigs = np.atleast_1d(np.squeeze(np.asarray(p)))
result = place_poles(A_mat, B_mat, placed_eigs, method='YT')
K = result.gain_matrix
return _ssmatrix(K)
def test_errors(self):
# Test input mistakes from user
A = np.array([0,7,0,0,0,0,0,7/3.,0,0,0,0,0,0,0,0]).reshape(4,4)
B = np.array([0,0,0,0,1,0,0,1]).reshape(4,2)
#should fail as the method keyword is invalid
assert_raises(ValueError, place_poles, A, B, (-2.1,-2.2,-2.3,-2.4),
method="foo")
#should fail as poles are not 1D array
assert_raises(ValueError, place_poles, A, B,
np.array((-2.1,-2.2,-2.3,-2.4)).reshape(4,1))
#should fail as A is not a 2D array
assert_raises(ValueError, place_poles, A[:,:,np.newaxis], B,
(-2.1,-2.2,-2.3,-2.4))
#should fail as B is not a 2D array
assert_raises(ValueError, place_poles, A, B[:,:,np.newaxis],
(-2.1,-2.2,-2.3,-2.4))
#should fail as there are too many poles
assert_raises(ValueError, place_poles, A, B, (-2.1,-2.2,-2.3,-2.4,-3))
#should fail as there are not enough poles
assert_raises(ValueError, place_poles, A, B, (-2.1,-2.2,-2.3))
#should fail as the rtol is greater than 1
assert_raises(ValueError, place_poles, A, B, (-2.1,-2.2,-2.3,-2.4),
rtol=42)
#should fail as maxiter is smaller than 1
assert_raises(ValueError, place_poles, A, B, (-2.1,-2.2,-2.3,-2.4),
maxiter=-42)
# should fail as rank(B) is two
assert_raises(ValueError, place_poles, A, B, (-2,-2,-2,-2))
#unctrollable system
assert_raises(ValueError, place_poles, np.ones((4,4)),
np.ones((4,2)), (1,2,3,4))
# Should not raise ValueError as the poles can be placed but should
# raise a warning as the convergence is not reached
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter("always")
fsf = place_poles(A, B, (-1,-2,-3,-4), rtol=1e-16, maxiter=42)
assert_(len(w) == 1)
assert_(issubclass(w[-1].category, UserWarning))
assert_("Convergence was not reached after maxiter iterations"
in str(w[-1].message))
assert_equal(fsf.nb_iter, 42)
# should fail as a complex misses its conjugate
assert_raises(ValueError, place_poles, A, B, (-2+1j,-2-1j,-2+3j,-2))
# should fail as A is not square
assert_raises(ValueError, place_poles, A[:,:3], B, (-2,-3,-4,-5))
# should fail as B has not the same number of lines as A
assert_raises(ValueError, place_poles, A, B[:3,:], (-2,-3,-4,-5))
# should fail as KNV0 does not support complex poles
assert_raises(ValueError, place_poles, A, B,
(-2+1j,-2-1j,-2+3j,-2-3j), method="KNV0")
def design_tsob(sys_c_ol,
Ca,
sampling_interval,
desired_settling_time,
desired_observer_settling_time=None,
spoles=None,
sopoles=None,
sapoles=None,
disp=True):
""" Design a digital full order observer tracking system with the desired settling time.
Args:
sys_c_ol (StateSpace): The continouous plant model
sampling_interval: The sampling interval for the digital control system in seconds.
desired_settling_time: The desired settling time in seconds
desired_observer_settling_time (optional): The desired observer settling time
in seconds. If not provided the observer settling time will be 4 times faster
than the overall settling time. Default is None.
spoles (optional): The desired closed loop poles. If not supplied, then optimal
poles will try to be used. Default is None.
sopoles (optional): The desired observer poles. If not supplied, then optimal
poles will try to be used. Default is None.
sapoles (optional): The poles of the reference input and disturbance vectors.
If not supplied the reference input is assumed to be a step. Default is None.
disp: Print debugging output. Default is True.
Returns:
tuple: (sys_d_ol, phia, gammaa, L1, L2, K) Where sys_d_ol is the discrete plant,
phia is the discrete additional dynamics A matrix, gammaa is the discrete
additional dynamics B matrix, L1 is the plant gain matrix, L2 is the
additional gain matrix, and K is the observer gain matrix.
"""
if disp:
print("Designing a tracking system with full order observer.")
# Make sure the system is in fact continuous and not discrete
if sys_c_ol.dt != None:
print("Error: Function expects continuous plant")
return None
A = sys_c_ol.A
B = sys_c_ol.B
C = sys_c_ol.C
D = sys_c_ol.D
num_states = A.shape[0]
num_inputs = B.shape[1]
num_outputs = C.shape[0]
num_tracked = Ca.shape[0]
# Convert to discrete system using zero order hold method
sys_d_ol = sys_c_ol.to_discrete(sampling_interval, method="zoh")
phi = sys_d_ol.A
gamma = sys_d_ol.B
# Check controlability of the discrete system
controllability_mat = control.ctrb(phi, gamma)
rank = LA.matrix_rank(controllability_mat)
if rank != num_states:
print(rank, num_states)
print("Error: System is not controlable")
return None
# Check observability of the discrete system
observability_mat = control.obsv(phi, C)
rank = LA.matrix_rank(observability_mat)
if rank != num_states:
print("Error: System is not observable")
return None
# Create the design model with additional dynamics
if sapoles is None:
# assume tracking a step input (s=0, z=1)
sapoles = [0]
zapoles = [
-p for p in np.poly(
control_poles.spoles_to_zpoles(sapoles, sampling_interval))
]
zapoles = np.delete(zapoles, 0) # the first coefficient isn't important
gammaa = np.transpose(np.matrix(zapoles))
q = gammaa.shape[0]
phia_left = np.matrix(gammaa)
phia_right = np.concatenate((np.eye(q - 1), np.zeros((1, q - 1))), axis=0)
phia = np.concatenate((phia_left, phia_right), axis=1)
if num_tracked > 1:
# replicate the additional dynamics
phia = np.kron(np.eye(num_tracked), phia)
gammaa = np.kron(np.eye(num_tracked), gammaa)
# Form the design matrix
phid_top_row = np.concatenate((phi, np.zeros(
(num_states, q * num_tracked))),
axis=1)
phid_bot_row = np.concatenate((gammaa * Ca, phia), axis=1)
phid = np.concatenate((phid_top_row, phid_bot_row), axis=0)
gammad = np.concatenate((gamma, np.zeros((gammaa.shape[0], num_tracked))),
axis=0)
# Choose poles if none were given
if spoles is None:
spoles = find_candadate_spoles(sys_c_ol, desired_settling_time, disp)
num_spoles_left = num_states - len(spoles)
if num_spoles_left > 0:
# Use normalized bessel poles for the rest
spoles.extend(
control_poles.bessel_spoles(num_spoles_left,
desired_settling_time))
zpoles = control_poles.spoles_to_zpoles(spoles, sampling_interval)
if disp:
print("spoles = ", spoles)
print("zpoles = ", zpoles)
# place the poles such that eig(phi - gamma*L) are inside the unit circle
full_state_feedback = signal.place_poles(phid, gammad, zpoles)
# Check the poles for stability just in case
for zpole in full_state_feedback.computed_poles:
if abs(zpole) >= 1:
print("Computed pole is not stable", zpole)
return None
L = full_state_feedback.gain_matrix
L1 = L[:, 0:num_states]
L2 = L[:, num_states:]
# Choose poles if none were given
if sopoles is None:
sopoles = []
if desired_observer_settling_time == None:
desired_observer_settling_time = desired_settling_time / 4
# TODO: Find existing poles based on the rules. For now just use bessel
num_sopoles_left = num_states - len(sopoles)
if num_sopoles_left > 0:
# Use normalized bessel poles for the rest
sopoles.extend(
control_poles.bessel_spoles(num_sopoles_left,
desired_observer_settling_time))
zopoles = control_poles.spoles_to_zpoles(sopoles, sampling_interval)
if disp:
print("sopoles = ", sopoles)
print("zopoles = ", zopoles)
# Find K such that eig(phi - KC) are inside the unit circle
full_state_feedback = signal.place_poles(np.transpose(phi),
np.transpose(C), zopoles)
# Check the poles for stability just in case
for zopole in full_state_feedback.computed_poles:
if abs(zopole) > 1:
print("Computed observer pole is not stable", zopole)
return None
K = np.transpose(full_state_feedback.gain_matrix)
return (sys_d_ol, phia, gammaa, np.matrix(L1), np.matrix(L2), np.matrix(K))
def design_regsf(sys_c_ol,
sampling_interval,
desired_settling_time,
spoles=None,
disp=True):
""" Design a digital full state feedback regulator with the desired settling time.
Args:
sys_c_ol (StateSpace): The continouous plant model
sampling_interval: The sampling interval for the digital control system in seconds.
desired_settling_time: The desired settling time in seconds
spoles (optional): The desired closed loop poles. If not supplied, then optimal
poles will try to be used. Default is None.
Returns:
tuple: (sys_d_ol, L) Where sys_d_ol is the discrete plant and L is the stablizing
gain matrix.
"""
# Make sure the system is in fact continuous and not discrete
if sys_c_ol.dt != None:
print("Error: Function expects continuous plant")
return None
A = sys_c_ol.A
B = sys_c_ol.B
C = sys_c_ol.C
D = sys_c_ol.D
num_states = A.shape[0]
num_inputs = B.shape[1]
num_outputs = C.shape[0]
# Convert to discrete system using zero order hold method
sys_d_ol = sys_c_ol.to_discrete(sampling_interval, method="zoh")
phi = sys_d_ol.A
gamma = sys_d_ol.B
# Check controlability of the discrete system
controllability_mat = control.ctrb(phi, gamma)
rank = LA.matrix_rank(controllability_mat)
if rank != num_states:
print("Error: System is not controlable")
return None
# Check observability of the discrete system
observability_mat = control.obsv(phi, C)
rank = LA.matrix_rank(observability_mat)
if rank != num_states:
print("Error: System is not observable")
return None
# Choose poles if none were given
if spoles is None:
spoles = find_candadate_spoles(sys_c_ol, desired_settling_time, disp)
num_spoles_left = num_states - len(spoles)
if num_spoles_left > 0:
# Use normalized bessel poles for the rest
spoles.extend(
control_poles.bessel_spoles(num_spoles_left,
desired_settling_time))
zpoles = control_poles.spoles_to_zpoles(spoles, sampling_interval)
if disp:
print("spoles = ", spoles)
print("zpoles = ", zpoles)
# place the poles such that ...
full_state_feedback = signal.place_poles(phi, gamma, zpoles)
# Check the poles for stability
for zpole in full_state_feedback.computed_poles:
if abs(zpole) > 1:
print("Computed pole is not stable")
return None
L = full_state_feedback.gain_matrix
return (sys_d_ol, np.matrix(L))
K1 = cnt.acker(A1, B1, des_poles)
K = np.matrix([K1.item(0), K1.item(1)])
ki = K1.item(2)
# ----------------------------------------- OBSERVER GAINS -----------------------------------------
# compute longitudinal observer gains
des_obs_char_poly = [1, 2 * zeta_h * wn_h_obs, wn_h_obs**2]
des_obs_poles = np.roots(des_obs_char_poly)
# Compute the gains if the system is observable
if np.linalg.matrix_rank(cnt.ctrb(A.T, C.T)) != 2:
print("The longitudinal observer system is not observable")
else:
# place_poles returns an object with various properties. The gains are accessed through .gain_matrix
# .T transposes the matrix
L = signal.place_poles(A.T, C.T, des_obs_poles).gain_matrix.T
print('----- Longitudinal -----')
print('K1', K1)
print('K: ', K)
print('ki: ', ki)
print('L^T: ', L.T)
# computer observer gains
# Augmented Matrices
A2 = np.concatenate((np.concatenate((A, B), axis=1), np.zeros((1, 3))), axis=0)
C2 = np.concatenate((C, np.zeros((1, 1))), axis=1)
des_obs_char_poly = np.convolve([1, 2 * zeta_h * wn_h_obs, wn_h_obs**2],
np.poly([dist_obsv_pole]))
des_obs_poles = np.roots(des_obs_char_poly)
def place(A, B, p):
"""Place closed loop eigenvalues
K = place(A, B, p)
Parameters
----------
A : 2-d array
Dynamics matrix
B : 2-d array
Input matrix
p : 1-d list
Desired eigenvalue locations
Returns
-------
K : 2-d array
Gain such that A - B K has eigenvalues given in p
Algorithm
---------
This is a wrapper function for scipy.signal.place_poles, which
implements the T**s and Yang algorithm [1]. It will handle SISO,
MISO, and MIMO systems. If you want more control over the algorithm,
use scipy.signal.place_poles directly.
[1] A.L. T**s and Y. Yang, "Globally convergent algorithms for robust
pole assignment by state feedback, IEEE Transactions on Automatic
Control, Vol. 41, pp. 1432-1452, 1996.
Limitations
-----------
The algorithm will not place poles at the same location more
than rank(B) times.
Examples
--------
>>> A = [[-1, -1], [0, 1]]
>>> B = [[0], [1]]
>>> K = place(A, B, [-2, -5])
See Also
--------
place_varga, acker
"""
from scipy.signal import place_poles
# Convert the system inputs to NumPy arrays
A_mat = np.array(A)
B_mat = np.array(B)
if (A_mat.shape[0] != A_mat.shape[1]):
raise ControlDimension("A must be a square matrix")
if (A_mat.shape[0] != B_mat.shape[0]):
err_str = "The number of rows of A must equal the number of rows in B"
raise ControlDimension(err_str)
# Convert desired poles to numpy array
placed_eigs = np.array(p)
result = place_poles(A_mat, B_mat, placed_eigs, method='YT')
K = result.gain_matrix
return K
