bfgs_alg.m

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

% Algorytm BFGS
% Algorytm obszaru wiarygodnosci, metody newtona
% funkcja celu: Q:R^n -> R
% dane wejsciowe:
%   main_tau - poczatkowe rozwiazanie
%   main_h0 - krok metody rk4
%   main_x0 - punkt startowy Ziemi
% dane wyjsciowe:
%   wynik_tau - znalezione rozwiazanie optymalne dla czasow tua
%   wynik_u2 - znalezione rozwiazanie optymalne dla sterowan u2
function [wynik_tau,wynik_u2] = bfgs_alg(main_x0,main_tau,main_h0)
global super_jednostka;
x = main_tau;

% 1) Inicjalizacja:
E0 = 1e-5;      % zadana dokladnosc optymalizacji
r = 1;          % parametr odnowy
numer_iteracji = 0;
gradnormy = [];
wskazniki = [];

% pierwsze ustalanie przedzialow dla u2:
super_jednostka = 0.3;
szerokosc = x(1);
ii = 3;
while (ii<=length(x))
    szerokosc = [szerokosc (x(ii)-x(ii-1))];
    ii = ii + 2;
end
u2 = [];    % wartosci wektora u2
tu2 = [];   % czasy dla u2
for ii=1:length(szerokosc)
    if (abs(floor(szerokosc(ii)/super_jednostka)-(szerokosc(ii)/super_jednostka)))<(1e-5)
        vv = 1;
        ilosc_jednostek = 1+(szerokosc(ii)/super_jednostka);
    else
        vv = 0;
        ilosc_jednostek = 2+floor(szerokosc(ii)/super_jednostka);
    end;
    if (ii==1)
        czas_startowy_przedzialu = 0;
    else
        czas_startowy_przedzialu = x(2*ii-2);
    end
    for iii=1:ilosc_jednostek
        u2 = [u2 (pi/4)];
        tu2 = [tu2 (czas_startowy_przedzialu+(iii-1)*super_jednostka)];
    end
    if vv==0
        tu2(length(tu2)) = czas_startowy_przedzialu + szerokosc(ii);
    end
end
if floor(length(x)/2)==(length(x)/2)
    u2 = [u2 (pi/4)];
    tu2 = [tu2 x(length(x))];
end
% display ('pierwsze u2:');
% u2
% tu2
% koniec pierwszego ustalania przedzialow dla u2

wskaz = bfgs_f_celu2(x,main_x0,main_h0,u2);
while (numer_iteracji<100)
    numer_iteracji = numer_iteracji + 1;
    wskaz = wskaz
    wskazniki(numer_iteracji) = wskaz;

    grad_akt = bfgs_f_celu_grad(x,main_x0,main_h0,u2,tu2);
    
    disp ('iteracja:  ');
    disp (numer_iteracji);
%     GRAD = grad_akt'
    GRAD_NORM = norm(grad_akt)
    gradnormy(numer_iteracji) = GRAD_NORM;
%     X_NA_POCZATKU = x'

    if (norm(grad_akt)<E0)
        break;
    end
    if r==1
        W = eye(length(x));
    elseif r==0
        r = grad_akt-gs;
        s = x - xs;
        W = W + (r*(r'))/((s')*r) - (W*s*(s')*W)/((s')*W*s);
    end
    d = -inv(W)*grad_akt;
    xs = x;
    gs = grad_akt;

    [nowe_x,nowe_wskaz] = bfgs_min_kier(x,d,main_x0,main_h0,u2,tu2,wskaz);
    if nowe_wskaz<wskaz
        x = nowe_x;
        wskaz = nowe_wskaz;
        r = 0;
    else
        if r==1
            disp ('Nastapilo zakonczenie mozliwosci poprawy');
            break;
        else
            r = 1;
            continue;
        end
    end
    
    % likwidowanie szpilek:
    dokladnosc = 1e-2;
    dlugosc_tau = length(x);
    if dlugosc_tau>1
        if x(dlugosc_tau)-x(dlugosc_tau-1)<dokladnosc   % likwidacja ostatniej
            x_stare = x(1:dlugosc_tau-1);
            x = x_stare;
            r = 1;
        elseif dlugosc_tau>2        % ewentualne likwidacje dwoch srodkowych
            for i = 2:dlugosc_tau
                if x(i)-x(i-1)<dokladnosc
                    x_stare = [];
                    for ii = 1:dlugosc_tau-1
                        if (ii~=i) && (ii~=(i-1))
                            x_stare = [x_stare;x(ii)];
                        end
                    end
                    x = [x_stare;x(dlugosc_tau)];
                    r = 1;
                    break;
                end
            end
        end
    end
%     X_PO_SZPILKACH = x'
%     koniec likwidowania szpilek
    
    % zmiana u2(tau) podyktowana zmiana tau:
    [u2,tu2] = zmien_u2(x,u2,tu2);
%     display ('zmiana u2:');
%     u2
%     tu2
    
    % optymalizacja wartosci u2(tau):
    [u2,wskaz] = bfgs_u2_alg(main_x0,(x'),main_h0,(u2'));
%     display ('u2 po optymalizacji:');
%     u2 = u2 
end
wynik_tau = x;
wynik_u2 = u2;
figure(11);
plot(1:length(wskazniki), wskazniki);
figure(12);
plot(1:length(gradnormy), gradnormy);
end % koniec funkcji