pyProject/example_siso_pulley_deepc.py

import numpy as np
import scipy.signal as scipysig
import cvxpy as cp
import matplotlib.pyplot as plt
from fontTools.misc.cython import returns

from typing import List
from cvxpy.expressions.expression import Expression
from cvxpy.constraints.constraint import Constraint
from pydeepc import DeePC
from pydeepc.utils import Data
from utils import System

from scipy.signal import step
from scipy import signal

### To ignore warnings about cp.ECOS not being available
import warnings
warnings.simplefilter(action='ignore', category=FutureWarning)
warnings.simplefilter(action='ignore', category=UserWarning)


# Define the loss function for DeePC
def loss_callback(u: cp.Variable, y: cp.Variable) -> Expression:
    horizon, M, P = u.shape[0], u.shape[1], y.shape[1]
    # Sum_t ||y_t - 1||^2
    return 1e3 * cp.norm(y-1,'fro')**2 + 1e-1 * cp.norm(u, 'fro')**2

# Define the constraints for DeePC
def constraints_callback(u: cp.Variable, y: cp.Variable) -> List[Constraint]:
    horizon, M, P = u.shape[0], u.shape[1], y.shape[1]
    # Define a list of input/output constraints (no constraints here)
    return []


# DeePC paramters
s = 1                       # How many steps before we solve again the DeePC problem
T_INI = 4                   # Size of the initial set of data
# T_list = [50, 500]          # Number of data points used to estimate the system
T_list = [1000]          # Number of data points used to estimate the system 训练数据的个数，熟练越多，效果越好
HORIZON = 10                # Horizon length defaut:10
LAMBDA_G_REGULARIZER = 20    # g regularizer (see DeePC paper, eq. 8)   defaut:1
LAMBDA_Y_REGULARIZER = 0    # y regularizer (see DeePC paper, eq. 8)   解决测量数据有噪声时的鲁棒性  defaut:1
LAMBDA_U_REGULARIZER = 0    # u regularizer
EXPERIMENT_HORIZON = 100    # Total number of steps

# Plant
# In this example we consider the three-pulley
# system analyzed in the original VRFT paper:
#
# "Virtual reference feedback tuning:
#      a direct method for the design offeedback controllers"
# -- Campi et al. 2003

dt = 0.05
num = [0.28261, 0.50666]
den = [1, -1.41833, 1.58939, -1.31608, 0.88642]
sys = System(scipysig.TransferFunction(num, den, dt=dt).to_ss(), noise_std=1e-2) #转换为状态空间

# 画图看看系统的响应
# # lti = scipysig.lti([1],[1,1])
# lti = scipysig.lti(num,den)
# t, y = scipysig.step(lti,None,None,10000)  # 计算单位阶跃响应，t是时间向量，y是系统输出向量
# # t, y = scipysig.impulse(lti)  # 计算单位阶跃响应，t是时间向量，y是系统输出向量
#
# # plt.plot(t[0:300], y[0:300])
# plt.plot(t,y)
# plt.xlabel('Time [s]')
# plt.ylabel('Amplitude')
# plt.title('Step Response')
# plt.grid()
# plt.show()
# exit()



fig, ax = plt.subplots(1, 2, figsize=(10,4))
plt.margins(x=0, y=0)


