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