root/applications/dual/SIDP/rizeni.m @ 743

function [ztrata control y b P]=rizeni(H,u,system,zac)

ztrata=0;
index=zac;
state=H(1,index,:)

    for i=1:size(H,1)-1
        best_control=u(i,index);
        y(i)=state(1,1,1);
        b(i)=state(1,1,2);
        P(i)=state(1,1,3);
        control(i)=best_control;
        
        next_state(1)=state(1)+system.b*best_control+system.sigma*randn;
        ztrata=ztrata+(next_state(1)-system.yr(i))^2;
        if i<size(H,1)-1
            %kalman
            K=best_control*state(3)/(state(3)*best_control^2+system.sigma^2);
            next_state(2)=state(2)+K*(next_state(1)-state(1)-state(2)*best_control);
            next_state(3)=(1-K*best_control)*state(3);         
            state(1,1,:)=next_state;
            index=find_in_hyperstate5(state,H(i+1,:,:));

            plot3(H(i+1,:,1),H(i+1,:,2),H(i+1,:,3),'g.');
            hold on
            plot3(H(i+1,index,1),H(i+1,index,2),H(i+1,index,3),'r+');
            plot3(state(1,1,1),state(1,1,2),state(1,1,3),'r+');
            hold off
            

        end
    end

        y(size(H,1))=state(1,1,1);
        b(size(H,1))=state(1,1,2);
        P(size(H,1))=state(1,1,3);
        control(size(H,1))=0;
end