sravan/Control-Project
1
0
Fork 0
mirror of https://github.com/ROB-535-F21-Team-3/Control-Project.git synced 2025-08-23 10:52:45 +00:00
Code Issues Packages Projects Releases Wiki Activity

MPC kinda works but fails to avoid boundaries well

Browse Source
This commit is contained in:
xdemenchuk
2021-12-09 18:00:14 -05:00
parent f27a3b82e3
commit 4165d10a72
1 changed files with 79 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

85
SimPkg_F21(student_ver)/xenia_nonlinearopt.m
View File

@@ -129,18 +129,18 @@ for i=1:length(T)-1
[Aeq,beq]=eq_cons(i,A,B,eY(:,i),npred_i,nstates,ninputs);
%generate boundary constraints
[Lb,Ub]=bound_cons(i,U_ref,npred_i,input_range,nstates,ninputs, Y(:,i), Y_ref(:,i));
[Lb,Ub]=bound_cons(i,U_ref,npred_i,input_range,nstates,ninputs,Y(:,i), Y_ref(:,i));
%cost for states
Q=[100,1,100,1,50,1];
Q=[5,1,5,0.1,1,1];
%cost for inputs
R=[100,1];
R=[0.1,0.01];
H=diag([repmat(Q,[1,npred_i+1]),repmat(R,[1,npred_i])]);
f=zeros(nstates*(npred_i+1)+ninputs*npred_i,1);
options = optimoptions('quadprog','Display','iter');
[x,fval] = quadprog(H,f,[],[],Aeq,beq,Lb,Ub,[],options);
%get linearized input
@@ -207,6 +207,63 @@ function x0vec = initvec(x0, u0)
end
%% mpc functions
% function [Aineq, bineq] =ineq_cons(initial_idx, x_initial, npred, nstates, ninputs, Q, R, currpos)
% n = size(x_initial,1);
% m = size(R,1);
%
% Y_curxy = [currpos(1)+x_initial(1); currpos(3)+x_initial(3)];
% sqdist_to_cl = (TestTrack.cline - Y_curxy).^2;
% dist_to_cl = (sqdist_to_cl(1,:) + sqdist_to_cl(2,:)).^0.5;
% [minDist, indMin] = min(dist_to_cl);
%
% prev_idx = max(indMin-1, 1);
% next_idx = min(indMin, 246);
%
% bound_X = [TestTrack.bl(1,prev_idx), TestTrack.bl(1,next_idx), ...
% TestTrack.br(1,prev_idx), TestTrack.br(1,next_idx)];
% bound_Y = [TestTrack.bl(2,prev_idx), TestTrack.bl(2,next_idx), ...
% TestTrack.br(2,prev_idx), TestTrack.br(2,next_idx)];
% %phi_init = TestTrack.theta(i);
%
% lb_x = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf];
% ub_x = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf];
%
% Axcons = [1 0 0 0 0 0;
% -1 0 0 0 0 0;
% 0 1 0 0 0 0;
% 0 -1 0 0 0 0;
% 0 0 1 0 0 0;
% 0 0 -1 0 0 0;
% 0 0 0 1 0 0;
% 0 0 0 -1 0 0;
% 0 0 0 0 1 0;
% 0 0 0 0 -1 0;
% 0 0 0 0 0 1;
% 0 0 0 0 0 -1];
%
% bXcons = [ub_x(1); lb_x(1); ub_x(2); lb_x(2); ub_x(3); lb_x(3); ub_x(4); lb_x(4); ub_x(5); lb_x(5); ub_x(6); lb_x(6)];
%
% % next we define the inequality constraints corresponding to bound
% % constraints we have 2 constraints for each state and input at each time
% % instance
% Aineq = zeros(2 * n * npred + 2 * m * (npred - 1),n * npred + m * (npred - 1 ));
% bineq = zeros(2 * n * npred + 2 * m * (npred - 1),1);
% % state constraints
% for i = 1:npred
% Aineq(((i - 1) * n * 2 + 1):(i * n * 2),((i - 1) * n + 1):(i * n)) = AXcons;
% bineq(((i - 1) * n * 2 + 1):(i * n * 2), 1) = bXcons;
% end
%
%
%
% end
function [Aeq,beq]=eq_cons(initial_idx,A,B,x_initial,npred,nstates,ninputs)
%build matrix for A_i*x_i+B_i*u_i-x_{i+1}=0
%in the form Aeq*z=beq
@@ -264,7 +321,7 @@ function [Lb,Ub]=bound_cons(initial_idx,U_ref,npred,input_range,nstates,ninputs,
%phi_init = TestTrack.theta(i);
lb_x = [min(bound_X)-Y_ref(1); -Inf; min(bound_Y)-Y_ref(3); -Inf; -pi; -Inf];
ub_x = [max(bound_X)-Y_ref(1); Inf; max(bound_Y)-Y_ref(3); Inf; +pi; Inf];
ub_x = abs([max(bound_X)-Y_ref(1); Inf; max(bound_Y)-Y_ref(3); Inf; +pi; Inf]);
Lb=[Lb;lb_x];
@@ -276,6 +333,22 @@ function [Lb,Ub]=bound_cons(initial_idx,U_ref,npred,input_range,nstates,ninputs,
Lb=[Lb;input_range(:,1)-U_ref(:,initial_idx+j-1)];
Ub=[Ub;input_range(:,2)-U_ref(:,initial_idx+j-1)];
end
% xsize=(npred+1)*nstates;
% usize=npred*ninputs;
% Lb = [];
% Ub = [];
% for j=1:ninputs
% Lb=[Lb;input_range(j,1)-U_ref(j,initial_idx:initial_idx+npred-1)];
% Ub=[Ub;input_range(j,2)-U_ref(j,initial_idx:initial_idx+npred-1)];
% end
%
% Lb=reshape(Lb,[usize,1]);
% Ub=reshape(Ub,[usize,1]);
%
% Lb=[-Inf(xsize,1);Lb];
% Ub=[Inf(xsize,1);Ub];
end