root/applications/dual/SIDP/SIDPS/motor_sidps/dualni_rizeni.asv @ 1327

function [ztrata omega]=dualni_rizeni(system,apriori)
kresli=0;
H_tilde=load('H_tilde.txt');
C=load('C.txt');    

horizont=system.horizont_rizeni;
ztrata=0;
H=apriori.H;
x_hat=H(:,1);

system.S=zeros(1,3);
for i=1:horizont
    [u system]=control(system,x_hat);
    u=u+interpol(H,H_tilde,C);
    system.x=get_next(system,system.x,u);
    H=kalman_filter(H,u,system);
    x_hat=H(:,1);
    omega(1,i)=system.x(3);
    omega(2,i)=x_hat(3);
    theta(1,i)=system.x(4);
    theta(2,i)=x_hat(4);
    ztrata= ztrata+u'*system.gamma*u+(system.x-system.x_opt)'*system.ksi*(system.x-system.x_opt);
    neurcitost(i)=H(4,5);
    zas(:,i)=u;
end
if (kresli)
figure
plot(zas(1,:)); hold on
plot(zas(2,:)); hold off
figure
plot(omega(1,:),'-'); hold on
plot(omega(2,:),'.-.'); hold off
legend('SKUTECNE','ODHAD');
figure
plot(theta(1,:),'-'); hold on
plot(theta(2,:),'.-.'); hold off
legend('SKUTECNE','ODHAD');
figure
plot(neurcitost)
ztrata
end

end