Modal_Control.py - master - Chronos/Control_lib

Modal_Control.py
443 строки18 KB
Chronos
Нет повести печальнее на Свете, чем татуха о Ромео и Джульетте
24 апр 2025, 20:52
24 апр 2025, 20:52ae09e83
import numpy as np
from scipy.optimize import root
from scipy.linalg import svd, qr, null_space
# Матрица управляемости системы
def controllability_matrix(A, B):
    n = A.shape[0]  # размерность состояния
    m = B.shape[1]  # количество входов

    # Создаем пустую матрицу управляемости
    C = np.empty((n, n*m))

    # Заполняем матрицу управляемости блоками [B AB A²B ... A^(n-1)B]
    for i in range(n):
        C[:, i*m:(i+1)*m] = np.linalg.matrix_power(A, i) @ B

    return C

def print_polynomial(poly_coeffs):
    """Функция для красивой печати полинома"""
    n = len(poly_coeffs) - 1
    terms = []
    for i, coeff in enumerate(poly_coeffs):
        power = n - i
        if power == 0:
            terms.append(f"{coeff:.1f}")
        elif power == 1:
            terms.append(f"{coeff:.1f}z")
        else:
            terms.append(f"{coeff:.1f}z^{power}")
    return " + ".join(terms).replace("+ -", "- ")


def matrix_poly(coeffs, A):
    n = A.shape[0]
    pA = np.zeros_like(A)
    for i, coeff in enumerate(coeffs):
        pA += coeff * np.linalg.matrix_power(A, n - i)
    return pA

# Формула Аккермана
def accerman(A,B,desired_poles):
    A = A.astype(np.float64)
    B  = B.astype(np.float64)
    if B.shape[1] != 1:
        print("Формула Аккермана работает только для одного столбца B!!!")
        return
    C  = controllability_matrix(A,B)
    if np.linalg.matrix_rank(C) != A.shape[0]:
        print("Система не является полностью управляемой, рассмотрите разложение Калмана!!!")
        return
    print("Матрица управляемости системы:\n", C)
    C_inv = np.linalg.inv(C)
    print("Обратная к матрице управляемости системы:\n",C_inv)
    # Желаемый полином
    pol = np.poly(desired_poles)
    print("\nЖелаемый характеристический полином:")
    print(print_polynomial(pol))
    pol_A = matrix_poly(pol,A)

    print("Желаемый характеристический полином от матрицы A:\n",pol_A)
    theta = - (C_inv[-1,:])@pol_A
    print("Линейное управление:\n",theta)
    print("Полученное размещение полюсов:\n",np.linalg.eigvals(A+B@theta.reshape(1,A.shape[0])))
    return theta


# Форма Фробениуса-Луенбергера
def frobenius_luenberger(A,B,desired_poles):
    A = A.astype(np.float64)
    B  = B.astype(np.float64)
    if B.shape[1] != 1:
        print("Формула Фробениуса-Луенбергера работает только для одного столбца B!!!")
        return
    C = controllability_matrix(A, B)
    if np.linalg.matrix_rank(C) != A.shape[0]:
        print("Система не является полностью управляемой, рассмотрите разложение Калмана!!!")
        return
    print("Матрица управляемости системы:\n", C)
    C_inv = np.linalg.inv(C)
    print("Обратная к матрице управляемости системы:\n", C_inv)
    n = A.shape[0]
    # Желаемый полином
    pol = np.poly(desired_poles)
    print("\nЖелаемый характеристический полином:")
    print(print_polynomial(pol))

    current_pol = np.poly(np.linalg.eigvals(A))
    print("Текущий характеристический полином:")
    print(print_polynomial(current_pol))

    A_hat = np.vstack((np.hstack((np.zeros((n-1,1)),np.eye(n-1))),-current_pol[1:][::-1]))
    print("Матрица A^~:\n",A_hat)
    B_hat = np.vstack((np.zeros((n-1,1)),np.array([1])))
    print("Матрица B^~:\n",B_hat)
    C_hat = controllability_matrix(A_hat,B_hat)
    print("Матрица управляемости новой системы в форме Фробениуса-Луенбергера:\n",C_hat)

    theta = current_pol[1:][::-1]-pol[1:][::-1]

    theta = theta.reshape(1,A.shape[0])

    theta = theta@(C_hat@C_inv)
    print("Линейное управление:\n",theta)
    print("Полученное размещение полюсов:\n",np.linalg.eigvals(A+B@theta.reshape(1,A.shape[0])))
    return theta


