def kalmd(sys, Q, R): """Solves for the steady state kalman gain and error covariance matrices. Keyword arguments: sys -- discrete state-space model Q -- process noise covariance matrix R -- measurement noise covariance matrix Returns: Kalman gain, error covariance matrix. """ m = sys.A.shape[0] observability_rank = np.linalg.matrix_rank(cnt.obsv(sys.A, sys.C)) if observability_rank != m: print("Warning: Observability of %d != %d, unobservable state", observability_rank, m) # Compute the steady state covariance matrix P_prior = sp.linalg.solve_discrete_are(a=sys.A.T, b=sys.C.T, q=Q, r=R) S = sys.C * P_prior * sys.C.T + R K = P_prior * sys.C.T * np.linalg.inv(S) P = (np.eye(m) - K * sys.C) * P_prior return K, P
def _reduce_state_space(self): x_al = self.fsf_dict['x_al'] x_sp_al = self.fsf_dict['x_sp_al'] x_er_al = self.fsf_dict['x_er_al'] u_al = self.fsf_dict['u_al'] u_sp_al = self.fsf_dict['u_sp_al'] u_er_al = self.fsf_dict['u_er_al'] al_sys = self.fsf_dict['al_sys'] #Construct reduced state space x_ral = x_al[7:] x_sp_ral = x_sp_al[7:] x_er_ral = x_er_al[7:] u_ral = u_al[np.ix_([0,1,3,4])] u_sp_ral = u_sp_al[np.ix_([0,1,3,4])] u_er_ral = u_er_al[np.ix_([0,1,3,4])] A = al_sys.A B = al_sys.B C = al_sys.C D = al_sys.D A_ral = A[np.ix_([7,8,9,10,11,12],[7,8,9,10,11,12])] B_ral = B[np.ix_([7,8,9,10,11,12],[0,1,3,4])] C_ral = C[np.ix_([7,8,9,10,11,12],[7,8,9,10,11,12])] D_ral = D[np.ix_([7,8,9,10,11,12],[0,1,3,4])] #Lateral and longitudinal state spaces ral_sys = control.StateSpace(A_ral, B_ral, C_ral, D_ral) ral_ctrb = control.ctrb(ral_sys.A, ral_sys.B) ral_obsv = control.obsv(ral_sys.A, ral_sys.C) #Update FSF dictionary self.fsf_dict.update(x_ral = x_ral) self.fsf_dict.update(x_sp_ral = x_sp_ral) self.fsf_dict.update(x_er_ral = x_er_ral) self.fsf_dict.update(u_ral = u_ral) self.fsf_dict.update(u_sp_ral = u_sp_ral) self.fsf_dict.update(u_er_ral = u_er_ral) self.fsf_dict.update(ral_sys = ral_sys)
def kalmd(A, C, Q, R): """Solves for the steady state kalman gain and error covariance matrices. Keyword arguments: sys -- discrete state-space model Q -- process noise covariance matrix R -- measurement noise covariance matrix Returns: Kalman gain, error covariance matrix. """ m = A.shape[0] observability_rank = np.linalg.matrix_rank(cnt.obsv(A, C)) if observability_rank != m: print( "Warning: Observability of %d != %d, unobservable state" % (observability_rank, m) ) # Compute the steady state covariance matrix # P_prior = sp.linalg.solve_discrete_are(a=A.T, b=C.T, q=Q, r=R) P_prior = np.array([[R]]) S = C * P_prior * C.T + R K = P_prior * C.T * np.linalg.inv(S) P = (np.eye(m) - K * C) * P_prior print(str(P_prior[0, 0]) + " " + str(S[0, 0]) + " " + str(P[0, 0]) + " " + str(K[0, 0])) return K, P
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 __isObservable(self, th=None): if th is None: th = self.__th F = np.array(self.__F(th)) C = np.array(self.__C(th)) n = self.__n obsv_matrix = control.obsv(F, C) rank = np.linalg.matrix_rank(obsv_matrix) return rank == n
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 feed_forward(self, A, B, C, D, K): """creates the feed forward matrix F to compute control inputs using a desired state""" F = np.linalg.pinv(-C * np.linalg.inv(A - B * K) * B) if np.linalg.matrix_rank(control.ctrb(A - B * K, B * F)) < A.shape[0]: print "System is not fully controllable" else: print "System is fully controllable" if np.linalg.matrix_rank(control.obsv(A - B * K, C - D * K)) < A.shape[0]: print "System is not fully observable" else: print "System is fully observable" return F
def kalmd(sys, Q, R): """Solves for the steady-state kalman gain matrix. Keyword arguments: sys -- discrete state-space model Q -- process noise covariance matrix R -- measurement noise covariance matrix Returns: K -- numpy.array(states x outputs), Kalman gain matrix. """ if np.linalg.matrix_rank(ct.obsv(sys.A, sys.C)) < sys.A.shape[0]: print( f"Warning: The system is unobservable\n\nA = {sys.A}\nC = {sys.C}\n" ) P = sp.linalg.solve_discrete_are(a=sys.A.T, b=sys.C.T, q=Q, r=R) S = sys.C @ P @ sys.C.T + R return np.linalg.solve(S.T, sys.C @ P.T).T
def design_regredob(sys_c_ol, sampling_interval, desired_settling_time, desired_observer_settling_time=None, spoles=None, sopoles=None, disp=True): """ Design a digital reduced order observer 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 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. disp: Print debugging output. Default is True. Returns: tuple: (sys_d_ol, L1, L2, K, F, G, H) Where sys_d_ol is the discrete plant, L is the stablizing gain matrix, and K is the observer 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] num_measured_states = num_outputs num_unmeasured_states = num_states - num_measured_states # 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 # 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(phi, gamma, 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") return None L = full_state_feedback.gain_matrix # 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_unmeasured_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)) if disp: print("Using normalized bessel for the remaining", num_sopoles_left, "sopoles") zopoles = control_poles.spoles_to_zpoles(sopoles, sampling_interval) if disp: print("sopoles = ", sopoles) print("zopoles = ", zopoles) # partition out the phi and gamma matrix phi11 = phi[:num_measured_states, :num_measured_states] phi12 = phi[:num_measured_states, num_measured_states:] phi21 = phi[num_measured_states, :num_measured_states] phi22 = phi[num_measured_states:, num_measured_states:] gamma1 = gamma[:num_measured_states] gamma2 = gamma[num_measured_states:] C1 = C[:num_measured_states, :num_measured_states] if num_measured_states >= num_states / 2 and LA.matrix_rank( phi12) == num_unmeasured_states: # case 1 if num_unmeasured_states % 2 == 0: F = np.matrix([[zopoles[0].real, zopoles[0].imag], [zopoles[1].imag, zopoles[1].real]]) else: # We only support 1 real pole F = np.matrix([zopoles[0].real]) cp = C1 * phi12 cp_t = np.transpose(cp) K = (phi22 - F) * np.linalg.inv(cp_t * cp) * cp_t elif num_measured_states == 1: # case 2 (unsupported) print("unsupported design with measured states = 1") np.poly(np.eig(phi22)) else: full_state_feedback = signal.place_poles(np.transpose(phi22), np.transpose(C1 * phi12), zopoles) K = np.transpose(full_state_feedback.gain_matrix) F = phi22 - K * C1 * phi12 H = gamma2 - K * C1 * gamma1 G = (phi21 - K * C1 * phi11) * np.linalg.inv(C1) + (F * K) # 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") return None return (sys_d_ol, np.matrix(L), np.matrix(K), np.matrix(F), np.matrix(G), np.matrix(H))
def design_regob(sys_c_ol, sampling_interval, desired_settling_time, desired_observer_settling_time=None, spoles=None, sopoles=None, disp=True): """ Design a digital full order observer 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 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. disp: Print debugging output. Default is True. Returns: tuple: (sys_d_ol, L, K) Where sys_d_ol is the discrete plant, L is the stablizing gain matrix, and K is the observer 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(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 eig(phi - gamma*L) are inside the unit circle full_state_feedback = signal.place_poles(phi, gamma, 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") return None L = full_state_feedback.gain_matrix # 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)) if disp: print("Using normalized bessel for the remaining", num_sopoles_left, "sopoles") 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") return None K = np.transpose(full_state_feedback.gain_matrix) return (sys_d_ol, np.matrix(L), np.matrix(K))
C = np.array([[1., 0., 0., 0.], [0, 0, 1, 0]]) D = 0 sys_Cont = ct.StateSpace(A, B, C, D) sys_Dis = cm.c2d(sys_Cont, dt) Ad = sys_Dis.A Bd = sys_Dis.B Cd = sys_Dis.C # Controlability test Controlable = ct.ctrb(Ad, Bd) # print(np.linalg.matrix_rank(Controlable)) # full rank - therefore controllable # Observability Test: Observable = ct.obsv(Ad, Cd) # print(np.linalg.matrix_rank(Observable)) # full rank - therefore observable def pendControl(sys, sysC, x0, x0_est): A = sys.A B = sys.B C = sys.C dt = sys.dt sim_Time = 8 t = np.linspace(0, sim_Time, sim_Time / dt) Overshoot_LQR = performance_specs[0] settle_LQR = performance_specs[1] Overshoot_KF = performance_specs[2]
print('K: ', K) print('ki: ', ki) #################################################### # Observer #################################################### # Observer design obs_th_wn = 5.0*th_wn obs_z_wn = 5.0*z_wn obs_des_char_poly = np.convolve([1,2.0*z_zeta*obs_z_wn,obs_z_wn**2], [1,2.0*th_zeta*obs_th_wn,obs_th_wn**2]) obs_des_poles = np.roots(obs_des_char_poly) if np.linalg.matrix_rank(cnt.obsv(A,C))!=4: print('System Not Observable') else: L = place(A.T,C.T,obs_des_poles).gain_matrix.T print('L:', L) ################################################# # Uncertainty Parameters ################################################# UNCERTAINTY_PARAMETERS = True if UNCERTAINTY_PARAMETERS: alpha = 0.0 # Uncertainty parameter m1 = m1*(1+2*alpha*np.random.rand()-alpha) # Mass of the pendulum, kg m2 = m2*(1+2*alpha*np.random.rand()-alpha) # Length of the rod, m l = l*(1+2*alpha*np.random.rand()-alpha) # Damping coefficient, Ns
def testConvert(self): """Test state space to transfer function conversion.""" verbose = self.debug from control.statesp import _mimo2siso #print __doc__ # Machine precision for floats. eps = np.finfo(float).eps for states in range(1, self.maxStates): for inputs in range(1, self.maxIO): for outputs in range(1, self.maxIO): # start with a random SS system and transform to TF then # back to SS, check that the matrices are the same. ssOriginal = matlab.rss(states, outputs, inputs) if (verbose): self.printSys(ssOriginal, 1) # Make sure the system is not degenerate Cmat = control.ctrb(ssOriginal.A, ssOriginal.B) if (np.linalg.matrix_rank(Cmat) != states): if (verbose): print(" skipping (not reachable)") continue Omat = control.obsv(ssOriginal.A, ssOriginal.C) if (np.linalg.matrix_rank(Omat) != states): if (verbose): print(" skipping (not observable)") continue tfOriginal = matlab.tf(ssOriginal) if (verbose): self.printSys(tfOriginal, 2) ssTransformed = matlab.ss(tfOriginal) if (verbose): self.printSys(ssTransformed, 3) tfTransformed = matlab.tf(ssTransformed) if (verbose): self.printSys(tfTransformed, 4) # Check to see if the state space systems have same dim if (ssOriginal.states != ssTransformed.states): print("WARNING: state space dimension mismatch: " + \ "%d versus %d" % \ (ssOriginal.states, ssTransformed.states)) # Now make sure the frequency responses match # Since bode() only handles SISO, go through each I/O pair # For phase, take sine and cosine to avoid +/- 360 offset for inputNum in range(inputs): for outputNum in range(outputs): if (verbose): print("Checking input %d, output %d" \ % (inputNum, outputNum)) ssorig_mag, ssorig_phase, ssorig_omega = \ control.bode(_mimo2siso(ssOriginal, \ inputNum, outputNum), \ deg=False, Plot=False) ssorig_real = ssorig_mag * np.cos(ssorig_phase) ssorig_imag = ssorig_mag * np.sin(ssorig_phase) # # Make sure TF has same frequency response # num = tfOriginal.num[outputNum][inputNum] den = tfOriginal.den[outputNum][inputNum] tforig = control.tf(num, den) tforig_mag, tforig_phase, tforig_omega = \ control.bode(tforig, ssorig_omega, \ deg=False, Plot=False) tforig_real = tforig_mag * np.cos(tforig_phase) tforig_imag = tforig_mag * np.sin(tforig_phase) np.testing.assert_array_almost_equal( \ ssorig_real, tforig_real) np.testing.assert_array_almost_equal( \ ssorig_imag, tforig_imag) # # Make sure xform'd SS has same frequency response # ssxfrm_mag, ssxfrm_phase, ssxfrm_omega = \ control.bode(_mimo2siso(ssTransformed, \ inputNum, outputNum), \ ssorig_omega, \ deg=False, Plot=False) ssxfrm_real = ssxfrm_mag * np.cos(ssxfrm_phase) ssxfrm_imag = ssxfrm_mag * np.sin(ssxfrm_phase) np.testing.assert_array_almost_equal( \ ssorig_real, ssxfrm_real) np.testing.assert_array_almost_equal( \ ssorig_imag, ssxfrm_imag) # # Make sure xform'd TF has same frequency response # num = tfTransformed.num[outputNum][inputNum] den = tfTransformed.den[outputNum][inputNum] tfxfrm = control.tf(num, den) tfxfrm_mag, tfxfrm_phase, tfxfrm_omega = \ control.bode(tfxfrm, ssorig_omega, \ deg=False, Plot=False) tfxfrm_real = tfxfrm_mag * np.cos(tfxfrm_phase) tfxfrm_imag = tfxfrm_mag * np.sin(tfxfrm_phase) np.testing.assert_array_almost_equal( \ ssorig_real, tfxfrm_real) np.testing.assert_array_almost_equal( \ ssorig_imag, tfxfrm_imag)
def obs(self): # whether the system is observable import control obs = control.obsv(self.A, self.C) rank = np.linalg.matrix_rank(obs) print("rank", rank, "n", self.n)
def observable(self): O_mat = control.obsv(self.F, self.H) d = numpy.linalg.matrix_rank(O_mat) return d
g = 9.8 # gravity d = 4.0 # dampping term ############################# # Linearized system and place pole A = np.array([[0, 1, 0, 0], [0, -d, -(g * m) / M, 0], [0, 0, 0, 1], [0, 0, (g * m + M * g) / (L * M), -0.1 * d]], dtype=np.float) B = np.array([[0], [1.0 / M], [0], [-1.0 / (L * M)]], dtype=np.float) C = np.array( [1, 0, 0, 0], dtype=np.float ) # only observable if x measured... because x can't be reconstructed O = control.obsv(A, C) print("Obsv A,C: ", O) print("rank(obsv): ", np.linalg.matrix_rank(O)) print('----------') print('Which measurements are best if we omit "x"') A = A[1:, 1:] B = B[1:] C = np.array([1, 0, 0], dtype=np.float) print('C: ', C) O = control.obsv(A, C) print("Obsv A,C: ", O) print("rank(obsv): ", np.linalg.matrix_rank(O)) C = np.array([0, 1, 0], dtype=np.float)
def check_ctrb_obsv(A, B, C): ctrb_matrix = ctrb(A, B.T) obsv_matrix = obsv(A, C) print("Controllability matrix rank: %d" % matrix_rank(ctrb_matrix)) print("Observability matrix rank: %d" % matrix_rank(obsv_matrix))
print np.matrix(B) A_eig = list(A.eigenvals().keys()) for i in range(0, len(A_eig)): A_eig[i]=sp.N(A_eig[i]) print "A_eig:=" print np.matrix(A_eig) # Controllability control_matrix = co.ctrb(A, B) control_matrix = sp.Matrix(control_matrix) # Observability: observe_matrix = co.obsv(A, C) observe_matrix = sp.Matrix(observe_matrix) print "Rank of control_matrix: "+str(control_matrix.rank()) print "Rank of observe_matrix: "+str(observe_matrix.rank()) ## tODO this is a problem!? # pole wish position: poles_wish = [-6, -6, -6, -6, -6, -6, -6, -6] A = np.matrix(A) B = np.matrix(B) C = np.matrix(C) A = A.astype(float) B = B.astype(float) C = C.astype(float)
import numpy as np from numpy.linalg import matrix_rank from control import ctrb, obsv g = 9.8 L = 0.43 # pendulum length d = 0.01 # friction coefficent A = np.matrix([[.0, 1., .0, .0], [-g / L, -d / L, .0, .0], [.0, .0, .0, 1.], [.0, .0, .0, .0]]) B = np.matrix([[.0, 1. / L, .0, 1.]]) C = np.matrix([[1.0, .0, 1., .0]]) ctrb_matrix = ctrb(A, np.transpose(B)) obsv_matrix = obsv(A, C) print "Ctrb matrix rank: %d" % matrix_rank(ctrb_matrix) print "Obsv matrix rank: %d" % matrix_rank(obsv_matrix)
def linearize_matricies(self, x, u, dt): """creates discritized linearized F,G,H & M matricies Inputs: x (array_like): linearization point-states u (array_like): linearization point-inputs dt (float): discrete interval """ dxdotdot_dxdot = -(self.A_x * self.Cd_x * self.rho * x[1]) / self.m dxdotdot_dtheta = -self.g * cos(x[10]) * cos(x[8]) + ( (self.Fb * cos(x[10]) * cos(x[8])) / self.m) dxdotdot_dpsi = self.g * sin(x[10]) * sin(x[8]) - ( (self.Fb * sin(x[10]) * sin(x[8])) / self.m) dxdotdot_dTL = 1 / self.m dxdotdot_dTR = 1 / self.m dydotdot_dydot = -(self.A_y * self.Cd_y * self.rho * x[3]) / self.m dydotdot_dphi = self.g * cos(x[6]) * cos(x[10]) - ( (self.Fb * cos(x[10]) * cos(x[6])) / self.m) dydotdot_dpsi = -self.g * sin(x[6]) * sin(x[10]) + ( (self.Fb * sin(x[10]) * sin(x[6])) / self.m) dydotdot_dTF = 1 / self.m dydotdot_dTB = 1 / self.m dzdotdot_dzdot = -(self.A_z * self.Cd_z * self.rho * x[5]) / self.m dzdotdot_dphi = -self.g * cos(x[8]) * sin(x[6]) + ( (self.Fb * cos(x[8]) * sin(x[6])) / self.m) dzdotdot_dtheta = -self.g * cos(x[6]) * sin(x[8]) + ( (self.Fb * cos(x[6]) * sin(x[8])) / self.m) dzdotdot_dTFL = 1 / self.m dzdotdot_dTFR = 1 / self.m dzdotdot_dTBL = 1 / self.m dzdotdot_dTBR = 1 / self.m dphidotdot_dydot=-.5*self.Ixx*self.rho*(2*self.A_yB*self.Cd_yB*self.dz_yB* \ (self.dz_yB*x[7]+x[3])+2*self.A_yT*self.Cd_yT*self.dz_yT*(self.dz_yT*x[7]+x[3])) dphidotdot_dzdot=-.5*self.Ixx*self.rho*(2*self.A_zL*self.Cd_zL*self.dy_zL* \ (self.dy_zL*x[7]+x[5])+2*self.A_zR*self.Cd_zR*self.dy_zR*(self.dy_zR*x[7]+x[5])) dphidotdot_dphi = (self.Fb / self.Ixx) * (self.phi_bz * cos(x[6]) * cos(x[8]) + self.phi_by * cos(x[8]) * sin(x[6])) dphidotdot_dphidot=-.5*self.Ixx*self.rho*(2*self.A_yB*self.Cd_yB*self.dz_yB**2* \ (self.dz_yB*x[7]+x[3])+2*self.A_yT*self.Cd_yT*self.dz_yT**2*(self.dz_yT*x[7]+x[3])+ \ 2*self.A_zL*self.Cd_zL*self.dy_zL**2*(self.dy_zL*x[7]+x[5])+2*self.A_zR*self.Cd_zR* \ self.dy_zR**2*(self.dy_zR*x[7]+x[5])) dphidotdot_dtheta = (self.Fb / self.Ixx) * (self.phi_by * cos(x[6]) * sin(x[8]) - self.phi_bz * sin(x[6]) * sin(x[8])) dphidotdot_dTFL = -self.phi_L / self.Ixx dphidotdot_dTFR = self.phi_R / self.Ixx dphidotdot_dTBL = -self.phi_L / self.Ixx dphidotdot_dTBR = self.phi_R / self.Ixx dthetadotdot_dxdot=-.5*self.Iyy*self.rho*(2*self.A_xB*self.Cd_xB*self.dz_xB* \ (self.dz_xB*x[9]+x[1])+2*self.A_xT*self.Cd_xT*self.dz_xT*(self.dz_xT*x[9]+x[1])) dthetadotdot_dzdot=-.5*self.Iyy*self.rho*(2*self.A_zB*self.Cd_zB*self.dx_zB* \ (self.dx_zB*x[9]+x[5])+2*self.A_zF*self.Cd_zF*self.dx_zF*(self.dx_zF*x[9]+x[5])) dthetadotdot_dphi = (-self.Fb / self.Iyy) * (self.theta_bx * cos(x[8]) * sin(x[6])) dthetadotdot_dtheta = (self.Fb / self.Iyy) * ( self.theta_bz * cos(x[8]) - self.theta_bx * cos(x[6]) * sin(x[8])) dthetadotdot_dthetadot=-.5*self.Iyy*self.rho*(2*self.A_xB*self.Cd_xB*self.dz_xB**2* \ (self.dz_xB*x[9]+x[1])+2*self.A_xT*self.Cd_xT*self.dz_xT**2*(self.dz_xT*x[9]+x[1])+ \ 2*self.A_zB*self.Cd_zB*self.dx_zB**2*(self.dx_zB*x[9]+x[5])+2*self.A_zF*self.Cd_zF* \ self.dx_zF**2*(self.dx_zF*x[9]+x[5])) dthetadotdot_dTFL = self.theta_F / self.Iyy dthetadotdot_dTFR = self.theta_F / self.Iyy dthetadotdot_dTBL = -self.theta_B / self.Iyy dthetadotdot_dTBR = -self.theta_B / self.Iyy dpsidotdot_dxdot=-.5*self.Izz*self.rho*(2*self.A_xL*self.Cd_xL*self.dy_xL* \ (self.dy_xL*x[11]+x[1])+2*self.A_xR*self.Cd_xR*self.dy_xR*(self.dy_xR*x[11]+x[1])) dpsidotdot_dydot=-.5*self.Izz*self.rho*(2*self.A_yBa*self.Cd_yBa*self.dx_yBa* \ (self.dx_yBa*x[11]+x[3])+2*self.A_yF*self.Cd_yF*self.dx_yF*(self.dx_yF*x[11]+x[3])) dpsidotdot_dphi = (self.Fb / self.Izz) * (self.psi_bx * cos(x[8]) * sin(x[6])) dpsidotdot_dtheta = (self.Fb / self.Izz) * ( -self.psi_by * cos(x[8]) - self.psi_bx * sin(x[6]) * sin(x[8])) dpsidotdot_dpsidot=-.5*self.Izz*self.rho*(2*self.A_xL*self.Cd_xL*self.dy_xL**2* \ (self.dy_xL*x[11]+x[1])+2*self.A_xR*self.Cd_xR*self.dy_xR**2*(self.dy_xR*x[11]+x[1])+ \ 2*self.A_yBa*self.Cd_yBa*self.dx_yBa**2*(self.dx_yBa*x[11]+x[3])+2*self.A_yF*self.Cd_yF* \ self.dx_yF**2*(self.dx_yF*x[11]+x[3])) dpsidotdot_dTL = self.psi_L / self.Izz dpsidotdot_dTR = -self.psi_R / self.Izz dpsidotdot_dTF = self.psi_F / self.Izz dpsidotdot_dTB = -self.psi_B / self.Izz # A should be a 12x12 # B should be a 12x8 # C should be a 10x12 (12x12 with DVL) # D should be a 10x8 (12x8 with DVL) A = np.array([[0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [ 0, dxdotdot_dxdot, 0, 0, 0, 0, 0, 0, dxdotdot_dtheta, 0, dxdotdot_dpsi, 0 ], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], [ 0, 0, 0, dydotdot_dydot, 0, 0, dydotdot_dphi, 0, 0, 0, dydotdot_dpsi, 0 ], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [ 0, 0, 0, 0, 0, dzdotdot_dzdot, dzdotdot_dphi, 0, dzdotdot_dtheta, 0, 0, 0 ], [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [ 0, 0, 0, dphidotdot_dydot, 0, dphidotdot_dzdot, dphidotdot_dphi, dphidotdot_dphidot, dphidotdot_dtheta, 0, 0, 0 ], [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0], [ 0, dthetadotdot_dxdot, 0, 0, 0, dthetadotdot_dzdot, dthetadotdot_dphi, 0, dthetadotdot_dtheta, dthetadotdot_dthetadot, 0, 0 ], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1], [ 0, dpsidotdot_dxdot, 0, dpsidotdot_dydot, 0, 0, dpsidotdot_dphi, 0, dpsidotdot_dtheta, 0, dxdotdot_dpsi, dpsidotdot_dpsidot ]]) B = np.array([[0, 0, 0, 0, 0, 0, 0, 0], [dxdotdot_dTL, dxdotdot_dTR, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0], [0, 0, dydotdot_dTF, dydotdot_dTB, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0], [ 0, 0, 0, 0, dzdotdot_dTFL, dzdotdot_dTFR, dzdotdot_dTBL, dzdotdot_dTBR ], [0, 0, 0, 0, 0, 0, 0, 0], [ 0, 0, 0, 0, dphidotdot_dTFL, dphidotdot_dTFR, dphidotdot_dTBL, dphidotdot_dTBR ], [0, 0, 0, 0, 0, 0, 0, 0], [ 0, 0, 0, 0, dthetadotdot_dTFL, dthetadotdot_dTFR, dthetadotdot_dTBL, dthetadotdot_dTBR ], [0, 0, 0, 0, 0, 0, 0, 0], [ dpsidotdot_dTL, dpsidotdot_dTR, dpsidotdot_dTF, dpsidotdot_dTB, 0, 0, 0, 0 ]]) C = np.array([[ 0, dxdotdot_dxdot, 0, 0, 0, 0, 0, 0, dxdotdot_dtheta, 0, dxdotdot_dpsi, 0 ], [ 0, 0, 0, dydotdot_dydot, 0, 0, dydotdot_dphi, 0, 0, 0, dydotdot_dpsi, 0 ], [ 0, 0, 0, 0, 0, dzdotdot_dzdot, dzdotdot_dphi, 0, dzdotdot_dtheta, 0, 0, 0 ], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0]]) D = np.array([[dxdotdot_dTL, dxdotdot_dTR, 0, 0, 0, 0, 0, 0], [0, 0, dydotdot_dTF, dydotdot_dTB, 0, 0, 0, 0], [ 0, 0, 0, 0, dzdotdot_dTFL, dzdotdot_dTFR, dzdotdot_dTBL, dzdotdot_dTBR ], [0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0]]) if np.linalg.matrix_rank(control.ctrb(A, B)) < A.shape[0]: print "System is not fully controllable" else: print "System is fully controllable" # print np.sum(control.obsv(A,C),axis=1) if np.linalg.matrix_rank(control.obsv(A, C)) < A.shape[0]: print "System is not fully observable" else: print "System is fully observable" state_space = StateSpace(A, B, C, D, dt) return state_space
#################################################### # Observer #################################################### # Observer design obs_th_wn = 5.0 * th_wn obs_z_wn = 5.0 * z_wn obs_h_wn = 5.0 * h_wn obs_des_char_poly_lat = np.convolve( [1, 2.0 * z_zeta * obs_z_wn, obs_z_wn**2], [1, 2.0 * th_zeta * obs_th_wn, obs_th_wn**2]) obs_des_char_poly_lon = [1, 2.0 * h_zeta * obs_h_wn, obs_h_wn**2] obs_des_poles_lat = np.roots(obs_des_char_poly_lat) obs_des_poles_lon = np.roots(obs_des_char_poly_lon) if np.linalg.matrix_rank(cnt.obsv(A_lat, C_lat)) != 4: print('Latitudinal System Not Observable') else: L_lat = place(A_lat.T, C_lat.T, obs_des_poles_lat).gain_matrix.T print('L_lat:', L_lat) if np.linalg.matrix_rank(cnt.obsv(A_lon, C_lon)) != 2: print('Longitudinal System Not Observable') else: L_lon = place(A_lon.T, C_lon.T, obs_des_poles_lon).gain_matrix.T print('L_lon:', L_lon) ################################################# # Uncertainty Parameters ################################################# UNCERTAINTY_PARAMETERS = True
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))
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
def main(): init_data = None with open('/vagrant/client/init_data.json') as f: init_data = json.load(f) num_states = init_data['num_states'] if type(num_states) is not int: raise TypeError("num_states must be int") num_inputs = init_data['num_inputs'] if type(num_inputs) is not int: raise TypeError("num_inputs must be int") delay = init_data['delay'] if type(delay) is not int: raise TypeError("delay must be int") nonlinearity = init_data['nonlinearity'] if type(nonlinearity) is not str: raise TypeError("nonlinearity must be str") error_dist_mu = init_data['error_dist_mu'] if type(error_dist_mu) is not float: raise TypeError("error_dist_mu must be float") error_dist_sigma = init_data['error_dist_sigma'] if type(error_dist_sigma) is not float: raise TypeError("error_dist_sigma must be float") feed_forward = init_data['feed_forward'] if type(feed_forward) is not list: raise TypeError("feed_forward must be list") A = init_data['A'] if type(A) is not list: raise TypeError("A must be list") B = init_data['B'] if type(B) is not list: raise TypeError("B must be list") C = init_data['C'] if type(C) is not list: raise TypeError("C must be list") D = init_data['D'] if type(D) is not list: raise TypeError("D must be list") controller = Controller(num_states, num_inputs) if not controller.set_matrix('A', A): sys.exit() if not controller.set_matrix('B', B): sys.exit() if not controller.set_matrix('C', C): sys.exit() if not controller.set_matrix('D', D): sys.exit() controller.calculate_observer_controller() dyn_process_session = DynamicProcessSession() dyn_process_session.set_num_states(json.dumps(num_states)) dyn_process_session.set_num_outputs(json.dumps(num_inputs)) dyn_process_session.set_delay(json.dumps(delay)) dyn_process_session.set_nonlinearity(nonlinearity) dyn_process_session.set_error_dist_mu(error_dist_mu) dyn_process_session.set_error_dist_sigma(error_dist_sigma) dyn_process_session.set_coefficient('A', json.dumps(A)) dyn_process_session.set_coefficient('B', json.dumps(B)) dyn_process_session.set_coefficient('C', json.dumps(C)) W_c = control.ctrb(controller.A, controller.B) W_o = control.obsv(controller.A, controller.C) print('The rank of controlability matrix is: {}'.format( np.linalg.matrix_rank(W_c))) print('The rank of observability matrix is: {}'.format( np.linalg.matrix_rank(W_o))) queue_ = queue.Queue() commands = Commands(logger, queue_) commands_thread = CommandsThread(commands) commands_thread.start() t0 = time.perf_counter() freq_counter = 0 freq = 0 while(True): freq_counter += 1 t = time.perf_counter() if (t - t0) > 1: freq = freq_counter freq_counter = 0 t0 = t logger.info("freq: {}".format(freq)) logger.info('------------------') y = dyn_process_session.get_output() y = json_to_np(y) logger.info('y: {}'.format(y)) u = controller.get_control_signal(y, feed_forward) dyn_process_session.set_input(np_to_json(u)) time.sleep(0.1) logger.info("====================================") try: [action, matrix_name, values] = queue_.get_nowait() if action is 'exit': sys.exit() getattr(controller, action)(matrix_name, values) except queue.Empty: pass
Q = np.eye(4,4) R = np.eye(2,2) #S1 = sp.linalg.solve_continuous_are(A, B, Q, R) #K1 = np.linalg.inv(R).dot(B.T).dot(S1) #E1 = np.linalg.eigvals(A-B.dot(K1)) #print("S1 =\n", S1) #print("K1 =\n", K1) #print("E1 =\n", E1) S2, E2, K2 = ct.care(Ap, Bp, Q, R) #print("\nS2 =\n", S2) #print("K2 =\n", K2) #print("E2 =\n", E2) print "ctrb = "+ str(np.linalg.matrix_rank(ct.ctrb(Ap,Bp))) print "obsv = " + str(np.linalg.matrix_rank(ct.obsv(Ap,Cp))) #K3, S3, E3 = ct.matlab.lqr(Ap, Bp, Q, R) #print("\nS3 =\n", S3) #print("K3 =\n", K3) #print("E3 =\n", E3) ss_P = ct.ss(Ap,Bp,Cp,Dp) print ss_P t = np.linspace(0, 3, 1000) stepy = ct.step(ss_P,t) #plt.plot(t,stepy) #ct.impulse(ss_P) ct.nyquist(ss_P) #bode plot only implemented for SISO system.omg #ct.bode(ss_P)
Aa = np.array([[A[0, 0], A[0, 1], 0], [A[1, 0], A[1, 1], 0], [np.dot(C, A)[0], np.dot(C, A)[1], 1]]) Ba = np.array([[B[0, 0], B[0, 1]], [B[1, 0], B[1, 1]], [np.dot(C, B)[0], np.dot(C, B)[1]]]) Ca = np.array([0, 0, 1]) Da = 0 # %% Controllability and Observability analysis Co = control.ctrb(Aa, Ba) if matrix_rank(Co) == len(Aa): print('The augmented system is controllable.') Ob = control.obsv(Aa, Ca) if matrix_rank(Ob) == len(Aa): print('The augmented system is observable.') # %% LQR controller # y | Delta*x1 | Delta*x2 Qlqr = np.diag([1, 100, 1]) Rlqr = 1 * np.eye(2) # K, X, eigVals = dlqr(Aa, Ba, Qlqr, Rlqr) # Recursive Riccati Difference Equation (RDE) P = 100 * np.eye(3) for k in range(300): P = np.dot(np.dot(Aa.T, P), Aa) - np.dot(
C, Qe, Re = model.compute_noise_cov(sys_i, sens) print('\n Sensors: ', sens) print('\nQe = ') print(Qe) print('eig(Qe)') print(np.linalg.eig(Qe)[0]) print('\nRe = ') print(Re) print('eig(Re)') print(np.linalg.eig(Re)[0]) print('\nRank of observability matrix: %d of %d' % (np.linalg.matrix_rank(control.obsv(model.rsys[sys_i].A, C)), nx)) Ts = 5 print('Mode frequencies:') print(np.abs(np.angle(np.diag(model.sys_eig[sys_i]))) / (2.0 * np.pi * Ts)) """ Save full model & final model """ pickle.dump(model, open('data/model_full.p', 'wb')) final_model = [model.rsys[sys_i], C, Qe, Re, sens] pickle.dump(final_model, open('data/model_sparse.p', 'wb')) """ Plot results """
Q = np.eye(4, 4) R = np.eye(2, 2) #S1 = sp.linalg.solve_continuous_are(A, B, Q, R) #K1 = np.linalg.inv(R).dot(B.T).dot(S1) #E1 = np.linalg.eigvals(A-B.dot(K1)) #print("S1 =\n", S1) #print("K1 =\n", K1) #print("E1 =\n", E1) S2, E2, K2 = ct.care(Ap, Bp, Q, R) #print("\nS2 =\n", S2) #print("K2 =\n", K2) #print("E2 =\n", E2) print "ctrb = " + str(np.linalg.matrix_rank(ct.ctrb(Ap, Bp))) print "obsv = " + str(np.linalg.matrix_rank(ct.obsv(Ap, Cp))) #K3, S3, E3 = ct.matlab.lqr(Ap, Bp, Q, R) #print("\nS3 =\n", S3) #print("K3 =\n", K3) #print("E3 =\n", E3) ss_P = ct.ss(Ap, Bp, Cp, Dp) print ss_P t = np.linspace(0, 3, 1000) stepy = ct.step(ss_P, t) #plt.plot(t,stepy) #ct.impulse(ss_P) ct.nyquist(ss_P) #bode plot only implemented for SISO system.omg #ct.bode(ss_P)
import sys sys.path.append('..') # add parent directory import matplotlib.pyplot as plt import control import pendulumParam as P from kalmanFilter import kalmanFilter, kalmanFilterEC from pendulumNonlinearDynamics import Pendulum from pendulumAnimation import pendulumAn from plotDataZ import plotData from signalGenerator import signalGen #Compute controller dpoles = np.array([-1.1, -1.2, -1.3, -1.4]) Kr = control.place(P.A * 1.01, P.B * 1.01, dpoles) print(control.obsv(P.A, P.C)) # which states of A, C are observable? print(np.linalg.matrix_rank(control.obsv( P.A, P.C))) # expect a rank of 0 if observable #Initialize and rename for convenience ref = signalGen(amplitude=.5, frequency=0.05, y_offset=0) pendulum = Pendulum(param=P) states = [np.array([[P.z0], [P.zdot0], [P.theta0], [P.thetadot0]])] states_est = [states[0]] mu = np.array([[P.z0 + 0.1], [P.zdot0 + 0.1], [P.theta0 + 0.1], [P.thetadot0 + 0.1]]) sigma = np.eye(4) * 0.00001 u = 0 #performs very simple first order integration
# _p = [-3.0, -3.1, -3.2, -3.3] # aggressive # _p = [-3.5, -3.6, -3.7, -3.8] # breaks # _p = [-10,-10.1,-10.2,-10.3] # breaks # _p = [-10,-10.1,-10.2,-10.3] # breaks p = np.array(_p) x0 = np.array([0, 0, -np.pi + 0.1, 1]) # intial x Q = np.array([[10, 0, 0, 0], [0, 1, 0, 0], [0, 0, 2, 0], [0, 0, 0, 2]]) R = 0.1 # K = control.place(A, B, p) K = control.lqr(A, B, Q, R)[0] # controllability = control.ctrb(A, B) observability = control.obsv(A, C) print(observability) sys = control.ss(A, B, C, D) def xDot(t, y): # print(t) error = y - [0, 0, -np.pi, 0] u = -K * ((error)[np.newaxis].T) return cartpend(y, m, M, L, g, d, u[0][0]) t0 = 0 t1 = 2 dt = 0.02
def dare(F, G1, G2, H): """Solves the discrete-time algebraic Riccati equation 0 = F ^ T * X * F - X - F ^ T * X * G1 * (G2 + G1 ^ T * X * G1) ^ -1 * G1 ^ T * X * F + H Under the assumption that X ^ -1 exists, this equation is equivalent to 0 = F ^ T * (X ^ -1 + G1 * G2 ^ -1 * G1 ^ T) ^ -1 * F - X + H Parameters ========== Inputs are real matrices: F : n x n G1 : n x m G2 : m x m, symmetric, positive definite H : n x n, symmetric, positive semi-definite Assumptions =========== (F, G1) is a stabilizable pair (C, F) is a detectable pair (where C is full rank factorization of H, i.e., C ^ T * C = H and rank(C) = rank(H). F is invertible Returns ======= Unique nonnegative definite solution of discrete Algrebraic Ricatti equation. Notes ===== This is an implementation of the Schur method for solving algebraic Riccati eqautions as described in dx.doi.org/10.1109/TAC.1979.1102178 """ # Verify that F is non-singular u, s, v = la.svd(F) assert(np.all(s > 0.0)) # Verify that (F, G1) controllable C = ctrb(F, G1) u, s, v = la.svd(C) assert(np.all(s > 0.0)) # Verify that (H**.5, F) is observable O = obsv(H**.5, F) u, s, v = la.svd(O) assert(np.all(s > 0.0)) n = F.shape[0] m = G2.shape[0] G = np.dot(G1, np.dot(inv(G2), G1.T)) Finv = inv(F) Finvt = Finv.T # Form symplectic matrix Z = empty((2*n, 2*n)) Z[:n, :n] = F + np.dot(G, np.dot(Finvt, H)) Z[:n, n:] = -np.dot(G, Finvt) Z[n:, :n] = -np.dot(Finvt, H) Z[n:, n:] = Finvt S, U, sdim = schur(Z, sort='iuc') # Verify that the n eigenvalues of the upper left block stable assert(sdim == n) U11 = U[:n, :n] U21 = U[n:, :n] return solve(U[:n, :n].T, U[n:, :n].T).T