dump/task_7_test.py - master - Chronos/TU_laba

dump/task_7_test.py
72 строки3 KB
Chronos
Живо то, что ещё не умерло
28 мар 2025, 09:12
28 мар 2025, 09:12f49a0f7
import numpy as np
from scipy.integrate import odeint
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from params import *
from full_unimod_lib import *

# Исходные матрицы системы
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]
])
B = np.asarray([
    [0],
    [0],
    [0.950],
    [-1.464]
])
# Матрица наблюдателя
C = np.asarray([[1.0,1.0,0.0,0.0]])

def design_controller(A, B, reg_spec):
    Q, Ac = full_unimodal_control_FL(A, B, reg_spec)
    return Q, Ac

def design_observer(A, C, obs_spec):
    L, Ao = full_unimodal_control_FL(A.T, C.T, obs_spec)
    L = -L.T
    return L.reshape(-1, 1), Ao

def build_combined_system(A, B, Q, L, C):
    M_top = np.hstack((A, B @ Q))
    M_bot = np.hstack((L @ C, A + B @ Q - L @ C))
    return np.vstack((M_top, M_bot))

def nonlinear_model(y, t, A, B, C, Q, L):
    theta1, theta2,theta1_dot, theta2_dot,zeta1, zeta2, zeta3, zeta4 = 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], [theta1_dot], [theta2_dot]])

    dzetas = A@zeta_arr+B.T*V+(L@C)@(theta_arr-zeta_arr)
    print(np.linalg.eigvals(A+B@Q-L@C))
    print("Спектр")
    # Общий знаменатель
    delta_theta = theta1 - theta2
    denominator = A11 * A22 - (A12**2) * np.cos(delta_theta)**2

    # Расчет theta1_ddot
    numerator1 = A12 * B2 * theta2_dot * np.cos(delta_theta)- A12 * g * l2 * m2 * np.sin(theta2) * np.cos(delta_theta)- A22 * (B1 + Kf*Ks) * theta1_dot+ A22 * g * (L1 * m2 + l1 * m1) * np.sin(theta1)+ A22 * Kf * V

    theta1_ddot = numerator1 / denominator

    # Расчет theta2_ddot
    numerator2 = -A11 * B2 * theta2_dot+ A11 * g * l2 * m2 * np.sin(theta2)+ A12 * theta1_dot * (B1 + Kf*Ks) * np.cos(delta_theta)- A12 * g * (m2*L1 + m1*l1) * np.sin(theta1) * np.cos(delta_theta)- A12 * Kf * V * np.cos(delta_theta)
    theta2_ddot = numerator2 / denominator

    return [theta1_dot, theta2_dot,theta1_ddot,  theta2_ddot, dzetas[0][0],dzetas[1][0],dzetas[2][0],dzetas[3][0]]

def linear_model(y, t, A, B, Q, L, C):
    theta_arr = y[:4]
    zeta_arr = y[4:]
    V = Q@zeta_arr
    theta_arr_dot = A@theta_arr+B.T*V
    zeta_arr_dot = A@zeta_arr+B.T*V+(L@C)@(theta_arr-zeta_arr)
    return np.concatenate((theta_arr_dot[0],zeta_arr_dot[0]))