rk4_adj.asv

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

% rk4-wstecz
% wejscia:
%   x0 - punkt startowy rakiety
%   tau - wektor przelaczania sterowania u1
%   h0 - krok metody
% wyjscia:
%   Eps - wartosc funkcji celu
%   polozenia - rownania stanu rakiety w czasie
%   f_przel - wektor wartosci funkcji przelaczen w czasie
function [polozenia,f_przel] = rk4_adj ( x0, tau, h0, u2 )
global RzRm bm az am c;
global kq;

% przygotowanie:
dtau = diff( [0 tau]);
n = ceil(dtau/h0);
nk = sum(n);
cn = cumsum([1 n]);
[x_rk4,t] = rk4(x0,tau,h0,u2);
%t = wylicz_t(nk+1,h0);
t_dlugosc = length(t);
wektor_u1 = wylicz_u1(tau,t);
wektor_u2 = wylicz_u2(tau,t,u2,h0);
polozenieT = x_rk4(length(x_rk4),1:5);
%rownania ruchu Marsa:
xmT = RzRm*cos((am*t(t_dlugosc))+bm);
ymT = RzRm*sin((am*t(t_dlugosc))+bm);
dxmT = -RzRm*am*sin((am*t(t_dlugosc))+bm);
dymT =  RzRm*am*cos((am*t(t_dlugosc))+bm);  
Eps = 

psi = zeros(t_dlugosc,5);
psi(t_dlugosc,:) = [kq(1)*(polozenieT(1)-xmT) kq(2)*(polozenieT(2)-dxmT) kq(3)*(polozenieT(3)-ymT) kq(4)*(polozenieT(4)-dymT) kq(5)];

z= [x_rk4(length(x_rk4),:) psi(t_dlugosc,:)];
f_przel = zeros(t_dlugosc,1);
u2_akt = wektor_u2(t_dlugosc);
f_przel(t_dlugosc) = (z(7)*cos(u2_akt)+z(9)*sin(u2_akt))/z(5) + c*z(10);

for j = length(tau):(-1):1
    if (n(j)>0)
        h = dtau(j)/n(j);
        h2 = h/2;
        h3 = h/3;
        h6 = h/6;
        for i = cn(j+1):(-1):(cn(j)+1)
            u1_akt = wektor_u1(i);
            z= [x_rk4(i,:) psi(i,:)];
            u2_akt = wektor_u2(i);
                dz1= rhs_adj(z,t(i),u1_akt,u2_akt);
                z1= z-h2*dz1;
            u2_akt = (u2_akt+wektor_u2(i-1))/2;
                dz2= rhs_adj(z1,t(i)-h2,u1_akt,u2_akt);
                z2= z-h2*dz2;
                dz3= rhs_adj(z2,t(i)-h2,u1_akt,u2_akt);
                z3= z-h*dz3;
            u2_akt = wektor_u2(i-1);
                dz4= rhs_adj(z3,t(i)-h,u1_akt,u2_akt);
                z= z-h3*(dz2+dz3)-h6*(dz1+dz4);
            psi(i-1,:)= z(6:10);
            polozenia(i-1,:)= z(1:5);
            f_przel(i-1) = (z(7)*cos(u2_akt)+z(9)*sin(u2_akt))/z(5) + c*z(10);
        end
    else
        disp ('blad');
        break;
    end
end
polozenia(t_dlugosc,:) = polozenieT;

end