# Дополняем матрицы линейно независимыми строками
def grow_matrix_(v):
    # Идея: собственный вектор ненулевой, значит есть ненулевой элемент
    # Берём только базисные элементы, где на этом месте СТОИТ НОЛЬ!!
    n = v.shape[1]
    # Найдём первый слева ненулевой элемент вектора v
    nonzero_pos = -1
    for i in range(n):
        if v[0,i] != 0:
            nonzero_pos = i
            break
    # Берём единичную матрицу
    I = np.eye(n)
    # И удаляем столбец и строку с этим ненулевым элементом
    if nonzero_pos != -1:
        I = np.delete(I, nonzero_pos, axis=0)
    matrix = np.vstack((v, I))
    return matrix

# Дополняем матрицы линейно независимыми строками (Случай двух линейно независимых векторов)
def grow_matrix(v1, v2):
    # Проверка входных данных
    # if v1.ndim != 1 or v2.ndim != 1 or len(v1) != len(v2):
    #     raise ValueError("Векторы должны быть одномерными и одинаковой длины")
    n = v1.shape[1]

    if n == 2:
        return np.vstack((v1, v2))

    # Проверка линейной независимости через ортогональность
    # if np.allclose(np.vdot(v1, v2), 0):
    #     raise ValueError("Векторы линейно зависимы")

    matrix = np.vstack((v1, v2))
    # QR-разложение для ступенчатого вида
    Q, R = np.linalg.qr(matrix.T, mode='reduced')

    # Находим ведущие столбцы (ненулевые диагональные элементы R)
    leading_cols = []
    for i in range(min(R.shape)):
        if not np.isclose(R[i, i], 0):
            leading_cols.append(i)

    if len(leading_cols) < 2:
        raise ValueError("Векторы линейно зависимы")

    # Добавляем базисные векторы для оставшихся столбцов
    all_cols = set(range(n))
    remaining_cols = sorted(all_cols - set(leading_cols))
    basis_vectors = [np.eye(1, n, k).flatten() for k in remaining_cols]

    # Собираем полную матрицу
    full_matrix = np.vstack((v1, v2, basis_vectors))
    return full_matrix[:n]



# Сдвиг одной моды (для нескольких мод можно строить управление итеративно!)
def move_one_pole(A,B,pole_from,pole_to):
    A = A.astype(np.float64)
    B  = B.astype(np.float64)
    n = A.shape[0]
    C = controllability_matrix(A, B)
    if np.linalg.matrix_rank(C) != A.shape[0]:
        print("Система не является полностью управляемой, рассмотрите разложение Калмана!!!")
        return
    print("Матрица управляемости системы:\n", C)
    # Находим левые собственные векторы (правые векторы A^T)
    eigenvals, left_eigenvectors = np.linalg.eig(A.T)

    # Находим индекс, соответствующий нужному lambda_desired (с учётом погрешности)
    idx = np.argmin(np.abs(eigenvals - pole_from))
    left_eigenvector = left_eigenvectors[:, idx]
    left_eigenvector /= np.linalg.norm(left_eigenvector)
    print("Левый собственный вектор:", left_eigenvector)

    left_eigenvector = left_eigenvector.reshape(1,A.shape[0])
    P_inv = grow_matrix_(left_eigenvector)
    print("Матрица P^{-1}:\n",P_inv)
    print("Её ранг:",np.linalg.matrix_rank(P_inv))

    P = np.linalg.inv(P_inv)
    print("Матрица P:\n",P)
    A_hat = P_inv@A@P
    print("Матрица A^~:\n",A_hat)
    B_hat = P_inv@B
    print("Матрица B^~:\n",B_hat)

    theta_hat = np.zeros((1,n))
    theta_hat[0,0] = (pole_to-pole_from)/B_hat[0,0]

    theta = theta_hat@P_inv
    print("Линейное управление:\n",theta)
    print("Полученное размещение полюсов:\n",np.linalg.eigvals(A+B@theta.reshape(1,A.shape[0])))
    return theta


