/
Chronos
/
TU_laba
Обзор
Документация
Войти
/
Chronos
/
TU_laba
Код
Запросы
0
Задачи
Вики
Пакеты
0
Релизы
0
Аналитика
Безопасность
master
task_14.py
572 строки
19 KB
Chronos
Победа не приходит без обеда
01 апр 2025, 19:36
01 апр 2025, 19:36
c2859d2
Код
Авторство
О чём код?
from scipy.integrate import odeint import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation from params import * from full_unimod_lib import * from A_B_computation import * from scipy.linalg import expm from scipy.linalg import solve_discrete_are final_time = 100.0 np.random.seed(0) # Матрицы системы # A = np.asarray([ # [0, 0, 1, 0], # [0, 0, 0, 1], # [74.574, -38.295, -56.072, 86.342], # [-114.886, 101.449, 86.383, -228.731] # ]) # Численный якобиан системы: # \left[\begin{matrix}0 & 0 & 1 & 0\\0 & 0 & 0 & 1\\74.5742544 & -38.2956276 & -1120.9474312 & 1726.857341\\-114.8868829 & 101.4493401 & 1726.8983435 & -4574.6354981\end{matrix}\right] # A = np.asarray([ # [0, 0, 1, 0], # [0, 0, 0, 1], # [74.5742544 , -38.2956276 , -1120.9474312 , 1726.857341], # [-114.8868829 , 101.4493401 ,1726.8983435 , -4574.6354981] # ]) # B = np.asarray([ # [0], # [0], # [0.9505409], # [-1.464375] # ]) A = np.asarray([ [0 , 0 , 1 , 0], [0 , 0 , 0 , 1], [74.4982357 , -38.2565903, -56.0726559, 86.3428671], [-114.7697709,101.3459259,86.3838696, -228.7317749] ]) B = np.asarray([ [0], [0], [0.9505409], [-1.464375] ]) h = 0.1 Ad = expm(A*h) # print("Ad: \n", a2l.to_ltx(Ad,frmt='{:6.7f}')) Bd = inv(A)@(Ad-np.eye(4))@B # A,B = compute_A_B() np.set_printoptions(precision=10,suppress=True) print(A) print(B) print("Исходный спектр матрицы A: \n") eigv = np.linalg.eigvals(A) print(eigv) left_eigv = eigv[eigv < 0.0] # print(eigv[eigv < 0.0]) # Матрица наблюдателя # C = np.asarray([[1.0,1.0,0.0,0.0]]) C = np.asarray([ [1.0,0.0,0.0,0.0], [0.0,1.0,0.0,0.0], ]) # Этап 1: строим управление для регулятора Q_lqr = np.eye(4) # Штраф за отклонение состояния R_lqr = np.eye(1) P_ric = solve_discrete_are(Ad, Bd, Q_lqr, R_lqr) print("Решение уравнения Риккати: \n") print(a2l.to_ltx(P_ric,frmt='{:6.7f}',arraytype='bmatrix')) # Оптимальная матрица обратной связи Q = -np.linalg.inv(R_lqr + Bd.T @ P_ric @ Bd) @ Bd.T @ P_ric @ Ad print("Управление для регулятора: \n") print(a2l.to_ltx(Q,frmt='{:6.7f}',arraytype='bmatrix')) # reg_spec = np.asarray([0.9,0.9, np.exp(-21.421*h), np.exp(-265.086*h)]) # Желаемый спектр регулятора # reg_spec = np.asarray([0.9,0.9, np.exp(-21.421*h), np.exp(-265.086*h)]) # Q,Ac = full_unimodal_control_FL(Ad,Bd,reg_spec) # Управление для регулятора # Q,Ac = full_unimodal_control_FL(A,B,reg_spec) # Управление для регулятора # print(np.linalg.eigvals(Ac)) # Этап 2: строим управление для наблюдателя # print("Управление для наблюдателя: \n") # obs_spec = np.asarray([0.9,0.9, np.exp(-21.421*h), np.exp(-265.086*h)]) # Желаемый спектр наблюдателя # [-10,-10, -21.421, -265.086] # L = ctrl.place(A.T,C.T,obs_spec) # L,Ao = full_unimodal_control_FL(Ad.T,C.T,obs_spec) # Управление для наблюдателя # print(np.linalg.eigvals(Ao)) # L = -L.T # L = L*0 # L = L.reshape(4,1) # Этап 3: строим всю систему целиком a1,a2,a3,a4 = Q[0] theta1_ddot_, theta2_ddot_ = solve_system() def model(y, t,V): theta1, theta2,theta1dot, theta2dot = y # Вычисление управляющего напряжения V (по состоянию наблюдателя) # V = a1*zeta1 + a2*zeta2 + a3*zeta3 + a4*zeta4 # V = a1*theta1 + a2*theta2 + a3*theta1_dot + a4*theta2_dot # Как меняется состояние наблюдателя # zeta_arr = np.array([[zeta1], [zeta2], [zeta3], [zeta4]]) # theta_arr = np.array([[theta1], [theta2], [theta1dot], [theta2dot]]) # # dzetas = A@zeta_arr+B*V+(L@C)@(theta_arr-zeta_arr) return [theta1dot, theta2dot,theta1_ddot_(V,theta1, theta2,theta1dot,theta2dot), theta2_ddot_(V,theta1, theta2,theta1dot,theta2dot)] # def linear_model(y, t): # theta_arr = y[:4].reshape(4,1) # # print(theta_arr) # zeta_arr = y[4:].reshape(4,1) # # print(zeta_arr) # V = (Q@zeta_arr) # # print("V",V) # theta_arr_dot = A@theta_arr+B*V # # print("theta_arr_dot",theta_arr_dot) # zeta_arr_dot = A@zeta_arr+B*V+(L@C)@(theta_arr-zeta_arr) # # print("zeta_arr_dot",zeta_arr_dot) # res = np.vstack((theta_arr_dot,zeta_arr_dot)).flatten() # # print(res) # return res # Q = Q.reshape(1,4) # print("Матрица всей линейной системы по принципу разделения:") # M_top = np.hstack((A,B@Q)) # M_bot = np.hstack((L@C,A+B@Q-L@C)) # M = np.vstack((M_top,M_bot)) # print(M) # print("Спектр всей системы по принципу разделения:") # print(np.linalg.eigvals(M)) # Временной интервал # t = np.linspace(0, 20.0, 20_000) # Начальные условия (близкие к нулю) # y0 = [1e-5,-1e-5, -1e-5, 0,0,0,0,0] # y0 = [0.001, 0.001, 0.001, 0.001] y0 = np.asarray([0.00032, 0.0, 0.0, 0.0]) # Решение нелинейной системы # print("Вектор в начале:") # print(model(y0,0)) # time = 0.0 # # zetas = np.asarray([ # [0], # [0], # [0], # [0] # ]) # sol_nonlinear = np.asarray([y0]) # # zeta_arr = zetas.T # while time <= 20: # zeta1,zeta2,zeta3,zeta4 = zetas.reshape(4) # V = a1*zeta1 + a2*zeta2 + a3*zeta3 + a4*zeta4 # # print(Q[0]) # # print(zetas) # # print(V) # # print(model(y0,time,V)) # nonlin_sol = odeint(model, y0, np.linspace(time,time+h,100), args=(V,)) # sol_nonlinear = np.vstack((sol_nonlinear,nonlin_sol)) # # # print(np.tile(zetas.T,(100,1))) # zeta_arr = np.vstack((zeta_arr,np.tile(zetas.T,(100,1)))) # # print(Ad) # # print(y0.reshape(4,1)-zetas) # # print(Ad@zetas) # # zetas = Ad@zetas+Bd*V+(L@C)@(y0.reshape(4,1)-zetas) # y0 = nonlin_sol[-1] # time += h # # zeta_arr = zeta_arr[1:,:] # t = np.linspace(0, 20.0, len(sol_nonlinear)) # sol_nonlinear = odeint(model, y0, t) # Добавляем в начале кода параметры фильтра Калмана # Шумы системы # Q_kalman = np.diag([0.01, 0.01, 0.1, 0.1]) # Ковариация шума процесса R_kalman = np.eye(2)*0.0000001 # Ковариация шума измерений # [Должен быть 2d шум !] # Инициализация фильтра Калмана x_est = np.array([[0.00032], [0.0], [0.0], [0.0]]) # Начальная оценка P_est = np.eye(4) *0.0001 # Начальная ковариация # Модифицируем основной цикл (нелинейная система) time = 0.0 sol_nonlinear = np.asarray([y0]) kalman_estimates = [x_est.flatten()] # Для хранения оценок фильтра V_values = np.asarray([0]) all_K_matrices = [] while time <= final_time: # Фильтр Калмана: шаг прогноза x_pred = Ad @ x_est + Bd @ (Q @ x_est) P_pred = Ad @ P_est @ Ad.T # Получаем зашумленное измерение y_meas = C @ y0.reshape(4,1)+np.random.multivariate_normal(np.zeros(2), R_kalman).reshape(2,1) # print(time) K = P_pred @ C.T @ np.linalg.inv(C @ P_pred @ C.T + R_kalman) # print(K) x_est = x_pred + K @ (y_meas - C @ x_pred) P_est = (np.eye(4) - K @ C) @ P_pred # Формируем управление на основе оценки V = Q @ x_est # print(V) # Интегрируем систему nonlin_sol = odeint(model, y0, np.linspace(time,time+h,100), args=(V.item(),)) V_values = np.hstack((V_values,np.tile(V[0],(100)))) # print(K) all_K_matrices.append(K) # K_values = np.hstack((K_values,np.tile(K[0][0],(100)))) # Сохраняем результаты sol_nonlinear = np.vstack((sol_nonlinear,nonlin_sol)) # kalman_estimates.append(x_est.flatten()) kalman_estimates = np.vstack((kalman_estimates,np.tile(x_est.T,(100,1)))) y0 = nonlin_sol[-1] time += h K_history = np.array(all_K_matrices) K_expanded = np.repeat(K_history[:, np.newaxis, :, :], 100, axis=1) K_flattened = K_expanded.reshape(-1, 4, 2) # (N*100, 4, 2) # Преобразуем оценки в numpy array kalman_estimates = np.array(kalman_estimates) t = np.linspace(0, final_time, len(sol_nonlinear)) # # # Решение линеаризованной системы # sol_linear = np.asarray([y0]) # # time = 0.0 # # zetas = np.asarray([ # [0], # [0], # [0], # [0] # ]) # # sol_nonlinear = np.asarray([y0]) # thetas = y0.reshape(4,1) # # linear_zeta_arr = zetas.T # # V_values = np.asarray([0]) # while time <= 20: # zeta1,zeta2,zeta3,zeta4 = zetas.reshape(4) # # print("zetas:",zetas) # V = a1*zeta1 + a2*zeta2 + a3*zeta3 + a4*zeta4 # # nonlin_sol = odeint(model, y0, np.linspace(time,time+h,100), args=V) # # V_values = np.hstack((V_values,np.tile(V,(100)))) # # # zetas = Ad@zetas+Bd*V+(L@C)@(thetas-zetas) # linear_zeta_arr = np.vstack((linear_zeta_arr,zetas.T)) # # # print(thetas) # # print(Ad) # # print(Ad@thetas) # # print(V) # thetas = Ad@thetas+Bd*V # sol_linear = np.vstack((sol_linear,thetas.T)) # # # y0 = nonlin_sol[-1] # time += h # # # print(sol_linear.shape) # # # t_lin = np.linspace(0, 20.0, len(sol_linear)) # t_lin_obs = np.linspace(0, 20.0, len(linear_zeta_arr)) # Построение графиков f, ((ax1,ax2),(ax3,ax4)) = plt.subplots(2,2) f.suptitle('Нелинейная система', fontsize=20,fontfamily = "Times New Roman") fs = 20 # Фазовые портреты нелинейной системы # plt.subplot(2, 2, 1) ax1.plot(t, sol_nonlinear[:,0], 'b',label="Реальное состояние") ax1.plot(t, kalman_estimates[:,0], 'g', label="Оценка фильтра") ax1.set_ylabel("$\\theta_1$",fontsize = fs) ax1.set_xlabel('$t$',fontsize = fs) # plt.title('Нелинейная система: фазовый портрет $\\theta_1$',fontfamily = "Times New Roman",fontsize = fs) # plt.xticks(fontfamily = "Times New Roman",fontsize = fs) # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) ax1.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax1.grid() ax1.legend() # plt.subplot(2, 2, 2) ax2.plot(t, sol_nonlinear[:,1], 'b',label="Реальное состояние") ax2.plot(t, kalman_estimates[:,1], 'g', label="Оценка фильтра") ax2.set_ylabel("$\\theta_2$",fontsize = fs) ax2.set_xlabel('$t$',fontsize = fs) # plt.title('Нелинейная система: фазовый портрет $\\theta_2$',fontfamily = "Times New Roman",fontsize = fs) # plt.xticks(fontfamily = "Times New Roman",fontsize = fs-2) # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) ax2.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax2.grid() ax2.legend() # plt.subplot(2, 2, 3) # ax3.plot(t, sol_nonlinear[:,2], 'b') ax3.plot(t, sol_nonlinear[:,2], 'b',label="Реальное состояние") ax3.plot(t, kalman_estimates[:,2], 'g', label="Оценка фильтра") ax3.set_ylabel("$\\dot{\\theta}_1$",fontsize = fs) ax3.set_xlabel('$t$',fontsize = fs) # plt.title('Нелинейная система: фазовый портрет $\\theta_1$',fontfamily = "Times New Roman",fontsize = fs) # plt.xticks(fontfamily = "Times New Roman",fontsize = fs) # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) ax3.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax3.grid() ax3.legend() # plt.subplot(2, 2, 4) # ax4.plot(t, sol_nonlinear[:,3], 'r') ax4.plot(t, sol_nonlinear[:,3], 'b',label="Реальное состояние") ax4.plot(t, kalman_estimates[:,3], 'g', label="Оценка фильтра") ax4.set_ylabel("$\\dot{\\theta}_2$",fontsize = fs) ax4.set_xlabel('$t$',fontsize = fs) # plt.title('Нелинейная система: фазовый портрет $\\theta_2$',fontfamily = "Times New Roman",fontsize = fs) # plt.xticks(fontfamily = "Times New Roman",fontsize = fs-2) # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) ax4.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax4.grid() ax4.legend() # # f, ((ax1,ax2),(ax3,ax4)) = plt.subplots(2,2) # f.suptitle('Линейная система', fontsize=20,fontfamily = "Times New Roman") # fs = 20 # # Фазовые портреты нелинейной системы # # plt.subplot(2, 2, 1) # ax1.plot(t_lin, sol_linear[:,0], 'b',label="Реальное состояние") # ax1.plot(t_lin, linear_zeta_arr[:,0], 'g',label="Вид наблюдателя") # ax1.set_ylabel("$\\theta_1$",fontsize = fs) # ax1.set_xlabel('$t$',fontsize = fs) # # plt.title('Нелинейная система: фазовый портрет $\\theta_1$',fontfamily = "Times New Roman",fontsize = fs) # # plt.xticks(fontfamily = "Times New Roman",fontsize = fs) # # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) # ax1.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') # ax1.grid() # ax1.legend() # # # plt.subplot(2, 2, 2) # ax2.plot(t_lin, sol_linear[:,1], 'b',label="Реальное состояние") # ax2.plot(t_lin, linear_zeta_arr[:,1], 'g',label="Вид наблюдателя") # ax2.set_ylabel("$\\theta_2$",fontsize = fs) # ax2.set_xlabel('$t$',fontsize = fs) # # plt.title('Нелинейная система: фазовый портрет $\\theta_2$',fontfamily = "Times New Roman",fontsize = fs) # # # plt.xticks(fontfamily = "Times New Roman",fontsize = fs-2) # # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) # ax2.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') # ax2.grid() # ax2.legend() # # # plt.subplot(2, 2, 3) # # ax3.plot(t, sol_nonlinear[:,2], 'b') # ax3.plot(t_lin, sol_linear[:,2], 'b',label="Реальное состояние") # ax3.plot(t_lin, linear_zeta_arr[:,2], 'g',label="Вид наблюдателя") # ax3.set_ylabel("$\\dot{\\theta}_1$",fontsize = fs) # ax3.set_xlabel('$t$',fontsize = fs) # # plt.title('Нелинейная система: фазовый портрет $\\theta_1$',fontfamily = "Times New Roman",fontsize = fs) # # plt.xticks(fontfamily = "Times New Roman",fontsize = fs) # # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) # ax3.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') # ax3.grid() # ax3.legend() # # plt.subplot(2, 2, 4) # # ax4.plot(t, sol_nonlinear[:,3], 'r') # ax4.plot(t_lin, sol_linear[:,3], 'b',label="Реальное состояние") # ax4.plot(t_lin, linear_zeta_arr[:,3], 'g',label="Вид наблюдателя") # ax4.set_ylabel("$\\dot{\\theta}_2$",fontsize = fs) # ax4.set_xlabel('$t$',fontsize = fs) # # plt.title('Нелинейная система: фазовый портрет $\\theta_2$',fontfamily = "Times New Roman",fontsize = fs) # # # plt.xticks(fontfamily = "Times New Roman",fontsize = fs-2) # # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) # ax4.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') # ax4.grid() # ax4.legend() f,ax = plt.subplots(1,1) f.suptitle("Управление",fontsize=20,fontfamily = "Times New Roman") ax.plot(t,V_values,'b') ax.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax.set_xlabel('$t$',fontsize = fs) ax.set_ylabel('$V$',fontsize = fs) ax.grid() f,ax = plt.subplots(1,1) f.suptitle("Коэффициент усиления Калмана",fontsize=20,fontfamily = "Times New Roman") ax.plot(t[:-1],K_flattened[:, 0, 0],label="$K_{1,1}$") ax.plot(t[:-1],K_flattened[:, 1, 0],label="$K_{2,1}$") ax.plot(t[:-1],K_flattened[:, 2, 0],label="$K_{3,1}$") ax.plot(t[:-1],K_flattened[:, 3, 0],label="$K_{4,1}$") ax.plot(t[:-1],K_flattened[:, 0, 1],label="$K_{1,2}$") ax.plot(t[:-1],K_flattened[:, 1, 1],label="$K_{2,2}$") ax.plot(t[:-1],K_flattened[:, 2, 1],label="$K_{3,2}$") ax.plot(t[:-1],K_flattened[:, 3, 1],label="$K_{4,2}$") ax.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax.set_xlabel('$t$',fontsize = fs) ax.set_ylabel('$K$',fontsize = fs) ax.legend(fontsize=fs) ax.grid() step = 10 # Шаг прореживания данных t_anim = t[::step] sol_anim = sol_nonlinear[::step] # Создаем фигуру для анимации fig_anim, ax = plt.subplots(figsize=(6,6)) ax.set_xlim(-0.3, 0.3) ax.set_ylim(0.0, 0.8) ax.set_aspect('equal') ax.grid() # Создаем элементы анимации line1, = ax.plot([], [], 'b-', lw=2) # Первое звено line2, = ax.plot([], [], 'r-', lw=2) # Второе звено point1, = ax.plot([], [], 'bo', markersize=10) # Шарнир 1 point2, = ax.plot([], [], 'ro', markersize=10) # Шарнир 2 line1_obs, = ax.plot([], [], 'g-', lw=2,alpha=0.2) # Первое звено line2_obs, = ax.plot([], [], 'g-', lw=2,alpha=0.2) # Второе звено point1_obs, = ax.plot([], [], 'go', markersize=10,alpha=0.2) # Шарнир 1 point2_obs, = ax.plot([], [], 'go', markersize=10,alpha=0.2) # Шарнир 2 time_text = ax.text(0.05, 0.9, '', transform=ax.transAxes) def init(): line1.set_data([], []) line2.set_data([], []) point1.set_data([], []) point2.set_data([], []) line1_obs.set_data([], []) line2_obs.set_data([], []) point1_obs.set_data([], []) point2_obs.set_data([], []) time_text.set_text('') return line1, line2, point1, point2, time_text def animate(i): theta1 = sol_anim[i, 0] theta2 = sol_anim[i, 1] zeta1 = kalman_estimates[i, 0] zeta2 = kalman_estimates[i, 1] # Координаты первого звена x1 = L1 * np.sin(theta1) y1 = L1 * np.cos(theta1) # Координаты второго звена x2 = x1 + L2 * np.sin(theta2) y2 = y1 + L2 * np.cos(theta2) # Обновление линий line1.set_data([0, x1], [0, y1]) line2.set_data([x1, x2], [y1, y2]) # Обновление точек point1.set_data([x1], [y1]) point2.set_data([x2], [y2]) # Координаты первого звена x1_obs = L1 * np.sin(zeta1) y1_obs = L1 * np.cos(zeta1) # Координаты второго звена x2_obs = x1_obs + L2 * np.sin(zeta2) y2_obs = y1_obs + L2 * np.cos(zeta2) # Обновление линий line1_obs.set_data([0, x1_obs], [0, y1_obs]) line2_obs.set_data([x1_obs, x2_obs], [y1_obs, y2_obs]) # Обновление точек point1_obs.set_data([x1_obs], [y1_obs]) point2_obs.set_data([x2_obs], [y2_obs]) time_text.set_text(f'Время = {t_anim[i]:.2f} с') return line1, line2, point1, point2,line1_obs, line2_obs, point1_obs, point2_obs ,time_text # return line1, line2, point1, point2 ,time_text # Создаем анимацию ani = FuncAnimation(fig_anim, animate, frames=len(sol_anim), init_func=init, blit=True, interval=20) # Для сохранения анимации # plt.rcParams['animation.ffmpeg_path'] = 'C:\\ffmpeg\\ffmpeg-master-latest-win64-gpl-shared\\bin\\ffmpeg.exe' # ani.save('task_14.mp4', fps=30) plt.tight_layout() plt.show()