From 567c51fa3c621b96cdc5c3a53a5adb5628d26fbb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christopher=20Str=C3=B8m?= Date: Sat, 13 Nov 2021 20:48:14 +0100 Subject: [PATCH 1/6] remove SVGs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Christopher Strøm --- Assignments/Assignment_3/P4/1b_map_pos.svg | 8446 ------- Assignments/Assignment_3/P4/1c_map_pos.svg | 4434 ---- Assignments/Assignment_3/P4/2a_chi.svg | 163 - .../Assignment_3/P4/2a_crab_sideslip.svg | 284 - .../Assignment_3/P4/2a_ned_yaw_yaw_rate.svg | 301 - .../P4/2a_surge_propeller_speed_rudder.svg | 381 - Assignments/Assignment_3/P4/2b_chi.svg | 163 - .../Assignment_3/P4/2b_crab_sideslip.svg | 284 - .../Assignment_3/P4/2b_ned_yaw_yaw_rate.svg | 301 - .../P4/2b_surge_propeller_speed_rudder.svg | 381 - Assignments/Assignment_3/P4/2c_map_pos.svg | 3411 --- Assignments/Assignment_3/P4/2d_pos_map.svg | 3411 --- Assignments/Assignment_3/P4/wtf.svg | 18737 ---------------- 13 files changed, 40697 deletions(-) delete mode 100644 Assignments/Assignment_3/P4/1b_map_pos.svg delete mode 100644 Assignments/Assignment_3/P4/1c_map_pos.svg delete mode 100644 Assignments/Assignment_3/P4/2a_chi.svg delete mode 100644 Assignments/Assignment_3/P4/2a_crab_sideslip.svg delete mode 100644 Assignments/Assignment_3/P4/2a_ned_yaw_yaw_rate.svg delete mode 100644 Assignments/Assignment_3/P4/2a_surge_propeller_speed_rudder.svg delete mode 100644 Assignments/Assignment_3/P4/2b_chi.svg delete mode 100644 Assignments/Assignment_3/P4/2b_crab_sideslip.svg delete mode 100644 Assignments/Assignment_3/P4/2b_ned_yaw_yaw_rate.svg delete mode 100644 Assignments/Assignment_3/P4/2b_surge_propeller_speed_rudder.svg delete mode 100644 Assignments/Assignment_3/P4/2c_map_pos.svg delete mode 100644 Assignments/Assignment_3/P4/2d_pos_map.svg delete mode 100644 Assignments/Assignment_3/P4/wtf.svg diff --git a/Assignments/Assignment_3/P4/1b_map_pos.svg b/Assignments/Assignment_3/P4/1b_map_pos.svg deleted file mode 100644 index 902ca71..0000000 --- a/Assignments/Assignment_3/P4/1b_map_pos.svg +++ /dev/null @@ -1,8446 +0,0 @@ - - -63°25'N63°30'N63°35'N63°40'NLatitude 9°40'E 9°50'E10°E10°10'E10°20'ELongitudeEsri, HERE, Garmin, USGS 2 mi 5 km diff --git a/Assignments/Assignment_3/P4/1c_map_pos.svg b/Assignments/Assignment_3/P4/1c_map_pos.svg deleted file mode 100644 index 41c896e..0000000 --- a/Assignments/Assignment_3/P4/1c_map_pos.svg +++ /dev/null @@ -1,4434 +0,0 @@ - - -63°30'N63°40'NLatitude 9°30'E 9°45'E10°E10°15'E10°30'ELongitudeEsri, HERE, Garmin, USGS 5 mi 5 km diff --git a/Assignments/Assignment_3/P4/2a_chi.svg b/Assignments/Assignment_3/P4/2a_chi.svg deleted file mode 100644 index fe22580..0000000 --- a/Assignments/Assignment_3/P4/2a_chi.svg +++ /dev/null @@ -1,163 +0,0 @@ - - - diff --git a/Assignments/Assignment_3/P4/2a_crab_sideslip.svg b/Assignments/Assignment_3/P4/2a_crab_sideslip.svg deleted file mode 100644 index 5a19678..0000000 --- a/Assignments/Assignment_3/P4/2a_crab_sideslip.svg +++ /dev/null @@ -1,284 +0,0 @@ - - -010002000300040005000600070008000900010000time (s)-4-2024Crab (deg)010002000300040005000600070008000900010000time (s)050100Sideslip (deg) diff --git a/Assignments/Assignment_3/P4/2a_ned_yaw_yaw_rate.svg b/Assignments/Assignment_3/P4/2a_ned_yaw_yaw_rate.svg deleted file mode 100644 index 1b1784f..0000000 --- a/Assignments/Assignment_3/P4/2a_ned_yaw_yaw_rate.svg +++ /dev/null @@ -1,301 +0,0 @@ - - - diff --git a/Assignments/Assignment_3/P4/2a_surge_propeller_speed_rudder.svg b/Assignments/Assignment_3/P4/2a_surge_propeller_speed_rudder.svg deleted file mode 100644 index dc19f8a..0000000 --- a/Assignments/Assignment_3/P4/2a_surge_propeller_speed_rudder.svg +++ /dev/null @@ -1,381 +0,0 @@ - - -010002000300040005000600070008000900010000time (s)0510Actual and desired surge velocities (m/s)010002000300040005000600070008000900010000time (s)0500Actual and commanded propeller speed (rpm)010002000300040005000600070008000900010000time (s)-50050Actual and commanded rudder angles (deg) diff --git a/Assignments/Assignment_3/P4/2b_chi.svg b/Assignments/Assignment_3/P4/2b_chi.svg deleted file mode 100644 index 18ea764..0000000 --- a/Assignments/Assignment_3/P4/2b_chi.svg +++ /dev/null @@ -1,163 +0,0 @@ - - - diff --git a/Assignments/Assignment_3/P4/2b_crab_sideslip.svg b/Assignments/Assignment_3/P4/2b_crab_sideslip.svg deleted file mode 100644 index 1ea06fb..0000000 --- a/Assignments/Assignment_3/P4/2b_crab_sideslip.svg +++ /dev/null @@ -1,284 +0,0 @@ - - -010002000300040005000600070008000900010000time (s)050100Crab (deg)010002000300040005000600070008000900010000time (s)-100-50050100Sideslip (deg) diff --git a/Assignments/Assignment_3/P4/2b_ned_yaw_yaw_rate.svg b/Assignments/Assignment_3/P4/2b_ned_yaw_yaw_rate.svg deleted file mode 100644 index d1d3b16..0000000 --- a/Assignments/Assignment_3/P4/2b_ned_yaw_yaw_rate.svg +++ /dev/null @@ -1,301 +0,0 @@ - - - diff --git a/Assignments/Assignment_3/P4/2b_surge_propeller_speed_rudder.svg b/Assignments/Assignment_3/P4/2b_surge_propeller_speed_rudder.svg deleted file mode 100644 index a7924fb..0000000 --- a/Assignments/Assignment_3/P4/2b_surge_propeller_speed_rudder.svg +++ /dev/null @@ -1,381 +0,0 @@ - - -010002000300040005000600070008000900010000time (s)0510Actual and desired surge velocities (m/s)010002000300040005000600070008000900010000time (s)0500Actual and commanded propeller speed (rpm)010002000300040005000600070008000900010000time (s)-50050Actual and commanded rudder angles (deg) diff --git a/Assignments/Assignment_3/P4/2c_map_pos.svg b/Assignments/Assignment_3/P4/2c_map_pos.svg deleted file mode 100644 index 9b4682e..0000000 --- a/Assignments/Assignment_3/P4/2c_map_pos.svg +++ /dev/null @@ -1,3411 +0,0 @@ - - -63°30'N63°35'N63°40'NLatitude 9°40'E 9°50'E10°E10°10'E10°20'ELongitudeEsri, HERE, Garmin, USGS 2 mi 5 km diff --git a/Assignments/Assignment_3/P4/2d_pos_map.svg b/Assignments/Assignment_3/P4/2d_pos_map.svg deleted file mode 100644 index a275403..0000000 --- a/Assignments/Assignment_3/P4/2d_pos_map.svg +++ /dev/null @@ -1,3411 +0,0 @@ - - -63°30'N63°35'N63°40'NLatitude 9°40'E 9°50'E10°E10°10'E10°20'ELongitudeEsri, HERE, Garmin, USGS 2 mi 5 km diff --git a/Assignments/Assignment_3/P4/wtf.svg b/Assignments/Assignment_3/P4/wtf.svg deleted file mode 100644 index 088f73e..0000000 --- a/Assignments/Assignment_3/P4/wtf.svg +++ /dev/null @@ -1,18737 +0,0 @@ - - -63°30'N63°40'N63°50'N64°N64°10'N64°20'N64°30'NLatitude 8°30'E 9°E 9°30'E10°E10°30'E11°ELongitudeEsri, HERE, Garmin, USGS 10 mi 20 km From 6cb34b573ddebfedfd78317704974392d5a36508 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christopher=20Str=C3=B8m?= Date: Sat, 13 Nov 2021 20:48:56 +0100 Subject: [PATCH 2/6] add guidance law MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Christopher Strøm --- Assignments/Assignment_3/P4/guidance.m | 58 +++++--------------------- 1 file changed, 11 insertions(+), 47 deletions(-) diff --git a/Assignments/Assignment_3/P4/guidance.m b/Assignments/Assignment_3/P4/guidance.m index 5d637df..3499f72 100644 --- a/Assignments/Assignment_3/P4/guidance.m +++ b/Assignments/Assignment_3/P4/guidance.m @@ -1,48 +1,12 @@ -function [chi_d, y_int_dot] = guidance(state_struct, current_waypoint, next_waypoint, ILOS_params) - % Important note regarding adpative lookahead distance - % A larger lookadhead distance is better when the system is closer to - % the desired waypoint, as it prevents overshoot better - % A smaller lookahead distance is preferable when the ships is further - % away, as it creates a more aggressive controller - - % Assumptions: - % - The values for position and waypoints are in NED, such that this - % function must be modified if used over larger areas - % - The function should be invoked for every iteration, or often - % enough, such that will generate a new desired course whenever one - % detects that the old desired course no longer is satisfying. Since a - % ship is a slow system, the function could be invoked for every tenth - % iteration or something - % - The funstion takes in the lookahead distance, such that this could - % be chosen spesifically between ships of different types. The same - % reason is also chosen for kappa - - % Extract values - lookahead = ILOS_params.lookahead; - kappa = ILOS_params.kappa; - - pos = state_struct.pos; - y_int = state_struct.y_int; - - % Calculate pi_p - diff_waypoints = next_waypoint - current_waypoint; - pi_p = atan2(diff_waypoints(2), diff_waypoints(1)); - - % Using eq. 12.55 to calculate the cross-track error - R = [cos(pi_p), -sin(pi_p); - sin(pi_p), cos(pi_p)]; - ep = R' * (pos - current_waypoint); - y_ep = ep(2); - - % Calculate controller gains according to 12.79 and 12.109 - Kp = 1/lookahead; - Ki = kappa*Kp; - - % Calculate desired course using eq. 12.78 - chi_d = pi_p - atan(Kp * y_ep + Ki*y_int); - - % Calculate y_int_dot using eq. 12.109 - % Should ideally be calculated outside of this function, but the - % consistency of this code is so terrible nonetheless... - y_int_dot = lookahead*y_ep / (lookahead^2 + (y_ep + kappa*y_int)^2); +function chi_d = guidance(pos, wp_ref, wp_t, lookahead) + % pos = (x, y) position of ownship in NED + % wp_ref = reference waypoint, waypoint 1 + % wp_t = target waypoint, waypoint 2 + % lookahead = tuning parameter + + pi_p = atan2(wp_t(2) - wp_ref(2), wp_t(1) - wp_ref(1)); + [~, ~, y_e] = crosstrack(wp_t(1), wp_t(2), ... + wp_ref(1), wp_ref(2), ... + pos(1), pos(2)); + chi_d = pi_p - atan(y_e / lookahead); end \ No newline at end of file From b8cc223cb798b0b8887ae5412fdfcc9f3eccd855 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christopher=20Str=C3=B8m?= Date: Sat, 13 Nov 2021 22:07:25 +0100 Subject: [PATCH 3/6] move reference model to own file MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Christopher Strøm --- Assignments/Assignment_3/P4/reference_model.m | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 Assignments/Assignment_3/P4/reference_model.m diff --git a/Assignments/Assignment_3/P4/reference_model.m b/Assignments/Assignment_3/P4/reference_model.m new file mode 100644 index 0000000..e3a79a2 --- /dev/null +++ b/Assignments/Assignment_3/P4/reference_model.m @@ -0,0 +1,30 @@ +function [r_d_out, a_d_out, psi_d_out] ... + = reference_model(r_d, a_d, psi_d, psi_ref, h) + + w_ref = 0.03; + zeta_ref = 1; + + r_max = 1 * pi/180; + a_max = 0.5 * pi/180; + + sat_rd = sat_value(r_d, r_max); + sat_ad = sat_value(a_d, a_max); + + psi_d_dot = sat_rd; + r_d_dot = sat_ad; + a_d_dot = -(2*zeta_ref + 1)*w_ref*sat_ad ... + -(2*zeta_ref + 1)*w_ref^2*sat_rd ... + + w_ref^3*(psi_ref - psi_d); + + psi_d_out = psi_d + h*psi_d_dot; + r_d_out = r_d + h*r_d_dot; + a_d_out = a_d + h*a_d_dot; + +end + +function sat_val = sat_value(val, abs_lim) + sat_val = val; + if abs(val) >= abs_lim + sat_val = sign(val)*abs_lim; + end +end From 45dcc435f459316b27d08f98ee1fabff28867ab7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christopher=20Str=C3=B8m?= Date: Sat, 13 Nov 2021 22:07:38 +0100 Subject: [PATCH 4/6] cleanup and working LOS MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Christopher Strøm --- Assignments/Assignment_3/P4/main.m | 171 +++++++---------------------- 1 file changed, 37 insertions(+), 134 deletions(-) diff --git a/Assignments/Assignment_3/P4/main.m b/Assignments/Assignment_3/P4/main.m index 1630e6f..95cac71 100644 --- a/Assignments/Assignment_3/P4/main.m +++ b/Assignments/Assignment_3/P4/main.m @@ -8,19 +8,10 @@ %% Input h = 0.1; % Sampling time [s] Ns = 100000; % Num samples - -use_ILOS = 1; % Set to 0 for regular LOS -use_crab_compensation = 0; % Set to 1 to compensate using crab - -% Initial reference for P1-P3 -%psi_ref_init = 10 * pi/180 * ones(1, Ns/4); % Desired yaw angle (rad) -%psi_ref_end = -20 * pi/180 * ones(1, (Ns - Ns/4) + 1); -%psi_ref = [psi_ref_init, psi_ref_end]; -%psi_ref = 10 * pi/180; -U_ref = 9; % Desired surge speed (m/s) +U_ref = 9; % Desired surge speed (m/s) %% Initial states -eta = [0 0 wrapTo2Pi(-110*pi/180)]'; +eta = [0 0 deg2rad(-110)]'; nu = [0.1 0 0]'; delta = 0; n = 0; @@ -28,7 +19,6 @@ x = [nu' eta' delta n Qm]'; %% Ship coefficients -% Rudder limitations rudder_max = 40 * pi/180; % Max rudder angle (rad) d_rudder_max = 5 * pi/180; % Max rudder derivative (rad/s) @@ -38,37 +28,11 @@ K = 0.0075; T = 169.5493; -%% Waypoints +%% Guidance load WP - -num_wpts = width(WP); -assert(num_wpts >= 1, "Not enough waypoints"); - -wpt_idx = 1; -Roa = 2*Loa; % Distance when switching between waypoints -lookahead = 10*Loa; -kappa = 0.1; - -ILOS_params.lookahead = lookahead; -ILOS_params.kappa = kappa; -if use_ILOS == 0, - ILOS_params.kappa = 0; -end - -y_int = 0; -state_struct.pos = eta(1:2); -state_struct.y_int = y_int; - -% Using current position to go towards first waypoint -curr_wpt = eta(1:2); -next_wpt = WP(:,wpt_idx); % Assuming WP = [x; y]; -if num_wpts >= 1, - curr_wpt = WP(:,wpt_idx); % Assuming WP = [x; y]; - next_wpt = WP(:,wpt_idx+1); -end - -[psi_ref, y_int_dot] = guidance(state_struct, curr_wpt, next_wpt, ILOS_params); -crab = atan2(x(2), x(1)); +lookahead = 15*Loa; +ROA = 5*Loa; +wp_n = 1; %% External forces Vc = 0; @@ -87,118 +51,69 @@ Nwind = 0; %% Controller -zeta_PID = 1; +zeta_n = 1; w_b = 0.06; -w_PID = 1/(sqrt(1-2*zeta_PID^2 + sqrt(4*zeta_PID^4 - 4*zeta_PID^2 + 2))) * w_b; +w_n = 1/(sqrt(1-2*zeta_n^2 + sqrt(4*zeta_n^4 - 4*zeta_n^2 + 2))) * w_b; % Using eq. 15.95, ex. 15.7 and algorithm 15.1 m = T/K; d = 1/K; -k = 0; % No 'spring'-force in yaw -Kp = m*w_PID^2 - k; -Kd = 2*zeta_PID*w_PID*m - d; -Ki = w_PID/10*Kp; +Kp = m*w_n^2; +Kd = 2*zeta_n*w_n*m - d; +Ki = w_n/10*Kp; %% Reference model -w_ref = 0.03; -zeta_ref = 1; - psi_d = 0; r_d = 0; a_d = 0; -% But shouldn't these also be used to represent physical limitations -% that is present in the ship/model? -r_max = 1 * pi/180; -a_max = 0.5 * pi/180; - e_psi_int = 0; delta_c = 0; n_c = 9; %% Simulation simdata = zeros(Ns+1,18); % Table of simulation data -psi_d_arr = zeros(1, Ns+1); for i=1:Ns+1 %% Time t = (i-1) * h; % Time (s) - %% Current disturbance - uc = Vc*cos(beta_vc - x(6)); - vc = Vc*sin(beta_vc - x(6)); - nu_c = [ uc vc 0 ]'; + %% Currents + nu_c = [Vc*cos(beta_vc - x(6)), Vc*sin(beta_vc - x(6)), 0 ]'; - %% Wind disturbance - if t >= 200, + %% Wind + if t >= 200 u_rw = x(1) - Vw*cos(beta_vw - x(6)); v_rw = x(2) - Vw*sin(beta_vw - x(6)); V_rw = norm([u_rw, v_rw], 2); + gamma_rw = -atan2(u_rw, v_rw); + dp = 1/2*rho_a*V_rw^2; - dynamic_pressure = 1/2*rho_a*V_rw^2; - Ywind = dynamic_pressure*cy*sin(gamma_rw)*A_Lw; - Nwind = dynamic_pressure*cn*sin(2*gamma_rw)*A_Lw*Loa; + Ywind = dp*cy*sin(gamma_rw)*A_Lw; + Nwind = dp*cn*sin(2*gamma_rw)*A_Lw*Loa; end tau_wind = [0 Ywind Nwind]'; - %% Waypoint - %x(6) = wrapTo2Pi(x(6)); - pos = x(4:5); - state_struct.pos = pos; - state_struct.y_int = y_int; + %% Guidance and reference model + wp_ref = WP(:, wp_n); + wp_t = WP(:, wp_n+1); - % Logic when switching waypoint - if norm(next_wpt - pos, 2) <= Roa, - %disp("Acheived new wpt at t = " + t + " with pos = "); - % Inside circle of acceptance - % Checking if there are any more waypoints - %assert(wpt_idx <= num_wpts, "Invalid number of changes in waypoints"); - wpt_idx = wpt_idx + 1; - if wpt_idx == num_wpts, - % Final waypoint. Turn of the engine and idle... - % Would be cool to enable DP or something here... - U_ref = 0; - curr_wpt = pos; - next_wpt = pos; - n_c = 0; - elseif wpt_idx <= num_wpts - 1, - curr_wpt = WP(:, wpt_idx); - next_wpt = WP(:, wpt_idx+1); + % Manage waypoint switching + if norm(wp_t - x(4:5), 2) <= ROA + wp_n = wp_n + 1; + if wp_n == width(WP) + break % End simulation if we reach the final waypoint end - %diff = next_wpt - curr_wpt; - %wrapTo2Pi(atan2(diff(2), diff(1))) * 180/pi - - %psi_ref = wrapTo2Pi(guidance(pos, curr_wpt, next_wpt, lookahead)) * 180/pi end - [psi_ref, y_int_dot] = guidance(state_struct, curr_wpt, next_wpt, ILOS_params); + + psi_ref = guidance(x(4:5), wp_ref, wp_t, lookahead); psi_ref = wrapTo2Pi(psi_ref); - if use_ILOS == 0 && use_crab_compensation == 1, - crab = wrapTo2Pi(crab); - psi_ref = wrapTo2Pi(psi_ref - crab); - end - - % Integrate the y_int with anti_windup() - y_int_dot = anti_windup(y_int_dot, delta_c, delta_c - x(7), rudder_max, d_rudder_max); - y_int = y_int + h*y_int_dot; - - u_d = U_ref; - - %% Reference model - sat_rd = sat_value(r_d, r_max); - sat_ad = sat_value(a_d, a_max); - psi_d_dot = sat_rd; - r_d_dot = sat_ad; - a_d_dot = -(2*zeta_ref + 1)*w_ref*sat_ad - (2*zeta_ref + 1)*w_ref^2*sat_rd + w_ref^3*(ssa(wrapTo2Pi(psi_ref) - psi_d)); % -(2*zeta_ref + 1)*w_ref*sat(a_d) - (2*zeta_ref + 1)*w_ref^2*sat(r_d) + w_ref^3*(psi_ref - psi_d); - - psi_d = psi_d + h*psi_d_dot; % Can see that the system gets a desired psi that greatly exceeds the maximum value... - r_d = r_d + h*r_d_dot; - a_d = a_d + h*a_d_dot; - psi_d_arr(i) = psi_d; + [r_d, a_d, psi_d] = reference_model(r_d, a_d, psi_d, psi_ref, h); %% Control law % Heading @@ -210,38 +125,33 @@ e_psi_int = e_psi_int + h*e_psi_int_dot; delta_c = -Kp*e_psi - Kd*e_r - Ki*e_psi_int; - - % Propeller speed - %n_c = 9;% x(8); % Propeller speed (rps) (But is it really in rps or in rpm as indicated in ship.m) - %% Ship dynamics u = [delta_c n_c]'; [xdot,u] = ship(x,u,nu_c,tau_wind); %% Sideslip and crab - % Using equation 10.138, it is given that the body-velocities are - % calculated in the ship dynamics - % Calculating crab and sideslip based on this crab = atan2(x(2), x(1)); R = Rzyx(0, 0, x(6)); U_ned = R'*[x(1), x(2), 0]'; - U_r = U_ned - [uc, vc, 0]'; - u_r = U_r(1); + U_r = U_ned - nu_c'; + v_r = U_r(2); - sideslip = asin(u_r/norm(U_r, 2)); + sideslip = asin(v_r/norm(U_r, 2)); %% Course chi = wrapTo2Pi(x(6) + crab); chi_ref = wrapTo2Pi(psi_ref); %% Store simulation data - simdata(i,:) = [t x(1:3)' x(4:5)' wrapTo2Pi(x(6)) x(7) x(8) u(1) u(2) u_d wrapTo2Pi(psi_d) r_d crab sideslip, chi, chi_ref]; + simdata(i,:) = [t x(1:3)' x(4:6)' x(7) x(8) u(1) u(2) U_ref psi_d r_d crab sideslip, chi, chi_ref]; %% Euler integration x = euler2(xdot,x,h); end +simdata( all(~simdata,2), : ) = []; % Cut trailing zeros + %% Plotting t = simdata(:,1); % s u = simdata(:,2); % m/s @@ -249,7 +159,7 @@ r = (180/pi) * simdata(:,4); % deg/s x = simdata(:,5); % m y = simdata(:,6); % m -psi = (180/pi) * simdata(:,7); % deg +psi = (180/pi) * wrapTo2Pi(simdata(:,7)); % deg delta = (180/pi) * simdata(:,8); % deg n = 60 * simdata(:,9); % rpm delta_c = (180/pi) * simdata(:,10); % deg @@ -318,13 +228,6 @@ pathplotter(x,y); %% Functions -function sat_val = sat_value(val, abs_lim) - sat_val = val; - if abs(val) >= abs_lim - sat_val = sign(val)*abs_lim; - end -end - function x_dot_aw = anti_windup(x_dot, param_0_check, param_1_check , abs_0, abs_1) x_dot_aw = x_dot; if abs(param_0_check) >= abs_0 || abs(param_1_check) >= abs_1 From a872badc763545bbf0598dcb53315bb2ca398536 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christopher=20Str=C3=B8m?= Date: Sat, 13 Nov 2021 22:47:16 +0100 Subject: [PATCH 5/6] add ILOS function MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Christopher Strøm --- Assignments/Assignment_3/P4/integral_guidance.m | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 Assignments/Assignment_3/P4/integral_guidance.m diff --git a/Assignments/Assignment_3/P4/integral_guidance.m b/Assignments/Assignment_3/P4/integral_guidance.m new file mode 100644 index 0000000..1be42c5 --- /dev/null +++ b/Assignments/Assignment_3/P4/integral_guidance.m @@ -0,0 +1,16 @@ +function [chi_d, dy_int] = integral_guidance(pos, wp_ref, wp_t, delta, kappa, y_int) + % pos = (x, y) position of ownship in NED + % wp_ref = reference waypoint, waypoint 1 + % wp_t = target waypoint, waypoint 2 + % lookahead = tuning parameter + + pi_p = atan2(wp_t(2) - wp_ref(2), wp_t(1) - wp_ref(1)); + [~, ~, y_e] = crosstrack(wp_t(1), wp_t(2), ... + wp_ref(1), wp_ref(2), ... + pos(1), pos(2)); + Kp = 1 / delta; + Ki = kappa * Kp; + + chi_d = pi_p - atan(Kp*y_e + Ki*y_int); + dy_int = delta*y_e / (delta^2 + (y_e + kappa*y_int)^2); +end \ No newline at end of file From 245bca54886650909618efe299bffe287a904c1c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christopher=20Str=C3=B8m?= Date: Sat, 13 Nov 2021 22:47:29 +0100 Subject: [PATCH 6/6] use ILOS in main MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Christopher Strøm --- Assignments/Assignment_3/P4/main.m | 39 ++++++++++++++++-------------- 1 file changed, 21 insertions(+), 18 deletions(-) diff --git a/Assignments/Assignment_3/P4/main.m b/Assignments/Assignment_3/P4/main.m index 95cac71..d262309 100644 --- a/Assignments/Assignment_3/P4/main.m +++ b/Assignments/Assignment_3/P4/main.m @@ -30,16 +30,18 @@ %% Guidance load WP -lookahead = 15*Loa; -ROA = 5*Loa; +lookahead = 15*Loa; +kappa = 2; +ROA = 4*Loa; wp_n = 1; +y_int = 0; %% External forces -Vc = 0; +Vc = 1; beta_vc = deg2rad(45); % Wind-coefficients -Vw = 0; +Vw = 1; beta_vw = deg2rad(135); rho_a = 1.247; cy = 0.95; @@ -81,7 +83,7 @@ %% Currents nu_c = [Vc*cos(beta_vc - x(6)), Vc*sin(beta_vc - x(6)), 0 ]'; - + %% Wind if t >= 200 u_rw = x(1) - Vw*cos(beta_vw - x(6)); @@ -96,6 +98,16 @@ end tau_wind = [0 Ywind Nwind]'; + %% Sideslip and crab + crab = atan2(x(2), x(1)); + + R = Rzyx(0, 0, x(6)); + U_ned = R'*[x(1), x(2), 0]'; + U_r = U_ned - nu_c'; + v_r = U_r(2); + + sideslip = asin(v_r/norm(U_r, 2)); + %% Guidance and reference model wp_ref = WP(:, wp_n); wp_t = WP(:, wp_n+1); @@ -107,10 +119,9 @@ break % End simulation if we reach the final waypoint end end - - - - psi_ref = guidance(x(4:5), wp_ref, wp_t, lookahead); + + [psi_ref, dy_int] = integral_guidance(x(4:5), wp_ref, wp_t, ... + lookahead, kappa, y_int); % - crab; % crab angle compensation psi_ref = wrapTo2Pi(psi_ref); [r_d, a_d, psi_d] = reference_model(r_d, a_d, psi_d, psi_ref, h); @@ -129,15 +140,6 @@ u = [delta_c n_c]'; [xdot,u] = ship(x,u,nu_c,tau_wind); - %% Sideslip and crab - crab = atan2(x(2), x(1)); - - R = Rzyx(0, 0, x(6)); - U_ned = R'*[x(1), x(2), 0]'; - U_r = U_ned - nu_c'; - v_r = U_r(2); - - sideslip = asin(v_r/norm(U_r, 2)); %% Course chi = wrapTo2Pi(x(6) + crab); @@ -148,6 +150,7 @@ %% Euler integration x = euler2(xdot,x,h); + y_int = y_int + h*dy_int; end simdata( all(~simdata,2), : ) = []; % Cut trailing zeros