root/applications/dual/SIDP/Kopie (2) - finalni implementace/rizeni.m @ 1340

function  [ztrata u y b P]=rizeni(H,ny,system,apriori,seed)

sigma=system.sigma;
N=size(H,1)+2;
%%chce to mzslet na PYB

y=zeros(1,N);
b=zeros(1,N);
P=zeros(1,N);
u=zeros(1,N-1);
ztrata=0;

y(1)=apriori(1);
b(1)=apriori(2);
P(1)=apriori(3);

    for i=1:size(H,1)
        eta=abs(y(i))/sigma;
        beta=abs(b(i))/sqrt(P(i));
        state=[eta beta];
        
        index=find_in_hyperstate5(state, H(i,:,:));
        
        %plot(H(i,:,1),H(i,:,2),'g.'); hold on; plot(H(i,index,1), H(i,index,2),'r+'); plot(state(1), state(2),'b.');  hold off
                
        znamenko=sign(y(i)*((-1)^(b(i)<0)));
        u(i)=-ny(i,index)*sigma/sqrt(P(i))*znamenko;
        
        y(i+1)=y(i)+system.b*u(i)+sigma*seed(i);
        K=u(i)*P(i)/(P(i)*u(i)^2+sigma^2);
        b(i+1)=b(i)+K*(y(i+1)-y(i)-b(i)*u(i));
        P(i+1)=(1-K*u(i))*P(i);   
        
        ztrata=ztrata+(y(i+1)-system.yr(i))^2;
    end
    i=i+1;
    eta=abs(y(i))/sigma;
    beta=abs(b(i))/sqrt(P(i));
    u(i)=-eta*beta/(1+beta^2)*sigma/sqrt(P(i))*sign(y(i)*b(i));
    
    y(i+1)=y(i)+system.b*u(i)+sigma*randn;
    K=u(i)*P(i)/(P(i)*u(i)^2+sigma^2);
    b(i+1)=b(i)+K*(y(i+1)-y(i)-b(i)*u(i));
    P(i+1)=(1-K*u(i))*P(i); 
    ztrata=ztrata+y(i+1)^2;
    
 end