def WDC_justvelocity_observer_highgain_GP(t, xhat, u, y, t0, init_control, GP, kwargs): x = reshape_pt1(xhat) assert np.any(kwargs.get('saturation')), 'Need to define a saturation ' \ 'value to use the combined ' \ 'observer-identifier framework.' xhat = reshape_pt1(x) y = reshape_pt1(y(t, kwargs)) u = reshape_pt1(u(t, kwargs, t0, init_control)) # Gain (needs to be large enough) g = kwargs.get('prior_kwargs').get('observer_gains').get('g') k1 = kwargs.get('prior_kwargs').get('observer_gains').get('k1') k2 = kwargs.get('prior_kwargs').get('observer_gains').get('k2') Gamma1 = reshape_pt1([k1 * g, k2 * g ** 2]) if GP: if 'GP' in GP.__class__.__name__: mean, var, lowconf, uppconf = GP.predict(reshape_pt1(xhat), reshape_pt1(u)) if not kwargs.get('continuous_model'): # discrete model so need to differentiate it in continuous obs mean = (mean - reshape_pt1(xhat[:, 1])) / GP.prior_kwargs.get( 'dt') # TODO better than Euler? GP.prior_kwargs['observer_gains'].update({'g': g, 'Gamma1': Gamma1, 'k1': k1, 'k2': k2}) else: mean = GP(reshape_pt1(xhat), reshape_pt1(u), kwargs.get( 'prior_kwargs')) else: mean = np.zeros_like(reshape_pt1(xhat[:, 1])) if np.any(kwargs.get('saturation')): # Saturate the estimate of the nonlinearity to guarantee contraction a_min = np.min([-kwargs.get('saturation'), kwargs.get('saturation')], axis=0) a_max = np.max([-kwargs.get('saturation'), kwargs.get('saturation')], axis=0) mean = np.clip(mean, a_min=a_min, a_max=a_max) A = reshape_pt1([[0, 1], [0, 0]]) B = reshape_pt1([[0], [1]]) ABmult = np.dot(A, reshape_pt1_tonormal(xhat)) + \ np.dot(B, reshape_pt1_tonormal(mean)) LC1 = reshape_pt1(Gamma1 * (y - xhat[:, 0])) xhatdot = reshape_pt1(ABmult + LC1 + u) # Also check eigenvalues of M for stability without high gain K = np.array([[k1, k2]]) C = np.zeros_like(xhat) C[0, 0] = 1 M = A - np.dot(K.T, C) eigvals = np.linalg.eigvals(M) for x in eigvals: if np.linalg.norm(np.real(x)) < 1e-5: logging.warning('The eigenvalues of the matrix M are dangerously ' 'small, low robustness of the observer! Increase ' 'the gains.') elif np.real(x) > 0: logging.warning('Some of the eigenvalues of the matrix M are ' 'positive. Change the gains to get a Hurwitz ' 'matrix.') return reshape_pt1(xhatdot)
def duffing_observer_Michelangelo_LS(t, xhat, u, y, t0, init_control, LS_deriv, kwargs): x = reshape_pt1(xhat) assert np.any(kwargs.get('saturation')), 'Need to define a saturation ' \ 'value to use the combined ' \ 'observer-identifier framework.' xhat = reshape_pt1(x[:, :-1]) xi = reshape_pt1(x[:, -1]) y = reshape_pt1(y(t, kwargs)) u = reshape_pt1(u(t, kwargs, t0, init_control)) # Gain (needs to be large enough) g = kwargs.get('prior_kwargs').get('observer_gains').get('g') k1 = kwargs.get('prior_kwargs').get('observer_gains').get('k1') k2 = kwargs.get('prior_kwargs').get('observer_gains').get('k2') k3 = kwargs.get('prior_kwargs').get('observer_gains').get('k3') Gamma1 = reshape_pt1([k1 * g, k2 * g ** 2]) Gamma2 = reshape_pt1([k3 * g ** 3]) if LS_deriv: mean_deriv = LS_deriv(reshape_pt1(xhat), reshape_pt1(u), kwargs.get('prior_kwargs')) else: mean_deriv = np.zeros_like(xhat) if np.any(kwargs.get('saturation')): # Saturate the derivative of the nonlinearity estimate to guarantee # contraction a_min = np.min([-kwargs.get('saturation'), kwargs.get('saturation')], axis=0) a_max = np.max([-kwargs.get('saturation'), kwargs.get('saturation')], axis=0) mean_deriv = np.clip(mean_deriv, a_min=a_min, a_max=a_max) A = reshape_pt1([[0, 1], [0, 0]]) B = reshape_pt1([[0], [1]]) ABmult = np.dot(A, reshape_pt1_tonormal(xhat)) + \ np.dot(B, reshape_pt1_tonormal(xi)) DfA = reshape_pt1(np.dot(reshape_pt1_tonormal(mean_deriv), reshape_pt1_tonormal(ABmult + u))) LC1 = reshape_pt1(Gamma1 * (y - xhat[:, 0])) LC2 = reshape_pt1(Gamma2 * (y - xhat[:, 0])) xhatdot = reshape_pt1(ABmult + LC1 + u) xidot = reshape_pt1(DfA + LC2) # Also check eigenvalues of M for stability without high gain AB = np.concatenate((A, B), axis=1) ABO = np.concatenate((AB, np.zeros_like(reshape_pt1(AB[0]))), axis=0) K = np.array([[k1, k2, k3]]) C = np.zeros_like(x) C[0, 0] = 1 M = ABO - np.dot(K.T, C) eigvals = np.linalg.eigvals(M) for x in eigvals: if np.linalg.norm(np.real(x)) < 1e-5: logging.warning('The eigenvalues of the matrix M are dangerously ' 'small, low robustness of the observer! Increase ' 'the gains.') elif np.real(x) > 0: logging.warning('Some of the eigenvalues of the matrix M are ' 'positive. Change the gains to get a Hurwitz ' 'matrix.') return np.concatenate((xhatdot, xidot), axis=1)
def true_dynamics(x, control): m1 = config.get('m1') m2 = config.get('m2') k1 = config.get('k1') k2 = config.get('k2') z = reshape_pt1(x) z3 = reshape_pt1(z[:, 2]) u = control v = reshape_pt1_tonormal(mass_spring_mass_v(z, config)) vdot = reshape_pt1_tonormal(mass_spring_mass_vdot(z, config)) return reshape_pt1(k1 * (m1 * m2) * (u - (m1 + m2) * z3) + (3 * k2) / (m1 * m2) * (u - (m1 + m2) * z3) * v**2 + (6 * k2) / m1 * v * vdot**2)
def MSM_continuous_Michelangelo_prior_mean_u(x, u, prior_kwargs): x = reshape_pt1(x) u = reshape_pt1(u) m1 = prior_kwargs.get('m1') m2 = prior_kwargs.get('m2') k1 = prior_kwargs.get('k1') k2 = prior_kwargs.get('k2') z = reshape_pt1(x) z3 = reshape_pt1(z[:, 2]) v = reshape_pt1_tonormal(mass_spring_mass_v(z, prior_kwargs)) vdot = reshape_pt1_tonormal(mass_spring_mass_vdot(z, prior_kwargs)) # phi = reshape_pt1( # k1 * (m1 * m2) * (u - (m1 + m2) * z3) + (3 * k2) / (m1 * m2) * ( # u - (m1 + m2) * z3) * v ** 2 + ( # 6 * k2) / m1 * v * vdot ** 2) phi = np.zeros((x.shape[0], 1)) return phi
def VanderPol_dynamics(t, x, u, t0, init_control, process_noise_var, kwargs): mu = kwargs.get('mu') x = reshape_pt1(x) u = reshape_pt1(u(t, kwargs, t0, init_control)) A = reshape_pt1([[0, 1], [-1, 0]]) F = reshape_pt1([0, mu * (1 - x[:, 0]**2) * x[:, 1] - x[:, 0]]) 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 harmonic_oscillator_dynamics(t, x, u, t0, init_control, process_noise_var, kwargs): k = kwargs.get('k') m = kwargs.get('m') x = reshape_pt1(x) u = reshape_pt1(u(t, kwargs, t0, init_control)) A = reshape_pt1([[0, 1], [-k / m, 0]]) xdot = reshape_pt1(np.dot(A, reshape_pt1_tonormal(x)) + u) if process_noise_var != 0: xdot += np.random.normal(0, np.sqrt(process_noise_var), xdot.shape) return xdot
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 VanderPol_observer_simplified(t, xhat, u, y, t0, init_control, GP, kwargs): mu = kwargs.get('mu') xhat = reshape_pt1(xhat) y = reshape_pt1(y(t, kwargs)) u = reshape_pt1(u(t, kwargs, t0, init_control)) # My gains l1 = mu - 5 # l2 = 1 - mu ** 2 - 2 * mu * xhat[:, 0] * xhat[:, 1] l2 = 1 - mu ** 2 - 2 * mu * xhat[:, 0] A = reshape_pt1([[0, 1], [-1, 0]]) F = reshape_pt1([0, mu * (1 - xhat[:, 0] ** 2) * xhat[:, 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 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 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 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 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 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 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: \ duffing_continuous_prior_mean(xl, ul, kwargs) xnext = 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) return reshape_pt1_tonormal(xnext)
def dynamics_traj_observer(x0, u, y, t0, dt, init_control, discrete=False, version=None, method='RK45', t_span=[0, 1], t_eval=[0.1], GP=None, **kwargs): if discrete: xtraj = np.zeros((len(t_eval), x0.shape[1])) xtraj[0] = reshape_pt1(x0) t = t0 i = 0 while (i < len(t_eval) - 1) and (t < t_eval[-1]): i += 1 xnext = reshape_pt1( version(t, xtraj[-1], u, y, t0, init_control, GP, **kwargs)) xtraj[i] = xnext t += dt else: sol = solve_ivp( lambda t, x: version(t, x, u, y, t0, init_control, GP, **kwargs), t_span=t_span, y0=reshape_pt1_tonormal(x0), method=method, t_eval=t_eval) xtraj = reshape_pt1(sol.y.T) return reshape_pt1(xtraj)
def predict(self, X, full_cov=False): means = reshape_pt1( np.array(self.models[0].predict(X, full_cov=full_cov)[0])) covars = reshape_pt1( np.array(self.models[0].predict(X, full_cov=full_cov)[1])) for i in range(1, self.nb_output_dims): means = np.concatenate( (means, reshape_pt1(self.models[i].predict(X, full_cov=full_cov)[0])), axis=1) covars = np.concatenate( (covars, reshape_pt1(self.models[i].predict(X, full_cov=full_cov)[1])), axis=1) assert means.shape[1] == self.nb_output_dims, \ 'Wrong shapes in multioutput GP predicted mean' assert covars.shape[1] == self.nb_output_dims, \ 'Wrong shapes in multioutput GP predicted covar' var = reshape_pt1(np.linalg.det(np.diag( reshape_pt1_tonormal(covars))))**(1 / self.nb_output_dims) return reshape_pt1(means), var
def dynamics_traj(x0, u, t0, dt, init_control, discrete=False, version=None, meas_noise_var=0, process_noise_var=0, method='RK45', t_span=[0, 1], t_eval=[0.1], solver_options={}, **kwargs): x0 = reshape_pt1(x0) if discrete: xtraj = np.zeros((len(t_eval), x0.shape[1])) xtraj[0] = reshape_pt1(x0) t = t0 i = 0 while (i < len(t_eval) - 1) and (t < t_eval[-1]): i += 1 xnext = reshape_pt1( version(t, xtraj[-1], u, t0, init_control, process_noise_var, **kwargs)) xtraj[i] = xnext t += dt else: sol = solve_ivp(lambda t, x: version(t, x, u, t0, init_control, process_noise_var, **kwargs), t_span=t_span, y0=reshape_pt1_tonormal(x0), method=method, t_eval=t_eval, **solver_options) xtraj = reshape_pt1(sol.y.T) if meas_noise_var != 0: xtraj += np.random.normal(0, np.sqrt(meas_noise_var), xtraj.shape) return reshape_pt1(xtraj)
def mass_spring_mass_ztox(z, kwargs): z = reshape_pt1(z) x = reshape_pt1(z) x[:, 2] = reshape_pt1_tonormal(mass_spring_mass_v(z, kwargs)) + z[:, 0] x[:, 3] = reshape_pt1_tonormal(mass_spring_mass_vdot(z, kwargs)) + z[:, 1] return reshape_pt1(x)