def fit_parameters(self, data_filename, fit_type="", dt=0.01):
"""
Use data to fit the model parameters of the velocity states (linvel, angvel)
"""
x_full, linvel, force, dt = self.load_data(data_filename, dt)
vdot_residual = derivative(linvel, dt)
vdot_residual -= self.subtract_nom_model(x_full, force)
X = x_full # Make X matrix
#Y = derivative(x_full, dt) # Make Y matrix
#Y[:,7:10] -= self.subtract_nom_model(x_full, force)
U = force # Make U matrix
print("Lifting data matrices...")
X_lift = self.create_observables(X) # Lift X
Y_lift = derivative(X_lift, dt) # Lift Y
self.n_lift = X_lift.shape[0]
print("Starting training...")
W = np.concatenate((Y_lift, X.transpose()), axis=0) # Create W matrix
V = np.concatenate((X_lift, U.transpose()), axis=0) # Create V matrix
M = np.dot(np.dot(W, V.transpose()),
np.linalg.pinv(np.dot(
V, V.transpose()))) # Do regression to find M
self.A = M[:self.n_lift, :self.n_lift] # Split M into A, B, C
self.B = M[:self.n_lift, self.n_lift:] # Split M into A, B, C
self.C = M[self.n_lift:, :self.n_lift] # Split M into A, B, C
print("Finished training...")
def fit_parameters(self, data_filename, fit_type, dt=0.01):
"""
Use data to fit the model parameters of the velocity states (linvel, angvel)
"""
# Load data
self.data_path = data_filename
self.fit_type = fit_type
data_format = os.path.splitext(data_filename)[1]
if (data_format == '.bag'):
time, position, orientation, linvel, angvel, force = self.read_ROSBAG(
data_filename, dt=dt, is_simulation=self.is_simulation)
elif (data_format == '.csv'):
pass
else:
exit("Data format should be 'rosbag' or 'csv'")
x_full = np.concatenate((position, orientation, linvel, angvel),
axis=1)
theta_xdt, theta_ydt, theta_zdt = self.create_observables(
x_full, force)
# Identify mass and hover throttle using z-data only:
theta_v = theta_zdt.reshape(-1, 1)
x_learn = linvel[:, 2].reshape(-1, 1)
dt = time[1] - time[0]
vdot = derivative(x_learn, dt).reshape(-1, 1)
self.estimator_pdt = LinearRegression(fit_intercept=False,
normalize=False)
self.estimator_pdt.fit(theta_v, vdot)
def Lotka_Volterra(x0, r, a, time):
def dynamical_system(y,t):
dy = np.zeros_like(y)
for i in range(4):
dy[i] = r[i]*y[i]*(1-a[i][0]*y[0]-a[i][1]*y[1]-a[i][2]*y[2]-a[i][3]*y[3])
return dy
x = odeint(dynamical_system,x0,time,mxstep=5000000)
dt = time[1]-time[0]
xdot = derivative(x,dt)
return x, xdot
def fit_parameters(self, data_filename, fit_type, is_simulation, dt=0.01):
"""
Use data to fit the model parameters of the velocity states (linvel, angvel)
"""
# Load data
self.data_path = data_filename
self.fit_type = fit_type
data_format = os.path.splitext(data_filename)[1]
if (data_format == '.bag'):
time, position, orientation, linvel, angvel, rcout = self.read_ROSBAG(
data_filename, dt=dt, is_simulation=is_simulation)
elif (data_format == '.csv'):
pass
else:
exit("Data format should be 'rosbag' or 'csv'")
x_full = np.concatenate((position, orientation, linvel, angvel),
axis=1)
x_learn = np.concatenate((linvel, angvel), axis=1)
u = self.mix_control_inputs(rcout)
dt = time[1] - time[0]
xdot_learn = derivative(x_learn, dt)
theta_xdt, theta_ydt, theta_zdt, theta_omg_x, theta_omg_y, theta_omg_z = self.create_observables(
x_full, u)
#Concatenate theta and xdot for position states to learn single coefficient for pos states
theta_p = np.concatenate((theta_xdt, theta_ydt, theta_zdt),
axis=0).reshape(-1, 1)
pdot = np.concatenate(
(xdot_learn[:, 0], xdot_learn[:, 1], xdot_learn[:, 2]),
axis=0).reshape(-1, 1)
self.estimator_pdt = sp.sindy(l2=0.0, solver='lstsq')
self.estimator_pdt.fit(theta_p, pdot)
self.estimator_omg_x = sp.sindy(l2=0.0, solver='lstsq')
self.estimator_omg_x.fit(theta_omg_x,
xdot_learn[:, 3].reshape(x_learn.shape[0], 1))
self.estimator_omg_y = sp.sindy(l2=0.0, solver='lstsq')
self.estimator_omg_y.fit(theta_omg_y,
xdot_learn[:, 4].reshape(x_learn.shape[0], 1))
self.estimator_omg_z = sp.sindy(l2=0.0, solver='lstsq')
self.estimator_omg_z.fit(theta_omg_z,
xdot_learn[:, 5].reshape(x_learn.shape[0], 1))
def Lorenz(x0, sigma, rho, beta, time):
"""
This small function runs a simulation of the Lorenz system.
Inputs
------
x0 : numpy array containing the initial condition.
sigma, rho, beta : parameters of the Lorenz system.
time : numpy array for the evaluation of the state of
the Lorenz system at some given time instants.
Outputs
-------
x : numpy two-dimensional array.
State vector of the vector for the time instants
specified in time.
xdot : corresponding derivatives evaluated using
central differences.
"""
def dynamical_system(y,t):
dy = np.zeros_like(y)
dy[0] = sigma*(y[1]-y[0])
dy[1] = y[0]*(rho - y[2]) - y[1]
dy[2] = y[0]*y[1] - beta*y[2]
return dy
x = odeint(dynamical_system,x0,time)
dt = time[1]-time[0]
from sparse_identification.utils import derivative
xdot = derivative(x,dt)
return x, xdot
def Lorenz(x0, sigma, rho, beta, time):
"""
This small function runs a simulation of the Lorenz system.
Inputs
------
x0 : numpy array containing the initial condition.
sigma, rho, beta : parameters of the Lorenz system.
time : numpy array for the evaluation of the state of
the Lorenz system at some given time instants.
Outputs
-------
x : numpy two-dimensional array.
State vector of the vector for the time instants
specified in time.
xdot : corresponding derivatives evaluated using
central differences.
"""
def dynamical_system(y, t):
dy = np.zeros_like(y)
dy[0] = sigma * (y[1] - y[0])
dy[1] = y[0] * (rho - y[2]) - y[1]
dy[2] = y[0] * y[1] - beta * y[2]
return dy
x = odeint(dynamical_system, x0, time)
dt = time[1] - time[0]
from sparse_identification.utils import derivative
xdot = derivative(x, dt)
return x, xdot
def Lotka_Volterra(x0, alpha, beta, time):
"""
This small function runs a simulation of the Lotka-Volterra system.
Inputs
------
x0 : numpy array containing the initial condition.
alpha, beta: Parameters of Lotka-Volterra system.
alpha is 1-dimensional, and beta is
a matrix.
time : numpy array for the evaluation of the state of
the Lotka-Volterra system at some given time instants.
Outputs
-------
x : numpy two-dimensional array.
State vector of the vector for the time instants
specified in time.
xdot : corresponding derivatives evaluated using
central differences.
"""
def dynamical_system(y, t):
dy = (alpha + beta @ y) * y
return dy
x = odeint(dynamical_system, x0, time)
dt = time[1] - time[0]
from sparse_identification.utils import derivative
xdot = derivative(x, dt)
return x, xdot
def fit_parameters(self, data_filename, fit_type, dt=0.01):
"""
Use data to fit the model parameters of the velocity states (linvel, angvel)
"""
# Load data
self.data_path = data_filename
self.fit_type = fit_type
data_format = os.path.splitext(data_filename)[1]
if (data_format=='.bag'):
time, position, orientation, linvel, angvel, force = self.read_ROSBAG(data_filename, dt=dt,
is_simulation=self.is_simulation)
elif (data_format=='.csv'):
pass
else:
exit("Data format should be 'rosbag' or 'csv'")
x_full = np.concatenate((position, orientation, linvel, angvel), axis=1)
dt = time[1] - time[0]
vdot_residual = derivative(linvel,dt)
vdot_residual -= self.subtract_nom_model(x_full, force)
#pdot_residual = self.normalize_x(derivative(pdot_residual,dt))
#pdot_residual = derivative(pdot_residual, dt)
self.poly_lib = PolynomialFeatures(degree=2, include_bias=True) #TODO: Set include_bias to True if cvx regressor is used
Theta = self.create_observables(x_full, force)
Theta = self.normalize_theta(Theta, prediction=False)
self.estimator = sp.sindy(l1=0.98, solver='lasso')
#self.estimator = Lasso(alpha = 1e-4, fit_intercept=True, normalize=True, max_iter=10000)
print("Start training...")
self.estimator.fit(Theta, vdot_residual)
print("Finish training")
#self.estimator.coef_ = np.divide(self.estimator.coef_, self.x_var)
print("Sparsity fraction (1 = no sparsity, 0 = completely sparse): ", float(np.count_nonzero(self.estimator.coef_))/float(self.estimator.coef_.size))
def fit_parameters(self, data_filename, fit_type, is_simulation, dt=0.01):
"""
Use data to fit the model parameters of the velocity states (linvel, angvel)
"""
# Load data
self.data_path = data_filename
self.fit_type = fit_type
data_format = os.path.splitext(data_filename)[1]
if (data_format == '.bag'):
time, position, orientation, linvel, angvel, rcout = self.read_ROSBAG(
data_filename, dt=dt, is_simulation=is_simulation)
elif (data_format == '.csv'):
pass
else:
exit("Data format should be 'rosbag' or 'csv'")
u = self.mix_control_inputs(rcout)
x_full = np.concatenate((position, orientation, linvel, angvel),
axis=1)
x_full = self.subtract_nom_model(x_full, u)
x_learn = x_full[:, 6:]
dt = time[1] - time[0]
xdot_learn = self.normalize_x(derivative(x_learn, dt))
self.poly_lib = PolynomialFeatures(degree=2, include_bias=True)
Theta = self.create_observables(x_full, u)
Theta = self.normalize_theta(Theta, prediction=False)
self.estimator = sp.sindy(l1=1.0, solver='lasso')
self.estimator.fit(Theta, xdot_learn)
self.estimator.coef_ = np.divide(self.estimator.coef_, self.x_var)
print(
"Sparsity fraction: ",
float(np.count_nonzero(self.estimator.coef_)) /
float(self.estimator.coef_.size))