N = np.shape(y)[0]
Ts = time_data[1] - time_data[0]
std_noise_V = 0.0 * 5.0
std_noise_I = 0.0 * 0.5
std_noise = np.array([std_noise_V, std_noise_I])
x_noise = np.copy(x) + np.random.randn(*x.shape) * std_noise
x_noise = x_noise.astype(np.float32)
y_noise = np.copy(y)
# Initialize optimization
ss_model = NeuralStateSpaceModel(n_x=2, n_u=1, n_feat=64) #NeuralStateSpaceModelLin(A_nominal*Ts, B_nominal*Ts)
nn_solution = NeuralStateSpaceSimulator(ss_model)
nn_solution.ss_model.load_state_dict(torch.load(os.path.join("models", "model_SS_1step.pkl")))
# nn_solution.ss_model.load_state_dict(torch.load(os.path.join("models", "model_ss_16step_from1.pkl")))
# nn_solution.ss_model.load_state_dict(torch.load(os.path.join("models", "model_ss_128step_from16.pkl")))
# In[Validate model]
t_val_start = 0
t_val_end = time_data[-1]
idx_val_start = int(t_val_start//Ts)#x.shape[0]
idx_val_end = int(t_val_end//Ts)#x.shape[0]
# Build fit data
u_val = u[idx_val_start:idx_val_end]
x_val = x_noise[idx_val_start:idx_val_end]
y_val = y[idx_val_start:idx_val_end]
) # torch.stack([time_torch_fit[batch_start[i]:batch_start[i] + seq_len] for i in range(batch_size)], dim=0)
batch_x0 = torch.tensor(
x_fit[batch_start]) # x_meas_torch_fit[batch_start, :] # (M, D)
batch_u = torch.tensor(
u_fit[batch_idx]
) # torch.stack([u_torch_fit[batch_start[i]:batch_start[i] + seq_len] for i in range(batch_size)], dim=0)
batch_x = torch.tensor(
x_fit[batch_idx]
) # torch.stack([x_meas_torch_fit[batch_start[i]:batch_start[i] + seq_len] for i in range(batch_size)], dim=0)
return batch_t, batch_x0, batch_u, batch_x
ss_model = NeuralStateSpaceModel(
n_x=2, n_u=1,
n_feat=64) # NeuralStateSpaceModelLin(A_nominal*Ts, B_nominal*Ts)
nn_solution = NeuralStateSpaceSimulator(ss_model)
# nn_solution.ss_model.load_state_dict(torch.load(os.path.join("models", "model_SS_1step.pkl")))
params = list(nn_solution.ss_model.parameters())
optimizer = optim.Adam(params, lr=1e-5)
end = time.time()
with torch.no_grad():
batch_t, batch_x0, batch_u, batch_x = get_batch(batch_size, seq_len)
batch_x_pred = nn_solution.f_sim_multistep(batch_x0, batch_u)
err = batch_x - batch_x_pred
loss_scale = torch.mean(err**2)
LOSS = []
start_time = time.time()
for itr in range(0, num_iter):
x0_torch = torch.from_numpy(x[0, :])
x_noise = np.copy(x) # np.random.randn(*x.shape)*std_noise
x_noise = x_noise.astype(np.float32)
Ts = time_data[1] - time_data[0]
# Get fit data
t_fit = time_data[-1] # use all data
n_fit = int(t_fit // Ts)
input_data = u[0:n_fit]
state_data = x_noise[0:n_fit]
u_torch = torch.from_numpy(input_data)
x_true_torch = torch.from_numpy(state_data)
# Setup neural model structure
ss_model = NeuralStateSpaceModel(n_x=2, n_u=1, n_feat=64, init_small=False)
nn_solution = NeuralStateSpaceSimulator(ss_model)
# Setup optimizer
optimizer = optim.Adam(nn_solution.ss_model.parameters(), lr=lr)
# Scale loss with respect to the initial one
with torch.no_grad():
x_est_torch = nn_solution.f_onestep(x_true_torch, u_torch)
err_init = x_est_torch - x_true_torch
scale_error = torch.sqrt(torch.mean(err_init**2, dim=0))
# Training loop
LOSS = []
start_time = time.time()
for itr in range(0, num_iter):
y_noise = x_noise[:, [y_var_idx]]
# Build validation data
t_val_start = 0
t_val_end = time_data[-1]
idx_val_start = int(t_val_start//Ts)
idx_val_end = int(t_val_end//Ts)
u_val = u[idx_val_start:idx_val_end]
x_meas_val = x_noise[idx_val_start:idx_val_end]
x_true_val = x[idx_val_start:idx_val_end]
y_val = y[idx_val_start:idx_val_end]
time_val = time_data[idx_val_start:idx_val_end]
# Setup neural model structure and load fitted model parameters
ss_model = NeuralStateSpaceModel(n_x=2, n_u=1, n_feat=64)
nn_solution = NeuralStateSpaceSimulator(ss_model)
model_filename = f"model_SS_{model_type}.pkl"
nn_solution.ss_model.load_state_dict(torch.load(os.path.join("models", model_filename)))
# Evaluate the model in open-loop simulation against validation data
x_0 = x_meas_val[0, :]
with torch.no_grad():
x_sim_torch = nn_solution.f_sim(torch.tensor(x_0), torch.tensor(u_val))
loss = torch.mean(torch.abs(x_sim_torch - torch.tensor(x_true_val)))
# Plot results
x_sim = np.array(x_sim_torch)
if not plot_input:
fig, ax = plt.subplots(2, 1, sharex=True, figsize=(6, 5.5))
else:
fig, ax = plt.subplots(3, 1, sharex=True, figsize=(6, 7.5))
def simulate_pendulum_MPC(sim_options):
use_NN_model = get_parameter(sim_options, 'use_NN_model')
if use_NN_model:
Ts_fit = 10e-3 # model was fitted with this sampling time
ss_model = CartPoleStateSpaceModel(Ts=Ts_fit)
nn_solution = NeuralStateSpaceSimulator(ss_model, Ts=Ts_fit)
model_name = "model_SS_1step_nonoise.pkl"
nn_solution.ss_model.load_state_dict(
torch.load(os.path.join("models", model_name)))
f_ODE = nn_solution.f_ODE
else:
f_ODE = f_ODE_wrapped
seed_val = get_parameter(sim_options, 'seed_val')
if seed_val is not None:
np.random.seed(seed_val)
Ac = get_parameter(sim_options, 'Ac')
Bc = get_parameter(sim_options, 'Bc')
Cc = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.]])
Dc = np.zeros((2, 1))
[nx, nu] = Bc.shape # number of states and number or inputs
ny = np.shape(Cc)[0]
Ts_MPC = get_parameter(sim_options, 'Ts_MPC')
ratio_Ts = int(Ts_MPC // Ts_fast)
Ts_MPC = (
(Ts_MPC //
Ts_fast)) * Ts_fast # make Ts_MPC an integer multiple of Ts_fast
# Brutal forward euler discretization
Ad = np.eye(nx) + Ac * Ts_MPC
Bd = Bc * Ts_MPC
Cd = Cc
Dd = Dc
# Standard deviation of the measurement noise on position and angle
std_npos = get_parameter(sim_options, 'std_npos')
std_nphi = get_parameter(sim_options, 'std_nphi')
# Force disturbance
std_dF = get_parameter(sim_options, 'std_dF')
# disturbance power spectrum
w_F = get_parameter(sim_options,
'w_F') # bandwidth of the force disturbance
tau_F = 1 / w_F
Hu = control.TransferFunction([1], [1 / w_F, 1])
Hu = Hu * Hu
Hud = control.matlab.c2d(Hu, Ts_MPC)
N_sim_imp = tau_F / Ts_MPC * 20
t_imp = np.arange(N_sim_imp) * Ts_MPC
t, y = control.impulse_response(Hud, t_imp)
y = y[0]
std_tmp = np.sqrt(np.sum(y**2)) # np.sqrt(trapz(y**2,t))
Hu = Hu / (std_tmp) * std_dF
N_skip = 1 #int(20 * tau_F // Ts_MPC) # skip initial samples to get a regime sample of d
t_sim_d = get_parameter(sim_options, 'len_sim') # simulation length (s)
N_sim_d = int(t_sim_d // Ts_MPC)
N_sim_d = N_sim_d + N_skip + 1
e = np.random.randn(N_sim_d)
te = np.arange(N_sim_d) * Ts_MPC
_, d, _ = control.forced_response(Hu, te, e)
d = d.ravel()
p_ref = d[N_skip:]
#td = np.arange(len(d)) * Ts_fast
Np = get_parameter(sim_options, 'Np')
Nc = get_parameter(sim_options, 'Nc')
Qx = get_parameter(sim_options, 'Qx')
QxN = get_parameter(sim_options, 'QxN')
Qu = get_parameter(sim_options, 'Qu')
QDu = get_parameter(sim_options, 'QDu')
# Reference input and states
xref_fun = get_parameter(sim_options, 'xref_fun') # reference state
xref_fun_v = np.vectorize(xref_fun, signature='()->(n)')
t0 = 0
xref_MPC = xref_fun(t0)
uref = get_parameter(sim_options, 'uref')
uminus1 = np.array(
[0.0]
) # input at time step negative one - used to penalize the first delta u at time instant 0. Could be the same as uref.
# Constraints
xmin = np.array([-1000, -1000, -1000, -1000])
xmax = np.array([1000, 1000.0, 1000, 1000])
umin = np.array([-10])
umax = np.array([10])
Dumin = np.array([-100 * Ts_MPC])
Dumax = np.array([100 * Ts_MPC])
# Initialize simulation system
phi0 = 0.0 #10*2*np.pi/360
x0 = np.array([0, 0, phi0, 0]) # initial state
# system_dyn = ode(f_ODE_wrapped).set_integrator('vode', method='bdf') # dopri5
system_dyn = ode(f_ODE).set_integrator('dopri5')
system_dyn.set_initial_value(x0, t0)
system_dyn.set_f_params(0.0)
QP_eps_rel = get_parameter(sim_options, 'QP_eps_rel')
QP_eps_abs = get_parameter(sim_options, 'QP_eps_abs')
# Emergency exit conditions
EMERGENCY_STOP = False
EMERGENCY_POS = 30.0
EMERGENCY_ANGLE = 90 * DEG_TO_RAD
K = MPCController(Ad,
Bd,
Np=Np,
Nc=Nc,
x0=x0,
xref=xref_MPC,
uminus1=uminus1,
Qx=Qx,
QxN=QxN,
Qu=Qu,
QDu=QDu,
xmin=xmin,
xmax=xmax,
umin=umin,
umax=umax,
Dumin=Dumin,
Dumax=Dumax,
eps_feas=1e3,
eps_rel=QP_eps_rel,
eps_abs=QP_eps_abs)
try:
K.setup(solve=True) # setup initial problem and also solve it
except:
EMERGENCY_STOP = True
if not EMERGENCY_STOP:
if K.res.info.status != 'solved':
EMERGENCY_STOP = True
# Basic Kalman filter design
Q_kal = get_parameter(sim_options, 'Q_kal')
R_kal = get_parameter(sim_options, 'R_kal')
L, P, W = kalman_design_simple(Ad,
Bd,
Cd,
Dd,
Q_kal,
R_kal,
type='predictor')
x0_est = x0
KF = LinearStateEstimator(x0_est, Ad, Bd, Cd, Dd, L)
# Simulate in closed loop
len_sim = get_parameter(sim_options, 'len_sim') # simulation length (s)
nsim = int(
np.ceil(len_sim / Ts_MPC)
) # simulation length(timesteps) # watch out! +1 added, is it correct?
t_vec = np.zeros((nsim, 1))
t_calc_vec = np.zeros(
(nsim, 1)) # computational time to get MPC solution (+ estimator)
status_vec = np.zeros((nsim, 1))
x_vec = np.zeros((nsim, nx))
x_ref_vec = np.zeros((nsim, nx))
y_vec = np.zeros((nsim, ny))
y_meas_vec = np.zeros((nsim, ny))
y_est_vec = np.zeros((nsim, ny))
x_est_vec = np.zeros((nsim, nx))
u_vec = np.zeros((nsim, nu))
x_MPC_pred = np.zeros(
(nsim, Np + 1, nx)) # on-line predictions from the Kalman Filter
nsim_fast = int(len_sim // Ts_fast)
t_vec_fast = np.zeros((nsim_fast, 1))
x_vec_fast = np.zeros(
(nsim_fast, nx)) # finer integration grid for performance evaluation
x_ref_vec_fast = np.zeros(
(nsim_fast, nx)) # finer integration grid for performance evaluatio
u_vec_fast = np.zeros(
(nsim_fast, nu)) # finer integration grid for performance evaluatio
Fd_vec_fast = np.zeros((nsim_fast, nu)) #
t_int_vec_fast = np.zeros((nsim_fast, 1))
emergency_vec_fast = np.zeros((nsim_fast, 1)) #
t_pred_all = t0 + np.arange(nsim + Np + 1) * Ts_MPC
Xref_MPC_all = xref_fun_v(t_pred_all)
t_step = t0
x_step = x0
u_MPC = None
for idx_fast in range(nsim_fast):
## Determine step type: fast simulation only or MPC step
idx_MPC = idx_fast // ratio_Ts
run_MPC = (idx_fast % ratio_Ts) == 0
# Output for step i
# Ts_MPC outputs
if run_MPC: # it is also a step of the simulation at rate Ts_MPC
t_vec[idx_MPC, :] = t_step
x_vec[idx_MPC, :] = x_step #system_dyn.y
xref_MPC = np.array([p_ref[idx_MPC], 0.0, 0.0, 0.0])
#xref_MPC = xref_fun(t_step) # reference state
t_pred = t_step + np.arange(Np + 1) * Ts_MPC
x_ref_vec[idx_MPC, :] = xref_MPC
if not EMERGENCY_STOP:
# u_MPC, info_MPC = K.output(return_x_seq=True, return_status=True) # u[i] = k(\hat x[i]) possibly computed at time instant -1
u_MPC, info_MPC = K.output(
return_status=True, return_x_seq=True
) # u[i] = k(\hat x[i]) possibly computed at time instant -1
else:
u_MPC = np.zeros(nu)
if not EMERGENCY_STOP:
if info_MPC['status'] != 'solved':
EMERGENCY_STOP = True
if not EMERGENCY_STOP:
#pass
x_MPC_pred[idx_MPC, :, :] = info_MPC[
'x_seq'] # x_MPC_pred[i,i+1,...| possibly computed at time instant -1]
u_vec[idx_MPC, :] = u_MPC
y_step = Cd.dot(x_step) # y[i] from the system
ymeas_step = np.copy(y_step)
ymeas_step[0] += std_npos * np.random.randn()
ymeas_step[1] += std_nphi * np.random.randn()
y_vec[idx_MPC, :] = y_step
y_meas_vec[idx_MPC, :] = ymeas_step
if not EMERGENCY_STOP:
status_vec[idx_MPC, :] = (info_MPC['status'] != 'solved')
if np.abs(ymeas_step[0]) > EMERGENCY_POS or np.abs(
ymeas_step[1]) > EMERGENCY_ANGLE:
EMERGENCY_STOP = True
# Ts_fast outputs
t_vec_fast[idx_fast, :] = t_step
x_vec_fast[idx_fast, :] = x_step #system_dyn.y
x_ref_vec_fast[idx_fast, :] = xref_MPC
u_fast = u_MPC
u_vec_fast[idx_fast, :] = u_fast
Fd_vec_fast[idx_fast, :] = 0.0
emergency_vec_fast[idx_fast, :] = EMERGENCY_STOP
## Update to step i+1
# Controller simulation step at rate Ts_MPC
if run_MPC:
time_calc_start = time.perf_counter()
# Kalman filter: update and predict
#KF.update(ymeas_step) # \hat x[i|i]
#KF.predict(u_MPC) # \hat x[i+1|i]
KF.predict_update(u_MPC, ymeas_step)
# MPC update
if not EMERGENCY_STOP:
#Xref_MPC = Xref_MPC_all[idx_MPC:idx_MPC + Np + 1]
K.update(
KF.x, u_MPC,
xref=xref_MPC) # update with measurement and reference
#K.update(KF.x, u_MPC, xref=xref_MPC) # update with measurement and reference
t_calc_vec[idx_MPC, :] = time.perf_counter() - time_calc_start
if t_calc_vec[idx_MPC, :] > 2 * Ts_MPC:
EMERGENCY_STOP = True
# System simulation step at rate Ts_fast
time_integrate_start = time.perf_counter()
system_dyn.set_f_params(u_fast)
system_dyn.integrate(t_step + Ts_fast)
x_step = system_dyn.y
#x_step = x_step + f_ODE(t_step, x_step, u_fast)*Ts_fast
t_int_vec_fast[
idx_fast, :] = time.perf_counter() - time_integrate_start
# Time update
t_step += Ts_fast
simout = {
't': t_vec,
'x': x_vec,
'u': u_vec,
'y': y_vec,
'y_meas': y_meas_vec,
'x_ref': x_ref_vec,
'x_MPC_pred': x_MPC_pred,
'status': status_vec,
'Fd_fast': Fd_vec_fast,
't_fast': t_vec_fast,
'x_fast': x_vec_fast,
'x_ref_fast': x_ref_vec_fast,
'u_fast': u_vec_fast,
'emergency_fast': emergency_vec_fast,
'KF': KF,
'K': K,
'nsim': nsim,
'Ts_MPC': Ts_MPC,
't_calc': t_calc_vec,
't_int_fast': t_int_vec_fast,
'use_NN_model': use_NN_model
}
return simout
COL_T = ['time']
COL_Y = ['p_meas', 'theta_meas']
COL_X = ['p', 'v', 'theta', 'omega']
COL_U = ['u']
df_X = pd.read_csv(os.path.join("data", "pendulum_data_MPC_ref.csv"))
t = np.array(df_X[COL_T], dtype=np.float32)
y = np.array(df_X[COL_Y], dtype=np.float32)
x = np.array(df_X[COL_X], dtype=np.float32)
u = np.array(df_X[COL_U], dtype=np.float32)
Ts = t[1] - t[0]
x_noise = x
n_x = x.shape[-1]
ss_model = CartPoleStateSpaceModel(Ts)
nn_solution = NeuralStateSpaceSimulator(ss_model)
#model_name = "model_SS_1step_nonoise.pkl"
# model_name = "model_SS_150step_nonoise.pkl"
#nn_solution.ss_model.load_state_dict(torch.load(os.path.join("models", model_name)))
n_fit = int(len_fit // Ts)
time_fit = t[0:n_fit]
u_fit = u[0:n_fit]
x_meas_fit = x_noise[0:n_fit]
y_meas_fit = y[0:n_fit]
batch_size = n_fit // seq_len
x_hidden_fit = torch.tensor(
x_meas_fit, requires_grad=True) # this is an optimization variable!
params = list(nn_solution.ss_model.parameters())
COL_R = ['r']
# Load dataset
df_X = pd.read_csv(os.path.join("data", "pendulum_data_oloop_id.csv"))
t = np.array(df_X[COL_T], dtype=np.float32)
y = np.array(df_X[COL_Y], dtype=np.float32)
x = np.array(df_X[COL_X], dtype=np.float32)
u = np.array(df_X[COL_U], dtype=np.float32)
Ts = t[1] - t[0]
# Add measurement noise
x_noise = np.copy(x)
# Setup neural model structure
ss_model = CartPoleStateSpaceModel(Ts)
nn_solution = NeuralStateSpaceSimulator(ss_model)
# Fit data to pytorch tensors #
n_fit = int(len_fit//Ts)
u_fit = u[0:n_fit]
x_fit = x_noise[0:n_fit]
t_fit = t[0:n_fit]
u_fit_torch = torch.from_numpy(u_fit)
x_meas_fit_torch = torch.from_numpy(x_fit)
# Setup optimizer
params = list(nn_solution.ss_model.parameters())
optimizer = optim.Adam(params, lr=lr)
end = time.time()
# Scale loss with respect to the initial one
df_X = pd.read_csv(os.path.join("data", "RLC_data_sat_FE.csv"))
time_data = np.array(df_X[COL_T], dtype=np.float32)
y = np.array(df_X[COL_Y], dtype=np.float32)
x = np.array(df_X[COL_X], dtype=np.float32)
u = np.array(df_X[COL_U], dtype=np.float32)
t = np.array(df_X[COL_T], dtype=np.float32)
x0_torch = torch.from_numpy(x[0,:])
Ts = time_data[1] - time_data[0]
n_x = 2
n_u = 1
n_hidden = 64
ss_model = NeuralStateSpaceModel(n_x, n_u, n_hidden)
nn_solution = NeuralStateSpaceSimulator(ss_model)
nn_solution.ss_model.load_state_dict(torch.load(os.path.join("models", "model_ARX_FE_sat.pkl")))
x_torch = torch.tensor(x)
x0_torch = torch.tensor(x[0,:])
u_torch = torch.tensor(u)
with torch.no_grad():
x_sim_torch = nn_solution.f_sim(x0_torch, u_torch)
loss = torch.mean(torch.abs(x_sim_torch - x_torch))
x_sim = np.array(x_sim_torch)
n_plot = t.size
fig,ax = plt.subplots(3,1,sharex=True)
ax[0].plot(t[:n_plot], x[:n_plot, 0], label='True')
ax[0].plot(t[:n_plot], x_sim[:n_plot, 0], label='Simulated')
N = np.shape(y)[0]
Ts = time_data[1] - time_data[0]
std_noise_V = 0.0 * 5.0
std_noise_I = 0.0 * 0.5
std_noise = np.array([std_noise_V, std_noise_I])
x_noise = np.copy(x) + np.random.randn(*x.shape) * std_noise
x_noise = x_noise.astype(np.float32)
y_noise = np.copy(y)
# Initialize optimization
ss_model = NeuralStateSpaceModel(
n_x=2, n_u=1,
n_feat=64) # NeuralStateSpaceModelLin(A_nominal*Ts, B_nominal*Ts)
nn_solution = NeuralStateSpaceSimulator(ss_model)
nn_solution.ss_model.load_state_dict(
torch.load(os.path.join("models", "model_SS_1step_nonoise.pkl")))
# In[Validate model]
t_val_start = 0
t_val_end = time_data[-1]
idx_val_start = int(t_val_start // Ts) # x.shape[0]
idx_val_end = int(t_val_end // Ts) # x.shape[0]
# Build fit data
u_val = u[idx_val_start:idx_val_end]
x_val = x_noise[idx_val_start:idx_val_end]
y_val = y[idx_val_start:idx_val_end]
time_val = time_data[idx_val_start:idx_val_end]
x_est = np.zeros((time_exp.shape[0], 2), dtype=np.float32)
x_est[:, 0] = np.copy(y_id[:, 0])
# Hidden state variable
x_hidden_fit = torch.tensor(
x_est, dtype=torch.float32,
requires_grad=True) # hidden state is an optimization variable
y_fit = y_id
u_fit = u_id
u_fit_torch = torch.tensor(u_fit)
y_fit_torch = torch.tensor(y_fit)
time_fit = time_exp
# Setup neural model structure
ss_model = CTSNeuralStateSpaceModel(n_x=2, n_u=1, n_feat=64, ts=ts)
nn_solution = NeuralStateSpaceSimulator(ss_model)
model_name = 'model_SS_256step'
hidden_name = 'hidden_SS_256step'
nn_solution.ss_model.load_state_dict(
torch.load(os.path.join("models", model_name + ".pkl")))
x_hidden_fit = torch.load(os.path.join("models", hidden_name + ".pkl"))
# Setup optimizer
params_net = list(nn_solution.ss_model.parameters())
params_hidden = [x_hidden_fit]
optimizer = optim.Adam([
{
'params': params_net,
'lr': lr
},
time_exp = np.arange(y_id.size).astype(np.float32) * ts
x_est = np.zeros((time_exp.shape[0], 2), dtype=np.float32)
x_est[:, 0] = np.copy(y_id[:, 0])
# Hidden state variable
x_hidden_fit = torch.tensor(
x_est, dtype=torch.float32,
requires_grad=True) # hidden state is an optimization variable
y_fit = y_id
u_fit = u_id
time_fit = time_exp
# Setup neural model structure
ss_model = CTSNeuralStateSpaceModel(n_x=2, n_u=1, n_feat=64, ts=ts)
nn_solution = NeuralStateSpaceSimulator(ss_model)
# Setup optimizer
params_net = list(nn_solution.ss_model.parameters())
params_hidden = [x_hidden_fit]
optimizer = optim.Adam([
{
'params': params_net,
'lr': lr
},
{
'params': params_hidden,
'lr': lr
},
],
lr=10 * lr)
'uref': np.array([0.0]), # N
'std_npos': 0 * 0.001, # m
'std_nphi': 0 * 0.00005, # rad
'std_dF': 0.1, # N
'w_F': 5, # rad
'len_sim': 40, #s
'Ac': Ac_def,
'Bc': Bc_def,
'Ts_slower_loop': Ts_slower,
'Q_kal': np.diag([0.1, 10, 0.1, 10]),
'R_kal': 1 * np.eye(2),
'seed_val': 42
}
ss_model = CartPoleStateSpaceModel(Ts=Ts_slower)
nn_solution = NeuralStateSpaceSimulator(ss_model, Ts=Ts_PID)
model_name = "model_SS_1step_nonoise.pkl"
nn_solution.ss_model.load_state_dict(
torch.load(os.path.join("models", model_name)))
f_ODE_wrapped = nn_solution.f_ODE
def get_parameter(sim_options, par_name):
return sim_options.get(par_name, DEFAULTS_PENDULUM_MPC[par_name])
def get_default_parameters(sim_options):
""" Which parameters are left to default ??"""
default_keys = [
key for key in DEFAULTS_PENDULUM_MPC if key not in sim_options
]
std_noise = np.array([std_noise_V, std_noise_I])
x_noise = np.copy(x) + np.random.randn(*x.shape) * std_noise
x_noise = x_noise.astype(np.float32)
Ts = time_data[1] - time_data[0]
n_fit = int(t_fit // Ts) #x.shape[0]
# Fit data to pytorch tensors #
input_data = u[0:n_fit]
state_data = x_noise[0:n_fit]
u_torch = torch.from_numpy(input_data)
x_true_torch = torch.from_numpy(state_data)
# Setup neural model structure
ss_model = NeuralStateSpaceModel(n_x=2, n_u=1, n_feat=64)
nn_solution = NeuralStateSpaceSimulator(ss_model)
# Setup optimizer
params = list(nn_solution.ss_model.parameters())
optimizer = optim.Adam(params, lr=1e-3)
# Scale loss with respect to the initial one
with torch.no_grad():
x_est_torch = nn_solution.f_sim(x0_torch, u_torch)
err_init = x_est_torch - x_true_torch
scale_error = torch.sqrt(torch.mean((err_init)**2, dim=(0)))
start_time = time.time()
LOSS = []
# Training loop
for itr in range(1, num_iter + 1):
