Skip to content
Snippets Groups Projects
Commit 3bef2ca0 authored by romanomatthew23's avatar romanomatthew23
Browse files

Put everything together. System is stable. Adjusted control limit for better... 

Put everything together. System is stable. Adjusted control limit for better performance. Not following reference commands currently.
parent ceea03b0
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Our Model/main.m
+ 113
− 4
View file @ 3bef2ca0
......@@ -63,21 +63,130 @@ P = blkdiag(Pxe, zeros(nu + nr));
%Constraints
lN = inf; %Large number (sometimes infinity doesn't work)
tMax = 0.2; %Maximum torque [units]
tMax = 10;%0.2; %Maximum torque [units]
%State Constraints
xlim.max = [lN, lN, lN, tMax, lN, lN, lN];
xlim.max = [lN, lN, lN, lN, lN, tMax, lN];
xlim.min = -xlim.max;
%Input constraints (TODO: review why this is just infinity and not tMax)
ulim.max = lN;
ulim.min = -ulim.max;
%Horizon
%% 4) Simulation of Closed Loop Response
% Horizon
N = 10;
[H, L, G, W, T, IMPC] = formQPMatrices(A, B, Q, R, P, xlim, ulim, N)
% QP Matrices
[H, L, G, W, T, IMPC] = formQPMatrices(A, B, Q, R, P, xlim, ulim, N);
% Augmented State Initial Conditions
x0 = [0; 0; 0; 0; 0; 0; 0];
u0 = 0;
% Trajectory Variables
TT = [];
XX = [];
UU = [];
XXX = [];
% ode45 incrementing constant
h = Ts/10;
% Augmented State and Input Variables
x = x0; % full "augmented" state [dx1; dx2; dx3; dx4; e; u; x3]
u = u0; % input, actual (not delta)
% Actual Initial State
x1 = 0; % keep track of x1 for ode45 stuff
x2 = 0; % keep track of x2 for ode45 stuff
x3 = 0; % keep track of x3 for ode45 stuff
x4 = 0; % keep track of x4 for ode45 stuff
% Lagrange Multipliers (lambdas)
lam = ones(size(G,1),1);
% Reference Step Value
Rad = 80 / 1000; % Radius of Wheel (was overwritten before)
step = 10 / Rad;
for t=0:Ts:10
% Generate a Delta Control Input
q = L*x;
b = W + T*x;
[ U, lam ] = myQP(H, q, G, b, lam);
delta_u = IMPC*U;
% Compute the Actual Control Input from Past and Delta
u = x(6) + delta_u;
% Deadband on Torque input
% if (abs(u) < 0.01)
% u = 0;
% end
% Simulate forward in time using ode45 and cont. time dynamics
U_ = ones(10,1)*u';
[Ti,X] = ode45( @(t,x) mip_model_fast(x,u) ,t+h:h:t+Ts,[x1;x2;x3;x4]);
% Capture This Step in our Trajectory Variables
TT = [TT; Ti];
XX = [XX; X];
UU = [UU; U_];
% Update the Augmented State
x = [X(end,1) - x1; % delta x1
X(end,2) - x2; % delta x2
X(end,3) - x3; % delta x3
X(end,4) - x4; % delta x4
x(5) + X(end,3) - x3; % new error
u; % new u
X(end,3)]; % new x3
% Capture the Aug State in a Traj Variable
XXX = [XXX, x*ones(1,10)];
% give step command
if(t == 5)
x(5) = x(5) - step;
end
% Update actual State Variables
x1 = X(end,1);
x2 = X(end,2);
x3 = X(end,3);
x4 = X(end,4);
% if we fell down, end the simulation
if (abs(x3) > pi/6)
break;
end
end
%% 5) Plot
figure();
plot(TT,XX(:,3));
hold on;
plot(TT,XX(:,4));
% plot([0 5],[0 0],'k--');
% plot([5 5],[0 step],'k--');
% plot([5 10],[step step],'k--');
% plot([0 10],[0.2 0.2],'r--');
% plot([0 10],[-0.2 -0.2],'r--');
hold off;
xlabel('Time (s)');
legend('\theta','\theta dot');
%%
figure();
plot(TT,XXX(5,:));
xlabel('Time (s)');
title('Error');
%% 6) Animate
% Segway_anim(t,phi,theta); phi is pend angle, theta is wheel angle
Segway_anim(TT,XX(:,3),XX(:,1),0.1);
Our Model/mip_simulate.asv deleted 100644 → 0
+ 0
− 52
View file @ ceea03b0
% mip_simulate
%% Closed Loop Simulation of MPC Controller (adapted from H3.2 h) )
N = 15;
%[H,L,G,W,T,IMPC] = formQPMatrices(A,B,Q,R,P,xlim,ulim,N);
x0 = [0; 0; 0; 0; 0; 0; 0];
u0 = 0;
TT = [];
XX = [];
UU = [];
h = Ts/10;
x = x0; % full "augmented" state [dx1; dx2; dx3; dx4; e; u; x3]
u = u0; % input, actual (not delta)
x1 = 0; % keep track of x1 for ode45 stuff
x2 = 0; % keep track of x2 for ode45 stuff
x3 = 0; % keep track of x2 for ode45 stuff
x4 = 0; % keep track of x2 for ode45 stuff
lam = ones(180,1); % 1s
step = -0.25;
for t=0:Ts:10
% Generate a Control Input
% q = L*x;
% b = W + T*x;
% [ U, lam ] = myQP(H, q, G, b, lam);
% delta_u = IMPC*U;
% u = x(4) + delta_u; %compute actual u from past and delta
u = 0.1;
% Simulate forward in time using ode45 and cont. time dynamics
U_ = ones(10,1)*u';
[Ti,X] = ode45( @(t,x) mip_model(x,u) ,t+h:h:t+Ts,[x1;x2;x3;x4]);
TT = [TT; Ti];
XX = [XX; X]; %
UU = [UU; U_]; %
x = [X(end,1) - x1; % delta x1
X(end,2) - x2; % delta x2
X(end,3) - x3; % delta x3
X(end,4) - x4; % delta x4
x(5) + X(end,3) - x3; % new error
u; % new u
X(end,3)]; % new x1
% give step command
if(t == 5)
x(3) = x(3) - step;
end
x1 = X(end,1);
x2 = X(end,2);
end
Our Model/mip_simulate.m deleted 100644 → 0
+ 0
− 90
View file @ ceea03b0
% mip_simulate
%% Closed Loop Simulation of MPC Controller (adapted from H3.2 h) )
N = 15;
Ts = 0.01;
%[H,L,G,W,T,IMPC] = formQPMatrices(A,B,Q,R,P,xlim,ulim,N);
x0 = [0; 0; 0; 0; 0; 0; 0];
u0 = 0;
TT = [];
XX = [];
UU = [];
h = Ts/10;
x = x0; % full "augmented" state [dx1; dx2; dx3; dx4; e; u; x3]
u = u0; % input, actual (not delta)
% Initial Actual State
x1 = 0; % keep track of x1 for ode45 stuff
x2 = 0; % keep track of x2 for ode45 stuff
x3 = 0.001; % keep track of x3 for ode45 stuff
x4 = 0; % keep track of x4 for ode45 stuff
lam = ones(180,1); % 1s
step = -0.25;
for t=0:Ts:10
% Generate a Control Input
% q = L*x;
% b = W + T*x;
% [ U, lam ] = myQP(H, q, G, b, lam);
% delta_u = IMPC*U;
% u = x(4) + delta_u; %compute actual u from past and delta
% if (t < 0.25)
% u = 0.05;
% else
% u = -1;
% end
u = 0;
% Deadband on Torque input
if (abs(u) < 0.01)
u = 0;
end
% Simulate forward in time using ode45 and cont. time dynamics
U_ = ones(10,1)*u';
[Ti,X] = ode45( @(t,x) mip_model_fast(x,u) ,t+h:h:t+Ts,[x1;x2;x3;x4]);
TT = [TT; Ti];
XX = [XX; X]; %
UU = [UU; U_]; %
x = [X(end,1) - x1; % delta x1
X(end,2) - x2; % delta x2
X(end,3) - x3; % delta x3
X(end,4) - x4; % delta x4
x(5) + X(end,3) - x3; % new error
u; % new u
X(end,3)]; % new x3
% give step command
if(t == 5)
x(5) = x(5) - step;
end
x1 = X(end,1);
x2 = X(end,2);
x3 = X(end,3);
x4 = X(end,4);
% if we fell down, end the simulation
if (abs(x3) > pi/6)
break;
end
end
%% Plot
figure();
plot(TT,XX(:,3));
hold on;
plot(TT,XX(:,4));
% plot([0 5],[0 0],'k--');
% plot([5 5],[0 step],'k--');
% plot([5 10],[step step],'k--');
% plot([0 10],[0.2 0.2],'r--');
% plot([0 10],[-0.2 -0.2],'r--');
hold off;
%title(['States of MSD vs. Time r = ' num2str(step)]);
xlabel('Time (s)');
legend('X(1)','X(2)');
%% Animate
% Segway_anim(t,phi,theta); phi is pend angle, theta is wheel angle
% theta = (y(:,1)/R);% - y(:,2);% + (pi/2);
Segway_anim(TT,XX(:,3),XX(:,1),0.05);
Our Model/myQP.m 0 → 100644
+ 33
− 0
View file @ 3bef2ca0
function [ U, lam ] = myQP(H, q, A, b, lam0)
%myQP Quadratic Programming Solver
% Solves the problem
% min (1/2) U'*H*U + q'*U subject to A*U <= b
% lam0 is the guess for the vector of dual variables
% Directly from Module 3 Slide 36
invH = inv(H); A_invH = A*invH; % see Note 1
Hd = A_invH*A'; % see Note 1
qd = A_invH*q + b;
Nit = 30;
lam = lam0;
L = norm(Hd);
k = 1;
df = Hd*lam + qd;
while k <= Nit, % see Note 2
lam = max(lam - 1/L*df,0);
df = Hd*lam + qd;
k = k + 1;
end
U = -invH*(A'*lam + q);
return
% Note 1: Can pre-compute these quantities and pass as arguments to myQP.
% In LQ-MPC setting, only q and b depend on x_0, makes no sense to
% constantly re-compute these
% Note 2: Change to another criterion as desired
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment