diff --git a/Experimentation/MPC_Class.m b/Experimentation/MPC_Class.m index 122940a..981e652 100644 --- a/Experimentation/MPC_Class.m +++ b/Experimentation/MPC_Class.m @@ -120,7 +120,7 @@ classdef MPC_Class % Calculate decision variable related quantities obj.npred = obj.T_p / obj.T_s; obj.npredstates = obj.npred + 1; - obj.npredinputs = obj.npred; + obj.npredinputs = obj.npred + 1; obj.ndecstates = obj.npredstates * obj.nstates; obj.ndecinputs = obj.npredinputs * obj.ninputs; @@ -187,12 +187,12 @@ classdef MPC_Class H = NaN(obj.ndec, 1); for i = 0:obj.npredstates-1 - start_idx = get_state_start_idx(i); + start_idx = obj.get_state_start_idx(i); H(start_idx+1:start_idx+obj.nstates) = obj.Q; end for i = 0:obj.npredinputs-1 - start_idx = get_input_start_idx(i); + start_idx = obj.get_input_start_idx(i); H(start_idx+1:start_idx+obj.ninputs) = obj.R; end @@ -222,7 +222,7 @@ classdef MPC_Class % State Limits for i = 0:obj.npredstates-1 - start_idx = get_state_start_idx(i); + start_idx = obj.get_state_start_idx(i); % x position limits Lb(start_idx+1) = obj.x_lims(1) - obj.Y_ref(obj.ref_idx+i, 1); @@ -235,7 +235,7 @@ classdef MPC_Class % Input Limits for i = 0:obj.npredinputs-1 - start_idx = get_input_start_idx(i); + start_idx = obj.get_input_start_idx(i); % delta_f input limits Lb(start_idx+1) = obj.delta_lims(1) - obj.U_ref(obj.ref_idx+i, 1); @@ -309,7 +309,7 @@ classdef MPC_Class % Construct constraint for each state for i = 0:obj.npredstates-1 % Get index of current state in decision variable - idx = get_start_idx(i); + idx = obj.get_state_start_idx(i); Y = Z(idx+1:idx+obj.nstates); % Get xy position from state vector @@ -337,7 +337,7 @@ classdef MPC_Class if ~in_track % Position not inside track - c(i) = 1; % c(Z_err) > 0, nonlinear inequality constraint violated + c(i+1) = 1; % c(Z_err) > 0, nonlinear inequality constraint violated % Skip to next constraint continue; @@ -345,22 +345,22 @@ classdef MPC_Class for j = 1:size(obj.Xobs_seen,2) % Check if position is in or on each obstacle - xv_obstacle = obj.Xobs_seen{i}(:,1); - yv_obstacle = obj.Xobs_seen{i}(:,2); + xv_obstacle = obj.Xobs_seen{j}(:,1); + yv_obstacle = obj.Xobs_seen{j}(:,2); [in_obstacle, on_obstacle] = inpolygon(p(1), p(2), xv_obstacle, yv_obstacle); if in_obstacle || on_obstacle % Point in or on obstacle - c(i) = 1; % c(Z_err) > 0, nonlinear inequality constraint violated + c(i+1) = 1; % c(Z_err) > 0, nonlinear inequality constraint violated % Skip remaining obstacle checking break; end end - if isnan(c(i)) + if isnan(c(i+1)) % If value not set, no constraints violated - c(i) = -1; % c(Z_err) <= 0, nonlinear inequality constraint satisfied + c(i+1) = -1; % c(Z_err) <= 0, nonlinear inequality constraint satisfied end end end @@ -368,79 +368,15 @@ classdef MPC_Class %% Private Kinematic Models methods (Access = private) - function dYdt = nonlinear_bike_model(obj, Y, U) - %nonlinear_bike_model Computes the derivative of the states - % based on current states and inputs using the full - % nonlinear bike model. - - % NOTE: I don't think this function is currently being used at all - - [~,u,~,v,psi,r,delta_f,F_x,F_yf,F_yr] = bike_model_helper(Y, U); - - % Vehicle Dynamics (Equation 1) - dx = u*cos(psi) - v*sin(psi); - du = (1/obj.m)*(-obj.f*obj.m*obj.g + obj.Nw*F_x - F_yf*sin(delta_f)) + v*r; - dy = u*sin(psi) + v*cos(psi); - dv = (1/obj.m)*(F_yf*cos(delta_f) + F_yr) - u*r; - dpsi = r; - dr = (1/obj.Iz)*(obj.a*F_yf*cos(delta_f) - obj.b*F_yr); - dYdt = [dx; du; dy; dv; dpsi; dr]; - end - function [A, B] = linearized_bike_model(obj) %linearized_bike_model Computes the discrete-time LTV % system matrices of the nonlinear bike model linearized % around the reference trajectory starting at the index % closest to `Y_curr` - syms x_var u_var y_var v_var psi_var r_var ... % states - delta_f_var F_x_var ... % inputs - F_yf_var F_yr_var % lateral forces - - % Vehicle Dynamics (Equation 1) - dx = u_var*cos(psi_var) - v_var*sin(psi_var); - du = (1/obj.m)*(-obj.f*obj.m*obj.g + obj.Nw*F_x_var - F_yf_var*sin(delta_f_var)) + v_var*r_var; - dy = u_var*sin(psi_var) + v_var*cos(psi_var); - dv = (1/obj.m)*(F_yf_var*cos(delta_f_var) + F_yr_var) - u_var*r_var; - dpsi = r_var; - dr = (1/obj.Iz)*(obj.a*F_yf_var*cos(delta_f_var) - obj.b*F_yr_var); - - % Jacobians of continuous-time linearized system - % TODO: Change A_c & B_c computation to regular functions (e.g., - % using `matlabFunction`) if speed is an issue - A_c_symb = [ ... - diff(dx, x_var), diff(dx, u_var), diff(dx, y_var), diff(dx, v_var), diff(dx, psi_var), diff(dx, r_var); - diff(du, x_var), diff(du, u_var), diff(du, y_var), diff(du, v_var), diff(du, psi_var), diff(du, r_var); - diff(dy, x_var), diff(dy, u_var), diff(dy, y_var), diff(dy, v_var), diff(dy, psi_var), diff(dy, r_var); - diff(dv, x_var), diff(dv, u_var), diff(dv, y_var), diff(dv, v_var), diff(dv, psi_var), diff(dv, r_var); - diff(dpsi,x_var), diff(dpsi,u_var), diff(dpsi,y_var), diff(dpsi,v_var), diff(dpsi,psi_var), diff(dpsi,r_var); - diff(dr, x_var), diff(dr, u_var), diff(dr, y_var), diff(dr, v_var), diff(dr, psi_var), diff(dr, r_var); - ]; - - B_c_symb = [ ... - diff(dx, delta_f_var), diff(dx, F_x_var); - diff(du, delta_f_var), diff(du, F_x_var); - diff(dy, delta_f_var), diff(dy, F_x_var); - diff(dv, delta_f_var), diff(dv, F_x_var); - diff(dpsi,delta_f_var), diff(dpsi,F_x_var); - diff(dr, delta_f_var), diff(dr, F_x_var); - ]; - - % Substitute values from reference trajectory into symbolic Jacobians - 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], ... - obj.bike_model_helper(obj.Y_ref(obj.ref_idx+i,:), obj.U_ref(obj.ref_idx+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], ... - obj.bike_model_helper(obj.Y_ref(obj.ref_idx+i,:), obj.U_ref(obj.ref_idx+i,:)) ... - ) ... - ); + % Continuous-time system + A_c = @(i) obj.A_c_func(i); + B_c = @(i) obj.B_c_func(i); % Discrete-time LTV system A = @(i) eye(obj.nstates) + obj.T_s*A_c(i); @@ -494,6 +430,56 @@ classdef MPC_Class delta_f = obj.clamp(delta_f, obj.delta_lims(1), obj.delta_lims(2)); F_x = obj.clamp(F_x, obj.F_x_lims(1), obj.F_x_lims(2)); end + + function A_c = A_c_func(obj, i) + %A_c Computes the continuous time `A` matrix + + % This function was generated by the Symbolic Math Toolbox version 9.0. + % 12-Dec-2021 15:05:59 + + [~,u_var,~,v_var,psi_var,r_var,~,~,~,~] ... + = obj.bike_model_helper( ... + obj.Y_ref(obj.ref_idx+i,:), ... + obj.U_ref(obj.ref_idx+i,:) ... + ); + + t2 = cos(psi_var); + t3 = sin(psi_var); + A_c = reshape([ ... + 0.0,0.0,0.0,0.0,0.0,0.0, ... + t2,0.0,t3,-r_var,0.0,0.0, ... + 0.0,0.0,0.0,0.0,0.0,0.0, ... + -t3,r_var,t2,0.0,0.0,0.0, ... + -t3.*u_var-t2.*v_var,0.0,t2.*u_var-t3.*v_var,0.0,0.0,0.0, ... + 0.0,v_var,0.0,-u_var,1.0,0.0 ... + ], ... + [6,6] ... + ); + end + + function B_c = B_c_func(obj, i) + %B_c_func Computes the continuous time `B` matrix + + % This function was generated by the Symbolic Math Toolbox version 9.0. + % 12-Dec-2021 15:05:59 + [~,~,~,~,~,~,delta_f_var,~,F_yf_var,~] ... + = obj.bike_model_helper( ... + obj.Y_ref(obj.ref_idx+i,:), ... + obj.U_ref(obj.ref_idx+i,:) ... + ); + + t2 = sin(delta_f_var); + B_c = reshape([ ... + 0.0,F_yf_var.*cos(delta_f_var).*(-7.142857142857143e-4), ... + 0.0,F_yf_var.*t2.*(-7.142857142857143e-4), ... + 0.0,F_yf_var.*t2.*(-5.061867266591676e-4), ... + 0.0,1.0./7.0e+2, ... + 0.0,0.0, ... + 0.0,0.0 ... + ], ... + [6,2] ... + ); + end end %% Private Helper Functions @@ -540,13 +526,13 @@ classdef MPC_Class Z_ref = zeros(obj.ndec, 1); for i = 0:obj.npredstates-1 - start_idx = get_state_start_idx(i); + start_idx = obj.get_state_start_idx(i); Z_ref(start_idx+1:start_idx+obj.nstates) ... = obj.Y_ref(obj.ref_idx+i, :); end for i = 0:obj.npredinputs-1 - start_idx = get_input_start_idx(i); + start_idx = obj.get_input_start_idx(i); Z_ref(start_idx+1:start_idx+obj.ninputs) ... = obj.U_ref(obj.ref_idx+i, :); end diff --git a/Experimentation/miscellaneous_stuff.m b/Experimentation/miscellaneous_stuff.m new file mode 100644 index 0000000..f75bf6e --- /dev/null +++ b/Experimentation/miscellaneous_stuff.m @@ -0,0 +1,68 @@ +%% Part 1 Testing Xenia's Code +close all; +figure(1) +hold on +plot(Y_ref(1,:), Y_ref(3,:), 'g-') +plot(Y(1,:), Y(3,:), 'b-') + +[Y_test, T_test] = forwardIntegrateControlInput(U', init); +traj_info = getTrajectoryInfo(Y_test(1:439,:), U(:,1:439)') +plot(Y_test(:,1), Y_test(:,3), 'k-') + +plot(TestTrack.bl(1,:), TestTrack.bl(2,:), 'r.') +plot(TestTrack.br(1,:), TestTrack.br(2,:), 'r.') + +%% Part 2 Symbolic Jacobians +close all; +clear all; +clc; + +Nw = 2; +f = 0.01; +Iz = 2667; +a = 1.35; +b = 1.45; +By = 0.27; +Cy = 1.2; +Dy = 0.7; +Ey = -1.6; +Shy = 0; +Svy = 0; +m = 1400; +g = 9.806; + +syms x_var u_var y_var v_var psi_var r_var ... % states + delta_f_var F_x_var ... % inputs + F_yf_var F_yr_var % lateral forces + +% Vehicle Dynamics (Equation 1) +dx = u_var*cos(psi_var) - v_var*sin(psi_var); +du = (1/m)*(-f*m*g + Nw*F_x_var - F_yf_var*sin(delta_f_var)) + v_var*r_var; +dy = u_var*sin(psi_var) + v_var*cos(psi_var); +dv = (1/m)*(F_yf_var*cos(delta_f_var) + F_yr_var) - u_var*r_var; +dpsi = r_var; +dr = (1/Iz)*(a*F_yf_var*cos(delta_f_var) - b*F_yr_var); + +% Jacobians of continuous-time linearized system +% TODO: Change A_c & B_c computation to regular functions (e.g., +% using `matlabFunction`) if speed is an issue +A_c_symb = [ ... + diff(dx, x_var), diff(dx, u_var), diff(dx, y_var), diff(dx, v_var), diff(dx, psi_var), diff(dx, r_var); + diff(du, x_var), diff(du, u_var), diff(du, y_var), diff(du, v_var), diff(du, psi_var), diff(du, r_var); + diff(dy, x_var), diff(dy, u_var), diff(dy, y_var), diff(dy, v_var), diff(dy, psi_var), diff(dy, r_var); + diff(dv, x_var), diff(dv, u_var), diff(dv, y_var), diff(dv, v_var), diff(dv, psi_var), diff(dv, r_var); + diff(dpsi,x_var), diff(dpsi,u_var), diff(dpsi,y_var), diff(dpsi,v_var), diff(dpsi,psi_var), diff(dpsi,r_var); + diff(dr, x_var), diff(dr, u_var), diff(dr, y_var), diff(dr, v_var), diff(dr, psi_var), diff(dr, r_var); +]; + +B_c_symb = [ ... + diff(dx, delta_f_var), diff(dx, F_x_var); + diff(du, delta_f_var), diff(du, F_x_var); + diff(dy, delta_f_var), diff(dy, F_x_var); + diff(dv, delta_f_var), diff(dv, F_x_var); + diff(dpsi,delta_f_var), diff(dpsi,F_x_var); + diff(dr, delta_f_var), diff(dr, F_x_var); +]; + +A_c = matlabFunction(A_c_symb, 'File', 'A_c_func'); +B_c = matlabFunction(B_c_symb, 'File', 'B_c_func'); \ No newline at end of file diff --git a/Experimentation/part2_test_controller.m b/Experimentation/part2_test_controller.m new file mode 100644 index 0000000..fd45817 --- /dev/null +++ b/Experimentation/part2_test_controller.m @@ -0,0 +1,21 @@ +%% Close Figures, Clear Workspace, and Clear Terminal +close all; +clear; +clc; + +%% Call simulation functions +[Y,U,t_total,t_update] = forwardIntegrate(); +info = getTrajectoryInfo(Y,U) + +%% Figures +% Plot segmented trajectory for debugging purposes +figure(1) +hold on; +grid on; + +plot(Y(:,1), Y(:,3), '-'); + +load('TestTrack.mat') +plot(TestTrack.bl(1,:), TestTrack.bl(2,:), '--r'); +plot(TestTrack.br(1,:), TestTrack.br(2,:), '--r'); +plot(TestTrack.cline(1,:), TestTrack.cline(2,:), '-.g'); \ No newline at end of file