root/applications/dual/SIDP/jednoduchy system/generate_realization.m @ 937

function cost=generate_realization(start, candidate, H,u,sigma,yr)

state=start;
best_control=candidate;
cost=0;
num_of_point=(size(H,2))^(1/3);

for i=1:size(H,1)
    
    %aplication of best_control
    next_state(1)=state(1)+(state(2)+state(3)*randn)*best_control+sigma*randn;
    
    %kalman
    K=best_control*state(3)/(state(3)*best_control^2+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);
    
    cost=cost+(state(1)-yr(i))^2;
    
    if i<size(H,1)
        state=next_state;
        
        %find in H
        index=0;
        for j=1:size(H,3)
            index=index+(max(min(round((state(j)-H(i,1,j))/(H(i,size(H,2),j)-H(i,1,j))*num_of_point),num_of_point),1)- j>1)^num_of_point^(j-1);
        end
        
        best_control=u(i,index);
    end
end

end