Homec4science
Diffusion QPFit (master) Browse

QPFit/simStar.m
3bfb25b5e70amaster

Last Change
Raw File

simStar.m
View Options

function sim = simStar(ctrl, x0, R, sys, fig)
n = 3;
m = 1;
nSim = size(R,2);
Ipos = sys.Ipos;
Ivel = sys.Ivel;
Itable = sys.Itable;
X = zeros(2*n, nSim);
U = zeros(2*m, nSim);
Y = zeros(2*n, nSim);
AA = blkdiag(sys.A, sys.A);
BB = blkdiag(sys.B, sys.B);
CC = blkdiag(sys.C, sys.C);
tic
X(:,1) = x0;
for i = 1:nSim
if toc > 2
fprintf('%i of %i\n', i, nSim);
tic
end
U(1,i) = ctrl(X(1:n,i), R(1,i));
U(2,i) = ctrl(X(n+1:end,i), R(2,i));
X(:,i+1) = AA*X(:,i) + BB*U(:,i);
Y(:,i) = CC*X(:,i);
end
fprintf('... done\n');
if fig
figure(fig); clf; grid on; hold on;
plot(Y(Ipos,:), Y(Ipos+n,:), 'b')
plot(R(1,:), R(2,:), 'r');
col = parula(5);
figure(fig+1); clf; grid on; hold on;
toPlot(1).var{1} = Y(Itable,:);
toPlot(1).var{2} = Y(Itable+n,:);
toPlot(1).bnd = sys.maxTableAngle;
toPlot(1).name = 'Table Angle';
toPlot(2).var{1} = Y(Ivel,:);
toPlot(2).var{2} = Y(Ivel+n,:);
toPlot(2).bnd = sys.maxBallVel;
toPlot(2).name = 'Ball Velocity';
toPlot(3).var{1} = Y(Ipos,:);
toPlot(3).var{2} = Y(Ipos+n,:);
toPlot(3).bnd = sys.maxBallPos;
toPlot(3).name = 'Ball Position';
toPlot(4).var{1} = U(1,:);
toPlot(4).var{2} = U(2,:);
toPlot(4).bnd = sys.maxTableAngle;
toPlot(4).name = 'Input';
t = [0:nSim-1]*sys.Ts;
plotBnd = @(bnd, col) patch([0;0;max(t);max(t)], ...
[-bnd;bnd;bnd;-bnd], col, 'linestyle','none', 'facealpha',0.2);
for i = 1:length(toPlot)
subplot(length(toPlot),1,i)
plotBnd(toPlot(i).bnd, col(i,:));
hold on;
for k = 1:length(toPlot(i).var)
stairs(t, double(toPlot(i).var{k}), 'linewidth', 2, 'color', col(i,:));
end
legend(toPlot(i).name);
axis([0,max(t),-toPlot(i).bnd*1.5,toPlot(i).bnd*1.5])
end
end
sim.X = X;
sim.Y = Y;
sim.U = U;
sim.Pos = [Y(Ipos,:); Y(Ipos+n,:)];
err = sim.Pos - R;
sim.MSE = err(:)'*err(:) / size(R,2);
c4science · Help