Add more sober P4

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

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

Assignments/Assignment_3/P4/main.m

@@ -8,27 +8,17 @@
 %% 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;
 Qm = 0;
 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,44 +28,20 @@
 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
-
+lookahead = 15*Loa; 
+kappa = 2;
+ROA = 4*Loa;
+wp_n = 1;
 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));
 
 %% 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;
@@ -87,118 +53,78 @@
 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 ]';
-
-    %% Wind disturbance
-    if t >= 200,
+    %% 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));
         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;
-
-    % 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); 
-        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
-
+    %% Sideslip and crab
+    crab = atan2(x(2), x(1));
 
-    [psi_ref, y_int_dot] = guidance(state_struct, curr_wpt, next_wpt, ILOS_params);
-    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;
+    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);
 
-    u_d = U_ref;
+    sideslip = asin(v_r/norm(U_r, 2));
 
-    %% 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);
+    %% Guidance and reference model
+    wp_ref = WP(:, wp_n);
+    wp_t = WP(:, wp_n+1);
 
-    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;
+    % 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
+    end
+
+    [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);
 
-    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,46 +136,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);
-
-    sideslip = asin(u_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);  
+    y_int = y_int + h*dy_int;
 end
 
+simdata( all(~simdata,2), : ) = []; % Cut trailing zeros
+
 %% Plotting
 t       = simdata(:,1);                 % s
 u       = simdata(:,2);                 % m/s
 v       = simdata(:,3);                 % m/s
 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 +231,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