Estimation_theory.py - master - Chronos/Control_lib

Estimation_theory.py
66 строк2 KB
Chronos
Final version (left only Kharitonov theorem)
06 май 2025, 13:14
06 май 2025, 13:1487d8d1a
import numpy as np

def least_squares(C,y):
    # Рассматривается измеряемый выход
    # y  = Cx + w, где w - шум
    T = C.T@C
    print("Матрица C^T C: \n", T)
    T_inv = np.linalg.inv(T)
    print("Матрица (C^T C)^(-1): \n", T_inv)
    print("Матрица (C^T C)^(-1)C^T: \n", T_inv@C.T)
    print("Матрица (C^T C)^(-1)C^Ty: \n", T_inv@C.T@y)
    x_hat = T_inv@C.T@y
    return x_hat


# Рекуррентный метод наименьших квадратов
# def recurrent_least_squares(C,y):
#     # Рассматривается измеряемый выход
#     # y  = Cx + w, где w - шум
#     H_k = np.zeros((C.shape[1],C.shape[1]))
#     x_hat_k = np.zeros((C.shape[1],1))
#     k = 1
#     counter = 1
#
#     print("---------------------------------")
#     while counter<=C.shape[0]:
#         print(f"k = {k-1}")
#         print("---------------------------------")
#         # Нужно выбрать размер C_0, чтобы
#         # C_0 имела полный ранг (ЕСЛИ РАНГ C < количества столбцов C)
#         if k == 1 and (np.linalg.matrix_rank(C) <= C.shape[1]):
#
#             while np.linalg.matrix_rank(C[:counter,:]) != C.shape[1]:
#                 counter += 1
#             C_k  = C[:counter,:]
#             y_k = y[:counter]
#         else:
#             C_k  = C[counter-1,:]
#             y_k = y[counter-1]
#
#         # if k == 2:
#         #     pass
#
#         H_k = H_k + C_k.T@C_k
#         z = y_k - C_k@x_hat_k
#         if len(C_k.shape) == 1:
#             C_k = C_k.reshape(1,C_k.shape[0])
#         if z.shape[0] == 1:
#             x_hat_k = x_hat_k + np.linalg.inv(H_k)@C_k.T*z
#         else:
#             x_hat_k = x_hat_k + np.linalg.inv(H_k)@C_k.T@z
#         print(f"y_{k-1} = ", y_k)
#         print(f"H_{k-1} = ", H_k)
#         print(f"C_{k-1} = ", C_k)
#         print(f"x_hat_{k-1} = ", x_hat_k)
#         print("---------------------------------")
#         k += 1
#         counter += 1
#     return x_hat_k

def variance_minima(C,y,X,W):
    return X@C.T@np.linalg.inv(W+C@X@C.T)@y

def weighted_least_squares(C,y,W):
    Omega = np.linalg.inv(W)
    return np.linalg.inv(C.T@Omega@C)@C.T@Omega@y