# Simulate for different values of T
i=0
for T in T_list:
    i=i+1
    print(f'Simulating with {T} initial samples...count={i}')
    sys.reset()
    # Generate initial data and initialize DeePC   data : u->1000*1 y-> 1000*1
    data = sys.apply_input(u = np.random.normal(size=T).reshape((T, 1)))
    data_train = data

    #Uf 10*987 Up 4*987 Yf 10*987 Yp 4*987  Tini=4  horizon=10
    deepc = DeePC(data, Tini = T_INI, horizon = HORIZON)
    # deepc = DeePC(data, Tini=T_INI, horizon=HORIZON, explained_variance=.9999)

    # Create initial data u:4*1 y:4*1
    data_ini = Data(u = np.zeros((T_INI, 1)), y = np.zeros((T_INI, 1)))
    sys.reset(data_ini = data_ini)

    deepc.build_problem(
        build_loss = loss_callback,
        build_constraints = constraints_callback,
        lambda_g = LAMBDA_G_REGULARIZER,
        lambda_y = LAMBDA_Y_REGULARIZER,
        lambda_u = LAMBDA_U_REGULARIZER)

    for idx in range(EXPERIMENT_HORIZON // s):  #整除
        # Solve DeePC  u_optimal:10*1
        u_optimal, info = deepc.solve(data_ini = data_ini, warm_start=True, solver=cp.ECOS)


        # Apply optimal control input  计算出来控制量u 取第1:s行,所有列数据
        _ = sys.apply_input(u = u_optimal[:s, :])

        # Fetch last T_INI samples 计算出来的u:4*1     y4*1
        data_ini = sys.get_last_n_samples(T_INI)

        if idx % 10 == 1 :
            print(f'idx={idx}/{EXPERIMENT_HORIZON // s}')

    # Plot curve
    data = sys.get_all_samples()
    ax[0].plot(data.y[T_INI:], label=f'$s={s}, T={T}, T_i={T_INI}, N={HORIZON}$')
    ax[1].plot(data.u[T_INI:], label=f'$s={s}, T={T}, T_i={T_INI}, N={HORIZON}$')

# plt.figure()
# plt.plot(data_train.u,data_train.y,'r-');
# plt.title('This is the trainning data')
# plt.grid()
#


# ax[0].set_ylim(0, 2)
# ax[1].set_ylim(-4, 4)
ax[0].set_xlabel('t')
ax[0].set_ylabel('y')
ax[0].grid()
ax[1].set_ylabel('u')
ax[1].set_xlabel('t')
ax[1].grid()
ax[0].set_title('Closed loop - output signal $y_t$')
ax[1].set_title('Closed loop - control signal $u_t$')
ax[0].legend(fancybox=True, shadow=True)

plt.show()



#
#
# # DeePC paramters
# s = 1                       # How many steps before we solve again the DeePC problem
# T_INI = 4                   # Size of the initial set of data
# T_list = [1000]              # Number of data points used to estimate the system
# HORIZON = 100               # Horizon length
# LAMBDA_G_REGULARIZER = 20   # g regularizer (see DeePC paper, eq. 8)
# LAMBDA_Y_REGULARIZER = 0    # y regularizer (see DeePC paper, eq. 8)
# LAMBDA_U_REGULARIZER = 0    # u regularizer
# EXPERIMENT_HORIZON = 100    # Total number of steps
#
# # Plant
# # In this example we consider the three-pulley
# # system analyzed in the original VRFT paper:
# #
# # "Virtual reference feedback tuning:
# #      a direct method for the design offeedback controllers"
# # -- Campi et al. 2003
#
# dt = 0.05
# num = [0.28261, 0.50666]
# den = [1, -1.41833, 1.58939, -1.31608, 0.88642]
# sys = System(scipysig.TransferFunction(num, den, dt=dt).to_ss(), noise_std=1e-2)
#
# fig, ax = plt.subplots(1, 2, figsize=(10,4))
# plt.margins(x=0, y=0)
#
#
# # Simulate for different values of T
# for T in T_list:
#     print(f'Simulating with {T} initial samples...')
#     sys.reset()
#     # Generate initial data and initialize DeePC
#     data = sys.apply_input(u = np.random.normal(size=T).reshape((T, 1)))
#     deepc = DeePC(data, Tini = T_INI, horizon = HORIZON, explained_variance = .99)
#
#     # Create initial data
#     data_ini = Data(u = np.zeros((T_INI, 1)), y = np.zeros((T_INI, 1)))
#     sys.reset(data_ini = data_ini)
#
#     deepc.build_problem(
#         build_loss = loss_callback,
#         build_constraints = constraints_callback,
#         lambda_g = LAMBDA_G_REGULARIZER,
#         lambda_y = LAMBDA_Y_REGULARIZER,
#         lambda_u = LAMBDA_U_REGULARIZER)
#
#     for idx in range(EXPERIMENT_HORIZON // s):
#         # Solve DeePC
#         u_optimal, info = deepc.solve(data_ini = data_ini, warm_start=True, solver=cp.ECOS)
#
#
#         # Apply optimal control input
#         _ = sys.apply_input(u = u_optimal[:s, :])
#
#         # Fetch last T_INI samples
#         data_ini = sys.get_last_n_samples(T_INI)
#
#         if idx % 10 == 1 :
#             print(f'idx={idx}/{EXPERIMENT_HORIZON // s}')
#
#
#     # Plot curve
#     data = sys.get_all_samples()
#     ax[0].plot(data.y[T_INI:], label=f'$s={s}, T={T}, T_i={T_INI}, N={HORIZON}$')
#     ax[1].plot(data.u[T_INI:], label=f'$s={s}, T={T}, T_i={T_INI}, N={HORIZON}$')
#
# ax[0].set_ylim(0, 2)
# ax[1].set_ylim(-4, 4)
# ax[0].set_xlabel('t')
# ax[0].set_ylabel('y')
# ax[0].grid()
# ax[1].set_ylabel('u')
# ax[1].set_xlabel('t')
# ax[1].grid()
# ax[0].set_title('Closed loop - output signal $y_t$')
# ax[1].set_title('Closed loop - control signal $u_t$')
# ax[0].legend(fancybox=True, shadow=True)
# plt.show()