[862] | 1 | function H=sidp(sidp_parameters, rsss_parameters,system,apriori)
|
---|
| 2 |
|
---|
| 3 | %pomocne promenne a konstanty
|
---|
| 4 | step=[1 sidp_parameters.n_grid sidp_parameters.n_grid^2];
|
---|
| 5 | rozsah_min=[1;0;0];
|
---|
| 6 | rozsah_max=[sidp_parameters.n_grid; sidp_parameters.n_grid-1; sidp_parameters.n_grid-1];
|
---|
| 7 | stav1=zeros(3,1);
|
---|
| 8 | sigma_na_druhou=system.sigma^2;
|
---|
| 9 |
|
---|
| 10 | zero=zeros(sidp_parameters.horizont,3); %zero(1,:) nema smysl, ale takhle je to pekne
|
---|
| 11 | range=zeros(sidp_parameters.horizont,3);
|
---|
| 12 | eigvec=zeros(sidp_parameters.horizont,3,3);
|
---|
| 13 |
|
---|
| 14 | [H,u]=init_hyperstate(sidp_parameters,system,apriori); %vytvori H a u, nastavi H0 a u*
|
---|
| 15 |
|
---|
| 16 | for i=1:sidp_parameters.n_pass
|
---|
| 17 | i
|
---|
| 18 | for j=1:sidp_parameters.n_iter
|
---|
| 19 | j
|
---|
| 20 | search_region=sidp_parameters.gama^(j-1)*sidp_parameters.lambda^(i-1)*sidp_parameters.search_region_init;
|
---|
| 21 | [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*
|
---|
| 22 |
|
---|
| 23 | for k=sidp_parameters.horizont:-1:1 %pozor na meze
|
---|
| 24 | level=sidp_parameters.horizont+1-k;
|
---|
| 25 | k
|
---|
| 26 |
|
---|
| 27 | for l=1:size(H,2)
|
---|
| 28 |
|
---|
| 29 | %if mod(l,1000)==0
|
---|
| 30 | % l
|
---|
| 31 | %end
|
---|
| 32 |
|
---|
| 33 | candidates=generate_candidates(u(k,l), search_region, sidp_parameters.num_of_candidates,sidp_parameters.generate_candidates_mode);
|
---|
| 34 |
|
---|
| 35 | %compare candidates
|
---|
| 36 |
|
---|
| 37 | %simple comparing
|
---|
| 38 | realization1=zeros(size(candidates,1),rsss_parameters.n0);
|
---|
| 39 | for m=1:sidp_parameters.num_of_candidates
|
---|
| 40 | for n=1:rsss_parameters.n0
|
---|
| 41 |
|
---|
| 42 | %generate realization
|
---|
| 43 | state=H(k,l,:);
|
---|
| 44 | best_control=candidates(m);
|
---|
| 45 |
|
---|
| 46 | for o=1:level
|
---|
| 47 |
|
---|
| 48 | %aplication of best_control
|
---|
| 49 | %next_state(1)=state(1)+(state(2)+state(3)*randn)*best_control+system.sigma*randn;
|
---|
| 50 | next_state(1)=state(1)+state(2)*best_control+system.sigma*randn;
|
---|
| 51 |
|
---|
| 52 | realization1(m,n)=realization1(m,n)+(next_state(1)-system.yr(k+o))^2;
|
---|
| 53 |
|
---|
| 54 | if o<level
|
---|
| 55 | %kalman
|
---|
| 56 | K=best_control*state(3)/(state(3)*best_control^2+sigma_na_druhou);
|
---|
| 57 | next_state(2)=state(2)+K*(next_state(1)-state(1)-state(2)*best_control);
|
---|
| 58 | next_state(3)=(1-K*best_control)*state(3);
|
---|
| 59 |
|
---|
| 60 | state(1,1,:)=next_state;
|
---|
| 61 |
|
---|
| 62 | %find in H
|
---|
| 63 | stav(1)=state(1,1,1)-zero(k+o,1);
|
---|
| 64 | stav(2)=state(1,1,2)-zero(k+o,2);
|
---|
| 65 | stav(3)=state(1,1,3)-zero(k+o,3);
|
---|
| 66 | %range2(:,1)=range(k+o,1,:);%fuj
|
---|
| 67 | for p=1:system.dim
|
---|
| 68 | 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);
|
---|
| 69 | end
|
---|
| 70 | index=step*max(min(round(stav1*(sidp_parameters.n_grid-1))+rozsah_min,rozsah_max),rozsah_min);
|
---|
| 71 |
|
---|
| 72 | % index= find_in_hyperstate5(state,H(k+o,:,:));
|
---|
| 73 | %if index== find_in_hyperstate5(state,H(k+o,:,:))
|
---|
| 74 | %else
|
---|
| 75 | % 1;
|
---|
| 76 | %end
|
---|
| 77 |
|
---|
| 78 | best_control=u(k+o,index);
|
---|
| 79 | end
|
---|
| 80 |
|
---|
| 81 | end
|
---|
| 82 |
|
---|
| 83 | end
|
---|
| 84 | end
|
---|
| 85 | mean_values=mean(realization1,2);
|
---|
| 86 | [min_val min_index]=min(mean_values);
|
---|
| 87 | best_control=candidates(min_index);
|
---|
| 88 | u(k,l)=best_control;
|
---|
| 89 | end
|
---|
| 90 | end
|
---|
| 91 |
|
---|
| 92 | vypis(:,:)=H(:,:,1);
|
---|
| 93 | save 'y.txt' vypis -ASCII;
|
---|
| 94 | vypis(:,:)=H(:,:,2);
|
---|
| 95 | save 'b.txt' vypis -ASCII;
|
---|
| 96 | vypis(:,:)=H(:,:,3);
|
---|
| 97 | save 'P.txt' vypis -ASCII;
|
---|
| 98 | save 'u.txt' u -ASCII;
|
---|
| 99 |
|
---|
| 100 | end
|
---|
| 101 |
|
---|
| 102 | end
|
---|
| 103 |
|
---|
| 104 | end
|
---|