def plot_outside_data_start(dyn_GP, data_dic, outside_folder=None):
if not outside_folder:
outside_folder = os.path.join(dyn_GP.results_folder, 'Data_outside_GP')
os.makedirs(outside_folder, exist_ok=True)
for key, val in data_dic.items():
val = reshape_dim1(val)
for i in range(val.shape[1]):
name = key + str(i) + '.pdf'
plt.plot(val[:, i], label=key + str(i))
plt.title(key)
plt.legend()
plt.xlabel('Time steps')
plt.ylabel(key)
plt.savefig(os.path.join(outside_folder, name),
bbox_inches='tight')
plt.close('all')
if all(k in data_dic for k in ('xtraj', 'xtraj_estim')):
xtraj = reshape_dim1(data_dic.get('xtraj'))
xtraj_estim = reshape_dim1(data_dic.get('xtraj_estim'))
if not xtraj.any():
return 0, 0, 0, 0, 0
dimmin = np.min([xtraj_estim.shape[1], xtraj.shape[1]])
for i in range(dimmin):
name = 'xtraj_xtrajestim_' + str(i) + '.pdf'
plt.plot(xtraj[:, i], label='True state', c='g')
plt.plot(xtraj_estim[:, i],
label='Estimated state',
c='orange',
alpha=0.9)
plt.title('True and estimated position over time')
plt.legend()
plt.xlabel('Time steps')
plt.ylabel('x_' + str(i))
plt.savefig(os.path.join(outside_folder, name),
bbox_inches='tight')
plt.close('all')
name = 'Estimation_RMSE_time'
RMSE = RMS(xtraj[:, :dimmin] - xtraj_estim[:, :dimmin])
SRMSE = RMSE / np.var(xtraj)
output_RMSE = RMS(xtraj[:, 0] - xtraj_estim[:, 0])
output_SRMSE = RMSE / np.var(xtraj[:, 0])
error = np.sqrt(
np.mean(np.square(xtraj[:, :dimmin] - xtraj_estim[:, :dimmin]),
axis=1))
error_df = pd.DataFrame(error)
error_df.to_csv(os.path.join(outside_folder, name + '_time.csv'),
header=False)
plt.plot(error, 'orange', label='Error')
plt.title('State estimation RMSE over last cycle')
plt.xlabel('Time steps')
plt.ylabel(r'$|x - \hat{x}|$')
plt.legend()
plt.savefig(os.path.join(outside_folder, name + '.pdf'),
bbox_inches='tight')
plt.close('all')
return error, RMSE, SRMSE, output_RMSE, output_SRMSE
def VanderPol_continuous_prior_mean_Michelangelo_deriv(x, u, prior_kwargs):
mu = prior_kwargs.get('mu')
x = reshape_pt1(x)
u = reshape_pt1(u)
deriv_x = reshape_dim1(-2 * mu * x[:, 0] * x[:, 1] - np.ones_like(x[:, 0]))
deriv_xdot = reshape_dim1(mu * (1 - x[:, 0]**2))
phi_deriv = reshape_pt1(np.concatenate((deriv_x, deriv_xdot), axis=1))
return phi_deriv
def duffing_continuous_prior_mean_Michelangelo_deriv(x, u, prior_kwargs):
alpha = prior_kwargs.get('alpha')
beta = prior_kwargs.get('beta')
delta = prior_kwargs.get('delta')
x = reshape_pt1(x)
deriv = reshape_dim1(-3 * beta * ((x[:, 0])**2) - alpha)
phi_deriv = reshape_pt1(
np.concatenate((deriv, reshape_dim1(-delta * np.ones_like(x[:, 0]))),
axis=1))
return phi_deriv
def pendulum_continuous_prior_mean_Michelangelo_deriv(x, u, prior_kwargs):
k = prior_kwargs.get('k')
m = prior_kwargs.get('m')
g = prior_kwargs.get('g')
l = prior_kwargs.get('l')
x = reshape_pt1(x)
deriv = reshape_dim1(-g / l * np.cos(x[:, 0]))
phi_deriv = reshape_pt1(
np.concatenate((deriv, reshape_dim1(-k / m * np.ones_like(x[:, 0]))),
axis=1))
return phi_deriv
def duffing_dynamics(t, x, u, t0, init_control, process_noise_var, kwargs):
alpha = kwargs.get('alpha')
beta = kwargs.get('beta')
delta = kwargs.get('delta')
x = reshape_pt1(x)
u = reshape_pt1(u(t, kwargs, t0, init_control))
A = reshape_pt1([[0, 1], [-alpha, -delta]])
F1 = reshape_dim1(np.zeros_like(x[:, 0]))
F2 = reshape_dim1(-beta * x[:, 0]**3)
F = reshape_pt1(np.concatenate((F1, F2), axis=1))
xdot = reshape_pt1(np.dot(A, reshape_pt1_tonormal(x)) + F + u)
if process_noise_var != 0:
xdot += np.random.normal(0, np.sqrt(process_noise_var), xdot.shape)
return xdot
def duffing_continuous_prior_mean(x, u, prior_kwargs):
alpha = prior_kwargs.get('alpha')
beta = prior_kwargs.get('beta')
delta = prior_kwargs.get('delta')
x = reshape_pt1(x)
u = reshape_pt1(u)
A = reshape_pt1([[0, 1], [-alpha, -delta]])
# Amult = np.array([np.dot(A, x[i, :]) for i in range(len(x))])
dot = lambda a: np.dot(A, a)
Amult = np.apply_along_axis(func1d=dot, axis=1, arr=x)
F1 = reshape_dim1(np.zeros_like(x[:, 0]))
F2 = reshape_dim1(-beta * (x[:, 0])**3)
F = reshape_pt1(np.concatenate((F1, F2), axis=1))
xdot = reshape_pt1(Amult + F + u)
return xdot
def wdc_arm_dynamics(t, x, u, t0, init_control, process_noise_var, kwargs):
inertia = kwargs.get('inertia')
m = kwargs.get('m')
lG = kwargs.get('lG')
g = kwargs.get('g')
x = reshape_pt1(x)
u = reshape_pt1(u(t, kwargs, t0, init_control))
A = reshape_pt1([[0, 1], [0, 0]])
F1 = reshape_dim1(np.zeros_like(x[:, 0]))
F2 = reshape_dim1(-m * g * lG / inertia * np.sin(x[:, 0]))
F = reshape_pt1(np.concatenate((F1, F2), axis=1))
xdot = reshape_pt1(np.dot(A, reshape_pt1_tonormal(x)) + F + u)
if process_noise_var != 0:
xdot += np.random.normal(0, np.sqrt(process_noise_var), xdot.shape)
return xdot
def sin_controller_02D(t, kwargs, t0, init_control):
gamma = kwargs.get('gamma')
omega = kwargs.get('omega')
if np.isscalar(t):
if t == t0:
u = reshape_pt1(init_control)
else:
u = reshape_pt1([[0, gamma * np.cos(omega * t)]])
else:
u = reshape_pt1(np.concatenate((reshape_dim1(
np.zeros(len(t))), reshape_dim1(gamma * np.cos(omega * t))),
axis=1))
if t[0] == t0:
u[0] = reshape_pt1(init_control)
return u
def harmonic_oscillator_continuous_prior_mean_Michelangelo_u(
x, u, prior_kwargs):
k = prior_kwargs.get('k')
m = prior_kwargs.get('m')
x = reshape_pt1(x)
phi = reshape_dim1(-k / m * x[:, 0] + u[:, 1])
return phi
def pendulum_continuous_prior_mean_Michelangelo(x, u, prior_kwargs):
k = prior_kwargs.get('k')
m = prior_kwargs.get('m')
g = prior_kwargs.get('g')
l = prior_kwargs.get('l')
x = reshape_pt1(x)
phi = reshape_dim1(-g / l * np.sin(x[:, 0]) - k / m * x[:, 1])
return phi
def duffing_cossquare_continuous_prior_mean_Michelangelo_u(x, u, prior_kwargs):
alpha = prior_kwargs.get('alpha')
beta = prior_kwargs.get('beta')
delta = prior_kwargs.get('delta')
x = reshape_pt1(x)
phi = reshape_dim1(-beta * (np.cos(x[:, 0])**2) - alpha * x[:, 0] -
delta * x[:, 1] + u[:, 1])
return phi
def harmonic_oscillator_continuous_prior_mean_Michelangelo_deriv(
x, u, prior_kwargs):
k = prior_kwargs.get('k')
m = prior_kwargs.get('m')
deriv = reshape_dim1(-k / m * np.ones_like(x[:, 0]))
phi_deriv = reshape_pt1(
np.concatenate((deriv, np.zeros_like(deriv)), axis=1))
return phi_deriv
def pendulum_dynamics(t, x, u, t0, init_control, process_noise_var, kwargs):
k = kwargs.get('k')
m = kwargs.get('m')
g = kwargs.get('g')
l = kwargs.get('l')
x = reshape_pt1(x)
u = reshape_pt1(u(t, kwargs, t0, init_control))
theta_before = x[:, 0]
thetadot_before = x[:, 1]
A = reshape_pt1([[0, 1], [0, 0]])
F1 = reshape_dim1(np.zeros_like(x[:, 0]))
F2 = reshape_dim1(-g / l * np.sin(theta_before) - k / m * thetadot_before)
F = reshape_pt1(np.concatenate((F1, F2), axis=1))
xdot = reshape_pt1(np.dot(A, reshape_pt1_tonormal(x)) + F + u)
if process_noise_var != 0:
xdot += np.random.normal(0, np.sqrt(process_noise_var), xdot.shape)
return xdot
def wdc_arm_continuous_prior_mean_Michelangelo_u(x, u, prior_kwargs):
inertia = prior_kwargs.get('inertia')
m = prior_kwargs.get('m')
lG = prior_kwargs.get('lG')
g = prior_kwargs.get('g')
x = reshape_pt1(x)
u = reshape_pt1(u)
# phi = reshape_dim1(-m * g * lG / inertia * np.sin(x[:, 0]))
phi = reshape_dim1(np.zeros_like(x[:, 0]) + u[:, 1])
return phi
def duffing_observer_Delgado(t, xhat, u, y, t0, init_control, GP, kwargs):
alpha = kwargs.get('alpha')
beta = kwargs.get('beta')
delta = kwargs.get('delta')
xhat = reshape_pt1(xhat)
y = reshape_pt1(y(t, kwargs))
u = reshape_pt1(u(t, kwargs, t0, init_control))
# My gains
l1 = delta - 5
l2 = alpha - delta ** 2 + 3 * beta * xhat[:, 0] ** 2
# # Delgado gains
# l1 = - 5
# l2 = - (alpha + 3 * xhat[:, 0] ** 2)
A = reshape_pt1([[0, 1], [-alpha, -delta]])
F1 = reshape_dim1(np.zeros_like(xhat[:, 0]))
F2 = reshape_dim1(- beta * xhat[:, 0] ** 3)
F = reshape_pt1(np.concatenate((F1, F2), axis=1))
LC = reshape_pt1([[l1 * (xhat[:, 0] - y), l2 * (xhat[:, 0] - y)]])
xhatdot = reshape_pt1(np.dot(A, reshape_pt1_tonormal(xhat)) + F + LC + u)
return xhatdot
def create_model(self, X, Y, likelihood, inference_method):
self.models = [
GPy.core.gp.GP(X,
reshape_dim1(Y[:, 0]),
self.kern[0],
likelihood,
inference_method=inference_method)
]
self.param_array = np.array(self.models[0].param_array)
for i in range(1, self.nb_output_dims):
self.models += [
GPy.core.gp.GP(X,
reshape_dim1(Y[:, i]),
self.kern[i],
likelihood,
inference_method=inference_method)
]
self.param_array = np.concatenate(
(self.param_array, self.models[i].param_array))
return self.models
def __init__(self, X, Y, kernel, likelihood, inference_method):
self.nb_output_dims = reshape_pt1(Y[0]).shape[1]
self.kern = kernel
self.models = [
GPy.core.gp.GP(X,
reshape_dim1(Y[:, 0]),
kernel=self.kern[0],
likelihood=likelihood,
inference_method=inference_method)
]
self.param_array = np.array(self.models[0].param_array)
for i in range(1, self.nb_output_dims):
self.models += [
GPy.core.gp.GP(X,
reshape_dim1(Y[:, i]),
self.kern[i],
likelihood,
inference_method=inference_method)
]
self.param_array = np.concatenate(
(self.param_array, self.models[i].param_array))
def mass_spring_mass_dynamics_z(t, z, u, t0, init_control, process_noise_var,
kwargs):
m1 = kwargs.get('m1')
m2 = kwargs.get('m2')
k1 = kwargs.get('k1')
k2 = kwargs.get('k2')
z = reshape_pt1(z)
z3 = reshape_pt1(z[:, 2])
u = reshape_pt1(u(t, kwargs, t0, init_control))
A = np.eye(z.shape[1], k=1)
F1 = reshape_dim1(np.zeros_like(z[:, :3]))
v = reshape_pt1_tonormal(mass_spring_mass_v(z, kwargs))
vdot = reshape_pt1_tonormal(mass_spring_mass_vdot(z, kwargs))
F2 = reshape_dim1(k1 / (m1 * m2) * (u - (m1 + m2) * z3) + (3 * k2) /
(m1 * m2) * (u - (m1 + m2) * z3) * v**2 +
(6 * k2) / m1 * v * vdot**2)
F = reshape_pt1(np.concatenate((F1, F2), axis=1))
zdot = reshape_pt1(np.dot(A, reshape_pt1_tonormal(z)) + F)
if process_noise_var != 0:
zdot += np.random.normal(0, np.sqrt(process_noise_var), zdot.shape)
return zdot
def chirp_controller(t, kwargs, t0, init_control):
gamma = kwargs.get('gamma')
f0 = kwargs.get('f0')
f1 = kwargs.get('f1')
t1 = kwargs.get('t1')
nb_cycles = int(np.floor(np.min(t) / t1))
t = t - nb_cycles * t1
if np.isscalar(t):
if t == t0:
u = reshape_pt1(init_control)
else:
u = reshape_pt1(
[[0, signal.chirp(t, f0=f0, f1=f1, t1=t1, method='linear')]])
else:
u = reshape_pt1(np.concatenate((reshape_dim1(np.zeros(len(t))),
reshape_dim1(
signal.chirp(t, f0=f0, f1=f1, t1=t1,
method='linear'))),
axis=1))
if t[0] == t0:
u[0] = reshape_pt1(init_control)
return gamma * u
def sin_controller_1D(t, kwargs, t0, init_control):
gamma = kwargs.get('gamma')
omega = kwargs.get('omega')
if np.isscalar(t):
if t == t0:
u = reshape_pt1(init_control)
else:
u = reshape_pt1([[gamma * np.cos(omega * t)]])
else:
u = reshape_dim1(gamma * np.cos(omega * t))
if t[0] == t0:
u[0] = reshape_pt1(init_control)
return u
def wdc_arm_continuous_justvelocity_prior_mean(x, u, prior_kwargs):
inertia = prior_kwargs.get('inertia')
m = prior_kwargs.get('m')
lG = prior_kwargs.get('lG')
g = prior_kwargs.get('g')
x = reshape_pt1(x)
u = reshape_pt1(u)
# F1 = reshape_dim1(np.zeros_like(x[:, 0]))
# F2 = reshape_dim1(-m * g * lG / inertia * np.sin(x[:, 0]))
# F = reshape_pt1(np.concatenate((F1, F2), axis=1))
F = reshape_pt1(np.zeros_like(u))
vdot = reshape_dim1(F[:, 1] + u[:, 1])
return vdot
def mass_spring_mass_dynamics_x(t, x, u, t0, init_control, process_noise_var,
kwargs):
m1 = kwargs.get('m1')
m2 = kwargs.get('m2')
k1 = kwargs.get('k1')
k2 = kwargs.get('k2')
x = reshape_pt1(x)
u = reshape_pt1(u(t, kwargs, t0, init_control))
A = reshape_pt1([[0, 1, 0, 0], [0, 0, 0, 0], [0, 0, 0, 1], [0, 0, 0, 0]])
B = reshape_pt1([[0], [0], [0], [1]])
F1 = reshape_dim1(np.zeros_like(x[:, 0]))
F2 = reshape_dim1(k1 / m1 * (x[:, 2] - x[:, 0]) + k2 / m1 *
(x[:, 2] - x[:, 0])**3)
F3 = reshape_dim1(np.zeros_like(x[:, 2]))
F4 = reshape_dim1(-k1 / m2 * (x[:, 2] - x[:, 0]) - k2 / m2 *
(x[:, 2] - x[:, 0])**3)
F = reshape_pt1(np.concatenate((F1, F2, F3, F4), axis=1))
xdot = reshape_pt1(
np.dot(A, reshape_pt1_tonormal(x)) + F +
np.dot(B, reshape_pt1_tonormal(u)))
if process_noise_var != 0:
xdot += np.random.normal(0, np.sqrt(process_noise_var), xdot.shape)
return xdot
def MSM_continuous_to_discrete_justvelocity_prior_mean(x, u, prior_kwargs):
dt = prior_kwargs.get('dt')
x = reshape_pt1(x)
u = reshape_pt1(u)
xu = np.concatenate((x, u), axis=1)
def dyns_1D(a):
x0 = reshape_pt1(a[:x.shape[1]])
u0 = reshape_pt1(a[x.shape[1]:])
version = \
lambda t, xl, ul, t0, init_control, process_noise_var, **kwargs: \
MSM_continuous_justvelocity_prior_mean(xl, ul, kwargs)
vnext = dynamics_traj(x0=x0,
u=u0,
t0=0,
dt=dt,
init_control=u0,
discrete=False,
version=version,
meas_noise_var=0,
process_noise_var=0,
method='RK45',
t_span=[0, dt],
t_eval=[dt],
kwargs=prior_kwargs)[:, -1]
return reshape_pt1_tonormal(vnext)
vnext = np.apply_along_axis(func1d=dyns_1D, axis=1, arr=xu)
# vnext = np.array([reshape_pt1_tonormal(
# dynamics_traj(x0=x[i, :], u=u[i, :], t0=0, dt=dt, init_control=u[i, :],
# discrete=False,
# version=lambda t, xl, ul, t0, init_control,
# process_noise_var, **kwargs:
# MSM_continuous_justvelocity_prior_mean(xl, ul, kwargs),
# meas_noise_var=0, process_noise_var=0, method='RK45',
# t_span=[0, dt], t_eval=[dt], kwargs=prior_kwargs))
# for i in range(len(x))])[:, -1]
return reshape_dim1(vnext)
def MSM_continuous_justvelocity_prior_mean(x, u, prior_kwargs):
u = reshape_pt1(u)
F = reshape_pt1(np.zeros_like(u))
vdot = reshape_dim1(F[:, -1])
return vdot
def VanderPol_continuous_prior_mean_Michelangelo_u(x, u, prior_kwargs):
mu = prior_kwargs.get('mu')
x = reshape_pt1(x)
u = reshape_pt1(u)
phi = reshape_dim1(mu * (1 - x[:, 0]**2) * x[:, 1] - x[:, 0] + u[:, 1])
return phi
def simulate_estimations(system,
observe_data,
t_eval,
t0,
tf,
dt,
meas_noise_var,
init_control,
init_state_estim,
controller,
observer,
optim_method,
dyn_config,
xtraj,
GP=None,
discrete=False,
verbose=False):
assert len(t_eval) == xtraj.shape[0], 'State trajectory and simulation ' \
'time over which to estimate the ' \
'states are not the same. Resample ' \
'your true trajectory to plot it ' \
'over the estimation time.'
y_observed = observe_data(xtraj)
if 'noisy_inputs' in system:
y_observed = reshape_pt1(
y_observed +
np.random.normal(0, np.sqrt(meas_noise_var), y_observed.shape))
t_y = np.concatenate((reshape_dim1(t_eval), y_observed), axis=1)
noisy_init_state_estim = np.concatenate(
(reshape_pt1(y_observed[0]), reshape_pt1(init_state_estim[:, 1:])),
axis=1)
if 'No_observer' in system:
logging.info('No observer has been specified, using true data for '
'learning.')
xtraj_estim = xtraj
else:
xtraj_estim = dynamics_traj_observer(
x0=reshape_pt1(noisy_init_state_estim),
u=controller,
y=lambda t, kwarg: interpolate(t, t_y, t0,
noisy_init_state_estim[:, 0]),
t0=t0,
dt=dt,
init_control=init_control,
discrete=discrete,
version=observer,
method=optim_method,
t_span=[t0, tf],
t_eval=t_eval,
GP=GP,
kwargs=dyn_config)
# Trajectory
dimmin = np.min([xtraj_estim.shape[1], xtraj.shape[1]])
plt.plot(t_eval, xtraj[:, 0], 'g', label='True position')
plt.plot(t_eval, y_observed, 'r', label='Observed position')
plt.plot(t_eval,
xtraj_estim[:, 0],
'orange',
label='Estimated position')
plt.title('True and estimated position over time')
plt.xlabel('t')
plt.ylabel('x')
plt.legend()
if verbose:
plt.show()
plt.plot(t_eval, xtraj[:, 1], 'g', label='True velocity')
plt.plot(t_eval,
xtraj_estim[:, 1],
'orange',
label='Estimated velocity')
plt.title('True and estimated velocity over time')
plt.xlabel('t')
plt.ylabel('x')
plt.legend()
if verbose:
plt.show()
if 'Michelangelo' in system:
plt.plot(t_eval,
xtraj_estim[:, -1],
'orange',
label='Estimated xi')
plt.title('Estimated xi over time')
plt.xlabel('t')
plt.ylabel('xi')
plt.legend()
if verbose:
plt.show()
elif 'adaptive' in system:
plt.plot(t_eval, xtraj_estim[:, -1], 'orange', label='Gain')
plt.title('Adaptive gain over time')
plt.xlabel('t')
plt.ylabel('g')
plt.legend()
if verbose:
plt.show()
elif dimmin > 2:
for i in range(2, dimmin):
plt.plot(t_eval, xtraj[:, i], 'g', label='True dim ' + str(i))
plt.plot(t_eval,
xtraj_estim[:, i],
'orange',
label='Estimated dim ' + str(i))
plt.title('True and estimated dim ' + str(i) + ' over time')
plt.xlabel('t')
plt.ylabel('x')
plt.legend()
if verbose:
plt.show()
# Phase portrait
plt.plot(xtraj[:, 0], xtraj[:, 1], 'g', label='True trajectory')
plt.plot(xtraj_estim[:, 0],
xtraj_estim[:, 1],
'orange',
label='Estimated trajectory')
plt.title('True and estimated phase portrait')
plt.xlabel('x')
plt.ylabel(r'$\dot{x}$')
plt.legend()
if verbose:
plt.show()
plt.close('all')
plt.clf()
# Error plot
plt.plot(np.sum(np.square(xtraj[:, :dimmin] - xtraj_estim[:, :dimmin]),
axis=1),
'orange',
label='True trajectory')
plt.title('Error plot')
plt.xlabel('t')
plt.ylabel(r'$|x - \hat{x}|$')
plt.legend()
if verbose:
plt.show()
plt.close('all')
plt.clf()
return y_observed, t_y, xtraj_estim
# True dynamics: (xt, ut) -> xt+1 if no observer, (xt, ut) -> phi(xt,ut) if
# Michelangelo. If no observer, simulate system for 10*dt starting at xt
# and return result at t+dt
if ('Michelangelo' in system) and ('Duffing' in system):
# Return xi_t instead of x_t+1 from x_t,u_t
true_dynamics = lambda x, control: \
- dyn_kwargs.get('beta') * x[:, 0] ** 3 - dyn_kwargs.get('alpha') \
* x[:, 0] - dyn_kwargs.get('delta') * x[:, 1]
elif ('justvelocity' in system) and ('Duffing' in system):
if not continuous_model:
true_dynamics = lambda x, control: dynamics_traj(
x0=reshape_pt1(x),
u=lambda t, kwarg, t0, init_control: interpolate(
t,
np.concatenate(
(reshape_dim1(np.arange(len(control))), control),
axis=1),
t0=t0,
init_value=init_control),
t0=t0,
dt=dt,
init_control=init_control,
version=dynamics,
meas_noise_var=0,
process_noise_var=process_noise_var,
method=optim_method,
t_span=[0, dt],
t_eval=[dt],
kwargs=dyn_kwargs)[:, -1]
else:
true_dynamics = lambda x, control: \
def model_kalman_rollout(dyn_GP,
observer,
observe_data,
discrete_observer,
folder,
init_state,
control_traj,
true_mean,
rollout_length=100,
title=None,
verbose=False,
save=False,
no_GP_in_observer=False,
only_prior=False):
rollout_length = int(np.min([rollout_length, len(true_mean) - 1]))
predicted_mean = np.zeros((rollout_length + 1, init_state.shape[1]))
predicted_lowconf = np.zeros((rollout_length + 1, init_state.shape[1]))
predicted_uppconf = np.zeros((rollout_length + 1, init_state.shape[1]))
predicted_var = np.zeros((rollout_length + 1, 1))
y_observed = reshape_dim1(observe_data(true_mean))
if 'noisy_inputs' in dyn_GP.system:
y_observed = reshape_pt1(y_observed + np.random.normal(
0, np.sqrt(dyn_GP.true_meas_noise_var), y_observed.shape))
init_state_estim = np.concatenate(
(reshape_pt1(y_observed[0]), np.zeros((1, init_state.shape[1] - 1))),
axis=1) # xhat0 = (x0_1 + noise,0,...,0)
predicted_mean[0] = init_state_estim
predicted_lowconf[0] = init_state_estim
predicted_uppconf[0] = init_state_estim
predicted_var[0] = np.zeros((1, 1))
time = np.arange(0, rollout_length + 1)
dt = dyn_GP.dt
kwargs = dyn_GP.config
kwargs.update(dict(kalman=True))
if no_GP_in_observer:
GP = dyn_GP.observer_prior_mean
else:
GP = dyn_GP
for t in range(rollout_length):
control = control_traj[t]
if t == 0:
if 'Michelangelo' in dyn_GP.system:
# True and predicted trajectory over time (random start, random
# control) with Euler to get xt+1 from GP xt, ut->phit
mean_next, varnext, next_lowconf, next_uppconf = \
dyn_GP.predict_euler_Michelangelo(
predicted_mean[t], control, only_prior=only_prior)
elif ('justvelocity' in dyn_GP.system) and \
not dyn_GP.continuous_model:
# True and predicted trajectory over time (random start, random
# control) with Euler to get xt+1 from GP xt, ut->x_nt+1
mean_next, varnext, next_lowconf, next_uppconf = \
dyn_GP.predict_euler_discrete_justvelocity(
predicted_mean[t], control, only_prior=only_prior)
elif ('justvelocity' in dyn_GP.system) and dyn_GP.continuous_model:
# True and predicted trajectory over time (random start, random
# control) with Euler to get xt+1 from GP xt, ut->xdot_nt
mean_next, varnext, next_lowconf, next_uppconf = \
dyn_GP.predict_euler_continuous_justvelocity(
predicted_mean[t], control, only_prior=only_prior)
else:
# True and predicted trajectory over time (random start, random
# control)
mean_next, varnext, next_lowconf, next_uppconf = dyn_GP.predict(
predicted_mean[t], control, only_prior=only_prior)
last_observation = reshape_pt1(init_state_estim.copy())
if 'Michelangelo' in dyn_GP.system:
last_observation = np.concatenate(
(last_observation, reshape_pt1([0])), axis=1)
else:
# Use the control and measurement recorded until current time
# step to build y(t) and u(t) functions, then solve the observer
# from t-1 to current t to restart GP prediction from a point
# corrected by current measure, and at next step restart observer
# solving from past corrected GP prediction (KF style)
t_u = np.concatenate((reshape_dim1(
time[:t + 1] * dt), reshape_dim1(control_traj[:t + 1])),
axis=1)
controller = lambda t_current, kwarg, t_init, u_init: \
interpolate(t_current, t_u, 0, reshape_pt1(control_traj[0]),
method='linear')
t_y = np.concatenate(
(reshape_dim1(time[:t + 1] * dt), y_observed[:t + 1]), axis=1)
measurement = lambda t_current, kwarg: \
interpolate(t_current, t_y, 0, init_state_estim[:, 0],
method='linear')
# Best would be to restart observer from init_observation,
# but takes too much time, so either solutions arguable (restart
# from last observation or from last GP prediction)
if 'Michelangelo' in dyn_GP.system:
previous_xi = reshape_pt1(
(predicted_mean[t, -1] - last_observation[:, -2]) / dt)
restart = np.concatenate(
(reshape_pt1(predicted_mean[t - 1]), previous_xi), axis=1)
last_observation = dynamics_traj_observer(
x0=reshape_pt1(restart), # last_observation?
u=controller,
y=measurement,
t0=(t - 1) * dt,
dt=dt,
init_control=reshape_pt1(control_traj[t - 1]),
discrete=discrete_observer,
version=observer,
method=dyn_GP.optim_method,
t_span=[(t - 1) * dt, t * dt],
t_eval=[t * dt],
GP=GP,
kwargs=dyn_GP.config)
elif ('No_observer' in dyn_GP.system) or ('observer'
not in dyn_GP.system):
last_observation = true_mean[t]
else:
last_observation = dynamics_traj_observer(
x0=reshape_pt1(predicted_mean[t - 1]), # last_observation?
u=controller,
y=measurement,
t0=(t - 1) * dt,
dt=dt,
init_control=reshape_pt1(control_traj[t - 1]),
discrete=discrete_observer,
version=observer,
method=dyn_GP.optim_method,
t_span=[(t - 1) * dt, t * dt],
t_eval=[t * dt],
GP=GP,
kwargs=dyn_GP.config)
if 'Michelangelo' in dyn_GP.system:
# True and predicted trajectory over time (random start, random
# control) with Euler to get xt+1 from GP xt, ut->phit
mean_next, varnext, next_lowconf, next_uppconf = \
dyn_GP.predict_euler_Michelangelo(
last_observation[:, :-1], control,
only_prior=only_prior)
elif ('justvelocity' in dyn_GP.system) and \
not dyn_GP.continuous_model:
# True and predicted trajectory over time (random start, random
# control) with Euler to get xt+1 from GP xt, ut->xt+1
mean_next, varnext, next_lowconf, next_uppconf = \
dyn_GP.predict_euler_discrete_justvelocity(
last_observation, control, only_prior=only_prior)
elif ('justvelocity' in dyn_GP.system) and dyn_GP.continuous_model:
# True and predicted trajectory over time (random start, random
# control) with Euler to get xt+1 from GP xt, ut->xdott
mean_next, varnext, next_lowconf, next_uppconf = \
dyn_GP.predict_euler_continuous_justvelocity(
last_observation, control, only_prior=only_prior)
else:
# True and predicted trajectory over time (random start, random
# control)
mean_next, varnext, next_lowconf, next_uppconf = dyn_GP.predict(
last_observation, control, only_prior=only_prior)
predicted_mean[t + 1] = mean_next
predicted_lowconf[t + 1] = next_lowconf
predicted_uppconf[t + 1] = next_uppconf
predicted_var[t + 1] = varnext
if save:
for i in range(predicted_mean.shape[1]):
if title:
name = title + 'kalman_rollout_model_predictions' + str(i) + \
'.pdf'
else:
name = 'Kalman_rollout_model_predictions' + str(i) + '.pdf'
plt.plot(time, true_mean[:, i], 'g', label='True trajectory')
plt.plot(time,
predicted_mean[:, i],
'b',
label='Predicted trajectory',
alpha=0.7)
plt.fill_between(time,
predicted_lowconf[:, i],
predicted_uppconf[:, i],
facecolor='blue',
alpha=0.2)
if not dyn_GP.ground_truth_approx:
plt.title('Kalman roll out of predicted and true trajectory '
'over time, random start, random control')
else:
if title and ('Test' in title):
plt.title('Kalman roll out of predicted and true '
'trajectory over time over testing data')
elif title and ('Val' in title):
plt.title('Kalman roll out of predicted and true '
'trajectory over time over validation data')
else:
plt.title(
'Kalman roll out of predicted and true '
'trajectory over time, random start, data control')
plt.legend()
plt.xlabel('Time steps')
plt.ylabel('State')
plt.savefig(os.path.join(folder, name), bbox_inches='tight')
if verbose:
plt.show()
plt.close('all')
for i in range(predicted_mean.shape[1] - 1):
if title:
name = title + 'kalman_rollout_phase_portrait' + str(
i) + '.pdf'
else:
name = 'Kalman_rollout_phase_portrait' + str(i) + '.pdf'
plt.plot(true_mean[:, i],
true_mean[:, i + 1],
'g',
label='True trajectory')
plt.plot(predicted_mean[:, i],
predicted_mean[:, i + 1],
'b',
label='Predicted trajectory',
alpha=0.7)
plt.fill_between(predicted_mean[:, i],
predicted_lowconf[:, i + 1],
predicted_uppconf[:, i + 1],
facecolor='blue',
alpha=0.2)
if not dyn_GP.ground_truth_approx:
plt.title('Kalman roll out of predicted and true phase '
'portrait, random start, random control')
else:
if title and ('Test' in title):
plt.title('Roll out of predicted and true phase portrait '
'over testing data')
elif title and ('Val' in title):
plt.title('Kalman roll out of predicted and true phase '
'portrait over validation data')
else:
plt.title('Kalman roll out of predicted and true phase '
'portrait, random start, data control')
plt.legend()
plt.xlabel('x_' + str(i))
plt.ylabel('x_' + str(i + 1))
plt.savefig(os.path.join(folder, name), bbox_inches='tight')
if verbose:
plt.show()
plt.close('all')
RMSE = RMS(predicted_mean - true_mean)
log_likelihood = log_multivariate_normal_likelihood(
true_mean[1:, :], predicted_mean[1:, :], predicted_var[1:, :])
return init_state, control_traj, true_mean, predicted_mean, predicted_var, \
predicted_lowconf, predicted_uppconf, RMSE, log_likelihood
def save_kalman_rollout_variables(results_folder,
nb_rollouts,
rollout_list,
step,
ground_truth_approx=False,
plots=True,
title=None):
if title:
folder = os.path.join(results_folder, title + '_' + str(step))
else:
folder = os.path.join(results_folder, 'Rollouts_' + str(step))
os.makedirs(folder, exist_ok=True)
for i in range(nb_rollouts):
rollout_folder = os.path.join(folder, 'Rollout_' + str(i))
filename = 'Predicted_mean_traj_kalman.csv'
file = pd.DataFrame(reshape_pt1(np.array(rollout_list)[i, 3]))
file.to_csv(os.path.join(rollout_folder, filename), header=False)
filename = 'Predicted_var_traj_kalman.csv'
file = pd.DataFrame(reshape_pt1(np.array(rollout_list)[i, 4]))
file.to_csv(os.path.join(rollout_folder, filename), header=False)
filename = 'Predicted_lowconf_traj_kalman.csv'
file = pd.DataFrame(reshape_pt1(np.array(rollout_list)[i, 5]))
file.to_csv(os.path.join(rollout_folder, filename), header=False)
filename = 'Predicted_uppconf_traj_kalman.csv'
file = pd.DataFrame(reshape_pt1(np.array(rollout_list)[i, 6]))
file.to_csv(os.path.join(rollout_folder, filename), header=False)
filename = 'RMSE_kalman.csv'
file = pd.DataFrame(reshape_pt1(np.array(rollout_list)[i, 7]))
file.to_csv(os.path.join(rollout_folder, filename), header=False)
filename = 'SRMSE_kalman.csv'
file = pd.DataFrame(reshape_pt1(np.array(rollout_list)[i, 8]))
file.to_csv(os.path.join(rollout_folder, filename), header=False)
filename = 'Log_likelihood_kalman.csv'
file = pd.DataFrame(reshape_pt1(np.array(rollout_list)[i, 9]))
file.to_csv(os.path.join(rollout_folder, filename), header=False)
filename = 'Standardized_log_likelihood_kalman.csv'
file = pd.DataFrame(reshape_pt1(np.array(rollout_list)[i, 10]))
file.to_csv(os.path.join(rollout_folder, filename), header=False)
true_mean = reshape_dim1(np.array(rollout_list)[i, 2])
predicted_mean = reshape_dim1(np.array(rollout_list)[i, 3])
predicted_lowconf = reshape_dim1(np.array(rollout_list)[i, 5])
predicted_uppconf = reshape_dim1(np.array(rollout_list)[i, 6])
time = np.arange(0, len(true_mean))
if plots:
for k in range(predicted_mean.shape[1]):
name = 'Kalman_rollout_model_predictions' + str(k) + '.pdf'
plt.plot(time, true_mean[:, k], 'g', label='True trajectory')
plt.plot(time,
predicted_mean[:, k],
'b',
label='Predicted trajectory',
alpha=0.7)
plt.fill_between(time,
predicted_lowconf[:, k],
predicted_uppconf[:, k],
facecolor='blue',
alpha=0.2)
if not ground_truth_approx:
plt.title('Kalman roll out of predicted and true '
'trajectory '
'over time, random start, random control')
else:
plt.title('Kalman roll out of predicted and true '
'trajectory '
'over time, random start, data control')
plt.legend()
plt.xlabel('Time steps')
plt.ylabel('State')
plt.savefig(os.path.join(rollout_folder, name),
bbox_inches='tight')
plt.close('all')
for k in range(predicted_mean.shape[1] - 1):
name = 'Kalman_rollout_phase_portrait' + str(k) + '.pdf'
plt.plot(true_mean[:, k],
true_mean[:, k + 1],
'g',
label='True trajectory')
plt.plot(predicted_mean[:, k],
predicted_mean[:, k + 1],
'b',
label='Predicted trajectory',
alpha=0.7)
plt.fill_between(predicted_mean[:, k],
predicted_lowconf[:, k + 1],
predicted_uppconf[:, k + 1],
facecolor='blue',
alpha=0.2)
if not ground_truth_approx:
plt.title('Kalman roll out of predicted and true '
'phase portrait over time, random start, '
'random control')
else:
plt.title('Kalman roll out of predicted and true '
'phase portrait over time, random start, '
'data control')
plt.legend()
plt.xlabel('x_' + str(k))
plt.ylabel('x_' + str(k + 1))
plt.savefig(os.path.join(rollout_folder, name),
bbox_inches='tight')
plt.close('all')
def dim1_observe_data(xtraj):
return reshape_dim1(xtraj[:, 0])
