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

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

horizont=system.horizont_rizeni;
if nargin<3
        seed=randn(6,horizont);
end
ztrata=0;
H=apriori.H;
x_hat=H(:,1);

system.S=zeros(1,3);
for i=1:horizont
    [u system]=control(system,x_hat,interpol(H,H_tilde,C,system.H_tilde_on));
    [u system]=control(system,x_hat,-20);
    rizeni(:,i)=u;
    [system.x system.pozorovani]=get_next(system,system.x,u,seed(:,i));
    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(3,4);
    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