%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                           %
% Computer exercise in:                                     %
%                                                           %
% 5B1872 Optimal Control                                    %
%                                                           %
% Main file (main.m)                                        %
%                                                           %
% Solves the optimal orbit transfer problem                 %
% with embedded shooting (Newton's method).'                %  
%                                                           %
% Matlab5 recommended (not tried for an older version)      %
%                                                           %
% This problem is taken from the book:                      %
% Applied Optimal Control                                   %
% Arthur E. Bryson and Yu-Chi Ho                            %
% Hemisphere Publishing Corporation, 1975                   %
%                                                           %
% and is made into a computer exercise by                   %
% Henrik Rehbinder & Petter Ögren                           %
%                                                           %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


clear, clc    
global c1 c2 c3 ti x0 tf

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                %
% Problem specific constants     %
%                                %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
c1=0.1405;  
c2=0.533;   
c3=3.32;    
ti = 0;
tf = 1;
x0 = [1;0;1];


% A starting guess
% (Consider what lambda_0 that are plausible.)
%

lambda_0(:,1) = [-1 ;-1 ;-1 ];

%
% Number of embeddings
%

N = 5;





% Plot the result of the starting guess.
figure(1), clf
[t,z] = ode45('fh',[ti tf],[x0;lambda_0]);
plotresults(t,z), hold on , drawnow




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                             %
% Use embedding to solve a set of equations   %
% with good starting guesses instead of one   %
% with a bad guess.                           %
%                                             %
%                                             %
% (after termination the optimal cost         %
% can be derived from the z vector)           %
%                                             %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


m_0=theta(lambda_0)
%return
lambda_k=lambda_0;
for k=1:N
    iteration=k
    m_k=(1-k/N).*m_0
    lambda_k=newton_solver('theta',m_k,lambda_k);
    th=theta(lambda_k)
    l_k=lambda_k
    [t,z] = ode45('fh',[ti tf],[x0;lambda_k]);
    plotresults(t,z), hold on , drawnow
end