Reference Trajectory States & Global Variable Clean-Up

- Add generated states from part 1 as variable
  `ROB535_ControlProject_part1_state` to
  `ROB535_ControlProject_part1_Team3.mat`
- Make step size and prediction horizon global variables
- Remove references to unused global variables
- Fix reference trajectory indexing with "i" based
  on `ROB535_ControlProject_part1_state` variable
- Replace `dt` with `T_s` in `linearized_bike_model`

Experimentation/part2_MPC_controller.m

@@ -40,6 +40,7 @@ state_init = [ ...
 ];
 
 % Simulation Parameters
+global T_s T_p
 T_s = 0.01; % Step Size [s]
 T_p = 0.5; % Prediction Horizon [s]
 
@@ -299,7 +300,7 @@ end
 
 %% Kinematic Bike Models
 function dXdt = nonlinear_bike_model(X,U)
-    global Nw f Iz a b By Cy Dy Ey Shy Svy m g delta_lims Fx_lims
+    global Nw f Iz a b m g
     
     [x,u,y,v,psi,r,delta_f,F_x,F_yf,F_yr] = bike_model_helper(X,U);
     
@@ -314,7 +315,7 @@ function dXdt = nonlinear_bike_model(X,U)
 end
 
 function [A, B] = linearized_bike_model(X_ref,U_ref)
-    global Nw f Iz a b By Cy Dy Ey Shy Svy m g
+    global Nw f Iz a b m g T_s
 
     syms x_var u_var y_var v_var psi_var r_var ... % states
          delta_f_var F_x_var ...                   % inputs
@@ -350,25 +351,24 @@ function [A, B] = linearized_bike_model(X_ref,U_ref)
     ];    
 
     % Substitute values from reference trajectory into symbolic Jacobians
-    % TODO: Fix reference trajectory indexing with "i"
     A_c = @(i) double( ...
         subs( ...
             A_c_symb, ...
             [x_var, u_var, y_var, v_var, psi_var, r_var, delta_f_var, F_x_var, F_yf_var, F_yr_var], ...
-            bike_model_helper(X_ref(i), U_ref(i)) ...
+            bike_model_helper(X_ref(i,:), U_ref(i,:)) ...
         ) ...
     );
     B_c = @(i) double( ...
         subs( ...
             B_c_symb, ...
             [x_var, u_var, y_var, v_var, psi_var, r_var, delta_f_var, F_x_var, F_yf_var, F_yr_var], ...
-            bike_model_helper(X_ref(i), U_ref(i)) ...
+            bike_model_helper(X_ref(i,:), U_ref(i,:)) ...
         ) ...
     );
 
     % Discrete-time LTV system
-    A = @(i) eye(6) + dt*A_c(i);
+    A = @(i) eye(6) + T_s*A_c(i);
-    B = @(i) dt * B_c(i);
+    B = @(i) T_s * B_c(i);
 end
 
 %% Helper Functions
@@ -383,7 +383,7 @@ function clamped_val = clamp(val, min, max)
 end
 
 function [x,u,y,v,psi,r,delta_f,F_x,F_yf,F_yr] = bike_model_helper(X,U)
-    global Nw f Iz a b By Cy Dy Ey Shy Svy m g
+    global Nw a b By Cy Dy Ey Shy Svy m g
 
     % Get state & input variables
     x   = X(1);