Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- Ad = np.array([[ 1.00000000e+00, 2.68170103e-02, 9.08934014e-02,
- -6.77884181e-04],
- [ 0.00000000e+00, 1.11897831e+00, -1.74689726e-03,
- 9.69312804e-02],
- [ 0.00000000e+00, 5.17634358e-01, 8.23599851e-01,
- -3.86676206e-03],
- [ 0.00000000e+00, 2.36782487e+00, -3.37194203e-02,
- 9.78620935e-01]])
- Bd = np.array([[0.02927989],
- [0.00561669],
- [0.56716864],
- [0.108416 ]])
- def obs_d_sys_creator(A, B, C, mu_ctrl = None, mu_obs = None, D = None):
- theta = control.place(A, B, mu_ctrl) *(-1) if (not mu_ctrl is None) else np.matrix(np.zeros(A.shape[1]))
- L = control.place(A.T, C.T, mu_obs).T if (not mu_obs is None) else np.matrix(np.zeros(A.shape[1])).T
- my_min = lambda x,y: (min(x[0],y[0]),min(x[1],y[1]))
- if D is None:
- D = np.zeros(A.shape[0])
- sys_matrix = np.block([
- [A, B@theta],
- [L@C,B@theta+ A - L@C]
- ])
- def rhs(X):
- X = np.matrix(X)
- return (sys_matrix@X.T).T
- return rhs
- Cd = np.matrix([
- [1,0,0,0],
- [0,1,0,0]
- ])
- eigs = np.linalg.eigvals(Ad)
- display(eigs)
- mu_ctrl = copy(eigs.tolist())
- mu_obs = copy(mu_ctrl)
- #case 1:(a)
- mu_obs[0] = -0.1
- mu_obs[1] = -0.2
- #case 2:(b)
- mu_obs[0] = 0.8 +0.3j
- mu_obs[1] = 0.8 - 0.3j
- #case 3:(c)
- mu_obs[0] = -0.1
- mu_obs[1] = 1
- mu_ctrl[0] = 0.8 + 0.3j
- mu_ctrl[1] = 0.8 - 0.3j
- obs_dis_sys = obs_d_sys_creator(Ad,Bd,Cd, mu_ctrl = mu_ctrl, mu_obs = mu_obs)
- L_coefs = control.place(Ad.T, Cd.T, mu_obs).T
- control_coefs = -control.place(Ad,Bd, mu_ctrl)
- control_coefs
- discret_t_arr = np.arange(0,4+0.1,h)
- discret_y0 = (0,np.pi/20,0,0,0,0,0,0)
- discret_y = [discret_y0]
- for i in range(1,len(discret_t_arr)):
- cur_x = obs_dis_sys(discret_y[-1])
- discret_y.append(cur_x.tolist()[0])
- discret_y = np.matrix(discret_y).T
- #display(discret_y.shape)
- u_arr = np.block([(control_coefs@x[0,4:8].T)[0,0] for x in discret_y.T][:-1])
- u_arr
- def to_plot_arr(arr_1, arr_2):
- plot_arr_1 = np.block([
- np.matrix(arr_1).T[:-1], np.matrix(arr_1).T[1:]
- ])
- plot_arr_2 = np.block([
- np.matrix(arr_2).T,np.matrix(arr_2).T
- ])
- return plot_arr_1.ravel().tolist()[0], plot_arr_2.ravel().tolist()[0]
- #используя полученные коэффициенты управления построим непрерывную линейную систему
- def discret_controled_system(A,B,u):
- def rhs(t,X):
- X = np.matrix(X)
- return (A@X.T+B*u).T
- return rhs
- lin_sol_arr = []
- cur_start_pos = discret_y0[:4]
- cur_t_limits = [0,0]
- for t,u in zip(discret_t_arr[1:],u_arr):
- discret_controled_lin_sys = discret_controled_system(nA,nB,u)
- cur_t_limits[0] = cur_t_limits[1]
- cur_t_limits[1] = t
- cur_sol = solve_ivp(discret_controled_lin_sys,y0 = cur_start_pos, t_span = cur_t_limits, t_eval = np.linspace(*cur_t_limits,10), rtol = 1e-12)
- cur_start_pos = cur_sol.y[:,-1]
- lin_sol_arr.append(cur_sol)
- lin_t_arr = np.block([x.t for x in lin_sol_arr])
- lin_y_arr = np.block([x.y for x in lin_sol_arr])
- from tqdm import tqdm
- #символьное вычисление функций
- #theta_ddot_func, phi_ddot_func = non_lin_theta_phi_funcs(lhs_1, lhs_2)
- #определение н.у.
- cur_start_pos = discret_y0[:4]
- cur_t_limits = [0,0]
- non_lin_sol_arr = []
- for t,u in tqdm(zip(discret_t_arr[1:],u_arr)):
- #расчёт системы для текущих н.у.
- controled_theta_ddot_func = theta_ddot_func.subs(parameters).subs({V:u})
- controled_phi_ddot_func = phi_ddot_func.subs(parameters).subs({V:u})
- theta_ddot_lamb, phi_ddot_lamb = non_lin_sys_lambdify(controled_theta_ddot_func, controled_phi_ddot_func)
- non_lin_sys = non_lin_sys_creator(theta_ddot_lamb, phi_ddot_lamb)
- cur_t_limits[0] = cur_t_limits[1]
- cur_t_limits[1] = t
- #численное решение
- non_lin_sol = solve_ivp(fun = non_lin_sys,y0 = cur_start_pos,t_span = cur_t_limits, t_eval = np.linspace(*cur_t_limits,10), rtol = 1e-12)
- cur_start_pos = non_lin_sol.y[:,-1]
- non_lin_sol_arr.append(non_lin_sol)
- non_lin_t_arr = np.block([x.t for x in non_lin_sol_arr])
- non_lin_y_arr = np.block([x.y for x in non_lin_sol_arr])
- fig, ax = plt.subplots(5,2,figsize = (20,10))
- ylabel_params = [r'\theta',r'\varphi',r'\dot{\theta}',r'\dot{\varphi}','V']
- for i in range(len(ylabel_params)):
- ax[i,0].set_ylabel(f"${ylabel_params[i]}$")
- ax[i,1].set_ylabel(f"${ylabel_params[i]}$")
- for i,y in enumerate(non_lin_y_arr):
- ax[i,0].plot(non_lin_t_arr,y, color = 'green', label = 'состояние нелин.')
- for i in range(4):
- cur_y = discret_y[4+i,:-1]
- x,y = to_plot_arr(discret_t_arr,cur_y)
- ax[i,0].plot(x,y, color = 'orange',linestyle='--', label = 'состояние наблюдателя.')
- for i in range(4):
- cur_y = discret_y[i,:-1]
- x,y = to_plot_arr(discret_t_arr,cur_y)
- ax[i,0].plot(x,y, color = 'pink',linestyle='--', label = 'состояние дискр. сис')
- for i in range(4):
- cur_y1 = discret_y[i,:-1]
- cur_y2 = discret_y[i+4,:-1]
- x,y = to_plot_arr(discret_t_arr,abs(cur_y2-cur_y1))
- ax[i,1].plot(x,y, color = 'pink',linestyle='--', label = 'невязка наблюдателя')
- for i,y in enumerate(lin_y_arr):
- ax[i,0].plot(lin_t_arr,y, color = 'red', label = 'состояние лин.')
- ax[4,0].plot(*to_plot_arr(discret_t_arr,u_arr))
- ax[0,0].legend()
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
