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)
