lambertlow.m

It s verry good for you

function [V1, V2, exposin] = lambertlow(r1vec, r2vec, tf, varargin)
%LAMBERTLOW                    Low-Thrust Lambert-targeter
%
%   Usage:
%   [V1, V2, exposin] = LAMBERTLOW(r1, r2, tf)
%   [V1, V2, exposin] = LAMBERTLOW(r1, r2, tf, k2)
%   [V1, V2, exposin] = LAMBERTLOW(r1, r2, tf, k2, N)
%   [V1, V2, exposin] = LAMBERTLOW(r1, r2, tf, k2, N, mu)
%   [V1, V2, exposin] = LAMBERTLOW(r1, r2, tf, k2, N, mu, way)
%
%   [V1, V2, exposin] = LAMBERTLOW(r1, r2, tf) determines the exponential 
%   sinuisoid between [r1] = [x1 y1 z1] and [r2] = [x2 y2 z2], that takes a 
%   transfer time [tf] in seconds. [V1] and [V2] are the velocity vectors 
%   at [r1] and [r2], and [exposin] is the exponential sinusoid in the form 
%   [k0, k1, k2, phi, tf, N, dth, gamma1, gamma_m, gamma_M] that satisfies 
%   the constraints.
%
%   LAMBERTLOW(r1, r2, tf, k2, N) defines the constants k2 and N which
%   appear in the equations of the exponential sinusoid (see Izzo, 2006). 
%   If left empty or omitted, LAMBERTLOW assumes [k2] = 1/12 and [N] = 0.
%   For some combinations of [k2] and [N] it is impossible to satisfy all 
%   constraints imposed by the exponential sinusoid-assumption. In such
%   cases, the results will be NaN. 
%
%   LAMBERTLOW(r1, r2, tf, k2, N, mu) takes the parameter [mu] as the 
%   standard gravitational parameter of the central body. This will make 
%   the Lambert targeter suited for use around other bodies than the Sun. 
%   [mu] may be empty, e.g., [mu] = [], in which case the Sun will be used 
%   as the central body.
%   
%   LAMBERTLOW(r1, r2, tf, k2, N, mu, way) allows selection of the 'long' or 
%   'short' way solution. By default, BOTH solutions are returned, which is
%   equivalent to [way] = 'both' (see next paragraph). The argument [way] 
%   may also take values equal to 'short' or 'long', in which case only the 
%   'short' or 'long' way solutions will be returned (small- and large 
%   values of the turn angle, respectively).
%
%   Inputs [r1] and [r2] may be of size (n x 3), as long as the input
%   [tf] is of corresponding size (n x 1). The inputs [k2] and [N] may be
%   vectors or scalars. In case they are vectors, they should also be of
%   conforming size (n x 1). In case they are scalars, the same value for
%   [k2] or [N] will be used for all trajectories defined by [r1] and [r2].
%   Inputs [r1] and [r2] may be of size (n x 3), as long as the inputs [tf]
%   and [m] are of corresponding size (n x 1). Normally, there are 2
%   solutions to the Lambert problem (for the given turn angle, and 2pi
%   minus that turn angle). The default outputs [V1] and [V2] are thus of 
%   size [n x 3 x 2]--the second solution is appended along the third
%   array dimension. If only one solution is desired ('short' or 'long'), 
%   the size of the outputs [V1] and [V2] will be [n x 3]. 
%
%   LAMBERTLOW uses the method developped by D. Izzo in his 2006 paper,
%   which was based on the exponential sinusoids introduced by Petropoulos
%   in his PhD. dissertation.
%
%   See also lamberthigh, newtonraphson.

