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)
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
import numpy as np
import time
from manoeuvre_model_rpa3 import ship_model
from ship_class import ship
from pid_small import PID
import matplotlib.pyplot as plt
ship = ship()
coef_ = np.genfromtxt('foo_rpa3.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 = 0.3
rpm = 0
# (speed, time)
speed_u = [(0, 0), (3, 20), (7, 140), (4, 250), (9, 450)]
end_input = 0
speed_u_position = 0
current_speed_setting = 0
t = 0
dt = 0.4 #future: add noise
t_end = 600
pid_speed = PID(P=25.6, I=-0.04, D=2.653) # -0.04646
#
u_ref = []
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(x[:11]),
np.asarray(x[11:25]),
np.asarray(x[25:])])
# 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_rpa_3(
u, v, r, hdg, df.loc[i, 'rpm'] / 60., df.loc[i, 'rsa'],
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 = 1000000000000
print(feature)
del df
return feature
1746.3946520449667, 1123.3868231692015, 1162.3140968147961,
1019.2068291887288, -1100.7324591224883, 11384.438046482417
]
coef_ = np.array(
[np.asarray(x[:11]),
np.asarray(x[11:25]),
np.asarray(x[25:])])
df_main = df_main[30:750]
# df_main.rsa = df_main.rsa*0.88
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_input = df_main[['rpm', 'rsa']]
print(u, v, r, hdg)
df_sim = pd.DataFrame([])
for i in df_main[:-1].index:
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,
df_main.loc[i, 'rpm'] / 60.,
df_main.loc[i, 'rsa'],
df_main.loc[i, 'delta_time'],
)