root/applications/dual/SIDP/jednoduchy system/sidp.m @ 1043

function H=sidp(sidp_parameters, rsss_parameters,system,apriori)

%pomocne promenne a konstanty
step=[1 sidp_parameters.n_grid sidp_parameters.n_grid^2];
rozsah_min=[1;0;0];
rozsah_max=[sidp_parameters.n_grid; sidp_parameters.n_grid-1; sidp_parameters.n_grid-1];
stav1=zeros(3,1);
sigma_na_druhou=system.sigma^2;

zero=zeros(sidp_parameters.horizont,3); %zero(1,:) nema smysl, ale takhle je to pekne
range=zeros(sidp_parameters.horizont,3);
eigvec=zeros(sidp_parameters.horizont,3,3);

[H,u]=init_hyperstate(sidp_parameters,system,apriori);  %vytvori H a u, nastavi H0 a u*

for i=1:sidp_parameters.n_pass
    i
    for j=1:sidp_parameters.n_iter
        j
        search_region=sidp_parameters.gama^(j-1)*sidp_parameters.lambda^(i-1)*sidp_parameters.search_region_init;
        [H zero range eigvec u]=update_hyperstate(H,zero, range, eigvec,u,sidp_parameters.n_grid,system.sigma,(i+j>2));   %vygeneruje trajektorie, v zasazenem regionu rovnomerne rozmisti body a prekopiruje nalezena u*
      
        for k=sidp_parameters.horizont:-1:1    %pozor na meze
            level=sidp_parameters.horizont+1-k;          
            k
            
            for l=1:size(H,2)  
                
                %if mod(l,1000)==0
                 %   l
                %end
                
                candidates=generate_candidates(u(k,l), search_region, sidp_parameters.num_of_candidates,sidp_parameters.generate_candidates_mode);
                
                %compare candidates
                
                    %simple comparing
                    realization1=zeros(size(candidates,1),rsss_parameters.n0);
                    for m=1:sidp_parameters.num_of_candidates
                        for n=1:rsss_parameters.n0

                            %generate realization      
                            state=H(k,l,:);
                            best_control=candidates(m);
                         
                            for o=1:level

                                %aplication of best_control
                                %next_state(1)=state(1)+(state(2)+state(3)*randn)*best_control+system.sigma*randn;
                                next_state(1)=state(1)+state(2)*best_control+system.sigma*randn;
                                
                                realization1(m,n)=realization1(m,n)+(next_state(1)-system.yr(k+o))^2;

                                if o<level
                                    %kalman
                                    K=best_control*state(3)/(state(3)*best_control^2+sigma_na_druhou);
                                    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;

                                    %find in H      
                                    stav(1)=state(1,1,1)-zero(k+o,1);
                                     stav(2)=state(1,1,2)-zero(k+o,2);
                                      stav(3)=state(1,1,3)-zero(k+o,3);
                                    %range2(:,1)=range(k+o,1,:);%fuj
                                    for p=1:system.dim
                                        stav1(p)=(stav(1)*eigvec(k+o,1,p)+stav(2)*eigvec(k+o,2,p)+stav(3)*eigvec(k+o,3,p))*range(k+o,p);
                                    end
                                    index=step*max(min(round(stav1*(sidp_parameters.n_grid-1))+rozsah_min,rozsah_max),rozsah_min);
                                    
                                     % index= find_in_hyperstate5(state,H(k+o,:,:));
                                     %if index== find_in_hyperstate5(state,H(k+o,:,:))
                                     %else
                                     %    1;
                                     %end
                                 
                                    best_control=u(k+o,index);
                                end

                            end

                        end
                    end
                    mean_values=mean(realization1,2);
                    [min_val min_index]=min(mean_values);
                    best_control=candidates(min_index);
                    u(k,l)=best_control;
            end
        end     
       
        vypis(:,:)=H(:,:,1);
        save 'y.txt' vypis -ASCII;
        vypis(:,:)=H(:,:,2);
        save 'b.txt' vypis -ASCII;
        vypis(:,:)=H(:,:,3);
        save 'P.txt' vypis -ASCII;
        save 'u.txt' u -ASCII;
        
    end
    
end

end