my_tests.py - master - Chronos/Control_lib

my_tests.py
444 строки12 KB
Chronos
Final version (left only Kharitonov theorem)
06 май 2025, 13:14
06 май 2025, 13:1487d8d1a
import unittest

import numpy as np

from LMI_regulators import *
from LQR import *
from Modal_Control import *
from Operator_Norms import *
from Estimation_theory import *
from Robust_stability import *
class TestModalControl(unittest.TestCase):
    def test_pole_placement_accerman(self):
        A = np.array([
            [1,1],
            [0,1]
        ])
        B = np.array([
            [1],
            [1]
        ])
        desired_poles = np.array([-1,-2])
        theta = accerman(A,B,desired_poles).reshape((1,A.shape[0]))
        res_poles = np.sort(np.linalg.eigvals(A+B@theta))
        desired_poles = np.sort(desired_poles)
        diff = np.linalg.norm(res_poles-desired_poles)

        assert diff < 1e-10

    def test_pole_placement_frobenius_luenberger(self):
        A = np.array([
            [1,1],
            [0,1]
        ])
        B = np.array([
            [1],
            [1]
        ])
        desired_poles = np.array([-1,-2])
        theta = frobenius_luenberger(A,B,desired_poles).reshape((1,A.shape[0]))
        res_poles = np.sort(np.linalg.eigvals(A+B@theta))
        desired_poles = np.sort(desired_poles)
        diff = np.linalg.norm(res_poles-desired_poles)

        assert diff < 1e-10

    def test_pole_placement_one_pole(self):
        A = np.array([
            [1,1],
            [0,1]
        ])
        B = np.array([
            [1],
            [1]
        ])
        pole_from = 1
        pole_to = -1
        theta = move_one_pole(A,B,pole_from,pole_to).reshape((1,A.shape[0]))
        res_poles = np.sort(np.linalg.eigvals(A+B@theta))
        desired_poles = np.sort(np.array([pole_to,1]))
        diff = np.linalg.norm(res_poles-desired_poles)

        assert diff < 1e-10

    def test_pole_placement_complex_pole(self):
        A = np.array([
            [1,1],
            [-1,1]
        ])
        B = np.array([
            [1],
            [1]
        ])
        pole_from = 1+1j
        poles_to = np.array([-1,-2])
        theta = move_complex_pole(A,B,pole_from,poles_to).reshape((1,A.shape[0]))
        res_poles = np.sort(np.linalg.eigvals(A+B@theta))
        desired_poles = np.sort(poles_to)
        diff = np.linalg.norm(res_poles-desired_poles)

        assert diff < 1e-10

    def test_calman_decomposition(self):
        A = np.array([
            [1,1,0],
            [0,1,0],
            [0,1,1]
        ])
        B = np.array([
            [0,1],
            [1,0],
            [0,1]
        ])
        C = np.array([
            [1,1,1]
        ])
        A_hat,B_hat,C_hat,T = kalman_decomposition(A,B,C)
        A_hat_target = np.array([
            [1,1,0],
            [0,1,0],
            [0,0,1]
        ])
        B_hat_target = np.array([
            [0,1],
            [1,0],
            [0,0]
        ])
        C_hat_target = np.array([[2],[1],[0]])

        # Запринтил матрицы и сравнил с правильным ответом, похоже
        assert True


    def test_lqr_continuous(self):
        A = np.array([
            [0,1],
            [0,1]
        ])

        B = np.array([
            [0],
            [1]
        ])

        Q = np.array([
            [1,0],
            [0,1]
        ])

        R = np.array([
            [1]
        ])

        # Нужно решить Q + XA+A^TX-XBR^-1B^TX = 0 - алгебраическое уравнение Риккати (в непрерывном случае)

        # НЕПРЕРЫВНЫЙ СЛУЧАЙ!!!
        theta,X = lqr_continuous(A,B,Q,R)
        # theta,X = lqr_discrete(A,B,Q,R)
        diff = np.linalg.norm(theta-np.array([[-1.0,-3.0]]))
        assert diff < 1e-3

    def test_lqr_discrete(self):
        A = np.array([
            [0,1],
            [0,1]
        ])

        B = np.array([
            [0],
            [1]
        ])

        Q = np.array([
            [1,0],
            [0,1]
        ])

        R = np.array([
            [1]
        ])

        # Нужно решить дискретное алгебраическое уравнение Риккати
        theta,X = lqr_discrete(A,B,Q,R)
        diff = np.linalg.norm(X-np.array([
            [1.0,0.0],
            [0.0,1.0+np.sqrt(3)]
        ]))
        assert diff < 1e-3










    # def test_h_inf_norm_continuous(self):
    #     A = np.array([[-1, -2], [2, -1]])
    #     B = np.array([
    #         [0],
    #         [1]
    #     ])
    #     C = np.array([[1, 0]])
    #
    #     # Вычисляем H∞ норму
    #
    #     gamma = h_inf_norm_lmi_continuous(A, B, C)
    #     assert np.abs(gamma - 0.5) < 1e-4
    #
    # def test_h_inf_norm_discrete(self):
    #     A = np.array([[0.5]])
    #     B = np.array([
    #         [1.0]
    #     ])
    #     C = np.array([[1.0]])
    #
    #     # Вычисляем H∞ норму
    #
    #     gamma = h_inf_norm_lmi_discrete(A, B, C)
    #     assert np.abs(gamma - 2.0) < 1e-4


    # def test_generalized_h_2_norm(self):
    #     A = np.array([[-1, 0], [0, -1]])
    #     B = np.array([[1], [0]])
    #     C = np.array([[1, 0]])
    #     h_2_norm = generalized_h_2_norm_lmi(A,B,C)
    #     # print(h_2_norm)
    #     a=2
    #     assert True
    #
    # def test_h_inf_norm_minimizing_control_continuous(self):
    #     A = np.array([[2.0]])
    #     Bu = np.array([[1.0]])  # Управление u
    #     Bv = np.array([[1.0]])  # Возмущение v
    #     C = np.array([[1.0]])
    #     D = np.array([[0.0]])   # Нет прямого вклада u в выход
    #
    #     h_inf_norm,theta = h_inf_norm_minimizing_control_lmi_continuous(A,Bu,Bv, C,D,tol=1e-6)
    #     print(h_inf_norm)
    #     print(theta)
    #     a = 2
    #     assert True
    #
    # def test_generalised_h_2_norm_minimizing_control(self):
    #     A = np.array([[-1, 0.5], [0.3, -2]])
    #     Bu = np.array([[1], [0.5]])  # Управление u
    #     Bv = np.array([[0.1], [0.2]])  # Возмущение v
    #     C = np.array([[1, 0]])
    #     D = np.array([[1.0]])
    #     h_2_norm,theta = generalized_h_2_norm_minimizing_control_lmi(A,Bu,Bv, C,D)
    #     print(h_2_norm)
    #     print(theta)
    #     a=2
    #     assert True





    def test_least_squares(self):
        f = lambda x: 3*x**2+2*x+1.0

        x = np.linspace(-5,5,200)
        values = f(x)
        x = x.reshape((len(x),1))
        noise = np.random.normal(scale=1.0,size=len(x))
        values_with_noise = values + noise

        values_with_noise = values_with_noise.reshape((len(values_with_noise),1))

        C = np.hstack(((x*x),x,np.ones((len(x),1))))

        y = values_with_noise
        # C = np.array([
        #     [0.0931, 1],
        #     [1.0986, 1],
        #     [1.3863, 1],  # C0 (3x3) будет иметь полный ранг
        #     [1.6094, 1],
        #     [1.7918, 1]
        # ])
        # y = np.array([[0.4055],[1.0986],[1.5041],[1.9459],[2.401]])
        x_hat = least_squares(C, y)
        a,b,c = x_hat.reshape((1,3))[0]
        # plt.plot(x,f(x))
        plt.scatter(x,values_with_noise)
        plt.plot(x,a*x**2+b*x+c,'--',color='red',label='Квадратичная аппроксимация')
        plt.show()
        assert abs(a-3) < 1e-1 and abs(b-2) < 1e-1 and abs(c-1) < 1e-1


    # def test_recurrent_least_squares(self):
    #     # C = np.array([
    #     #     [0.0931, 1],
    #     #     [1.0986, 1],
    #     #     [1.3863, 1],  # C0 (3x3) будет иметь полный ранг
    #     #     [1.6094, 1],
    #     #     [1.7918, 1]
    #     # ])
    #     # y = np.array([[0.4055],[1.0986],[1.5041],[1.9459],[2.401]])
    #
    #
    #     f = lambda x: 0.5*x**2+2*x+1.0
    #
    #     x = np.linspace(-1,1,6)
    #     values = f(x)
    #     x = x.reshape((len(x),1))
    #     noise = np.random.normal(scale=0.01,size=len(x))
    #     values_with_noise = values + noise
    #
    #     values_with_noise = values_with_noise.reshape((len(values_with_noise),1))
    #
    #     C = np.hstack(((x*x),x,np.ones((len(x),1))))
    #
    #     y = values_with_noise
    #
    #     x_hat = recurrent_least_squares(C, y)
    #     a,b,c = x_hat.reshape((1,3))[0]
    #     # plt.plot(x,f(x))
    #     plt.scatter(x,values_with_noise)
    #
    #
    #     w = np.linspace(-1,1,100)
    #
    #     plt.plot(w,a*w**2+b*w+c,'--',color='red',label='Квадратичная аппроксимация')
    #     plt.show()
        # C = np.array([
        #     [2,1,0],
        #     [0,np.sqrt(3),6],
        #     [1,3,0],  # C0 (3x3) будет иметь полный ранг
        #     [0,1,-3],
        #     [2,0,0]
        # ])
        # y = np.array([[4],[6],[1],[-7],[-8]])

        # x_hat = recurrent_least_squares(C, y)
        # x_hat_2 = least_squares(C, y)
        # print(x_hat)
        # print(x_hat_2)
        # assert np.linalg.norm(x_hat - least_squares(C, y)) < 1e-5


    def test_kharitonov_theorem(self):
        lower_coeffs = [5,1,28,30,8,15][::-1]
        upper_coeffs = [20,12,50,35,3,7]
        res = check_robust_stability_kharitonov(lower_coeffs, upper_coeffs)
        assert res == False

    def test_robust_stability_square(self):
        ncffs = np.array([1,1.5,0.5,0.3,4])

        # задаем границы изменения коэффициентов
        bcffs = np.array([1,0.5,0.5,0.2,1])

        robust_stability_square(ncffs, bcffs)

    def test_robust_stability_disk(self):
        ncffs = np.array([433.5,667.25,502.72,251.25,80.25,14,1])

        # задаем границы изменения коэффициентов
        bcffs = np.array([43.35,33.36,25.137,15.075,5.6175,1.4,0.1])
        robust_stability_disk(ncffs, bcffs)

    def test_robust_stability_radii(self):
        A = np.array([
            [-1,-2],
            [2,-1]
        ])
        B = np.array([
            [0],
            [1]
        ])
        C = np.array([
            [1,0]
        ])
        r_c = robust_stability_radii(A,B,C)
        # print(r_c)
        assert np.abs(r_c - 2.0) < 1e-4

    def test_stabilizing_regulator_continuous(self):
        A = np.array([
            [-1,2,0],
            [0,0.5,1],
            [1,0,-2]
        ])
        B = np.array([
            [1,0],
            [0,1],
            [1,0]
        ])
        theta = build_stabilizing_regulator_continuous(A,B)
        assert np.max(np.linalg.eigvals(A+np.dot(B,theta)).real) < 0.0

    def test_stabilizing_regulator_discrete(self):
        A = np.array([
            [-1,2,0],
            [0,0.5,1],
            [1,0,-2]
        ])
        B = np.array([
            [1,0],
            [0,1],
            [1,0]
        ])
        theta = build_stabilizing_regulator_discrete(A,B)
        assert np.max(np.abs(np.linalg.eigvals(A+np.dot(B,theta)))) < 1.0

    def test_check_matrix_D_stability(self):
        A = np.array([
            [-4.2386,-0.2026,0.7193],
            [2.6649,-2.8342,0.0175],
            [0.0344,0.0005,-3.1772]
        ])

        L1 = np.array([[2]])
        L2 = np.array([
            [-3,3],
            [3,-3]
        ])
        L = block_diag(L1,L2)

        M1 = np.array([[0.5]])
        M2 = np.array([
            [0.0,1.0],
            [0.0,0.0]
        ])

        M = block_diag(M1,M2)
        assert check_matrix_D_stability(A,L,M) == True

    def test_check_D_stabilizing_regulator(self):
        A = np.array([
            [0.5,0,0],
            [0,-2,10],
            [0,1,-2]
        ])
        B = np.array([
            [1,0],
            [-2,2],
            [0,1]
        ])

        L1 = np.array([
            [1.0]
        ])
        L2 = np.array([
            [-1,0.0],
            [0.0,-1.0]
        ])

        L = block_diag(L1,L2)
        #
        M1 = np.array([
            [1.0]
        ])
        M2 = np.array([
            [0,1],
            [0,0]
        ])
        M = block_diag(M1,M2)
        theta  = build_D_stabilizing_regulator(A,B,L,M)
        eigvals = np.linalg.eigvals(A+np.dot(B,theta))
        assert np.max(np.abs(eigvals)) < 1.0 and np.max(eigvals.real) < -0.5