def get_closed_loop_gain(self, p=None): """ Get the closed loop gain for the specified poles. :param p: pole list, defaults to self.p :type p: [type], optional :return: [description] :rtype: [type] """ if p is None: p = self.poles A = np.array(self.A).tolist() B = np.array(self.B).tolist() L = place(A, B, p) # Set L as ca.DM self.L = ca.DM.zeros(1, 4) self.L[0, 0] = L[0, 0] self.L[0, 1] = L[0, 1] self.L[0, 2] = L[0, 2] self.L[0, 3] = L[0, 3] return self.L
def e_12(): print('\n--E--\n') bnb = Sim.BallAndBeam() max_force = 15 damping_ratio = 1 / (2**(1 / 2)) # Tuning variables rise_time_theta = .5 bandwidth_separation = 2.4 integrator_pole = 2. natural_frequency_theta = np.pi / (2 * rise_time_theta * (1 - damping_ratio**2)**(1 / 2)) rise_time_z = rise_time_theta * bandwidth_separation natural_frequency_z = np.pi / (2 * rise_time_z * (1 - damping_ratio**2)**(1 / 2)) coefs_theta = [ 1, 2 * damping_ratio * natural_frequency_theta, natural_frequency_theta**2 ] coefs_z = [ 1, 2 * damping_ratio * natural_frequency_z, natural_frequency_z**2 ] coefs_I = [1, integrator_pole] roots = np.roots(np.convolve(coefs_I, np.convolve(coefs_theta, coefs_z))) A = bnb.dynamics.A B = bnb.dynamics.B C = bnb.dynamics.C A_I, B_I = augment_matrices(A, B, C[0:1, :]) gains = ctrl.place(A_I, B_I, roots) dim = A.shape[1] feedback_gain = gains[0:1, 0:dim] integrator_gain = gains[0, dim] print( f'Feedback Gain: {feedback_gain}\nIntegrator Gain: {integrator_gain}') bnb.add_controller(Control.BallAndBeam, feedback_gain, integrator_gain, max_force=max_force) requests = sg.generator(sg.square, amplitude=0.15, y_offset=0.25, frequency=0.1, t_step=bnb.seconds_per_sim_step, t_final=90) handle = bnb.view_animation(requests) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handle
def d_11(): print('\n--D--\n') msd = Sim.MassSpringDamper() max_force = 2.0 damping_ratio = 1 / (2**(1 / 2)) rise_time = 2.0 # mass = msd.dynamics.mass # spring_const = msd.dynamics.spring_const # natural_frequency = ((spring_const + max_force) / mass) ** (1 / 2) natural_frequency = np.pi / (2 * rise_time * (1 - damping_ratio**2)**(1 / 2)) # Part A coefs = [1, 2 * damping_ratio * natural_frequency, natural_frequency**2] # Small error roots = np.roots(coefs) print(f"Roots: {roots}") # Part B A = msd.dynamics.A B = msd.dynamics.B C = msd.dynamics.C # Part C control_mat = ctrl.ctrb(A, B) rank = np.linalg.matrix_rank(control_mat) order = A.shape[0] print(f"controllability matrix rank: {rank}") print(f"System order: {order}") print(f"{'Controllable' if rank == order else 'Not Controllable'}") # Part D feedback_gain = ctrl.place(A, B, roots) print(f'Feedback Gain: {feedback_gain}') reference_gain = -1 / (C @ np.linalg.inv(A - B @ feedback_gain) @ B) print(f"Reference gain: {reference_gain}") # Part E msd.add_controller(SSController.MassSpringDamper, feedback_gain, reference_gain, max_force) request = sg.generator(sg.constant, amplitude=2 / 3, t_step=msd.seconds_per_sim_step, t_final=20) handle = msd.view_animation(request) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handle
def d_12(): print('\n--D--\n') msd = Sim.MassSpringDamper() max_force = 2.0 damping_ratio = 1 / (2**(1 / 2)) rise_time = 2.0 # mass = msd.dynamics.mass # spring_const = msd.dynamics.spring_const # natural_frequency = ((spring_const + max_force) / mass) ** (1 / 2) natural_frequency = np.pi / (2 * rise_time * (1 - damping_ratio**2)**(1 / 2)) coefs = [1, 2 * damping_ratio * natural_frequency, natural_frequency**2] integrator_root = 1.0 roots = np.roots(np.convolve(coefs, [1, integrator_root])) print(f"Roots: {roots}") # Part B A = msd.dynamics.A B = msd.dynamics.B C = msd.dynamics.C A_I, B_I = augment_matrices(A, B, C) gains = ctrl.place(A_I, B_I, roots) feedback_gain = gains[:, 0:2] integrator_gain = gains[0, 2] print( f'Feedback Gain: {feedback_gain}\nIntegrator Gain: {integrator_gain}') msd.add_controller(Control.MassSpringDamper, feedback_gain, integrator_gain, max_force) request = sg.generator(sg.constant, amplitude=2 / 3, t_step=msd.seconds_per_sim_step, t_final=20) handle = msd.view_animation(request) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handle
def f_12(): print('\n--F--\n') pvt = Sim.PlanarVTOL() max_force = 10 damping_ratio = 1 / (2**(1 / 2)) + 0.2 # Tuning variable rise_time_h = 2.5 rise_time_theta = 0.1 bandwidth_separation = 7 integrator_pole_lat = 1.5 integrator_pole_lon = 1 natural_frequency_h = np.pi / (2 * rise_time_h * (1 - damping_ratio**2)**(1 / 2)) natural_frequency_theta = np.pi / (2 * rise_time_theta * (1 - damping_ratio**2)**(1 / 2)) rise_time_z = bandwidth_separation * rise_time_theta natural_frequency_z = np.pi / (2 * rise_time_z * (1 - damping_ratio**2)**(1 / 2)) coefs_theta = [ 1, 2 * damping_ratio * natural_frequency_theta, natural_frequency_theta**2 ] coefs_z = [ 1, 2 * damping_ratio * natural_frequency_z, natural_frequency_z**2 ] coefs_I_lat = [1, integrator_pole_lat] roots_lat = np.roots( np.convolve(coefs_I_lat, np.convolve(coefs_theta, coefs_z))) print(f"Latitudinal roots: {roots_lat}") coefs_h = [ 1, 2 * damping_ratio * natural_frequency_h, natural_frequency_h * 2 ] coefs_I_lon = [1, integrator_pole_lon] roots_lon = np.roots(np.convolve(coefs_h, coefs_I_lon)) print(f"Longitudinal roots: {roots_lon}") # Part B A_lat = pvt.dynamics.A_lat B_lat = pvt.dynamics.B_lat C_lat = pvt.dynamics.C_lat A_I_lat, B_I_lat = augment_matrices(A_lat, B_lat, C_lat[0:1, :]) A_lon = pvt.dynamics.A_lon B_lon = pvt.dynamics.B_lon C_lon = pvt.dynamics.C_lon A_I_lon, B_I_lon = augment_matrices(A_lon, B_lon, C_lon) gains_lat = ctrl.place(A_I_lat, B_I_lat, roots_lat) dim_lat = A_lat.shape[1] feedback_gain_lat = gains_lat[0:1, 0:dim_lat] int_gain_lat = gains_lat[0, dim_lat] print( f'Latitudinal feedback Gain: {feedback_gain_lat}\nLatitudinal integrator gain: {int_gain_lat}' ) gains_lon = ctrl.place(A_I_lon, B_I_lon, roots_lon) dim_lon = A_lon.shape[1] feedback_gain_lon = gains_lon[0:1, 0:dim_lon] int_gain_lon = gains_lon[0, dim_lon] print( f'Longitudinal feedback Gain: {feedback_gain_lon}\nLongitudinal integrator gain: {int_gain_lon}' ) request = zip( sg.generator(sg.constant, y_offset=1, t_step=pvt.seconds_per_sim_step, t_final=20), sg.generator(sg.sin, frequency=0.08, y_offset=0, amplitude=2.5, t_step=pvt.seconds_per_sim_step, t_final=20), ) pvt.add_controller(Control.PlanarVTOL, feedback_gain_lat, int_gain_lat, feedback_gain_lon, int_gain_lon, max_force) handel = pvt.view_animation(request) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handel
def testPlace(self, siso): """Call place()""" place(siso.ss1.A, siso.ss1.B, [-2, -2.5])
def main(): # 物理パラメータの定義 ------------- M = 5 m = 0.4 l = 0.3 D_theta = 0.001 D_x = 0.002 g = 9.80665 N = (4. * M + m) * l / 3. # ---------------------------- # システム行列の定義 -------------- A = np.array( [[0., 0., 1., 0.], [0., 0., 0., 1.], [0., -m * g * l / N, -4 * l * D_x / (3 * N), D_theta / N], [0., (M + m) * g / N, D_x / N, -(M + m) * D_theta / (N * m * l)]]) B = np.array([[0.], [0.], [4. * l / (3. * N)], [-1. / N]]) #C = np.eye(2, 4) C = np.array([[0., 0., 0., 1.]]) D = np.array([[0]]) # ----------------------------- # 最適レギュレータ重みの定義 -------------- R = np.array([[1]]) Q = np.diag([0.0, 0.0, 0.0, 1.0]) #Q = np.diag([1.0, 10000.0, 1.0]) #Q = np.sqrt(C.T @ C) # ----------------------------------- # オブザーバシステム行列の定義 ------------ p_obs = np.array([-10, -20, -10, -10]) # 2次元配列にするとplaceでエラー出る #p_obs = np.array([-30, -25, -20]) # 2次元配列にするとplaceでエラー出る K_obs = place(A.T, C.T, p_obs) A_obs = A - K_obs.T @ C B_obs = np.c_[B, K_obs.T] C_obs = np.eye(4, 4) # ----------------------------------- """ A = np.array([[1.1, 2.0], [0.0, 0.95]]) B = np.array([[0], [0.0787]]) C = np.array([[-2, 1]]) D = np.array([[0]]) #Q = np.diag([5, 3]) Q = C.T @ C R = np.array([[1]]) """ print("A: ") print(A) print("B: ") print(B) print("C: ") print(C) print("Q: ") print(Q) print("R: ") print(R) print("p_obs: ") print(p_obs) print("K_obs: ") print(K_obs) # システムが可制御・可観測でなければ終了 if check_ctrb(A, B) == -1: print("システムが可制御でないので終了") return 0 #if check_obsv(A, C) == -1 : # print("システムが可観測でないので終了") # return 0 # 最適レギュレータの設計 K, P, e = _clqr(A, B, Q, R) # 結果表示 print("リカッチ方程式の解:\n", P) print("状態フィードバックゲイン:\n", K) print("閉ループ系の固有値:\n", e) dt = 0.001 simtime = 10 time = 0.0 x = np.array([[0.0], [0.1], [0.0], [0.0]]) u = np.zeros([B.shape[1]]) y = np.zeros([C.shape[0], 1]) t_history = [] dx_history = np.zeros([0, len(x)]) x_history = np.zeros([0, len(x)]) y_history = np.zeros([0, len(y)]) u_history = np.zeros([0, len(u)]) x_ = np.array([[0.0], [0.0], [0.0], [0.0]]) y_ = np.zeros([C.shape[0], 1]) dx__history = np.zeros([0, len(x)]) x_history = np.zeros([0, len(x)]) #y__history = np.zeros([0, len(y_)]) #u__history = np.zeros([0, len(u_)]) while (time <= simtime): # プラント側の計算 ------------------ u = -K @ x_ # 最適ゲインによる状態フィードバック dx = A @ x + B @ u # 状態微分 x = x + dx * dt #+ 0.01*np.random.randn(4, 1) # 状態遷移(オイラー積分) y = C @ x #+ D @ u # 状態観測 # ------------------------------- # オブザーバ側の計算 ---------------- dx_ = A @ x_ + B @ u + K_obs.T @ (y - y_) x_ = x_ + dx_ * dt y_ = C @ x_ # ------------------------------- dx_history = np.r_[dx_history, dx.T] x_history = np.r_[x_history, x.T] y_history = np.r_[y_history, y.T] u_history = np.r_[u_history, u.T] t_history.append(time) time += dt plt.figure() plt.subplot(211) plt.plot(t_history, u_history, color="blue", label=u"$\ u $") plt.ylabel(u"$u(t)$", fontsize=16) plt.grid(True) plt.legend() plt.tight_layout() plt.subplot(212) plt.plot(t_history, x_history[:, 0], label=u"$\ x $") plt.plot(t_history, x_history[:, 1], label=u"$\ θ $") plt.plot(t_history, x_history[:, 2], label=u"$\ \.{x} $") plt.plot(t_history, x_history[:, 3], label=u"$\ \.{θ} $") plt.xlabel(u"$t [sec]$", fontsize=16) plt.ylabel(u"$x(t)$", fontsize=16) plt.grid(True) plt.legend() plt.tight_layout() plt.show()
def __init__(self): try: param_namespace = '/whirlybird' self.param = rospy.get_param(param_namespace) except KeyError: rospy.logfatal('Parameters not set in ~/whirlybird namespace') rospy.signal_shutdown('Parameters not set') g = self.param['g'] l1 = self.param['l1'] l2 = self.param['l2'] m1 = self.param['m1'] m2 = self.param['m2'] d = self.param['d'] h = self.param['h'] r = self.param['r'] Jx = self.param['Jx'] Jy = self.param['Jy'] Jz = self.param['Jz'] km = self.param['km'] self.r_in = 0.0 self.l_in = 0.0 self.theta_e = 0 self.Fe = 0.85 * (m1 * l1 - m2 * l2) * (g / l1) # initialize xhat for lat and lon # specify rise time, damping, omega, and observer omega for theta, phi and psi tr_phi = 0.3 / 5.0 # 5 times faster than controller rise times tr_psi = tr_phi * 9.0 tr_theta = 1.0 / 5.5 zeta = 0.707 wn_phi = 2.2 / tr_phi wn_psi = 2.2 / tr_psi wn_theta = 2.2 / tr_theta theta = 0.0 # Create A, B, C and L matrices for lateral and Longitude self.Alon = np.matrix([[0, 1], [(m1 * l1 - m2 * l2) * g * sin(theta) / (m1 * l1**2 + m2 * l2**2 + Jy), 0]]) self.Blon = np.matrix([[0], [l1 / (m1 * l1**2 + m2 * l2**2 + Jy)]]) self.Clon = np.matrix([[1, 0]]) desiredpoles_lon = np.roots([1.0, 2.0 * wn_theta * zeta, wn_theta**2]) self.Llon = ctrl.place(self.Alon.T, self.Clon.T, desiredpoles_lon) self.Llon = self.Llon.T # what the poles should be #self.Llon = np.matrix([[17.1094], # [146.4]]) # poles are currently [[17.1],[0.49]] print 'Llon', self.Llon self.Alat = np.matrix( [[0, 0, 1, 0], [0, 0, 0, 1], [0, 0, 0, 0], [l1 * self.Fe / (m1 * l1**2 + m2 * l2**2 + Jz), 0, 0, 0]]) self.Blat = np.matrix([[0], [0], [1 / Jx], [0]]) self.Clat = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0]]) desiredpoles_lat = np.roots( np.convolve([1, 2 * wn_psi * zeta, wn_psi**2], [1, 2 * wn_phi * zeta, wn_phi**2])) self.Llat = ctrl.place(self.Alat.T, self.Clat.T, desiredpoles_lat) self.Llat = self.Llat.T self.xhat_lon = np.matrix([[0.], [0.]]) self.xhat_lat = np.matrix([[0.], [0.], [0.], [0.]]) print 'Llat', self.Llat self.prev_time = rospy.Time.now() self._sub_ = rospy.Subscriber('whirlybird', Whirlybird, self.whirlybirdCallback, queue_size=5) self.command_sub_ = rospy.Subscriber('command', Command, self.commandCallback, queue_size=5) self._pub_ = rospy.Publisher('estimator', Twist, queue_size=5) while not rospy.is_shutdown(): # wait for new messages and call the callback when they arrive rospy.spin()
# 最適レギュレータの設計 K, P, e = _clqr(A, B, Q, R) # 結果表示 print("リカッチ方程式の解:\n", P) print("状態フィードバックゲイン:\n", K) print("閉ループ系の固有値:\n", e) dt = 0.015 simtime = 10 time = 0.0 # オブザーバシステム行列の定義 ------------ p_obs = np.array([-17.1+0j, -17.3-0j, -17.0+0j, -17.5-0j]) # 2次元配列にするとplaceでエラー出る #p_obs = 1 * e.real # 2次元配列にするとplaceでエラー出る K_obs = place(A.T, C.T, p_obs) A_obs = A - K_obs.T @ C B_obs = np.c_[B, K_obs.T] C_obs = np.eye(4, 4) print("A_obs:\n", A_obs) print("B_obs:\n", B_obs) eval_obs, evec_obs = la.eig(A_obs) print("オブザーバの固有値:\n", eval_obs) print("オブザーバの状態フィードバックゲイン:\n", K_obs) # ----------------------------------- """ # カルマンフィルタ型 オブザーバの重みの定義 ------ #V = np.array([[0.3, 0.5, 0.002, 0.1]])
def e_11(): print('\n--E--\n') bnb = Sim.BallAndBeam() max_force = 15 damping_ratio = 1 / (2**(1 / 2)) # Tuning variables rise_time_theta = .5 bandwidth_separation = 2.4 natural_frequency_theta = np.pi / (2 * rise_time_theta * (1 - damping_ratio**2)**(1 / 2)) rise_time_z = rise_time_theta * bandwidth_separation natural_frequency_z = np.pi / (2 * rise_time_z * (1 - damping_ratio**2)**(1 / 2)) # Part A coefs_theta = [ 1, 2 * damping_ratio * natural_frequency_theta, natural_frequency_theta**2 ] coefs_z = [ 1, 2 * damping_ratio * natural_frequency_z, natural_frequency_z**2 ] roots = np.concatenate(( np.roots(coefs_theta), np.roots(coefs_z), )) # Part B A = bnb.dynamics.A B = bnb.dynamics.B C = bnb.dynamics.C # Part C control_mat = ctrl.ctrb(A, B) rank = np.linalg.matrix_rank(control_mat) order = A.shape[0] print(f"controllability matrix rank: {rank}") print(f"System order: {order}") print(f"{'Controllable' if rank == order else 'Not Controllable'}") # Part D feedback_gain = ctrl.place(A, B, roots) print(f'Feedback Gain: {feedback_gain}') reference_gain = -1 / (C[0] @ np.linalg.inv(A - B @ feedback_gain) @ B) print(f"Reference gain: {reference_gain}") # Part E bnb.add_controller(SSController.BallAndBeam, feedback_gain, reference_gain, max_force) requests = sg.generator(sg.square, amplitude=0.15, y_offset=0.25, frequency=0.05, t_step=bnb.seconds_per_sim_step, t_final=20) handle = bnb.view_animation(requests) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handle
def f_11(): print('\n--F--\n') pvt = Sim.PlanarVTOL() max_force = 10 damping_ratio = 1 / (2**(1 / 2)) + 0.2 # Tuning variable rise_time_h = 2.5 rise_time_theta = 0.1 bandwidth_separation = 7 natural_frequency_h = np.pi / (2 * rise_time_h * (1 - damping_ratio**2)**(1 / 2)) natural_frequency_theta = np.pi / (2 * rise_time_theta * (1 - damping_ratio**2)**(1 / 2)) rise_time_z = bandwidth_separation * rise_time_theta natural_frequency_z = np.pi / (2 * rise_time_z * (1 - damping_ratio**2)**(1 / 2)) # Part A coefs_theta = [ 1, 2 * damping_ratio * natural_frequency_theta, natural_frequency_theta**2 ] coefs_z = [ 1, 2 * damping_ratio * natural_frequency_z, natural_frequency_z**2 ] roots_lat = np.concatenate(( np.roots(coefs_theta), np.roots(coefs_z), )) print(f"Latitudinal roots: {roots_lat}") coefs_h = [ 1, 2 * damping_ratio * natural_frequency_h, natural_frequency_h * 2 ] roots_lon = np.roots(coefs_h) print(f"Longitudinal roots: {roots_lon}") # Part B A_lat = pvt.dynamics.A_lat B_lat = pvt.dynamics.B_lat C_lat = pvt.dynamics.C_lat A_lon = pvt.dynamics.A_lon B_lon = pvt.dynamics.B_lon C_lon = pvt.dynamics.C_lon # Part C control_mat_lat = ctrl.ctrb(A_lat, B_lat) rank = np.linalg.matrix_rank(control_mat_lat) order = A_lat.shape[0] print(f"Latitudinal controllability matrix rank: {rank}") print(f"Latitudinal system order: {order}") print(f"{'Controllable' if rank == order else 'Not Controllable'}") control_mat_lon = ctrl.ctrb(A_lon, B_lon) rank = np.linalg.matrix_rank(control_mat_lon) order = A_lon.shape[0] print(f"Longitudinal Controllability matrix rank: {rank}") print(f"Longitudinal system order: {order}") print(f"{'Controllable' if rank == order else 'Not Controllable'}") # Part D feedback_gain_lat = ctrl.place(A_lat, B_lat, roots_lat) print(f'Latitudinal feedback Gain: {feedback_gain_lat}') reference_gain_lat = -1 / ( C_lat[0] @ np.linalg.inv(A_lat - B_lat @ feedback_gain_lat) @ B_lat) print(f"Latitudinal reference gain: {reference_gain_lat}") feedback_gain_lon = ctrl.place(A_lon, B_lon, roots_lon) print(f'Longitudinal feedback Gain: {feedback_gain_lon}') reference_gain_lon = -1 / ( C_lon @ np.linalg.inv(A_lon - B_lon @ feedback_gain_lon) @ B_lon) print(f"Longitudinal reference gain: {reference_gain_lon}") # Part E request = zip( sg.generator(sg.constant, y_offset=1, t_step=pvt.seconds_per_sim_step, t_final=20), sg.generator(sg.sin, frequency=0.08, y_offset=0, amplitude=2.5, t_step=pvt.seconds_per_sim_step, t_final=20), ) pvt.add_controller(SSController.PlanarVTOL, feedback_gain_lat, reference_gain_lat, feedback_gain_lon, reference_gain_lon, max_force) handel = pvt.view_animation(request) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handel
Cr = np.array([[0, 0, 1, 0]]) # confirm that the system in controllable C_AB = ctl.ctrb(A, B) if np.linalg.matrix_rank(C_AB) != 4: print('not controllable') # two rise times (one set slightly higher than the other) tr1 = 0.5 # rise time 1 zeta = 0.707 # damping print('tr1', tr1) wn1 = np.pi / 2.0 / tr1 / np.sqrt(1 - zeta**2) tr2 = 0.1 # rise time 2 zeta = 0.707 # damping print('tr2', tr2) wn2 = np.pi / 2.0 / tr2 / np.sqrt(1 - zeta**2) # find desired poles based on the rise times and damping ratios poly = np.convolve([1, 2 * zeta * wn1, wn1**2], [1, 2 * zeta * wn2, wn2**2]) p = np.roots(poly) print('desired poles', p) # place the poles at the desired locations K = ctl.place(A, B, p) print('K', K) # find kr gain to ensure the DC gain is zero kr = -1.0 / (np.dot(Cr, np.dot(np.linalg.inv(A - B * K), B))) print('kr', kr)
Clon = np.matrix([[1,0]]) Dlon = np.matrix([[0]]) C_ABlon = ctl.ctrb(Alon,Blon) ## altitude rise time tr_h = 1.25 zeta = .707 wn = np.pi/2/tr_h/np.sqrt(1-zeta**2) p = np.roots([1,2*zeta*wn,wn**2]) Klon = ctl.place(Alon,Blon,p) krlon = -1/(Clon*np.linalg.inv(Alon-Blon*Klon)*Blon) F = mtot*g Alat = np.matrix([[0,0,1,0], [0,0,0,1], [0,-F/mtot,-mu/mtot,0], [0,0,0,0]]) Blat = np.matrix([[0], [0], [0], [1/(Jc+2*mr*d**2)]])
def LTI_CLQE(system,saveComputation): A = system.A B = system.B C = system.C G = system.G g = system.g tol = system.tol size_x = A.shape[1] size_u = B.shape[1] size_y = C.shape[0] size_l = G.shape[0] G = svd_suit(G, tol) N = G.null R = G.row_space #E = [N, R] ################################################################## # Derivative-State Constraints and Effective States #D1*dx/dt + D2*x = 0 dof = int(size_x/2) D1 = np.hstack([np.eye(dof), np.zeros((dof,dof))]) D2 = np.hstack([np.zeros((dof,dof)), np.eye(dof)]) GG = vertcat(horzcat(G.M, np.zeros((size_l, size_x))),horzcat(D1, D2)) GG = svd_suit(GG, tol) NA = svd_suit(N.T@A,tol) temp = svd_suit(np.hstack([NA.null,N,GG.row_space[size_x:,:]]),tol) R_used = temp.left_null size_z = N.shape[1] size_zeta = R_used.shape[1] size_chi = size_z+size_zeta E = np.hstack([N, R_used]) N1 = np.hstack([N, np.zeros((size_x, size_zeta))]) ################################################################## class OutStruct: def __init__(self): self.matrix_chi = None self.matrix_u= None self.matrix_y= None self.vector = None self.map= None class Output: def __init__(self): self.sizes = None self.initialConditions = None self.desired = None self.closed_loop = None self.matrices = None self.observer = OutStruct() self.controller = OutStruct() output = Output() output.sizes = {'size_x': size_x, 'size_u': size_u, 'size_y': size_y, 'size_l': size_l, 'size_z': size_z, 'size_zeta': size_zeta, 'size_xi': size_chi} ################################################################## if system.controllerSettings is not None: if system.controllerSettings.Q.shape[1] == 1: costQ = np.diag(system.controllerSettings.Q[:size_z,:].squeeze()) else: costQ = np.diag(system.controllerSettings.Q) if system.controllerSettings.R.shape[1] == 1: costR = np.diag(system.controllerSettings.R[:size_z].squeeze()) else: costR = np.diag(system.controllerSettings.R) Kz = lqr(N.T@A@N, N.T@B, costQ, costR)[0] else: p = system.controllerSettings.poles[:N.shape[1]] Kz = place(N.T@A@N, N.T@B, p) if system.observerSettings is not None: if system.observerSettings.Q.shape[1] == 1: costQ = np.diag(system.observerSettings.Q[:size_z].squeeze()) else: costQ = np.diag(system.observerSettings.Q) if system.observerSettings.R.shape[1] == 1: costR = np.diag(system.observerSettings.R[:size_z].squeeze()) else: costR = np.diag(system.observerSettings.R) L = lqr((N1.T@A@E).T, [email protected], costQ, costR)[0] else: p = system.observerSettings.poles[:E.shape[1]] L = place((N1.T@A@E).T, [email protected], p) L = L.T Kzeta = np.linalg.pinv(N.T@B) @ N.T@A@R_used K = np.hstack((Kz, Kzeta)) ################################################################## #observer structure output.controller.matrix_chi = K output.observer.matrix_chi = N1.T@A@E - L@C@E output.observer.matrix_u = N1.T@B output.observer.matrix_y = L output.observer.vector = N1.T@g output.observer.map = E output.matrices = {'G': G, 'N': N, 'R': R, 'R_used': R_used, 'E': E, 'Kz': Kz, 'Kzeta': Kzeta, 'K': K, 'L': L, 'N1': N1} if saveComputation==2: return output ################################################################## ### desired class Desired: class Derivation: def __init__(self): self.NB = None self.NB_projector= None self.M = None self.zdz_corrected = None def __init__(self): self.x = None self.dx = None self.z = None self.dz = None self.z_corrected= None self.dz_corrected = None self.u_corrected = None self.derivation = Desired.Derivation() desired = Desired() desired.x = system.x_desired desired.dx = system.dx_desired desired.z = [email protected] desired.dz = [email protected] desired.derivation.NB = svd_suit(N.T@B,tol) desired.derivation.NB_projector = desired.derivation.NB.left_null @ desired.derivation.NB.left_null.T desired.derivation.M = svd_suit(np.hstack([[email protected]@A@N, desired.derivation.NB_projector]), tol) desired.derivation.zdz_corrected = [email protected][email protected]@g + [email protected] @ np.vstack([desired.z, desired.dz]) desired.z_corrected = desired.derivation.zdz_corrected[:size_z] desired.dz_corrected = desired.derivation.zdz_corrected[size_z:] if np.linalg.norm( [email protected]_null.T@ (desired.dz_corrected - N.T@A@[email protected]_corrected - N.T@g) ) < tol: desired.u_corrected = desired.derivation.NB.pinv @ (desired.dz_corrected - N.T@A@[email protected]_corrected - N.T@g) output.controller.vector = desired.u_corrected else: raise Warning('cannot create a node') if saveComputation == 1: output.desired = desired return output ################################################################## ### IC class InitialConditions: def __init__(self): self.x self.z self.zeta self.chiEstimateRandom x_initial = system.x_initial initialConditions = InitialConditions() initialConditions.x = x_initial initialConditions.z = N.T@x_initial initialConditions.zeta = R_used.T@x_initial InitialConditions.chi_estimate_random = 0.01*np.random.randn(size_chi, 1) output.initialConditions = initialConditions zeta = initialConditions.zeta u_des = desired.u_corrected z_des = desired.z_corrected x_des = N@z_des + R_used@zeta desired.zeta_corrected = zeta desired.x_corrected = x_des output.desired = desired ################################################################## ### Projected LTI in z-chi coordinates class ClosedLoop: class Coords: def __init__(self): self.matrix = None self.vector = None self.ode_func = None self.Y0 = None self.M = None def __init__(self): self.z_chi = ClosedLoop.Coords() self.x_chi = ClosedLoop.Coords() closed_loop = ClosedLoop() closed_loop.z_chi.matrix = np.vstack(np.hstack([N.T@A@N, -N.T@B@K]), np.hstack([L@C@N, (N1.T@A@E - N1.T@B@K - L@C@E)])) closed_loop.z_chi.vector = np.vstack([N.T@A@R_used@zeta + N.T @B@Kz@z_des + N.T @B@u_des + N.T @g, L@C@R_used@zeta + N1.T@B@Kz@z_des + N1.T@B@u_des + N1.T@g]) closed_loop.z_chi.ode_fnc = lambda y: closed_loop.z_chi.matrix @ y + closed_loop.z_xi.vector closed_loop.z_chi.Y0 = np.hstack([initialConditions.z,initialConditions.chi_estimate_random]) ################################################################## ### LTI in x-chi coordinates closed_loop.x_chi.M = np.vstack([ np.hstack([np.eye(size_x), np.zeros((size_x, size_chi)), -R]), np.hstack([np.zeros((size_chi, size_x)), np.eye((size_chi,size_chi)),np.zeros((size_chi, size_l))]), np.hstack([G.M,np.zeros((size_l, size_chi)),np.zeros((size_l, size_l))]) ]) iM = np.linalg.pinv(closed_loop.x_chi.M) iM11 = iM[:(size_x+size_chi), :(size_x+size_chi)] closed_loop.x_chi.matrix = [email protected]([np.hstack([A, -B@K]), np.hstack([L@C, (N1.T@A@E - N1.T@B@K - L@C@E)])]) closed_loop.x_chi.vector = [email protected]([B@Kz@z_des + B@u_des+ g,N1.T@B@Kz@z_des + N1.T@B@u_des + N1.T@g]) closed_loop.x_chi.ode_fnc = lambda y: closed_loop.x_chi.matrix @ y + closed_loop.x_xi.vector closed_loop.x_chi.Y0 = np.vstack([initialConditions.x, initialConditions.chi_estimate_random]) output.closed_loop = closed_loop return output