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
] #, (50.0, 50.0,225, 50)]#, (0.0, 50.,270.0, 150)]#,(-50.0, -50.,270.0, 250)]#,(-50.0, 0.,180.0, 300),(-50.0, 0.,180.0, 450)]#, (0.0, 0,290, 120)]#, (50., 100., 100)]#,(100., 100., 200),(100., 0., 300)]#], (0., 100., 180)]#, (0., 0., 190)]#, (20,70), (10, 150)] end_input = 0 input_position = 0 t = 0 dt = 1. #future noise t_end = 520 pid_position_x = PID(P=0.6, I=0.0, D=25.) pid_position_y = PID(P=0.6, I=0.0, D=25.) pid_attitude = PID(P=.1, I=0., D=16.5) # pid_position_x = PID(P=2.5, I=0.0, D=50.) # pid_position_y = PID(P=2.5, I=0.0, D=50.) # pid_attitude = PID(P=.85, I=0.0, D=25.0) # pid_attitude = PID(P=.265, I=0.0, D=.9) ship_model = ship_model(0, 0, 0, ship, coef_) x, x_old = 0, 0 y, y_old = 0, 0 u, v, r, hdg = 0.0, 0, 0, 45 rpm_1, rpm_2 = 0, 0 input_ref = [] traj_x, traj_y, hdg_list = [], [], [] last_coordinate = [0, 0] sign_attitude_control = 1 current_input_setting_attitude = 0 turbo_polyp = Turbo_Polyp(x, y, hdg) t_polyp = 0.0
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 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
from sklearn.model_selection import GridSearchCV
##
from ship_class import ship
from manoeuvre_model_borkum import ship_model
ship = ship()
df_main = pd.read_csv('test_1_large.csv', sep=',')
df_main = df_main[:100]
coef_ = np.genfromtxt('foo_evo_general.csv', delimiter=',')
u, v, r, hdg = df_main.loc[df_main.index[0], 'u'], df_main.loc[
df_main.index[0], 'v'], df_main.loc[df_main.index[0],
'r'], df_main.loc[df_main.index[0],
'hdg']
ship_model = ship_model(df_main.loc[df_main.index[0], 'u_dot'],
df_main.loc[df_main.index[0], 'v_dot'],
df_main.loc[df_main.index[0], 'r_dot'], ship, coef_)
df_sim = pd.DataFrame([])
# coef_[2][5] = coef_[2][5] -1000000
# coef_[2][1] = coef_[2][1] +10000
for i in df_main[:-1].index:
if i % 5 == 0:
u, v, r, hdg = df_main.loc[i, 'u'], df_main.loc[i, 'v'], df_main.loc[
i, 'r'], df_main.loc[i, 'hdg']
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,