rk4.m

Control optimisation. It is example of use BFGS algorithm to control satellite form Earth atmosphere

% algorytm rozwiazywania rownan rozniczkowych
function [ x_rk4, t ] = rk4 ( x0, tau, h0, u2)
global u1max;

otau = [0 tau];
dtau = diff(otau);
n = ceil(dtau/h0);
nk = sum(n);
cn = cumsum([1 n]);
x = zeros(5,nk+1);
x(:,1) = x0;

t = [0];
wektor_u1 = [];
wektor_u2 = [];
u1_akt = 0;
% t = wylicz_t(nk+1,h0);
% wektor_u1 = wylicz_u1(tau,t);
% wektor_u2 = wylicz_u2(tau,t,u2,h0);

for j=1:length(tau)
    if n(j)
        h = dtau(j)/n(j);
        h2= h/2;
        h3= h/3;
        h6= h/6;
        
%         t_frag = otau(j):h:(otau(j+1));
%         if (j>1)
%             t = t(1:length(t)-1);
%         end
%         t = [t t_frag];
        t = [t (otau(j)+h):h:(otau(j+1)+0.1*h)];
        
        if u1_akt
            u1_akt = 0;
        else
            u1_akt = u1max;
        end

%         for i=1:length(t_frag)-1
%             wektor_u1 = [wektor_u1 u1akt];
%         end
%         
        if (j==1)
            tau_frag = tau(1);
        else
            tau_frag = tau(1:j);
        end
        wektor_u2 = wylicz_u2(tau_frag,t,u2,h0);
        
        for i= cn(j):(cn(j+1)-1)
            %u1_akt = wektor_u1(i);
            z= x(:,i);
            u2_akt = wektor_u2(i);
                dx1= rhs(z,t(i),u1_akt,u2_akt);
            u2_akt = (wektor_u2(i+1)+u2_akt)/2;
                x1= z+h2*dx1;
                dx2= rhs(x1,t(i)+h2,u1_akt,u2_akt);
                x2= z+h2*dx2;
                dx3= rhs(x2,t(i)+h2,u1_akt,u2_akt);
            u2_akt = wektor_u2(i+1);
                x3= z+h*dx3;
                dx4= rhs(x3,t(i+1),u1_akt,u2_akt);  
            x(:,i+1)= z+h3*(dx2+dx3)+h6*(dx1+dx4);
        end
    else
        disp ('blad');
        break;
    end
end
x_rk4 = x';
end