root/applications/dual/SIDP/generate_trajectories.m @ 743

function H=generate_trajectories(H,u,sigma)
sigma_na_druhou=sigma^2;

for i=1:size(H,1)-1
    for j=1:size(H,2)
        %aplication of best_control
        %H(i+1,j,1)=H(i,j,1)+(H(i,j,2)+H(i,j,3)*randn)*u(i,j)+sigma*randn;
        H(i+1,j,1)=H(i,j,1)+H(i,j,2)*u(i,j)+sigma*randn;

        %kalman
        K=u(i,j)*H(i,j,3)/(H(i,j,3)*u(i,j)^2+sigma_na_druhou);
        H(i+1,j,2)=H(i,j,2)+K*( H(i+1,j,1)-H(i,j,1)-H(i,j,2)*u(i,j));
        H(i+1,j,3)=(1-K*u(i,j))*H(i,j,3);
    end
   
end
end