def regulator_with_observer(sys, poles=None, po=None, ts=None, extra_poles=[], ck=[]): final_poles = [] if poles is not None: final_poles = poles if po is not None and ts is not None: log_po = np.log(100 / po) psi = log_po / np.sqrt(np.pi**2 + log_po**2) wn = 4 / psi / ts s1 = -psi * wn + 1j * wn * np.sqrt(1 - psi**2) s2 = -psi * wn - 1j * wn * np.sqrt(1 - psi**2) final_poles.append(s1) final_poles.append(s2) for p in extra_poles: final_poles.append(p) rank = np.linalg.matrix_rank(ct.ctrb(sys.A, sys.B)) total_poles = len(final_poles) if total_poles < rank: raise BaseException( f"you have {total_poles} but you need {rank} to implement a FSFB") k = ct.acker(sys.A, sys.B, final_poles) mo = ct.obsv(sys.A, sys.C) obs_rank = np.linalg.matrix_rank(mo) print("obs_rank=", obs_rank) final_poles_obs = 5 * final_poles L = ct.acker(sys.A.T, sys.C.T, final_poles_obs) L = L.T print("") print("L=") print(L) pa = np.concatenate([sys.A, -sys.B * k], axis=1) pb = np.concatenate([L * sys.C, sys.A - sys.B * k - L * sys.C], axis=1) alc = np.concatenate([pa, pb], axis=0) blc = np.concatenate( [np.zeros((sys.A.shape[0], 1)), np.zeros((sys.A.shape[0], 1))]) clc = np.concatenate([sys.C, -sys.D * k], axis=1) dlc = np.array([[0]]) return ct.ss(alc, blc, clc, dlc)
def full_observer(system, poles_o, poles_s, debug=False) -> ObserverResult: """implementation of the complete observer for an autonomous system :param system : tuple (a, b, c) of system matrices :param poles_o: tuple of complex poles/eigenvalues of the observer dynamics :param poles_s: tuple of complex poles/eigenvalues of the closed system :param debug: output control for debugging in unittest(False:normal output,True: output local variables and normal output) :return: dataclass ObserverResult (controller function, feedback matrix, observer_gain, and pre-filter) """ # systemically relevant matrices a = system[0] b = system[1] c = system[2] d = system[3] # ignore the PendingDeprecationWarning for built-in packet control warnings.filterwarnings('ignore', category=PendingDeprecationWarning) ctr_matrix = ctr.ctrb(a, b) # controllabilty matrix assert np.linalg.det(ctr_matrix) != 0, 'this system is not controllable' # State feedback f_t = ctr.acker(a, b, poles_s) # pre-filter a1 = a - b * f_t v = (-1 * (c * a1**(-1) * b)**(-1)) # pre-filter obs_matrix = ctr.obsv(a, c) # observability matrix assert np.linalg.det(obs_matrix) != 0, 'this system is unobservable' # calculate observer gain l_v = ctr.acker(a.T, c.T, poles_o).T # controller function # in this case controller function is: -1 * feedback * states + pre-filter * reference output # disturbance value ist not considered # t = sp.Symbol('t') # sys_input = -1 * (f_t * sys_state)[0] # input_func = sp.lambdify((sys_state, t), sys_input, modules='numpy') # this method returns controller function, feedback matrix, observer gain and pre-filter result = ObserverResult(f_t, l_v, v, None) # return internal variables for unittest if debug: c_locals = Container(fetch_locals=True) result.debug = c_locals return result
def __init__(self, save_data=True, barriers=True): self.number_of_agents = rand.randint(3, 10) self.initial_poses = np.array([]) self.poses = np.array([]) self.desired_poses = np.zeros((self.number_of_agents, 3)) self.desired_vels = np.zeros((self.number_of_agents, 3)) self.crazyflie_objects = {} self.time = time.time() self.x_state = dict() self.vel_prev = dict() self.des_vel_prev = dict() self.xd = dict() self.u = dict() self.orientation_real = dict() self.pose_real = dict() self.dt = 0.03 self.count = 0 self.time_record = dict() self.x_record = dict() self.input_record = dict() self.orientation_record = dict() self.AA = np.array([[0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1], [0, 0, 0, 0]]) self.bb = np.array([[0], [0], [0], [1]]) self.Kb = np.asarray( acker(self.AA, self.bb, [-12.2, -12.4, -12.6, -12.8])) self.robotarium_simulator_plot = None self.barriers = barriers self.save_flag = save_data solvers.options['show_progress'] = False
def calculate_pp_gains(A, B, C, D, dp): # pole placement via ackermann's formula K = ctl.acker(A, B, dp) # calculate Nu, Nx matrices for including reference input Nu, Nx = getInputMatrices(A, B, C, D) return K, Nu, Nx
def gen_chain_of_integrators(): """Defines a chain of integrator for xyz: position, velocity, acceleration and jerk. Dynamics of the form: xd = Ax + bu Returns ------- A : ndarray b : ndarray Kb : ndarray """ # Define chain of integrator dynamics and controller gains. A = np.array([[0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1], [0, 0, 0, 0]]) b = np.array([[0], [0], [0], [1]]) Kb = np.array(acker( A, b, [-12.2, -12.4, -12.6, -12.8])) # Generate gains using pole placement. return A, b, Kb
def regulator(sys, poles=None, po=None, ts=None, extra_poles=[], ck=[]): final_poles = [] if poles is not None: final_poles = poles if po is not None and ts is not None: log_po = np.log(100 / po) psi = log_po / np.sqrt(np.pi**2 + log_po**2) wn = 4 / psi / ts s1 = -psi * wn + 1j * wn * np.sqrt(1 - psi**2) s2 = -psi * wn - 1j * wn * np.sqrt(1 - psi**2) final_poles.append(s1) final_poles.append(s2) for p in extra_poles: final_poles.append(p) rank = np.linalg.matrix_rank(ct.ctrb(sys.A, sys.B)) total_poles = len(final_poles) if total_poles < rank: raise BaseException( f"you have {total_poles} but you need {rank} to implement a FSFB") kk = ct.acker(sys.A, sys.B, final_poles) ak = sys.A - sys.B * kk bk = np.zeros((sys.A.shape[0], 1)) default = np.zeros((1, sys.A.shape[0])) default[0] = 1 ck2 = np.array(ck) if ck else default dk = np.array([[0]]) return ct.ss(ak, bk, ck2, dk)
zeta = 0.707 tr = 2.1 wn = 2.2 / tr # -- PID -- # kp = wn**2 * m - k kd = 2.0 * zeta * wn * m - b ki = 3.7 # -- FULL STATE -- # A = np.matrix([[0.0, 1.0], [-k / m, -b / m]]) B = np.matrix([[0.0], [1.0 / m]]) C = np.matrix([[1.0, 0.0]]) cl_poles = list(np.roots([1, 2 * wn * zeta, wn**2])) K = ctrl.acker(A, B, cl_poles) kr = -1.0 / np.dot(C, np.dot(np.linalg.inv(A - np.dot(B, K)), B)).item(0) # -- FULL STATE INTEGRATOR -- # USE_FULL_STATE_INTEGRATOR = True if USE_FULL_STATE_INTEGRATOR: A1 = np.concatenate((A, -C), 0) A1 = np.concatenate((A1, np.zeros((A1.shape[0], C.shape[0]))), 1) B1 = np.concatenate((B, np.zeros((C.shape[0], B.shape[1]))), 0) int_pole = -8.0 cl_poles.append(int_pole) K = ctrl.acker(A1, B1, cl_poles) ki = K.take([-1]).item(0) K = K.take(range(0, K.shape[1] - 1))
A1 = np.matrix([[0.0, 1.0, 0.0], [0.0, -1.0 * P.b / P.m / (P.ell**2), 0.0], [-1.0, 0.0, 0.0]]) B1 = np.matrix([[0.0], [3.0 / P.m / (P.ell**2)], [0.0]]) # gain calculation wn = 2.2 / tr # natural frequency des_char_poly = np.convolve([1, 2 * zeta * wn, wn**2], np.poly(integrator_pole)) des_poles = np.roots(des_char_poly) # Compute the gains if the system is controllable if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 3: print("The system is not controllable") else: K1 = cnt.acker(A1, B1, des_poles) K = np.matrix([K1.item(0), K1.item(1)]) ki = K1.item(2) # observer design des_obsv_char_poly = [1, 2 * zeta_obs * wn_obs, wn_obs**2] des_obsv_poles = np.roots(des_obsv_char_poly) # Compute the gains if the system is controllable if np.linalg.matrix_rank(cnt.ctrb(A.T, C.T)) != 2: print("The system is not observerable") else: L = cnt.acker(A.T, C.T, des_obsv_poles).T print('K: ', K) print('ki: ', ki)
th_alpha0 = th_wn**2 integrator_pole_long = -10.0 #-10.0 # Desired Poles des_char_poly_long = np.convolve([1, th_alpha1, th_alpha0], np.poly(integrator_pole_long)) des_poles_long = np.roots(des_char_poly_long) print 'des_poles_long' print des_poles_long # Controllability Matrix if np.linalg.matrix_rank(cnt.ctrb(A1_long, B1_long)) != 3: print("The longitudinal system is not controllable") else: K1_long = cnt.acker(A1_long, B1_long, des_poles_long) K_long = K1_long[0, 0:2] ki_long = K1_long[0, 2] print('K1_long: ', K1_long) print('K_long: ', K_long) print('ki_long: ', ki_long) ############################################### ############ Lateral ########################## ############################################### A_lat = np.matrix( [[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 0.0], [l1 * F_eq / (m1 * (l1**2) + m2 * (l2**2) + Jz), 0.0, 0.0, 0.0]]) B_lat = np.matrix([[0.0], [0.0], [1.0 / Jx], [0.0]])
# gain calculation wn = 2.2 / tr # natural frequency #des_char_poly = np.convolve( # [1, 2*zeta*wn, wn**2], # np.poly(integrator_pole)) #des_poles = np.roots(des_char_poly) des_char_poly = np.array([1., 2. * zeta * wn_obs, wn_obs**2.]) des_poles_poly = np.roots(des_char_poly) des_poles = np.concatenate((des_poles_poly, integrator_pole * np.ones(1))) # Compute the gains if the system is controllable if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 3: print("The system is not controllable") else: K1 = cnt.acker(A1, B1, des_poles) K = np.array([K1.item(0), K1.item(1)]) ki = K1.item(2) # observer design # Augmented Matrices A2 = np.concatenate((np.concatenate((A, B), axis=1), np.zeros((1, 3))), axis=0) B2 = np.concatenate((B, np.zeros((1, 1))), axis=0) C2 = np.concatenate((C, np.zeros((1, 1))), axis=1) #des_obsv_char_poly = np.convolve( # [1, 2*zeta*wn_obs, wn_obs**2], # np.poly(dist_obsv_pole)) #des_obsv_poles = np.roots(des_obsv_char_poly) des_char_est = np.array([1., 2. * zeta * wn_obs, wn_obs**2.]) des_poles_est = np.roots(des_char_est)
def get_observer(A,C,poles): C_tr=np.matrix.transpose(C) A_tr=np.matrix.transpose(A) L_tr=control.acker(A_tr,C_tr,poles) return np.matrix.transpose(L_tr)
# S**2 + alpha1*S + alpha0 h_alpha1 = 2.0*h_zeta*h_wn h_alpha0 = h_wn**2 # Desired Poles des_char_poly_lat = np.convolve([1,z_alpha1,z_alpha0],[1,th_alpha1,th_alpha0]) des_char_poly_lon = [1,h_alpha1,h_alpha0] des_poles_lat = np.roots(des_char_poly_lat) des_poles_lon = np.roots(des_char_poly_lon) # Latitudinal Controllability Matrix if np.linalg.matrix_rank(cnt.ctrb(A_lat,B_lat))!=4: print("The Latitudinal system is not controllable") else: K_lat = cnt.acker(A_lat,B_lat,des_poles_lat) kr_lat = -1.0/(C_lat*np.linalg.inv(A_lat-B_lat*K_lat)*B_lat) # Longitudinal Controllability Matrix if np.linalg.matrix_rank(cnt.ctrb(A_lon,B_lon))!=2: print("The Longitudinal system is not controllable") else: K_lon = cnt.acker(A_lon,B_lon,des_poles_lon) kr_lon = -1.0/(C_lon*np.linalg.inv(A_lon-B_lon*K_lon)*B_lon) print('K_lat: ', K_lat) print('kr_lat: ', kr_lat) print('K_lon: ', K_lon) print('kr_lon: ', kr_lon) #################################################
# init ploting handles Cfplt = dict() cfs = dict() CfCoord = dict() for i in range(N): Cfplt[i] = Shpere3D(p0[i][0, :], ax) CfCoord[i] = Coord3D(ax, p0[i][0, :]) cfs[i] = CF(p0[i][0, :], i) plt.draw() # pole placement for CLF and CBF AA = np.array([[0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1], [0, 0, 0, 0]]) bb = np.array([[0], [0], [0], [1]]) #Kb=acker(AA,bb,[-2.2,-2.4,-2.6,-2.8]); #Kb=acker(AA,bb,[-5.2,-5.4,-5.6,-5.8]); Kb = np.asarray(acker(AA, bb, [-12.2, -12.4, -12.6, -12.8])) # Bezier Interpolation Cout = dict() T = 4.0 #time to complete, also scaling factor for i in range(N - 1): Cout[i] = BezierInterp(T, p0[i], p1[i]) Cout[4] = BezierInterp(T, pc[0], pc[1]) # start auto mode program CBF_on = 1 tk = 0 dt = 0.02 t_total = 35 phat = dict() #nominal state from interpolation uhat = dict() #nominal control
0.0, -3 * P.m1 * P.g / 4 / (.25 * P.m1 + P.m2), -P.b / (.25 * P.m1 + P.m2), 0.0 ], [ 0.0, 3 * (P.m1 + P.m2) * P.g / 2 / (.25 * P.m1 + P.m2) / P.ell, 3 * P.b / 2 / (.25 * P.m1 + P.m2) / P.ell, 0.0 ]]) B = np.matrix([[0.0], [0.0], [1 / (.25 * P.m1 + P.m2)], [-3.0 / 2 / (.25 * P.m1 + P.m2) / P.ell]]) C = np.matrix([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0]]) # gain calculation wn_th = 2.2 / tr_theta # natural frequency for angle wn_z = 2.2 / tr_z # natural frequency for position des_char_poly = np.convolve([1, 2 * zeta_z * wn_z, wn_z**2], [1, 2 * zeta_th * wn_th, wn_th**2]) des_poles = np.roots(des_char_poly) # Compute the gains if the system is controllable if np.linalg.matrix_rank(cnt.ctrb(A, B)) != 4: print("The system is not controllable") else: K = cnt.acker(A, B, des_poles) kr = -1.0 / (C[0] * np.linalg.inv(A - B * K) * B) print('K: ', K) print('kr: ', kr)
# Augmented Longitudinal Parameters A1 = np.matrix([[0.0, 1.0, 0.0], [0.0, 0.0, 0.0], [-1.0, 0.0, 0.0]]) B1 = np.matrix([[0], [1 / (P.mc + 2 * P.mr)], [0.0]]) # gain calculation des_char_poly = np.convolve([1, 2 * zeta_h * wn_h, wn_h**2], np.poly([h_integrator_pole])) des_poles = np.roots(des_char_poly) # Compute the gains if the system is controllable if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 3: print("The longitudinal system is not controllable") else: 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
[0.0], [b_th] ]) C_lon = np.matrix([ [1.0, 0.0] ]) A1_lon = np.concatenate(( np.concatenate((A_lon, np.zeros((2,1))), 1), np.concatenate((-C_lon, np.zeros((1,1))), 1)), 0) B1_lon = np.concatenate((B_lon, np.zeros((1,1)))) i_pole_lon = -1*wn_th cl_poles_lon = list(np.roots([1,2*zeta_lon*wn_th,wn_th**2])) + [i_pole_lon] K1_lon = ctrl.acker(A1_lon, B1_lon, cl_poles_lon) K_lon = K1_lon[0,:2] ki_lon = K1_lon[0,2] #kr_lon = -1.0 / (C_lon * (np.linalg.inv(A_lon - B_lon*K_lon) * B_lon)).item(0) # Lateral c = l1*Feq / (m1*l1**2 + m2*l2**2 + Jz) A_lat = np.matrix([ [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 0.0], [c , 0.0, 0.0, 0.0] ]) B_lat = np.matrix([ [0.0], [0.0],
C_lat = np.array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0]]) ## LONGITUDINAL GAINS CALCULATIONS wn_h = 2.2/tr_h # natural frequency for angle des_char_poly_long = [1, 2*zeta_h*wn_h, wn_h**2] des_poles_long = np.roots(des_char_poly_long) # Compute the gains if the system is controllable if np.linalg.matrix_rank(cnt.ctrb(A_long, B_long)) != 2: print('The system is not controllable') else: # A just turn K matrix into a numpy array K_long = (cnt.acker(A_long, B_long, des_poles_long)).A kr_long = -1.0/(C_long @ np.linalg.inv(A_long - B_long@K_long) @ B_long) print('K_long: ', K_long) print('kr_long: ', kr_long) ## LATERAL GAINS CALCULATIONS wn_th = 2.2/tr_th # natural frequency for angle wn_z = 2.2/tr_z # natural frequency for position des_char_poly_lat = np.convolve([1, 2*zeta_z*wn_z, wn_z**2], [1, 2*zeta_th*wn_th, wn_th**2]) des_poles_lat = np.roots(des_char_poly_lat) # Compute the gains if the system is controllable if np.linalg.matrix_rank(cnt.ctrb(A_lat, B_lat)) != 4:
pid.setKp(6) A = np.matrix('[2 -3; 1 3]') B = np.matrix('[1;-1]') C = np.matrix('[1 0]') D = 0 A2 = np.matrix('[0 1; -2 -3]') B2 = np.matrix('[0;-1]') C2 = np.matrix('[1 0]') D2 = 0 sys = control.ss(A, B, C, D) tf = control.ss2tf(sys) poles = control.pole(sys) acker_k = control.acker(A, B, ["-1", "-1"]) sys_stabil = control.ss(A2, B2, C2, D2) Acl = A - B * acker_k sys2 = control.ss(Acl, B, C, D) pid_ss = pid.create_tf() sys3 = control.series(sys2, pid_ss) sys4 = control.feedback(sys3, 1, -1) while True: value = socket.recv_pyobj() sys_step = control.tf(value[0], [0, 1]) sys_command = control.series(sys_step, sys4) T, yout = control.step_response(sys_command)
def state_feedback(system: tuple, poles_o: list, sys_state: sp.Matrix, eqrt: list, yr, debug=False) -> StateFeedbackResult: """ :param system : tuple (a, b, c, d) of system matrices :param poles_o: tuple of closed-loop poles of the system :param sys_state: states of nonlinear system :param eqrt: equilibrium points of the system :param yr: reference output :param debug: output control for debugging in unittest(False:normal output,True: output local variables and normal output) :return: dataclass StateFeedbackResult (controller function, feedback matrix and pre-filter) """ # ignore the PendingDeprecationWarning for built-in packet control warnings.filterwarnings('ignore', category=PendingDeprecationWarning) # system matrices a = system[0] b = system[1] c = system[2] d = system[3] ctr_matrix = ctr.ctrb(a, b) # controllabilty matrix assert np.linalg.det(ctr_matrix) != 0, 'this system is not controllable' # full state feedback f_t = ctr.acker(a, b, poles_o) # pre-filter a1 = a - b * f_t v = -1 * (c * a1**(-1) * b)**(-1) # pre-filter '''Since the controller, which is designed on the basis of a linearized system, is a small signal model, the states have to be converted from the large signal model to the small signal model. i.e. the equilibrium points of the original non-linear system must be subtracted from the returned states. x' = x - x0 x: states of nonlinear system (large signal model) x0: equilibrium points x': states of controller (small signal model) And for the same reason, the equilibrium position of the input must be added to controller function. ''' t = sp.Symbol('t') # convert states to small signal model small_state = sys_state - sp.Matrix( [eqrt[i][1] for i in range(len(sys_state))]) # creat controller function # in this case controller function is: -1 * feedback * states + pre-filter * reference output # disturbance value ist not considered sys_input = -1 * (f_t * small_state)[0] + v[0] * yr + eqrt[len(sys_state)][1] input_func = sp.lambdify((sys_state, t), sys_input, modules='numpy') # this method returns controller function, feedback matrix and pre-filter result = StateFeedbackResult(input_func, f_t, v, None) # return internal variables for unittest if debug: c_locals = Container(fetch_locals=True) result.debug = c_locals return result
while (s < 1) or (sum(fflag) < N): cfs[i].send_cmd_diff(0, 0, 0, thrust_hover[i]) t = rospy.Time.now() s = min((t - t0).to_sec() / t_wp, 1.0) for i in range(N): fflag[i] = cfs[i].gotoT(pc[0][0, :], s) print 'Initialization Done! Experiment Starts!' #-------------------------Actual control experiment--------------------# # pole placement for CLF and CBF AA = np.array([[0, 1, 0], [0, 0, 1], [0, 0, 0]]) bb = np.array([[0], [0], [1]]) #Kb=acker(AA,bb,[-2.2,-2.4,-2.6,-2.8]) #Kb=acker(AA,bb,[-5.2,-5.4,-5.6,-5.8]) Kb = np.asarray(acker(AA, bb, [-8.2, -8.4, -8.6])) # start auto mode program print 'Auto mode: differential flatness' CBF_on = 0 Cout = dict() # Bezier Control points tk = 0 dt = 0.02 t_total = sum(Tf) + 1 phat = dict() #nominal state from interpolation uhat = dict() #nominal control x = dict() #actual states xd = dict() #derivative of x for i in range(N): x[i] = pc[0][0:-1, :] xd[i] = np.zeros((3, 3))
cfs[i].send_cmd_diff(0, 0, 0, thrust_hover[i]) t = rospy.Time.now() s = min((t - t0).to_sec() / t_wp, 1.0) for i in range(N): fflag[i] = cfs[i].gotoT(pc[0][0, :], s) print 'Initialization Done! Experiment Starts!' #-------------------------Actual control experiment--------------------# # pole placement for CLF and CBF AA = np.array([[0, 1, 0], [0, 0, 1], [0, 0, 0]]) bb = np.array([[0], [0], [1]]) #Kb=np.asarray(acker(AA,bb,[-2.2,-2.4,-2.6])) #Kb=acker(AA,bb,[-5.2,-5.4,-5.6,-5.8]) #Kb=np.asarray(acker(AA,bb,[-6.6,-6.8,-7.0])) # works, err 0.2 Kb = np.asarray(acker(AA, bb, [-6.6, -6.8, -7.0])) # works, err 0.2 #Kb=np.asarray(acker(AA,bb,[-12.2,-6.4,-2.6])) # works when kbz set to 0 #Kb=np.asarray(acker(AA,bb,[-12.2,-6.4,-2.6])) Kb[0, 2] = Kb[0, 2] * 1.0 # start auto mode program print 'Auto mode: differential flatness' CBF_on = 0 Cout = dict() # Bezier Control points tk = 0 dt = 0.02 t_total = sum(Tf) + 1 phat = dict() #nominal state from interpolation uhat = dict() #nominal control x = dict() #actual states xnext = dict() #actual states
ax0.plot(t, ref, color="k", label='desired') ax0.set_title("Step response of close loop system", fontsize='small') ax0.legend(loc='lower right', shadow=True, fontsize='small') ax0.set_xlabel("time {s}") ax0.set_ylabel("Height {m}") ax0.set_yticks(h_ticks) ax0t.set_ylabel("Velocity {m/s}") ax0t.set_yticks(v_ticks) #*****************************************# # FULL STATE FEEDBACK CONTROLLER #*****************************************# #desired controller poles p1= -3, p2, -4 dcp = np.array([-3.0, -3.0]) K = ctl.acker(A, B, dcp) print("Controller gains: {},{}".format(K.item(0, 0), K.item(0, 1))) # The system with full state feedback is modified as # x'(t) = (A-BK)x + B(Nu + KNx)*r # Nx = np.matrix([[0.0], [1.0]]) Nu = 0.0 #**************** ADDED LQR Controller # Define performance index weights for Vertical Dynamics Qx1 = np.diag([10, 100]) Qu1a = np.diag([1]) (K, X, E) = ctl.lqr(A, B, Qx1, Qu1a) K = np.matrix(K)
th_alpha0 = th_wn**2 # Desired Poles des_char_poly_lat = np.convolve( np.convolve([1, s_alpha1, s_alpha0], [1, p_alpha1, p_alpha0]), np.poly(integrator_pole_lat)) des_char_poly_lon = np.convolve([1, th_alpha1, th_alpha0], np.poly(integrator_pole_lon)) des_poles_lat = np.roots(des_char_poly_lat) des_poles_lon = np.roots(des_char_poly_lon) # Latitudinal Controllability Matrix if np.linalg.matrix_rank(cnt.ctrb(A1_lat, B1_lat)) != 5: print("The Latitudinal system is not controllable") else: K1_lat = cnt.acker(A1_lat, B1_lat, des_poles_lat) K_lat = K1_lat[0, 0:4] ki_lat = K1_lat[0, 4] # Longitudinal Controllability Matrix if np.linalg.matrix_rank(cnt.ctrb(A1_lon, B1_lon)) != 3: print("The Longitudinal system is not controllable") else: K1_lon = cnt.acker(A1_lon, B1_lon, des_poles_lon) K_lon = K1_lon[0, 0:2] ki_lon = K1_lon[0, 2] UNCERTAINTY_PARAMETERS = False if UNCERTAINTY_PARAMETERS: alpha = 0.2 l1 = 0.85 * (1 + 2 * alpha * np.random.rand() - alpha
des_poles = np.array([-5 + 0.1j, -5 - 0.1j]) polesI = np.append(des_poles, -5) if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 3: print('The system is not controllable.') else: K1 = cnt.place(A1, B1, polesI) K = np.array([K1.item(0), K1.item(1)]) ki2 = K1.item(2) # observer calculation obsv_poles = 10 * des_poles # compute gains if the system is observable if np.linalg.matrix_rank(cnt.ctrb(A.T, C.T)) != 2: print('The system is not observable.') else: L = cnt.acker(A.T, C.T, obsv_poles).T print('K: ', K) print('ki2: ', ki2) print('L^T: ', L.T) # ---------------------------------------------- # simulation parameters t_start = 0.0 t_end = 40.0 Ts = 0.01 t_plot = 0.1 sigma = 0.05
def reduced_observer(system, poles_o, poles_s, x0, tt, debug=False): """ :param system: tuple (A, B, C) of system matrices :param poles_o: tuple of complex poles/eigenvalues of the observer dynamics :param poles_s: tuple of complex poles/eigenvalues of the closed system :param x0: initial condition :param tt: vector for the time axis :param debug: output control for debugging in unittest (False:normal output,True: output local variables and normal output) :return yy: output of the system :return xx: states of the system :return tt: time of the simulation """ # renumber the matrices to construct the matrices # in the form of the reduced observer a, b, c, d = ob_func.transformation(system, debug=False) # row rank of the output matrix rank = np.linalg.matrix_rank(c[0]) # submatrices of the system matrix A # and the input matrix a_11 = a[0:rank, 0:rank] a_12 = a[0:rank, rank:a.shape[1]] a_21 = a[rank:a.shape[1], 0:rank] a_22 = a[rank:a.shape[1], rank:a.shape[1]] b_1 = b[0:rank, :] b_2 = b[rank:b.shape[0], :] # construct the observability matrix # q_1 = a_12 # q_2 = a_12 * a_22 # q = np.vstack([q_1, q_2]) obs_matrix = ctr.obsv(a_22, a_12) # observability matrix rank_q = np.linalg.matrix_rank(obs_matrix) assert np.linalg.det(obs_matrix) != 0, 'this system is unobservable' # ignore the PendingDeprecationWarning for built-in packet control warnings.filterwarnings('ignore', category=PendingDeprecationWarning) # state feedback f_t = ctr.acker(a, b, poles_s) c_ = c - d * f_t f_1 = f_t[:, 0:1] f_2 = f_t[:, 1:] l_ = ctr.acker(a_22.T, a_12.T, poles_o).T # State representation of the entire system # Original system and observer error system, # Dimension: 4 x 4) sys = ob_func.state_space_func(a, a_11, a_12, a_21, a_22, b, b_1, b_2, c_, l_, f_1, f_2) # simulation for the proper movement yy, tt, xx = ctr.matlab.initial(sys, tt, x0, return_x=True) result = yy, xx, tt, sys # return internal variables for unittest if debug: c_locals = Container(fetch_locals=True) return result, c_locals return result
# Longitudinal Parameters Alon = np.matrix([[0.0, 1.0], [0.0, 0.0]]) Blon = np.matrix([[0], [1 / (P.mc + 2 * P.mr)]]) Clon = np.matrix([[1.0, 0.0]]) # gain calculation des_char_poly = [1, 2 * zeta_h * wn_h, wn_h**2] des_poles = np.roots(des_char_poly) # Compute the gains if the system is controllable if np.linalg.matrix_rank(cnt.ctrb(Alon, Blon)) != 2: print("The system is not controllable") else: K_lon = cnt.acker(Alon, Blon, des_poles) kr_lon = -1.0 / (Clon[0] * np.linalg.inv(Alon - Blon * K_lon) * Blon) # Lateral parameters # State Space Equations # xdot = A*x + B*u # y = C*x Fe = P.m * P.g Alat = np.matrix( [[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0], [0.0, -Fe / (P.mc + 2 * P.mr), -P.mu / (P.mc + 2 * P.mr), 0.0], [0.0, 0.0, 0.0, 0.0]]) Blat = np.matrix([[0.0], [0.0], [0.0], [1 / (P.Jc + 2 * P.mr * P.d**2)]])
# tuning parameters tr = 0.4 zeta = 0.707 # State Space Equations # xdot = A*x + B*u # y = C*x A = np.array([[0.0, 1.0], [0.0, -1.0 * P.b / P.m / (P.ell**2)]]) B = np.array([[0.0], [3.0 / P.m / (P.ell**2)]]) C = np.array([[1.0, 0.0]]) # gain calculation wn = 2.2 / tr # natural frequency des_char_poly = [1, 2 * zeta * wn, wn**2] des_poles = np.roots(des_char_poly) # Compute the gains if the system is controllable if np.linalg.matrix_rank(cnt.ctrb(A, B)) != 2: print("The system is not controllable") else: #.A just turns K matrix into a numpy array K = (cnt.acker(A, B, des_poles)).A kr = -1.0 / (C @ np.linalg.inv(A - B @ K) @ B) print('K: ', K) print('kr: ', kr)
wn_th = 2.2 / tr_theta # natural frequency for angle wn_z = 2.2 / tr_z # natural frequency for latral wn_h = 2.2 / tr_h # natural frequency for longitudinal #-----------------------------latral----------------------------------------- des_char_poly_z = np.convolve( np.convolve([1, 2 * zeta_z * wn_z, wn_z**2], [1, 2 * zeta_th * wn_th, wn_th**2]), [1, integrator_pole_z]) des_poles_z = np.roots(des_char_poly_z) # Compute the gains if the system is controllable for latral if np.linalg.matrix_rank(cnt.ctrb(A_z, B_z)) != 4: print("The system is not controllable") else: K1_z = cnt.acker(A_z_1, B_z_1, des_poles_z) K_z = np.matrix([K1_z.item(0), K1_z.item(1), K1_z.item(2), K1_z.item(3)]) ki_z = K1_z.item(4) print('K_z: ', K_z) print('ki_z: ', ki_z) #----------------------------longi---------------------------------------- des_char_poly_h = np.convolve( [1, 2 * zeta_h * wn_h, wn_h**2], [1, integrator_pole_h]) #np.poly(integrator_pole_h)) des_poles_h = np.roots(des_char_poly_h) # Compute the gains if the system is controllable if np.linalg.matrix_rank(cnt.ctrb(A_h, B_h)) != 2: print("The system is not controllable")
# gain calculation wn_th = 2.2 / tr_theta # natural frequency for angle wn_z = 2.2 / tr_z # natural frequency for latral wn_h = 2.2 / tr_h # natural frequency for longitudinal #-----------------------------latral----------------------------------------- des_char_poly_z = np.convolve([1, 2 * zeta_z * wn_z, wn_z**2], [1, 2 * zeta_th * wn_th, wn_th**2]) des_poles_z = np.roots(des_char_poly_z) # Compute the gains if the system is controllable for latral if np.linalg.matrix_rank(cnt.ctrb(A_z, B_z)) != 4: print("The system is not controllable") else: K_z = cnt.acker(A_z, B_z, des_poles_z) Cr_z = np.array([[1.0, 0.0, 0.0, 0.0]]) kr_z = -1.0 / (Cr_z * np.linalg.inv(A_z - B_z @ K_z) @ B_z) print('K_z: ', K_z) print('kr_z: ', kr_z) #----------------------------longi---------------------------------------- des_char_poly_h = [1, 2 * zeta_h * wn_h, wn_h**2] des_poles_h = np.roots(des_char_poly_h) # Compute the gains if the system is controllable if np.linalg.matrix_rank(cnt.ctrb(A_h, B_h)) != 2: print("The system is not controllable") else: K_h = cnt.acker(A_h, B_h, des_poles_h)
# full state # ------------------------------ # d_0 = (1.0 / 3 * m2 * l**2 + m1 * z0**2) a_0 = (-m1 * g) / d_0 b_0 = l / d_0 A = np.matrix([[0, 0, 1, 0], [0, 0, 0, 1], [0, -g, 0, 0], [a_0, 0, 0, 0]]) B = np.matrix([[0], [0], [0], [b_0]]) C = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0]]) Cr = C[0, :] Cab = ctrl.ctrb(A, B) cl_poles = list(np.roots([1, 2 * zeta_z * wn_z, wn_z**2])) + list( np.roots([1, 2 * zeta_th * wn_th, wn_th**2])) K = ctrl.acker(A, B, cl_poles) kr = -1.0 / np.dot(C, np.dot(np.linalg.inv(A - np.dot(B, K)), B)).item(0) # ------------------------------ # # full state with integrator # ------------------------------ # USE_FULL_STATE_INTEGRATOR = True if USE_FULL_STATE_INTEGRATOR: A1 = np.concatenate((A, -Cr), 0) A1 = np.concatenate((A1, np.zeros((A1.shape[0], Cr.shape[0]))), 1) B1 = np.concatenate((B, np.zeros((Cr.shape[0], B.shape[1]))), 0) int_pole = -4.0 * wn_th cl_poles.append(int_pole)