function ildp tic %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %pocatecni konstanty Iterace = 2; %iterace K = 20; %casy N = 50; %vzorky sigma = 1; Sigmas = [[sigma^2 0 0]; [0 0 0]; [0 0 0]]; h = 0; hdx = [0; 0; 0]; hdxdx = [[0 0 0]; [0 0 0]; [0 0 0]]; Rk = ones(1, K); x0 = [0; 0; 1]; %velikost okoli pro lokalni metodu rho = 5; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %globalni promenne Kpi = ones(4, K); Kpi(4, :) = zeros(1, K); Wv = zeros(10, K); Xkn = zeros(3, K, N); Xstripe = zeros(3, K); gka = 0; gnu = 0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %iteracni smycka % clf reset % PtMin = 0.0001; % UU=zeros(K,N); for i = 1:Iterace, %generovani stavu % hold off for n = 1:N, Xkn(:, 1, n) = x0 + [sigma*randn(); 0; 0]; for k = 1:K-1, Uk = uPi(k, Xkn(:, k, n)); % UU(k,n) = Uk; Kk = Uk*Xkn(3, k, n)/(Uk^2*Xkn(3, k, n) + sigma^2); Xkn(1, k+1, n) = Xkn(1, k, n) + Xkn(2, k, n)*Uk + sigma*randn(); Xkn(2, k+1, n) = Xkn(2, k, n) + Kk*(Xkn(1, k+1, n) - Xkn(1, k, n) - Xkn(2, k, n)*Uk); Xkn(3, k+1, n) = (1 - Kk*Uk)*Xkn(3, k, n); end % hold all % subplot(4,1,1); % plot(1:K,Xkn(1,:,n)) % hold all % subplot(4,1,2); % plot(1:K,Xkn(2,:,n)) % hold all % subplot(4,1,3); % plot(1:K,Xkn(3,:,n)) % hold all % subplot(4,1,4); % plot(1:K,UU(:,n)) end Xstripe = mean(Xkn, 3); % hold all % subplot(4,1,1); % plot(1:K,Xstripe(1,:),'-ro') % hold all % subplot(4,1,2); % plot(1:K,Xstripe(2,:),'-ro') % hold all % subplot(4,1,3); % plot(1:K,Xstripe(3,:),'-ro') %hold off % Epsl = randn(3,N); %iterace pro k = K-1..1 for k = K-1:-1:1, gka = k; % 1] for n = 1:N, %krive okoli Xkn(1, k, n) = Xstripe(1, k) + rho*randn(); Xkn(2, k, n) = Xstripe(2, k) + rho*randn(); Xkn(3, k, n) = Xstripe(3, k)*rho*exp(randn()); % %rovne okoli % Xkn(1, k, n) = Xstripe(1, k) + Epsl(1,n); % Xkn(2, k, n) = Xstripe(2, k) + Epsl(2,n); % Xkn(3, k, n) = Xstripe(3, k)*exp(Epsl(3,n)); end % Xkn(:,k,:) % 2] for n = 1:N, gnu = n; [Uopt(n), Hmin(n)] = fminunc(@Hamilt, uPi(k, Xkn(:, k, n)), optimset('GradObj','on','Display','notify')); % [i, k, n] % % interv = -1000:1:1000; % for ll = 1:2001, % hodnot(ll) = Hamilt(interv(ll)); % end % plot(interv, hodnot,'-b',Uopt(n),Hmin(n),'rs',uPi(k, Xkn(:, k, n)),Hamilt(uPi(k, Xkn(:, k, n))),'go') % prah = 100; % if(Uopt(n) > prah) % Uopt(n) = prah; % Hmin(n) = Hamilt(prah); % disp('u > horni mez'); % elseif(Uopt(n) < -prah) % Uopt(n) = -prah; % Hmin(n) = Hamilt(-prah); % disp('u < dolni mez'); % end % if(extfl < 1) % disp('exitflag < 1') % end end % 3] for n = 1:N, % V??? nema to byt k+1? ale asi ne Vn(n) = Hmin(n) + Vtilde(k+1, Xkn(:, k, n)); % xxx(n) = Xkn(1,k,n); % xxy(n) = Xkn(2,k,n); % xxz(n) = Xkn(3,k,n); % Vn(n) = xxx(n).*xxx(n).*xxx(n); end % xxx2 = sort(xxx); % xxy2 = sort(xxy); % xxz2 = sort(xxz); % 4] Epsilon = zeros(3, N); for n = 1:N, Epsilon(:, n) = Xkn(:, k, n) - Xstripe(:, k); end mFi = matrixFi(Epsilon); FiFiTInvFi = (mFi*mFi')\mFi; Wv(:,k) = FiFiTInvFi * Vn'; % Wv = zeros(10,1); % Wv(1, k) = Wvtmp(1); % Wv(2, k) = Wvtmp(2); % Wv(3, k) = Wvtmp(3); % Wv(4, k) = Wvtmp(4); % FiFiTInvFi = (mFi'*mFi)\mFi'; % Wv(:, k) = FiFiTInvFi' * Vn'; % UU(k,:) = Uopt; for n = 1:N, yt(n) = Xkn(1, k, n); bt(n) = Xkn(2, k, n); pt(n) = Xkn(3, k, n); end mPsi = [yt',... bt'.*Uopt',... pt'.*Uopt',... Uopt']; PsiPsiTInvPsi = (mPsi'*mPsi)\mPsi'; Kpi(:,k) = PsiPsiTInvPsi * (Rk(k)*ones(N,1)); % for nn=1:N, % vlv(nn) = Vtilde(k, [xxx2(nn);xxy2(nn);xxz2(nn)]); % end % subplot(3,1,1); % plot(xxx,Vn,'rs',xxx2,vlv,'-b') % subplot(3,1,2); % plot(xxy,Vn,'rs',xxy2,vlv,'-b') % subplot(3,1,3); % plot(xxz,Vn,'rs',xxz2,vlv,'-b') end % clf reset % for n=1:N, % % hold all % subplot(4,1,1); % plot(1:K,Xkn(1,:,n),'-b') % hold all % subplot(4,1,2); % plot(1:K,Xkn(2,:,n),'-b') % hold all % subplot(4,1,3); % plot(1:K,Xkn(3,:,n),'-b') % hold all % % subplot(4,1,4); % % plot(1:K,UU(:,n),'-b') % end end %%%%%%%%%%% toc Kpi %graficky vystup X = zeros(3, K); UU = zeros(1,K); X(:,1) = x0 + [sigma*randn(); 0; 0]; for k = 1:K-1, Upi = uPi(k, X); UU(k) = Upi; Ktmp = Upi*X(3,k)/(Upi^2*X(3,k) + sigma^2); X(1,k+1) = X(1,k)+X(2,k)*Upi + sigma*randn(); X(2,k+1) = X(2,k) + Ktmp*(X(1,k+1) - X(1,k) - X(2,k)*Upi); X(3,k+1) = X(3,k)*(1-Ktmp*Upi); end % X % hold off subplot(4,1,1); plot(1:K,X(1,:),'-gs') % hold off subplot(4,1,2); plot(1:K,X(2,:),'-gs') % hold off subplot(4,1,3); plot(1:K,X(3,:),'-gs') % hold off subplot(4,1,4); plot(1:K,UU,'-gs') % X = zeros(1, K); % b = 0; % UU = zeros(1,K); % % X(1) = sigma*randn(); % for k = 1:K-1, % Upi = uPi(k, [X,b,0]); % UU(k) = Upi; % X(k+1) = X(k) + b*Upi + sigma*randn(); % end % % % hold off % subplot(2,1,1); % plot(1:K,X,'-gs') % % hold off % subplot(2,1,2); % plot(1:K,UU,'-gs') %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %pomocne funkce function [val_uPi] = uPi(k_uPi, x_uPi) val_uPi = (Rk(k_uPi) - Kpi(1, k_uPi)*x_uPi(1))/(Kpi(2, k_uPi)*x_uPi(2) + Kpi(3, k_uPi)*x_uPi(3) + Kpi(4, k_uPi)); end function [val_ham, val_grad] = Hamilt(u_ham) % Vtddx = Vtilde_dx(gka+1, Xkn(:, gka, gnu)); val_ham = (Xkn(1, gka, gnu) + Xkn(2, gka, gnu)*u_ham - Rk(gka+1))^2 + Xkn(3, gka, gnu)*u_ham^2 ... + sigma^2 ... %ztrata l + [Xkn(1, gka, gnu) + Xkn(2, gka, gnu)*u_ham; ... Xkn(2, gka, gnu) + Xkn(3, gka, gnu)*u_ham*(Xkn(1, gka+1, gnu) - Xkn(1, gka, gnu) - Xkn(2, gka, gnu)*u_ham)/(Xkn(3, gka, gnu)*u_ham^2 + sigma^2); ... Xkn(3, gka, gnu) - Xkn(3, gka, gnu)^2*u_ham^2/(Xkn(3, gka, gnu)*u_ham^2 + sigma^2)]' ... %fce f *Vtilde_dx(gka+1, Xkn(:, gka, gnu)) ... + Wv(5, gka+1)*sigma; val_grad = 2*(Xkn(1, gka, gnu) + Xkn(2, gka, gnu)*u_ham - Rk(gka+1))*Xkn(2, gka, gnu) + 2*Xkn(3, gka, gnu)*u_ham ... %ztrata du + [Xkn(2, gka, gnu);... (2*u_ham^2*Xkn(3, gka, gnu)^2*(Xkn(1, gka, gnu) - Xkn(1, gka+1, gnu) + u_ham*Xkn(2, gka, gnu)))/(sigma^2 + u_ham^2*Xkn(3, gka, gnu))^2 - (u_ham*Xkn(2, gka, gnu)*Xkn(3, gka, gnu))/(sigma^2 + u_ham^2*Xkn(3, gka, gnu)) - (Xkn(3, gka, gnu)*(Xkn(1, gka, gnu) - Xkn(1, gka+1, gnu) + u_ham*Xkn(2, gka, gnu)))/(sigma^2 + u_ham^2*Xkn(3, gka, gnu));... (2*u_ham^3*Xkn(3, gka, gnu)^3)/(sigma^2 + u_ham^2*Xkn(3, gka, gnu))^2 - (2*u_ham*Xkn(3, gka, gnu)^2)/(sigma^2 + u_ham^2*Xkn(3, gka, gnu))]' ... %fce f du * Vtilde_dx(gka+1, Xkn(:, gka, gnu)); %derivace Bellman fce end function [val_Vt] = Vtilde(k_Vt, x_Vt) if(k_Vt == K) val_Vt = h; else val_Vt = vectFi(x_Vt - Xstripe(:, k_Vt))' * Wv(:,k_Vt); end end function [val_Vt] = Vtilde_dx(k_Vt, x_Vt) if(k_Vt == K) val_Vt = hdx; else val_Vt = difFi(x_Vt - Xstripe(:, k_Vt))' * Wv(:,k_Vt); end end % function [val_Vt] = Vtilde_dx_dx(k_Vt, x_Vt) % if(k_Vt == K) % val_Vt = hdxdx; % else % val_Vt = zeros(3,3); % val_Vt(1,1) = 2*Wv(5,k_Vt); % val_Vt(2,2) = 2*Wv(8,k_Vt); % val_Vt(3,3) = 2*Wv(10,k_Vt); % % val_Vt(1,2) = Wv(6,k_Vt); % val_Vt(1,3) = Wv(7,k_Vt); % val_Vt(2,3) = Wv(9,k_Vt); % % val_Vt(2,1) = val_Vt(1,2); % val_Vt(3,1) = val_Vt(1,3); % val_Vt(3,2) = val_Vt(2,3); % end % end % function [val_Vt] = trSgVt(k_Vt) % if(k_Vt == K) % val_Vt = 0; % else % val_Vt = 2*Wv(5,k_Vt)*sigma; % end % end function [val_Fi] = vectFi(x_Fi) val_Fi = [ ... 1; ... x_Fi(1); ... x_Fi(2); ... x_Fi(3); ... x_Fi(1)^2; ... x_Fi(1)*x_Fi(2); ... x_Fi(1)*x_Fi(3); ... x_Fi(2)^2; ... x_Fi(2)*x_Fi(3); ... x_Fi(3)^2; ... ]; end function [val_Fi] = matrixFi(x_Fi) val_Fi = [ ... ones(1, N); ... x_Fi(1, :); ... x_Fi(2, :); ... x_Fi(3, :); ... x_Fi(1, :).^2; ... x_Fi(1, :).*x_Fi(2, :); ... x_Fi(1, :).*x_Fi(3, :); ... x_Fi(2, :).^2; ... x_Fi(2, :).*x_Fi(3, :); ... x_Fi(3, :).^2; ... ]; % val_Fi = [ ... % ones(1, N); ... % x_Fi(1, :); ... % x_Fi(2, :); ... % x_Fi(3, :); ... % ]; end function [val_Fi] = difFi(x_Fi) val_Fi = [ ... 0 0 0; ... 1 0 0; ... 0 1 0; ... 0 0 1; ... 2*x_Fi(1) 0 0; ... x_Fi(2) x_Fi(1) 0; ... x_Fi(3) 0 x_Fi(1); ... 0 2*x_Fi(2) 0; ... 0 x_Fi(3) x_Fi(2); ... 0 0 2*x_Fi(3); ... ]; end end