Main.asv

clear all
close all
clc

NSUs = 30;
repeats = 5;
for i = 1:length(NSUs)
    NSU = NSUs(i);
    disp(NSUs(i))
    for j = 1:repeats
        all(j).result = runOPT(NSU);
%         filename = ['Solutions/WithConstraint/NoiseCheck_', num2str(NSUs(i)), 'SUs', num2str(j), '.mat'];
%         save(filename, 'result')
        
    end
end


for j = 1:repeats
    T = result(end).params.T;
    h = T/(result(end).params.NperSU-1);
    for i = 1:length(all(1).result)
        obj(i,j) = result(i).obj;
        K(:,i,j) = result(i).X(end-1:end);
        [uperSU, uall,meanxx] = plotresult(result(i), 0);
        meanu(:,i,j) = mean(uall,3);
        stdu(:,i,j) = std(uall,[],3);
        meanx(i,:,j) = meanxx(1,:);
        rmsu = rms(uall);
        RMS(i,j) = mean(rmsu(:));
   end
end

figure
subplot(2,1,1)
meantrajs = mean(meanx,3);
stdtrajs = std(meanx,[],3);
plot([0:h:T], meantrajs([1 4:end],:)', 'linewidth', 1.5)
xlabel('Time [s]')
ylabel('Arm Angle [rad]')

subplot(2,1,2)
meantorque = mean(meanu,3);
stdtorque = mean(stdu,3);
% plotstds = meantorque-stdtorque;
% plotstds(:,end+1:2*end) = meantorque(:,end:-1:1)+stdtorque(:,end:-1:1);
% xstds = [0:h:T T:-h:0];
plot([0:h:T], meantorque(:,1), 'r', 'linewidth', 1.5)
hold on
errorbar([0:h:T], meantorque(:,end),stdtorque(:,end), 'k', 'linewidth', 1.5)
%legend('\sigma = 0', '\sigma = 1', 'FontSize', 12)
xlabel('Time [s]')
ylabel('Input u_0+ K x  [Nm]')
xlim([0 10])