%   Author: Rody P.S. Oldenhuis
%   Delft University of Technology
%   E-mail: [e-mail]
%   Last edited 26/Feb/2009.

    % basic errortrap
    error(nargchk(3, 7, nargin)); 
            
    % parameters   
    tol1 = 1e1;      % 10 second accuracy
    tol2 = 1e-6;     % constraint equation accuracy to be met
    
    % constants    
    muS     = 132712439940;    %  gravitational parameter Sun (from NASA/JPL Horizons)
    nvecs   = size(r1vec, 1);
    sqrtmuS = sqrt(muS);      
    
    % parse input
    k2 = 1/12 * ones(size(tf)); % default values are k = 1/12 and N = 0 for all problems
    N  = zeros(size(tf));
    whichsol = 'both';          % by default, compute both solutions
    if (nargin >= 4)
        k2 = varargin{1};
        if (numel(k2) == 1), k2 = k2*ones(size(tf)); end
        if isempty(k2), k2 = 1/12 * ones(size(tf)); end
    end
    if (nargin >= 5)
        N = varargin{2};
        if (numel(N) == 1), N = N*ones(size(tf)); end
        if isempty(N), N = zeros(size(tf)); end
    end
    if (nargin >= 6)
        mu = varargin{3};        
        if isempty(mu), mu = muS; end
        muS = mu;
    end
    if (nargin == 7)
        whichsol = varargin{4};
        if ~ischar(whichsol)
            % errortrap
            warning('lambertlow:IncorrectSols',...
                    ['Solutions should be selected by providing strings "both",',...
                     ' "short" or "long". Defaulting to "both"...']);
            whichsol = 'both';
        end
        if isempty(whichsol)
            whichsol = 'both';
        end
    end
    if strcmpi(whichsol, 'both')
        whichsol = 0;
        numsols  = 2;
    elseif strcmpi(whichsol, 'short')
        whichsol = 1;
        numsols  = 1;
    elseif strcmpi(whichsol, 'long')
        whichsol = 2;
        numsols  = 1;
    end
        
    % less basic errortraps    
    if (size(k2) ~= size(tf))
        error('lambertlow:incorrectsizeK2', ...
              'Parameter [k2] should be the same size as [tf].')
    end
    if (size(N) ~= size(tf))
        error('lambertlow:incorrectsizeN', ...
              'Parameter [N] should be the same size as [tf].')
    end
    if (size(tf, 1) ~= size(r1vec, 1))
        error('lambertlow:incorrectsizeTOF', ...
              'Parameter [tf] should have the same number of rows as the radius vectors.')
    end 
                
    % default output is pessimistic
    exposin = NaN(nvecs, 10, numsols);
    V1      = NaN(nvecs, 3, numsols);
    V2      = V1;    
            
    % initial values
    r1      = sqrt(sum(r1vec.^2, 2));  
    r2      = sqrt(sum(r2vec.^2, 2));
    dotprod = sum(r1vec.*r2vec, 2)./r1./r2;
    ln      = log(r1./r2);    
    
    % heliocentric angle(s) between target and departure    
    Psi = acos(dotprod);    
    direction = 1;
    if (whichsol == 2) 
        Psi = 2*pi - Psi;
        direction = -1;        
    elseif  (whichsol == 0)
        Psi = [Psi, 2*pi - Psi];
        direction = [direction, -1];
        N   = [N, N];
        k2  = [k2, k2];
        ln  = [ln, ln];
    end     
        
    % more initial values     
    r1unit = [1, 0, 0];
    r1th   = [0, 1, 0];    
    r2vecn = [r2.*cos(Psi(:, 1)), r2.*sin(Psi(:, 1)), zeros(nvecs, 1)];
    r2vecm = sqrt(sum(r2vecn.^2, 2));
    r2unit = r2vecn./[r2vecm, r2vecm, r2vecm];
    r2th   = [-r2unit(:, 2), r2unit(:, 1), zeros(nvecs, 1)];
    dth    = Psi + 2*pi*N.*sign(Psi);
    k2th   = k2.*dth;    
    Delta  = 2.*(1 - cos(k2th))./(k2.^4) - ln.^2;  
           
    % limit search space for gamma       
    gam1m = atan( k2/2 .* ( -ln.*cot(k2th./2) - sqrt(Delta) ));
    gam1M = atan( k2/2 .* ( -ln.*cot(k2th./2) + sqrt(Delta) ));
    
    % kill warnings caused by quadgk
    warning('off', 'MATLAB:quadgk:NonFiniteValue');
    
    % two possible solutions
    for sol = 1:numsols
        
        % nvecs different inputs
        for pair = 1:nvecs
            
            % extract appropriate values
            tfi    = tf(pair);            r1i   = r1(pair);
            lni    = ln(pair, sol);       Ni    = N(pair, sol);
            k2i    = k2(pair, sol);       gamm  = gam1m(pair, sol);
            gamM   = gam1M(pair, sol);    dthij = dth(pair, sol);  
            k2thij = k2th(pair, sol);     r1vp  = r1vec(pair, :);    
            r2vp   = r2vec(pair, :);
            
            % Delta is negative -- no solution
            if (~isreal(gamm) || ~isreal(gamM))
                continue;
            end                   

            % time-of-flight equation, and its derivative
            TOF  = @(gam) quadgk(@(th)tff(th,gam,k2i,k2thij,r1i,lni,1),0,dthij)/sqrtmuS-tfi;
            TOFp = @(gam) quadgk(@(th)tff(th,gam,k2i,k2thij,r1i,lni,2),0,dthij)/sqrtmuS/2;
                            
            % check whether solution is feasible
            if (TOF(gamm) > 0) || (TOF(gamM) < 0)
                continue;
            end           
            
            % get good initial value
            
            % step through the search space until the sign changes
            gm0 = (gamm/4 + gamM)/2;             
            step = (gamM - gamm)/10;  
            a  = TOF(gm0);   ap = a;
            if (a > 0), step = -step; end
            while a*ap > 0
                ap = a;             gm0p = gm0;  
                gm0 = gm0 + step;   a = TOF(gm0);  
            end
            
            % initial estimate comes from straight line through final points
            m = (a - ap)/(gm0 - gm0p);
            gam0 = gm0p - ap/m;

            % find gamma with Newton-Raphson            
            [gm, do, not, care, badinds] = newtonraphson(TOF, TOFp, gam0, tol1, gamm, gamM);
                
            % Newton-Raphson might fail. Use Regula-Falsi in that case
            if any(badinds(:))            
                [gm, do, not, care, badinds] = ...
                                           regulafalsi(TOF, gm0, gm0p, [], tol1, gamm, gamM);
            end
            
            % Regula-Falsi might also fail. Issue warning and continue with
            % the next solution
            if any(badinds(:))
                warning('lambertlow:gamma1_cannot_be_found', ...
                       ['The time-of-flight equation does not converge.\n',...
                        'Check whether the integral evaluates and is finite.']);
                continue;
            end  
            
            % get constants for converged gamma
            [dontcare, k0, k1, phi] = tff(0, gm, k2i, k2thij, r1i, lni, []);
            
            % some frequently used quantities
            k22    = k2i^2;       k1k22   = k1*k22;
            k12k24 = k1k22^2;     tangm12 = tan(gm)^2;
            
            % constraint for physically realistic solutions            
            cnstr1  = abs(k1k22) < 1;
             
            % constraint that follows from tangential thrust
            % (cos^2 + sin^2 = 1)
            cnstr2  = abs(k12k24 - k22*tangm12 - k12k24*sin(phi)^2) < tol2;         % departure                
            tangm22 = (tan(gamm) + tan(gamM) - tan(gm))^2;
            cnstr3  = abs(k12k24 - k22*tangm22 - k12k24*sin(k2thij + phi)^2) < tol2;% target
            
            % constraint for k2       
            cnstr4 = k22 <= (tangm12+2*k1k22*sin(phi)*lni)/lni^2;
            
            % only if all constraints are met, output results
            if cnstr1 && cnstr2 && cnstr3 && cnstr4
            
                % get Euler angles for transformation
                crossprd = [r1vp(2)*r2vp(3) - r1vp(3)*r2vp(2),...
                    r1vp(3)*r2vp(1) - r1vp(1)*r2vp(3),...
                    r1vp(1)*r2vp(2) - r1vp(2)*r2vp(1)];   % CROSS would slow down the loop
                X = r1vp/r1i;
                Z = crossprd/sqrt(sum(crossprd.^2, 2));
                beta  = atan2(sqrt(Z(1)^2 + Z(2)^2), Z(3));
                alpha = atan2(Z(1), -Z(2));
                gamma = atan2(X(3), Z(1)*X(2)-Z(2)*X(1));
                
                % determine rotation matrix for coordinate transformation
                csa = cos(alpha); sna = sin(alpha);
                csb = cos(beta ); snb = sin(beta );
                csg = cos(gamma); sng = sin(gamma);
                R = [csa*csg - sng*csb*sna,  csa*csb*sng + sna*csg, snb*sng;
                    -sna*csg*csb - csa*sng,  csa*csg*csb - sna*sng, snb*csg;
                     snb*sna              , -csa*snb              , csb    ];
                
                % output exposin 
                Phi = dth(pair, sol);
                exposin(pair, :, sol) = [k0, k1, k2i, phi, tfi, Ni, ...
                                         direction(sol)*Phi, gm, gamm, gamM];                 
                
                % define derivatives 
                r     = @(th) k0.*exp(k1.*sin(k2i.*th + phi));
                thdot = @(th) sqrt(              (muS./r(th).^3) ./...
                              (tan(gm).^2 + k1.*k2i.^2.*sin(k2i.*th + phi) + 1));
                rdot  = @(th) r(th).* thdot(th) .* k1.*k2i.*cos(k2i.*th + phi);

                % velocities (with coordinate transformation)
                Vth1 = direction(sol)*r(0).*thdot(0);
                Vr1  = rdot(0);                
                Vth2 = direction(sol)*r(Phi).*thdot(Phi);
                Vr2  = rdot(Phi);                 
                V1(pair, :, sol) = ([Vr1 , Vr1 , Vr1 ].*r1unit + ...
                                    [Vth1, Vth1, Vth1].*r1th)*R;
                V2(pair, :, sol) = ([Vr2 , Vr2 , Vr2 ].*r2unit(pair, :) + ...
                                    [Vth2, Vth2, Vth2].*r2th(pair, :))*R;
            end             
        end
    end
    
    % revive warnings 
    warning('on', 'MATLAB:quadgk:NonFiniteValue');
    
