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