def __init__(self, settings, ship, ship_model, coef_, acc_lim):
pygame.init()
self.screen_size = (1000, 1000) #width height
self.screen = pygame.display.set_mode(self.screen_size)
self.bg_color = (140, 153, 173)
pygame.display.set_caption('tugboat control')
self.scale_image = (114, 260)
self.pixel_meter_ratio = 3.0
self.spawn_location = (500, 500)
self.tugboat_img = pygame.image.load('tugboat.png')
self.tugboat_img = pygame.transform.scale(self.tugboat_img,
self.scale_image)
self.rpm_const = 0.0
self.az_angle = 0.
print(self.tugboat_img.get_rect())
self.t_reg = time.time()
self.x = 0.0
self.y = 0.0
self.heading = 10.0
self.u = 0.0
self.v = 0.0
self.r = 0.0
self.u_dot = 0.0
self.v_dot = 0.0
self.r_dot = 0.0
self.coef_ = coef_
self.ship = ship()
self.acc_lim = acc_lim
self.ship_model = ship_model(0, 0, 0, self.ship, self.coef_,
self.acc_lim)
def traj_error(x):
ship_it = ship()
df = df_main
coef__ = np.array(
[np.asarray(x[:10]),
np.asarray(x[10:25]),
np.asarray(x[25:40])])
# print(coef__)
u, v, r, hdg = df.loc[df.index[0], 'u'], df.loc[
df.index[0], 'v'], df.loc[df.index[0], 'r'], df.loc[df.index[0],
'hdg']
ship_it_model = ship_model(df.loc[df.index[0], 'u_dot'],
df.loc[df.index[0], 'v_dot'],
df.loc[df.index[0],
'r_dot'], ship_it, coef__)
df_sim = pd.DataFrame([])
for i in df[:-1].index:
if i % traj_len == 0:
u, v, r, hdg = df.loc[i, 'u'], df.loc[i, 'v'], df.loc[
i, 'r'], df.loc[i, 'hdg']
u, v, r, hdg, delta_x_0, delta_y_0, delta_r_0, u_dot, v_dot, r_dot = ship_it_model.manoeuvre_model_rt_evolution(
u, v, r, hdg, df.loc[i, 'rpm_0'] / 600.,
df.loc[i, 'rpm_1'] / 600., df.loc[i, 'rpm_2'] / 600.,
df.loc[i, 'rsa_0'], df.loc[i, 'rsa_1'], df.loc[i, 'rsa_2'],
df.loc[i, 'delta_time'])
# print(u, v, r, hdg)
df_temp = pd.DataFrame({
'index_sim': [i + 1],
'x_delta_sim': [delta_x_0],
'y_delta_sim': [delta_y_0],
'hdg_delta_sim': [delta_r_0]
})
df_sim = pd.concat([df_sim, df_temp], axis=0)
df = pd.merge(df,
df_sim,
how='left',
left_on=df.index,
right_on='index_sim')
df['x_real_sim'] = df.x_delta_sim.cumsum()
df['y_real_sim'] = df.y_delta_sim.cumsum()
df['psi_sim'] = df.hdg_delta_sim.cumsum()
df['x_real'] = df.x.cumsum()
df['y_real'] = df.y.cumsum()
df['x_diff'] = abs(df.x_delta_sim - df.x)
df['y_diff'] = abs(df.y_delta_sim - df.y)
df['error'] = np.sqrt(df.x_diff**2 + df.y_diff**2)
plt.plot(df.x_real.tolist()[:], df.y_real.tolist()[:])
plt.plot(df.x_real_sim.tolist()[:], df.y_real_sim.tolist()[:])
# plt.plot(df.traj_error)
plt.show()
# print(df)
feature = df.error.cumsum().iloc[-1]
# print(feature)
# print(feature)
del ship_it_model
if feature == np.inf or math.isnan(feature):
feature = 10000000000000000
print(feature)
del df
return feature
def traj_error(x):
ship_it = ship()
ship_it.D_p = x[0]
df_main = pd.read_csv('test_sunday_evening.csv', sep=',')
ship_it.I_e = x[1]*ship_it.I_e
print(x[0])
# ship_it.Mass = ship_it.Mass*x[1]
df = df_main[20:800]
df.rpm = df.rpm/60.0*x[2]
df = df.dropna()
# df.rsa = df.rsa*x[2]
# df['beta'] = np.degrees(np.arctan((1-ship_it.w)/(0.7*np.pi*df.rpm*ship_it.D_p)))
df['beta'] = np.rad2deg(np.arctan((df.u)/(0.7*np.pi*df.rpm*ship_it.D_p)))
# df['beta'] = df.apply(lambda row: row['beta'] + 180 if row['u']>=0 and row['u']<0400 else (row['beta'] + 180 if row['u']<0 and row['rpm']<0 else (row['beta'] + 360 if row['u'] <0 and row['rpm']>=0 else row['beta'])) ,axis=1)
df['beta'] = df.beta.apply(lambda x: x-360 if x>360.0 else (x+360 if x<0.0 else x))
df['f_p_40'] = (1-ship_it.t)*ship_it.beta_coef(df.beta)*0.5*ship_it.rho*(((((1-ship_it.w)*df.u)**2)+ (0.7*np.pi*df.rpm*ship_it.D_p)**2))*np.pi/4*ship_it.D_p**2
# df['f_p_40'] = df.apply(lambda row: 0 if row['rpm']<5 and row['rpm']>-5 else row['f_p_40'], axis=1 )
df['u_dot_spec'] = df.u_dot.shift(1)
df = df[2:]
u = df.u.to_numpy()[:, newaxis]
v = df.v.to_numpy()[:, newaxis]
r = df.r.to_numpy()[:, newaxis]
u_dot = df.u_dot.to_numpy()[:,newaxis]
v_dot = df.v_dot.to_numpy()[:,newaxis]
r_dot = df.r_dot.to_numpy()[:,newaxis]
rsa = df.rsa.to_numpy()[:, newaxis]
f_p_40 = df.f_p_40.to_numpy()[:,newaxis]
u_dot_spec = df.u_dot_spec.to_numpy()[:, newaxis]
X = np.concatenate([u_dot_spec, u*u, u*u*u, u*v, u*r, v*v, r*r, v*r, u*v*v, r*v*u, u*r*r], axis=1)
Y = np.concatenate([v_dot,v, u*v, u*r, u*u*r, u*u*v, v*v*v, r*r*r, r*r*v, v*v*r, abs(v)*v, abs(r)*v, r*abs(v), abs(r)*r], axis=1)
N = np.concatenate([r_dot,r, u*v, u*r, u*u*r, u*u*v, v*v*v, r*r*r, r*r*v, v*v*r, abs(v)*v, abs(r)*v, r*abs(v), abs(r)*r], axis=1)
F_r = -21.1* ship_it.A_r*u*u*rsa
y_x = ship_it.Mass*(u_dot_spec-r*v)-1.5*f_p_40 - F_r*np.sin(np.deg2rad(rsa))
y_y = ship_it.Mass*(v_dot+r*u)-F_r*np.cos(np.radians(rsa))
y_r = ship_it.I_e*r_dot - F_r*ship_it.x_r*np.cos(np.radians(rsa))
model = Ridge(fit_intercept=False)
cv = RepeatedKFold(n_splits=10, n_repeats=3, random_state=1)
grid = dict()
grid['alpha'] = np.arange(0, 0.0011, 0.0001)
search = GridSearchCV(model, grid, scoring='neg_mean_absolute_error', cv=cv, n_jobs=-1)
results_x = search.fit(X, y_x)
model = Ridge(fit_intercept=False)
cv = RepeatedKFold(n_splits=10, n_repeats=3, random_state=1)
grid = dict()
grid['alpha'] = np.arange(0, 0.0011, 0.0001)
search = GridSearchCV(model, grid, scoring='neg_mean_squared_error', cv=cv, n_jobs=-1)
results_y = search.fit(Y, y_y)
model = Ridge(fit_intercept=False)
cv = RepeatedKFold(n_splits=10, n_repeats=3, random_state=1)
grid = dict()
grid['alpha'] = np.arange(0, 0.0011, 0.0001)
search = GridSearchCV(model, grid, scoring='neg_mean_absolute_error', cv=cv, n_jobs=-1)
results_r = search.fit(N, y_r)
print(results_x.best_estimator_.score(X,y_x), results_x.best_estimator_.alpha)
print(results_y.best_estimator_.score(Y,y_y), results_y.best_estimator_.alpha)
print(results_r.best_estimator_.score(N,y_r), results_r.best_estimator_.alpha)
a_list = [list(results_x.best_estimator_.coef_[0]),list(results_y.best_estimator_.coef_[0]),list(results_r.best_estimator_.coef_[0])]
row_lengths = []
for row in a_list:
row_lengths.append(len(row))
max_length = max(row_lengths)
for row in a_list:
while len(row) < max_length:
row.append(None)
balanced_array = np.array([np.asarray(a_list[0]),np.asarray(a_list[1]),np.asarray(a_list[2])])
df = df_main[30:700]
u, v, r, hdg = df.loc[df.index[0], 'u'],df.loc[df.index[0], 'v'], df.loc[df.index[0], 'r'], df.loc[df.index[0], 'hdg']
ship_it_model = ship_model(df.loc[df.index[0], 'u_dot'],df.loc[df.index[0], 'v_dot'], df.loc[df.index[0], 'r_dot'], ship_it, balanced_array)
df_input = df[['rpm', 'rsa']]
# print(u, v, r, hdg)
df_sim = pd.DataFrame([])
for i in df[:-1].index:
u, v, r, hdg, delta_x_0, delta_y_0, delta_r_0, u_dot, v_dot, r_dot = ship_it_model.manoeuvre_model_rpa_3(u, v, r, hdg,
df.loc[i, 'rpm'],
df.loc[i, 'rsa'],
df.loc[i, 'delta_time'],
)
df_temp = pd.DataFrame({
'index_sim' : [i+1],
'x_delta_sim': [delta_x_0],
'y_delta_sim': [delta_y_0],
'hdg_delta_sim': [delta_r_0]
})
df_sim = pd.concat([df_sim, df_temp], axis=0)
df = pd.merge(df, df_sim, how='left', left_on=df.index, right_on='index_sim')
df['x_real_sim'] = df.x_delta_sim.cumsum()
df['y_real_sim'] = df.y_delta_sim.cumsum()
df['x_real'] = df.x.cumsum()
df['y_real'] = df.y.cumsum()
# calculate rho
df['x_sim_diff_avg'] = df['x_real_sim'] - df.x_delta_sim.mean()
df['y_sim_diff_avg'] = df['y_real_sim'] - df.y_delta_sim.mean()
df['x_real_diff_avg'] = df['x_real'] - df.x.mean()
df['y_real_diff_avg'] = df['y_real'] - df.y.mean()
rho_x = (df['x_sim_diff_avg']*df['x_real_diff_avg']).sum()/np.sqrt((df['x_sim_diff_avg']**2).sum()*(df['x_real_diff_avg']**2).sum() )
rho_y = (df['y_sim_diff_avg']*df['y_real_diff_avg']).sum()/np.sqrt((df['y_sim_diff_avg']**2).sum()*(df['y_real_diff_avg']**2).sum() )
df['traj_error'] = (np.sqrt((df['x_real_sim'] - df['x_real'])**2 + (df['y_real_sim'] - df['y_real'])**2)).cumsum()
# plt.plot(df.traj_error)
plt.plot(df.x_real.tolist()[:],df.y_real.tolist()[:])
plt.plot(df.x_real_sim.tolist()[:],df.y_real_sim.tolist()[:])
plt.show()
# value = df.loc[df.index[-1], 'traj_error']
# if np.isnan(value):
# value = 0.00001
print(rho_x*rho_y)
del df
return abs(rho_x*rho_y)
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Thu Oct 29 16:32:59 2020
@author: erwinlodder
"""
import numpy as np
import time
from manoeuvre_model_evo import ship_model
from ship_class import ship
from pid_small import PID
import matplotlib.pyplot as plt
ship = ship()
coef_ = np.genfromtxt('foo_evo_general.csv', delimiter=',')
# P_range = np.arange(0,10.2,0.2)
# I_range = np.arange(0,10.2,0.2)
# D_range = np.arange(0,10.2,0.2)
#initialise ship
u, v, r, hdg = 0, 0, 0, 0
ship_model = ship_model(0, 0, 0, ship, coef_)
max_rpm_second = 1.2
rpm = 0
class MA_filter:
def __init__(self, periods):
self.filter_array = np.zeros(periods)
def PID_tuner(x):
#initialise ship
u, v, r, hdg = 0, 0, 0, 0
ship_ = ship()
ship_model_ = ship_model(0, 0, 0, ship_, coef_)
max_rpm_second = 0.25
rpm = 0
# (distance, time)
speed_u = [(0, 0), (10, 40)]
end_input = 0
speed_u_position = 0
current_speed_setting = 0
t = 0
dt = 0.4 #future noise
t_end = 400
pid_position = PID(P=x[0], I=x[1], D=x[2], Ibounds_speed=(-90, 90))
#
u_ref = []
u_real = []
x = 0
while t < t_end:
# calculate speed input
if end_input == 0:
if speed_u[speed_u_position][1] <= t:
current_speed_setting = speed_u[speed_u_position][0]
pid_position.setPoint_speed(current_speed_setting)
speed_u_position += 1
if speed_u_position == len(speed_u):
end_input = 1
control_input = pid_position.update(x)
sign_control_input = np.sign(control_input)
# calculate speed control iput
if abs(abs(control_input) - abs(rpm)) / dt > max_rpm_second:
rpm = dt * sign_control_input * max_rpm_second + rpm
if abs(rsa_1) > abs(pid_position.Integrator_max):
rpm = np.sign(rpm) * abs(pid_position.Integrator_max)
# print(rpm)
# model
u, v, r, hdg, delta_x_0, delta_y_0, delta_r_0, u_dot, v_dot, r_dot = ship_model.manoeuvre_model_rt_evolution(
u,
v,
r,
hdg,
0,
rpm_1,
rpm_2,
180,
rsa_1,
rsa_2,
dt,
# u_dot_arr, v_dot_arr, r_rot_arr
)
x = x + delta_y_0
u_ref.append(current_speed_setting)
u_real.append(u)
t = t + dt
u_ref_array = np.asarray(u_ref)
u_real_array = np.asarray(u_real)
u_score = abs(u_ref_array - u_real_array)
u_score = u_score.sum()
print(u_score)
return u_score
def PID_tuner(x):
#initialise ship
u, v, r, hdg = 0, 0, 0, 0
ship_ = ship()
ship_model_ = ship_model(0, 0, 0, ship_, coef_)
max_rpm_second = 0.25
rpm = 0
# (speed, time)
speed_u = [(0, 0), (5, 20), (9, 140), (2, 250)]
end_input = 0
speed_u_position = 0
current_speed_setting = 0
t = 0
dt = 0.4 #future noise
t_end = 400
pid_speed = PID(P=x[0], I=x[1], D=x[2])
#
u_ref = []
u_real = []
while t < t_end:
# calculate speed input
if end_input == 0:
if speed_u[speed_u_position][1] <= t:
current_speed_setting = speed_u[speed_u_position][0]
pid_speed.setPoint_speed(current_speed_setting)
speed_u_position += 1
if speed_u_position == len(speed_u):
end_input = 1
control_input = pid_speed.update(u)
sign_control_input = np.sign(control_input)
# calculate speed control iput
if abs(abs(control_input) - abs(rpm)) / dt > max_rpm_second:
rpm = dt * sign_control_input * max_rpm_second + rpm
if abs(rpm) > abs(pid_speed.Integrator_max):
rpm = np.sign(rpm) * abs(pid_speed.Integrator_max)
# print(rpm)
# model
u, v, r, hdg, delta_x_0, delta_y_0, delta_r_0, u_dot, v_dot, r_dot = ship_model_.manoeuvre_model_rpa_3(
u,
v,
r,
hdg,
rpm,
0, #rudder
dt,
)
u_ref.append(current_speed_setting)
u_real.append(u)
t = t + dt
u_ref_array = np.asarray(u_ref)
u_real_array = np.asarray(u_real)
u_score = abs(u_ref_array - u_real_array)
u_score = u_score.sum()
print(u_score)
return u_score
def traj_error(x):
ship_it = ship()
ship_it.man_eng = ship_it.man_eng * x[0]
ship_it.x_g = ship_it.x_g * x[1]
ship_it.y_1, ship_it.y_2 = ship_it.y_1 * x[2], ship_it.y_2 * x[2]
ship_it.I_e = ship_it.I_e * x[3]
ship_it.Mass = ship_it.Mass * x[4]
print(x)
df = pd.read_csv('test_1_large_turbopolyp.csv', sep=',')[2200:]
df = df.dropna()
# df = df_main[1:]
df.rpm_0 = df.rpm_0 / 600.0
df.rpm_1 = df.rpm_1 / 600.0
df.rpm_2 = df.rpm_2 / 600.0
df['u_a_2'] = (1 - ship_it.w) * (
(df.u + df.r * abs(ship_it.y_2)) * -1 * np.cos(np.deg2rad(df.rsa_2)) +
(df.v + df.r * abs(ship_it.x_2)) * -1 * np.sin(np.deg2rad(df.rsa_2))
) #(1-ship.w)*
df['u_a_1'] = (1 - ship_it.w) * (
(df.u - df.r * abs(ship_it.y_1)) * -1 * np.cos(np.deg2rad(df.rsa_1)) +
(-df.v - df.r * abs(ship_it.x_1)) * -1 * np.sin(np.deg2rad(df.rsa_1))
) #(1-ship.w)*
df['u_a_0'] = (1 - ship_it.w) * (
(df.u) * -1 * np.cos(np.deg2rad(df.rsa_0)) +
((-df.v + df.r * abs(ship_it.x_0)) * -1 * np.sin(np.deg2rad(df.rsa_0)))
) #(1-ship.w)*
df['beta_2'] = np.rad2deg(
np.arctan((df.u_a_2) / (0.7 * np.pi * df.rpm_2 * ship_it.D_p)))
df['beta_2'] = df.beta_2.apply(lambda x: x + 360 if x < 0 else x)
df['beta_1'] = np.rad2deg(
np.arctan((df.u_a_1) / (0.7 * np.pi * df.rpm_1 * ship_it.D_p)))
df['beta_1'] = df.beta_1.apply(lambda x: x + 360 if x < 0 else x)
df['beta_0'] = np.rad2deg(
np.arctan((df.u_a_0) / (0.7 * np.pi * df.rpm_0 * ship_it.D_p)))
df['beta_0'] = df.beta_0.apply(lambda x: x + 360 if x < 0 else x)
df['f_p_40_2'] = 1. * (
(1 - ship_it.t) * ship_it.beta_coef(df.beta_2) * 0.5 * ship_it.rho *
(((((1 - ship_it.w) * df.u_a_2)**2) +
(0.7 * np.pi * df.rpm_2 * ship_it.D_p)**2)) * np.pi / 4 *
ship_it.D_p**2) #(1-df['t_21_phi'])*(1-df['t_20_phi'])*
df['f_p_40_1'] = 1. * (
(1 - ship_it.t) * ship_it.beta_coef(df.beta_1) * 0.5 * ship_it.rho *
(((((1 - ship_it.w) * df.u_a_1)**2) +
(0.7 * np.pi * df.rpm_1 * ship_it.D_p)**2)) * np.pi / 4 *
ship_it.D_p**2) #(1-df['t_12_phi'])*(1-df['t_10_phi'])*
df['f_p_40_0'] = 1. * (
(1 - ship_it.t) * ship_it.beta_coef(df.beta_0) * 0.5 * ship_it.rho *
(((((1 - ship_it.w) * df.u_a_0)**2) +
(0.7 * np.pi * df.rpm_0 * ship_it.D_p)**2)) * np.pi / 4 * ship_it.D_p
**2) #.rolling(20).mean() #(1-df['t_02_phi'])*(1-df['t_01_phi'])*
df = df[20:-7]
u = df.u.to_numpy()[:, newaxis]
v = df.v.to_numpy()[:, newaxis]
r = df.r.to_numpy()[:, newaxis]
u_a_2 = df.u_a_2.to_numpy()[:, newaxis]
u_a_1 = df.u_a_1.to_numpy()[:, newaxis]
u_a_0 = df.u_a_0.to_numpy()[:, newaxis]
u_dot = df.u_dot.to_numpy()[:, newaxis]
v_dot = df.v_dot.to_numpy()[:, newaxis]
r_dot = df.r_dot.to_numpy()[:, newaxis]
rsa_0 = df.rsa_0.to_numpy()[:, newaxis]
rsa_1 = df.rsa_1.to_numpy()[:, newaxis]
rsa_2 = df.rsa_2.to_numpy()[:, newaxis]
f_p_40_0 = df.f_p_40_0.to_numpy()[:, newaxis]
f_p_40_2 = df.f_p_40_2.to_numpy()[:, newaxis]
f_p_40_1 = df.f_p_40_1.to_numpy()[:, newaxis]
X = np.concatenate([
u_dot,
u,
u * u,
u * u * u,
v * v,
r * r,
v * r,
u * v * v,
r * v * u,
u * r * r,
],
axis=1)
Y = np.concatenate([
v_dot, v, v * v, u * v, u * r, u * u * r, u * u * v, v * v * v,
r * r * r, r * r * v, v * v * r,
abs(v) * v,
abs(r) * v, r * abs(v),
abs(r) * r
],
axis=1) #, abs(v)*v, abs(r)*v, r*abs(v), abs(r)*r
N = np.concatenate([
r_dot,
r,
r * r,
v * r,
u * r,
u * u * r,
u * u * v,
v * v * v,
r * r * r,
r * r * v,
v * v * r,
abs(v) * v,
abs(r) * v,
r * abs(v),
abs(r) * r,
],
axis=1)
y_x = ship_it.Mass * (u_dot - r * v) - 1.0 * (
-np.cos(np.deg2rad(rsa_0)) * (f_p_40_0) - np.cos(np.deg2rad(rsa_1)) *
(f_p_40_1) - np.cos(np.deg2rad(rsa_2)) * (f_p_40_2))
y_y = ship_it.Mass * (v_dot + r * u) - 1.0 * (
-np.sin(np.deg2rad(rsa_0)) * (f_p_40_0) - np.sin(np.deg2rad(rsa_1)) *
(f_p_40_1) - np.sin(np.deg2rad(rsa_2)) *
(f_p_40_2)) #np.sin(rsa_0)*abs(f_p_40_0)+np.sin(rsa_1)*abs(f_p_40_1)+
y_r = ship_it.I_e * r_dot - 1 * (
ship_it.x_0 * -1 * np.sin(np.deg2rad(rsa_0)) *
(f_p_40_0) + ship_it.x_2 * -1 * np.sin(np.deg2rad(rsa_2)) *
(f_p_40_2) + ship_it.x_1 * -1 * np.sin(np.deg2rad(rsa_1)) *
(f_p_40_1) - ship_it.y_2 * -1 * np.cos(np.deg2rad(rsa_2)) *
(f_p_40_2) - ship_it.y_1 * -1 * np.cos(np.deg2rad(rsa_1)) * (f_p_40_1))
model = Ridge(fit_intercept=False)
cv = RepeatedKFold(n_splits=5, n_repeats=10, random_state=25)
grid = dict()
grid['alpha'] = np.logspace(1.0, -4.0, num=10)
search = GridSearchCV(model,
grid,
scoring='neg_mean_squared_error',
cv=cv,
n_jobs=-1)
results_x = search.fit(X, y_x)
one_mse_value = search.cv_results_['mean_test_score'][
-1] - search.cv_results_['mean_test_score'].std()
a = [
i for i in range(len(search.cv_results_['mean_test_score']))
if search.cv_results_['mean_test_score'][i] > one_mse_value
]
# search.param_grid['alpha'][0][int(a[0])]
clf_x = Ridge(alpha=search.param_grid['alpha'][int(a[0])])
clf_x.fit(X, y_x)
# clf.score(X, y_x)
model = Ridge(fit_intercept=False)
cv = RepeatedKFold(n_splits=5, n_repeats=10, random_state=25)
grid = dict()
grid['alpha'] = np.logspace(5.0, -4.0, num=200)
search = GridSearchCV(model,
grid,
scoring='neg_mean_squared_error',
cv=cv,
n_jobs=-1)
results_x = search.fit(Y, y_y)
one_mse_value = search.cv_results_['mean_test_score'][
-1] - search.cv_results_['mean_test_score'].std()
a = [
i for i in range(len(search.cv_results_['mean_test_score']))
if search.cv_results_['mean_test_score'][i] > one_mse_value
]
# search.param_grid['alpha'][0][int(a[0])]
clf_y = Ridge(alpha=search.param_grid['alpha'][int(a[0])])
clf_y.fit(Y, y_y)
model = Ridge(fit_intercept=False)
cv = RepeatedKFold(n_splits=5, n_repeats=10, random_state=25)
grid = dict()
grid['alpha'] = np.logspace(1.0, -4.0, num=10)
search = GridSearchCV(model,
grid,
scoring='neg_mean_squared_error',
cv=cv,
n_jobs=-1)
results_x = search.fit(N, y_r)
one_mse_value = search.cv_results_['mean_test_score'][
-1] - search.cv_results_['mean_test_score'].std()
a = [
i for i in range(len(search.cv_results_['mean_test_score']))
if search.cv_results_['mean_test_score'][i] > one_mse_value
]
# search.param_grid['alpha'][0][int(a[0])]
clf_n = Ridge(alpha=search.param_grid['alpha'][int(a[0])])
clf_n.fit(N, y_r)
coef__ = np.array([
np.asarray(clf_x.coef_[0]),
np.asarray(clf_y.coef_[0]),
np.asarray(clf_n.coef_[0])
])
# print(coef__)
u, v, r, hdg = df.loc[df.index[0],
'u'], df.loc[df.index[0],
'v'], df.loc[df.index[0],
'r'], df.loc[df.index[0],
'hdg']
ship_it_model = ship_model(df.loc[df.index[0],
'u_dot'], df.loc[df.index[0], 'v_dot'],
df.loc[df.index[0], 'r_dot'], ship_it, coef__)
df_sim = pd.DataFrame([])
for i in df[:-1].index:
if i % 50 == 0:
u, v, r, hdg = df.loc[i,
'u'], df.loc[i,
'v'], df.loc[i,
'r'], df.loc[i,
'hdg']
u, v, r, hdg, delta_x_0, delta_y_0, delta_r_0, u_dot, v_dot, r_dot = ship_it_model.manoeuvre_model_borkum(
u, v, r, hdg, df.loc[i, 'rpm_0'], df.loc[i, 'rpm_1'],
df.loc[i, 'rpm_2'], df.loc[i, 'rsa_0'], df.loc[i, 'rsa_1'],
df.loc[i, 'rsa_2'], df.loc[i, 'delta_time'])
# print(u, v, r, hdg)
df_temp = pd.DataFrame({
'index_sim': [i + 1],
'x_delta_sim': [delta_x_0],
'y_delta_sim': [delta_y_0],
'hdg_delta_sim': [delta_r_0]
})
df_sim = pd.concat([df_sim, df_temp], axis=0)
df = pd.merge(df,
df_sim,
how='left',
left_on=df.index,
right_on='index_sim')
df['x_real_sim'] = df.x_delta_sim.cumsum()
df['y_real_sim'] = df.y_delta_sim.cumsum()
df['psi_sim'] = df.hdg_delta_sim.cumsum()
df['x_real'] = df.x.cumsum()
df['y_real'] = df.y.cumsum()
df['x_diff'] = abs(df.x_delta_sim - df.x)
df['y_diff'] = abs(df.y_delta_sim - df.y)
df['error'] = np.sqrt(df.x_diff**2 + df.y_diff**2)
plt.plot(df.x_real.tolist()[:], df.y_real.tolist()[:])
plt.plot(df.x_real_sim.tolist()[:], df.y_real_sim.tolist()[:])
# plt.plot(df.traj_error)
plt.show()
# print(df)
feature = df.error.cumsum().iloc[-1]
# print(feature)
# print(feature)
print(feature)
if feature == np.inf or math.isnan(feature):
feature = 1000000000000000000000000000000000
return feature
def traj_error(x):
ship_it = ship()
ship_it.D_p = ship_it.D_p*x[0]
df_main = pd.read_csv('test_1_large.csv', sep=',')
ship_it.I_e = x[1]*ship_it.I_e
ship_it.Mass = x[2]*ship_it.Mass
print(x[0],'a')
# ship_it.Mass = ship_it.Mass*x[1]
df = df_main[20:-20]
df.rpm_0 = df.rpm_0/60.0*x[3]
df.rpm_1 = df.rpm_1/60.0*x[3]
df.rpm_2 = df.rpm_2/60.0*x[3]
df = df.dropna()
df['az_speed_0'] = df.rsa_0.diff()
df['az_speed_1'] = df.rsa_1.diff()
df['az_speed_2'] = df.rsa_2.diff()
# df.az_speed_0 = df.az_speed_0.apply(lambda x: x-360. if x>360. else ())
#azimuth 2 port side
df['u_a_2'] = (1-ship_it.w)*((-df.u+df.r*abs(ship_it.y_2))*np.cos(np.deg2rad(df.rsa_2)) + (-df.v+df.r*abs(ship_it.x_2))*np.sin(np.deg2rad(df.rsa_2))) #(1-ship_it.w)*
df['u_a_1'] = (1-ship_it.w)*((-df.u-df.r*abs(ship_it.y_1))*np.cos(np.deg2rad(df.rsa_1)) + (-df.v+df.r*abs(ship_it.x_1))*np.sin(np.deg2rad(df.rsa_1))) #(1-ship_it.w)*
df['u_a_0'] = (1-ship_it.w)*((df.u)*+1*np.cos(np.deg2rad(df.rsa_0)) + ((-df.v - df.r*abs(ship_it.x_0))*np.sin(np.deg2rad(df.rsa_0))) ) #(1-ship_it.w)*
# df['u_a_2'] = df.u
# df['u_a_1'] = df.u
# df['u_a_0'] = df.u
df['beta_2'] = np.rad2deg(np.arctan((df.u_a_2)/(0.7*np.pi*df.rpm_2*ship_it.D_p)))
#change invalse hoek acoording to engine stand
# df['beta_2'] = df.apply(lambda row: row['beta_2'] + 180 if row['u_a_2']>=0 else (row['beta_2'] + 0 if row['u_a_2']<0 and row['rsa_2']<90 or row['rsa_2']>270. else (row['beta_2'] + 180 if row['u_a_2'] <0 and 90.<row['rsa_2']<270. else row['beta_2'])) ,axis=1)
# df['beta_2'] = df.beta_2.apply(lambda x: x-360 if x>360 else x)
df['beta_2'] = df.beta_2.apply(lambda x: x+360 if x<0 else x)
df['beta_1'] = np.rad2deg(np.arctan((df.u_a_1)/(0.7*np.pi*df.rpm_1*ship_it.D_p)))
# df['beta_1'] = df.apply(lambda row: row['beta_1'] + 180 if row['u_a_1']>=0 and row['rsa_1']<90 or row['rsa_1']>270. else (row['beta_1'] + 180 if row['u_a_1']<0 and row['rsa_1']<90 or row['rsa_1']>270. else (row['beta_1'] + 360 if row['u_a_1'] <0 and 90.<row['rsa_1']<270. else row['beta_1'])) ,axis=1)
# df['beta_1'] = df.beta_1.apply(lambda x: x-360 if x>360 else x)
df['beta_1'] = df.beta_1.apply(lambda x: x+360 if x<0 else x)
df['beta_0'] = np.rad2deg(np.arctan((df.u_a_0)/(0.7*np.pi*df.rpm_0*ship_it.D_p)))
# df['beta_0'] = df.apply(lambda row: row['beta_0'] + 180 if row['u_a_0']>=0 and row['rsa_0']<90 or row['rsa_0']>270. else (row['beta_0'] + 180 if row['u_a_0']<0 and row['rsa_0']<90 or row['rsa_0']>270. else (row['beta_0'] + 360 if row['u_a_0'] <0 and 90.<row['rsa_0']<270. else row['beta_0'])) ,axis=1)
# df['beta_0'] = df.beta_0.apply(lambda x: x-360 if x>360 else x)
df['beta_0'] = df.beta_0.apply(lambda x: x+360 if x<0 else x)
# first engine listed experiences thrust decrease, t_21 means thrust reduction ratio due to downstream flow caused by engine 1
# df['t_21_phi'] = df.apply(lambda row: thruster_interaction_coefficient(ship_it.x_1, ship_it.y_1, row['rsa_1'], 25.0, 100.0, ship_it.x_2, ship_it.y_2, row['rsa_2']), axis=1)
# df['t_20_phi'] = df.apply(lambda row: thruster_interaction_coefficient(ship_it.x_0, ship_it.y_0, row['rsa_0'], 25.0, 100.0, ship_it.x_2, ship_it.y_2, row['rsa_2']), axis=1)
df['f_p_40_2'] = 1.0*((1-ship_it.t)*ship_it.beta_coef(df.beta_2)*0.5*ship_it.rho*(((((1-ship_it.w)*df.u_a_2)**2)+(0.7*np.pi*df.rpm_2*ship_it.D_p)**2))*np.pi/4*ship_it.D_p**2) #(1-df['t_21_phi'])*(1-df['t_20_phi'])*
##*(1-df['t_21_phi'])*(1-df['t_20_phi'])
# df['t_12_phi'] = df.apply(lambda row: thruster_interaction_coefficient(ship_it.x_2, ship_it.y_2, row['rsa_2'], 25.0, 100.0, ship_it.x_1, ship_it.y_1, row['rsa_1']), axis=1)
# df['t_10_phi'] = df.apply(lambda row: thruster_interaction_coefficient(ship_it.x_0, ship_it.y_0, row['rsa_0'], 25.0, 100.0, ship_it.x_1, ship_it.y_1, row['rsa_1']), axis=1)
df['f_p_40_1'] = 1.0*((1-ship_it.t)*ship_it.beta_coef(df.beta_1)*0.5*ship_it.rho*(((((1-ship_it.w)*df.u_a_1)**2)+(0.7*np.pi*df.rpm_1*ship_it.D_p)**2))*np.pi/4*ship_it.D_p**2) #(1-df['t_12_phi'])*(1-df['t_10_phi'])*
#*(1-df['t_12_phi'])*(1-df['t_10_phi'])
# df['t_02_phi'] = df.apply(lambda row: thruster_interaction_coefficient(ship_it.x_2, ship_it.y_2, row['rsa_2'], 25.0, 100.0, ship_it.x_0, ship_it.y_0, row['rsa_0']), axis=1)
# df['t_01_phi'] = df.apply(lambda row: thruster_interaction_coefficient(ship_it.x_1, ship_it.y_1, row['rsa_1'], 25.0, 100.0, ship_it.x_0, ship_it.y_0, row['rsa_0']), axis=1)
df['f_p_40_0'] = 1.0*((1-ship_it.t)*ship_it.beta_coef(df.beta_0)*0.5*ship_it.rho*(((((1-ship_it.w)*df.u_a_0)**2)+(0.7*np.pi*df.rpm_0*ship_it.D_p)**2))*np.pi/4*ship_it.D_p**2) #(1-df['t_02_phi'])*(1-df['t_01_phi'])*
#
# (1-df['t_02_phi'])*(1-df['t_01_phi'])*
# J =(1-w)*u/(n_p*D_p); % Advance ratio of the propeller J =(1-w)*u/(n_p*D_p)
# F_r = - 577 * A_r * (u*1.3)^2 * sin(deg2rad(delta)); % Rudder force with respect to speed and rudder angle
# F_p = (1-t)*rho * n_p^2 * D_p^4 * interp1(K_t(:,1),K_t(:,2),J,'linear','extrap'); % Propeller force with respect to speed and propeller rpm
# df['t_21_phi'] = df.apply(lambda row: thruster_interaction_coefficient(ship_it.x_1, ship_it.y_1, row['rsa_1'], 25.0, 100.0, ship_it.x_2, ship_it.y_2, row['rsa_2']), axis=1)
#slide u_dot
df['u_dot_spec'] = df.u_dot.shift(1)
df['v_dot_spec'] = df.v_dot.shift(1)
df['r_dot_spec'] = df.r_dot.shift(1)
# df = df[df.u>0.0]
# print(df[['u_dot']].head(100))
df = df.dropna()
u = df.u.to_numpy()[:, newaxis]
v = df.v.to_numpy()[:, newaxis]
r = df.r.to_numpy()[:, newaxis]
u_dot_spec = df.u_dot_spec.to_numpy()[:, newaxis]
u_dot = df.u_dot.to_numpy()[:, newaxis]
v_dot = df.v_dot.to_numpy()[:, newaxis]
v_dot_spec = df.v_dot_spec.to_numpy()[:, newaxis]
r_dot = df.r_dot.to_numpy()[:, newaxis]
r_dot_spec = df.r_dot_spec.to_numpy()[:, newaxis]
rsa_0 = df.rsa_0.to_numpy()[:, newaxis]
rsa_1 = df.rsa_1.to_numpy()[:, newaxis]
rsa_2 = df.rsa_2.to_numpy()[:, newaxis]
az_speed_0 = df.az_speed_0.to_numpy()[:, newaxis]
az_speed_1 = df.az_speed_1.to_numpy()[:, newaxis]
az_speed_2 = df.az_speed_2.to_numpy()[:, newaxis]
f_p_40_0 = df.f_p_40_0.to_numpy()[:, newaxis]
f_p_40_2 = df.f_p_40_2.to_numpy()[:, newaxis]
f_p_40_1 = df.f_p_40_1.to_numpy()[:, newaxis]
# X = u uu uuu vv rr vr uvv rvu urr
# Y = v uv ur uur uuv vvv rrr rrv vvr abs(v)v abs(r)v rabs(v) abs(r)r
# N = r uv ur uur uuv vvv rrr rrv vvr abs(v)v abs(r)v rabs(v) abs(r)r
X = np.concatenate([u_dot, u, u*u, u*u*u, v*v, r*r, v*r, u*v*v, r*v*u, u*r*r], axis=1)
Y = np.concatenate([v_dot, v, v*v, u*v, u*r, u*u*r, u*u*v, v*v*v, r*r*r, r*r*v, v*v*r, abs(v)*v, abs(r)*v, r*abs(v), abs(r)*r], axis=1)
N = np.concatenate([r_dot, r, r*r, v*r, u*r, u*u*r, u*u*v, v*v*v, r*r*r, r*r*v, v*v*r, abs(v)*v, abs(r)*v, r*abs(v), abs(r)*r ], axis=1)
y_x = ship_it.Mass*(u_dot-r*v)+1*(np.cos(np.deg2rad(rsa_0))*(f_p_40_0)+np.cos(np.deg2rad(rsa_1))*(f_p_40_1)+np.cos(np.deg2rad(rsa_2))*(f_p_40_2))
y_y = ship_it.Mass*(v_dot+r*u)+1*(np.sin(np.deg2rad(rsa_0))*(f_p_40_0)+np.sin(np.deg2rad(rsa_1))*(f_p_40_1)+np.sin(np.deg2rad(rsa_2))*(f_p_40_2)) #np.sin(rsa_0)*abs(f_p_40_0)+np.sin(rsa_1)*abs(f_p_40_1)+
y_r = ship_it.I_e*r_dot + 1*(abs(ship_it.x_0)*np.sin(np.deg2rad(rsa_0))*(f_p_40_0) - abs(ship_it.x_2)*np.sin(np.deg2rad(rsa_0))*(f_p_40_2) - (ship_it.x_1)*np.sin(np.deg2rad(rsa_1))*(f_p_40_1) - abs(ship_it.y_2)*np.cos(np.deg2rad(rsa_2))*(f_p_40_2) + abs(ship_it.y_1)*np.cos(np.deg2rad(rsa_1))*(f_p_40_1))
model = Lasso(fit_intercept=False)
cv = RepeatedKFold(n_splits=5, n_repeats=3, random_state=1)
grid = dict()
grid['alpha'] = np.arange(0.001, 0.011, 0.001)
search = GridSearchCV(model, grid, scoring='neg_mean_absolute_error', cv=cv, n_jobs=-1)
results_x = search.fit(X, y_x)
model = Lasso(fit_intercept=False)
cv = RepeatedKFold(n_splits=5, n_repeats=3, random_state=1)
grid = dict()
grid['alpha'] = np.arange(0.001, 0.011, 0.001)
search = GridSearchCV(model, grid, scoring='neg_mean_absolute_error', cv=cv, n_jobs=-1)
results_y = search.fit(Y, y_y)
model = Lasso(fit_intercept=False)
cv = RepeatedKFold(n_splits=10, n_repeats=3, random_state=1)
grid = dict()
grid['alpha'] = np.arange(0.0000001, 0.0000011, 0.0000001)
search = GridSearchCV(model, grid, scoring='neg_mean_absolute_error', cv=cv, n_jobs=-1)
results_r = search.fit(N, y_r)
print(results_x.best_estimator_.score(X,y_x), results_x.best_estimator_.alpha)
print(results_y.best_estimator_.score(Y,y_y), results_y.best_estimator_.alpha)
print(results_r.best_estimator_.score(N,y_r), results_r.best_estimator_.alpha)
# acc_lim = np.asarray([[df.u_dot.max(),df.u_dot.diff().max()],
# [df.u_dot.min(),df.u_dot.diff().min()],
# [abs(df.v_dot).max(),abs(df.v_dot.diff()).max() ],
# [abs(df.r_dot).max(),abs(df.r_dot.diff()).max() ]])
acc_lim = np.asarray([[df_main.u_dot.quantile(0.95),df_main.u_dot.diff().quantile(0.95)],
[df_main.u_dot.quantile(0.05),df_main.u_dot.diff().quantile(0.05)],
[abs(df.v_dot).quantile(0.95),abs(df.v_dot.diff()).quantile(0.95) ],
[abs(df.r_dot).quantile(0.95),abs(df.r_dot.diff()).quantile(0.95) ]])
a_list = [list(results_x.best_estimator_.coef_),list(results_y.best_estimator_.coef_),list(results_r.best_estimator_.coef_)]
row_lengths = []
for row in a_list:
row_lengths.append(len(row))
max_length = max(row_lengths)
for row in a_list:
while len(row) < max_length:
row.append(None)
coef_ = np.array([np.asarray(a_list[0]),np.asarray(a_list[1]),np.asarray(a_list[2])])
df = df_main[20:1000]
u, v, r, hdg = df.loc[df.index[0], 'u'],df.loc[df.index[0], 'v'], df.loc[df.index[0], 'r'], df.loc[df.index[0], 'hdg']
ship_it_model = ship_model(df.loc[df.index[0], 'u_dot'],df.loc[df.index[0], 'v_dot'], df.loc[df.index[0], 'r_dot'], ship_it, coef_, acc_lim)
print(df.rpm_0[:100])
df_sim = pd.DataFrame([])
for i in df[:-1].index:
u, v, r, hdg, delta_x_0, delta_y_0, delta_r_0, u_dot, v_dot, r_dot = ship_it_model.manoeuvre_model_rt_evolution(u, v, r, hdg,
df.loc[i, 'rpm_0'], df.loc[i, 'rpm_1'], df.loc[i, 'rpm_2'],
df.loc[i, 'rsa_0'], df.loc[i, 'rsa_1'], df.loc[i, 'rsa_2'],
df.loc[i, 'delta_time']
)
# print(u, v, r, hdg)
df_temp = pd.DataFrame({
'index_sim' : [i+1],
'x_delta_sim': [delta_x_0],
'y_delta_sim': [delta_y_0],
'hdg_delta_sim': [delta_r_0]
})
df_sim = pd.concat([df_sim, df_temp], axis=0)
df = pd.merge(df, df_sim, how='left', left_on=df.index, right_on='index_sim')
df['x_real_sim'] = df.x_delta_sim.cumsum()
df['y_real_sim'] = df.y_delta_sim.cumsum()
df['psi_sim'] = df.hdg_delta_sim.cumsum()
df['x_real'] = df.x.cumsum()
df['y_real'] = df.y.cumsum()
df['traj_error'] = (np.sqrt((df['x_real_sim'] - df['x_real'])**2 + (df['y_real_sim'] - df['y_real'])**2)).cumsum()
df['x_sim_diff_avg'] = df['x_real_sim'] - df.x_delta_sim.mean()
df['y_sim_diff_avg'] = df['y_real_sim'] - df.y_delta_sim.mean()
df['x_real_diff_avg'] = df['x_real'] - df.x.mean()
df['y_real_diff_avg'] = df['y_real'] - df.y.mean()
rho_x = (df['x_sim_diff_avg']*df['x_real_diff_avg']).sum()/np.sqrt((df['x_sim_diff_avg']**2).sum()*(df['x_real_diff_avg']**2).sum() )
rho_y = (df['y_sim_diff_avg']*df['y_real_diff_avg']).sum()/np.sqrt((df['y_sim_diff_avg']**2).sum()*(df['y_real_diff_avg']**2).sum() )
plt.plot(df.x_real.tolist()[:],df.y_real.tolist()[:])
plt.plot(df.x_real_sim.tolist()[:],df.y_real_sim.tolist()[:])
# plt.plot(df.traj_error)
plt.show()
print(rho_x*rho_y)
del df
return rho_x*rho_y
def traj_error(x):
ship_it = ship()
df = df_main
# ship_it.Mass = ship_it.Mass*x[1]
# coef__i = np.genfromtxt('foo_evo_general.csv', delimiter=',')
coef__ = np.array(
[np.asarray(coef__i[0]),
np.asarray(coef__i[1]),
np.asarray(x)])
# print(coef__)
u, v, r, hdg = df.loc[df.index[0],
'u'], df.loc[df.index[0],
'v'], df.loc[df.index[0],
'r'], df.loc[df.index[0],
'hdg']
ship_it_model = ship_model(df.loc[df.index[0],
'u_dot'], df.loc[df.index[0], 'v_dot'],
df.loc[df.index[0],
'r_dot'], ship_it, coef__, acc_lim)
df_sim = pd.DataFrame([])
for i in df[:-1].index:
if i % 400 == 0:
u, v, r, hdg = df.loc[i,
'u'], df.loc[i,
'v'], df.loc[i,
'r'], df.loc[i,
'hdg']
u, v, r, hdg, delta_x_0, delta_y_0, delta_r_0, u_dot, v_dot, r_dot = ship_it_model.manoeuvre_model_rt_evolution(
u, v, r, hdg, df.loc[i, 'rpm_0'] / 60., df.loc[i, 'rpm_1'] / 60.,
df.loc[i, 'rpm_2'] / 60., df.loc[i, 'rsa_0'], df.loc[i, 'rsa_1'],
df.loc[i, 'rsa_2'], df.loc[i, 'delta_time'])
# print(u, v, r, hdg)
df_temp = pd.DataFrame({
'index_sim': [i + 1],
'x_delta_sim': [delta_x_0],
'y_delta_sim': [delta_y_0],
'hdg_delta_sim': [delta_r_0]
})
df_sim = pd.concat([df_sim, df_temp], axis=0)
df = pd.merge(df,
df_sim,
how='left',
left_on=df.index,
right_on='index_sim')
df['x_real_sim'] = df.x_delta_sim.cumsum()
df['y_real_sim'] = df.y_delta_sim.cumsum()
df['psi_sim'] = df.hdg_delta_sim.cumsum()
df['x_real'] = df.x.cumsum()
df['y_real'] = df.y.cumsum()
df['x_diff'] = abs(df.x_delta_sim - df.x)
df['y_diff'] = abs(df.y_delta_sim - df.y)
df['error'] = np.sqrt(df.x_diff**2 + df.y_diff**2)
plt.plot(df.x_real.tolist()[:], df.y_real.tolist()[:])
plt.plot(df.x_real_sim.tolist()[:], df.y_real_sim.tolist()[:])
# plt.plot(df.traj_error)
plt.show()
# print(df)
feature = df.error.cumsum().iloc[-1]
# print(feature)
# print(feature)
del df
del ship_it_model
if feature == np.inf:
feature = 1000000000000
print(feature)
return feature
