Homec4science
Diffusion QPFit (master) Browse

QPFit/analyze_double_track.m
3bfb25b5e70amaster

Last Change
Raw File

analyze_double_track.m
View Options

%% Learned model
load data/double_track_fit.mat
if strcmp(type, 'QPNet')
fit.F = double(F);
fit.f = double(f(:));
fit.L = double(L);
% min .5*z'*Q*z + (F*x + f')'*z
% s.t. z >= 0
nVar = size(fit.L,2);
nParam = size(fit.F,2);
fit.Q = fit.L'*fit.L + 1e-3*eye(nVar);
z = sdpvar(nVar,1);
x = sdpvar(nParam,1);
obj = 0.5*z'*fit.Q*z + (fit.F*x+fit.f)'*z;
g = sdpsettings('solver','gurobi+');
g.gurobi.IterationLimit = double(maxIter);
fit.opt= optimizer(z >= 0, obj, g, x, z);
fit.prob = Opt([z >= 0; -ROI <= x <= ROI],obj,x,z);
% fit.prob.bth = ROI * ones(4,1);
% fit.sol = fit.prob.solve();
fit.ctrl = @(x) min(1, max(-1, O*fit.opt(x)+o(:)));
% ul = fit.ctrl(X);
elseif strcmp(type, 'GradNet')
dat.F = F;
dat.f = f;
dat.Q = Q;
dat.O = O;
dat.o = o;
dat.numItr = numItr;
dat.umax = double(umax);
dat.nVar = size(Q,2);
dat.nParam = size(F,2);
fit.ctrl = @(x) gradEval(dat, x(:)');
else
error('Unknown network type')
end
%%
load data/double_track.mat
fU = 0*U;
for i = 1:size(X,2)
fU(:,i) = fit.ctrl(X(:,i));
end
NN = 10000;
figure(1);
clf
plot3(X(1,1:NN),X(2,1:NN),U(:,1:NN),'r.');
hold on
plot3(X(1,1:NN),X(2,1:NN),fU(:,1:NN),'b.');
figure(2);
clf
plot3(X(1,1:NN),X(2,1:NN),U(:,1:NN) - fU(:,1:NN),'r.');
% clf
% for ref = [-5,5]
% I = X(3,:) == ref;
% x = X(1:2,I); u = U(:,I);
% plot3(x(1,:),x(2,:),u,'r.');
% hold on; grid on;
%
% fU = 0*u;
% for i = 1:size(x,2)
% fU(:,i) = fit.ctrl([x(:,i);ref]);
% end
% plot3(x(1,:),x(2,:),fU,'b.');
% end
%%
sys.A = [1 1; 0 1];
sys.B = [1; 0.5];
sys.C = [1 0];
sys.D = 0;
n = 2;
m = 1;
ROI = 5;
clf
maxN = 20;
for i = 1:10
ref = 5*rand*[ones(1,20) -ones(1,20)];
maxN = 40;
x0 = [0;0];
[Y, X, U] = simsys(sys, x0, fit.ctrl, maxN, ref);
plot(Y,'bo-')
hold on
gnd = @(x) [1 zeros(1,9)] * test_mex(x);
[Y, X, U] = simsys(sys, x0, gnd, maxN, ref);
plot(Y,'ro-')
end
c4science · Help