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])