def find_theta(A, B, trace_target, det_target):
    """
    Находит theta = [theta_1, theta_2], такие что:
    - trace(A + B @ theta) = trace_target
    - det(A + B @ theta) = det_target
    """
    # Проверка размеров
    assert A.shape == (2, 2), "A должна быть матрицей 2x2"
    assert B.shape == (2, 1), "B должен быть столбцом 2x1"

    a11, a12 = A[0, 0], A[0, 1]
    a21, a22 = A[1, 0], A[1, 1]
    b1, b2 = B[0, 0], B[1, 0]

    def system(theta):
        theta1, theta2 = theta
        # След матрицы A + B*theta
        trace = (a11 + b1*theta1) + (a22 + b2*theta2)
        # Определитель матрицы A + B*theta
        det = (a11 + b1*theta1)*(a22 + b2*theta2) - (a12 + b1*theta2)*(a21 + b2*theta1)
        return [trace - trace_target, det - det_target]

    # Начальное приближение (можно корректировать)
    initial_guess = np.array([0.0, 0.0])

    # Решение системы нелинейных уравнений
    result = root(system, initial_guess, method='lm')  # Используем метод Левенберга-Марквардта

    if result.success:
        theta_opt = result.x
        # Проверка точности
        A_new = A + B @ theta_opt.reshape(1, -1)
        assert np.isclose(np.trace(A_new), trace_target), "Ошибка следа"
        assert np.isclose(np.linalg.det(A_new), det_target), "Ошибка определителя"
        return theta_opt
    else:
        raise ValueError(f"Решение не найдено: {result.message}")
# Сдвиг комплексно сопряжённой пары мод в необходимую пару
def move_complex_pole(A,B,pole_from,poles_to):
    A = A.astype(np.float64)
    B  = B.astype(np.float64)
    n = A.shape[0]
    C = controllability_matrix(A, B)
    if np.linalg.matrix_rank(C) != A.shape[0]:
        print("Система не является полностью управляемой, рассмотрите разложение Калмана!!!")
        return
    print("Матрица управляемости системы:\n", C)
    # Находим левые собственные векторы (правые векторы A^T)
    eigenvals, left_eigenvectors = np.linalg.eig(A.T)

    # Находим индекс, соответствующий нужному lambda_desired (с учётом погрешности)
    idx = np.argmin(np.abs(eigenvals - pole_from))
    left_eigenvector = left_eigenvectors[:, idx]
    left_eigenvector /= np.linalg.norm(left_eigenvector)
    print("Левый собственный вектор:", left_eigenvector)

    v1 = np.real(left_eigenvector).reshape(1,n)
    v2 = np.imag(left_eigenvector).reshape(1,n)

    P_inv = grow_matrix(v1,v2)
    print("Матрица P^{-1}:\n",P_inv)
    print("Её ранг:",np.linalg.matrix_rank(P_inv))
    #
    P = np.linalg.inv(P_inv)
    print("Матрица P:\n",P)
    A_hat = P_inv@A@P
    print("Матрица A^~:\n",A_hat)
    B_hat = P_inv@B
    print("Матрица B^~:\n",B_hat)

    theta_hat = find_theta(A_hat[:2,:2],B_hat[:2,:],np.sum(poles_to),np.prod(poles_to)).reshape(1,2)
    theta_hat = np.hstack((theta_hat,np.zeros((1,n-2))))
    theta = theta_hat@P_inv
    print("Линейное управление:\n",theta)
    print("Полученное размещение полюсов:\n",np.linalg.eigvals(A+B@theta.reshape(1,A.shape[0])))
    return theta


def observability_matrix(A, C):
    return controllability_matrix(A.T, C.T).T

def image_basis(A):
    U, S, Vh = np.linalg.svd(A)
    tol = 1e-10
    rank = np.sum(S > tol)
    return U[:, :rank]

def kernel_basis_qr(A, tol=1e-10):
    """
    Альтернативная реализация через QR-разложение
    """
    Q, R = np.linalg.qr(A.T)
    # Диагональные элементы R
    diag = np.abs(np.diag(R))
    rank = np.sum(diag > tol)
    nullity = A.shape[1] - rank

    if nullity == 0:
        return np.empty((A.shape[1], 0))

    return Q[:, rank:]