end

function [tofeqs, k0, k1, phi] = tff(th, gam, k2, k2dth, r1, ln, type)
    
    % some substitutions
    tofeqs = [];
    tangam = tan(gam);
    k22    = k2^2;
    
    % compute all the constants
    k1sgn  = (ln+tangam*sin(k2dth)/k2) / (1-cos(k2dth));
    k1     = sign(k1sgn)*sqrt(k1sgn^2 + tangam^2/k22);
    phi    = acos(tangam/k1/k2);
    k0     = r1 * exp(-k1*sin(phi));
    
    % more substitutions
    s     = sin(k2*th + phi);
    c     = cos(k2*th + phi);
    k12sf = k1*k22*s;    
    tangm = k1*k2*c;
    tanfc = tangm.^2 + k12sf + 1;
    r     = k0*exp(k12sf ./ k22);    

    % equation for transfer time    
    if type == 1
        tofeqs = sqrt(r.^3.*tanfc);
        
    % and the derivative thereof
    elseif type == 2
        
        % some more substitutions
        tangam2  = tangam^2;
        sec2gam  = sec(gam)^2;
        sink2dth = sin(k2dth);
        cosk2dth = cos(k2dth);        
        
        % compute the derivatives (yes, they're nasty)
        dk1dg   = tangam*sec2gam/k1/k22*(sink2dth/(1-cosk2dth)^2*(k2*ln/tangam+sink2dth)+1);
        dphidg  = (dk1dg*tangam-k1*sec2gam)/abs(k1)/sqrt(k22*k1^2-tangam2);
        dk0dg   = -k0*(dk1dg*sin(phi)+dphidg*k1*cos(phi));
        Phi     = dk1dg*s+dphidg*k1*c;
        dPhidth = k2*(dk1dg*c-dphidg*k1*s);
       
        % equation for derivative of transfer time
        tofeqs = sqrt(r.^3./tanfc).*(3*tanfc.*(dk0dg./k0 + Phi) + dPhidth + k22*Phi);    
        
    end
end