Bunch of Fixes & Add Some Testing Scripts

- Fix missing references to `obj`
- Fix `npredinputs` so it matches `npredstates` as specified by
  `forwardIntegrate.m`
- Fix typo: `get_start_idx` -> `obj.get_state_start_idx`
- Fix nonlinear constraint indexing issue
- Fix nonlinear constraint obstacle indexing typo
- Remove unused `nonlinear_bike_model` function
- Compute continuous system matrices offline and convert
  to function with `matlabFunction`: `A_c_func` & `B_c_func`
- Add `miscellaneous_stuff.m` which has some random testing
  and continuous system matrix generation code
- Add `part2_test_controller.m` for testing part2 deliverable
This commit is contained in:
Sravan Balaji
2021-12-12 15:27:37 -05:00
parent 6361cfd40c
commit 0812db21ef
3 changed files with 156 additions and 81 deletions

View File

@@ -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

View File

@@ -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');

View File

@@ -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');