def fill_basis(T2, basis_C):
    if T2.size == 0:
        return basis_C
    """
    Дополняет матрицу T2 до полного столбцового базиса матрицы basis_C

    Параметры:
    T2 : numpy.ndarray (n x k)
        Матрица, которую нужно дополнить до базиса
    basis_C : numpy.ndarray (n x m)
        Матрица, чей столбцовый базис мы хотим получить

    Возвращает:
    numpy.ndarray (n x (m-k))
        Матрица T1, которая вместе с T2 образует базис basis_C
    """
    # Проверка размеров
    assert T2.shape[0] == basis_C.shape[0], "Число строк в матрицах должно совпадать"

    # 1. Находим базис пространства, порожденного basis_C
    U, S, _ = np.linalg.svd(basis_C, full_matrices=False)
    rank = np.sum(S > 1e-10)
    basis_full = U[:, :rank]

    # 2. Проецируем T2 на этот базис
    # Находим коэффициенты разложения T2 по базису basis_full
    coeffs = np.linalg.lstsq(basis_full, T2, rcond=None)[0]

    # 3. Находим ортогональное дополнение к span(T2) в span(basis_C)
    # Сначала находим ядро coeffs.T (векторы, ортогональные всем столбцам coeffs)
    kernel = null_space(coeffs.T)

    if kernel.size == 0:
        return np.empty((basis_C.shape[0], 0))

    # 4. Преобразуем эти векторы обратно в исходное пространство
    T1 = basis_full @ kernel

    # 5. Проверяем, что объединение T1 и T2 дает базис basis_C
    combined = np.hstack((T2, T1))
    if not np.allclose(np.linalg.matrix_rank(combined), np.linalg.matrix_rank(basis_full)):
        raise ValueError("Не удалось построить полный базис")

    return T1

# Дополняет до столбцового базиса Rn
def append_basis(Matrix, tol=1e-10):
    """
    Дополняет столбцы матрицы до полного базиса в ℝⁿ

    Параметры:
    Matrix : numpy.ndarray (n x m)
        Исходная матрица (m столбцов высоты n)
    tol : float, optional
        Порог для определения линейной независимости (по умолчанию 1e-10)

    Возвращает:
    numpy.ndarray (n x (n-m))
        Матрица, столбцы которой дополняют входную матрицу до базиса ℝⁿ
    """
    n, m = Matrix.shape

    # Проверка на полный ранг (если m >= n, возвращаем пустую матрицу)
    if m >= n:
        return np.empty((n, 0))

    # 1. Находим базис пространства строк Matrix (через SVD)
    U, S, Vh = np.linalg.svd(Matrix, full_matrices=True)
    rank = np.sum(S > tol)

    # 2. Базис пространства строк - первые rank строк Vh
    row_basis = Vh[:rank]

    # 3. Находим ядро (нуль-пространство) row_basis
    # Это будут векторы, ортогональные всем строкам row_basis
    kernel_basis = null_space(row_basis)

    # 4. Проверяем размерность
    if kernel_basis.shape[1] != n - rank:
        raise ValueError("Неожиданная размерность ядра")

    return kernel_basis


def kalman_decomposition(A, B, C):
    A = A.astype(np.float64)
    B  = B.astype(np.float64)
    C = C.astype(np.float64)
    n = A.shape[0]
    ControllabilityMatrix = controllability_matrix(A, B)
    print("Матрица управляемости системы:\n", ControllabilityMatrix)
    ObservabilityMatrix = observability_matrix(A, C)
    print("Матрица наблюдаемости системы:\n", ObservabilityMatrix)
    r_C = np.linalg.matrix_rank(ControllabilityMatrix)
    r_O = np.linalg.matrix_rank(ObservabilityMatrix)
    # 1. Находим образ Cm и ядро Om
    # Базис образа Cm
    if r_C == n:
        basis_C = ControllabilityMatrix
    else:
        basis_C = image_basis(ControllabilityMatrix)
    print("Базис образа ControllabilityMatrix:\n", basis_C)
    # Базис ядра Om
    if r_O == n:
        basis_O = ObservabilityMatrix
    else:
        basis_O = kernel_basis_qr(ObservabilityMatrix)
    print("Базис ядра ObservabilityMatrix:\n", basis_O)
    # Находим базис пересечения
    T2 = null_space(np.hstack((basis_C, basis_O)))



    print("Базис пересечения T2:\n", T2)
    # Блок 1 будет дополнять до базиса образа Cm
    T1 = fill_basis(T2, basis_C)
    print("Блок T1:\n", T1)
    # Блок 4 будет дополнять до базиса ядра Om
    T4 = fill_basis(T2, basis_O)
    print("Блок T4:\n", T4)
    # Блок T3 дополняет до базиса всю матрицу
    T3 = append_basis(np.hstack((T1, T2, T4)))
    print("Блок T3:\n", T3)

    T = np.hstack((T1, T2, T3, T4))
    print("Матрица T:\n", T)
    T_inv = np.linalg.inv(T)
    print("Матрица T^{-1}:\n", T_inv)
    A_hat = T_inv@A@T
    B_hat = T_inv@B
    C_hat = C@T
    print("Матрица A^~:\n",A_hat)
    print("Матрица B^~:\n",B_hat)
    print("Матрица C^~:\n",C_hat)
    return A_hat,B_hat,C_hat,T