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