BUG

[rodyo]

Email exchange:

From: Han Cai [email protected]
Sent: 17 March 2017 00:47
To: Rody Oldenhuis [email protected]
Subject: Re: A bug about the Lambert code

Hi, Rody:

Thanks for you reply. I'm not quite sure that the equations of THR2 and phr are the same. THR2 is definitely between -1 and 1 for any input according to its equation:

THR2 = DATAN2(QSQFMI, 2D0*Q)/PI.

But the equation of phr is different:

phr = mod(2atan2(1 - q^2, 2q), 2*pi);

the value of phr is between 0 to 2*pi which related its input q. I find another code (please see the attached code line 513) that use the equation below to define thr2, which is similar to Gooding's definition:

thr2 = atan2(qsqfm1, 2.0 * q) / pi;

I'm still confused after I compared Gooding's paper with your code again. The variables dth, and phr are both appeared in line 523:

x0 = x*(1 - (1 + m + (dth - 1/2)) / (1 + 0.15m)x(W/2 + 0.03x*sqrt(W))) + xM;

line 535:

W = x00 + 1.7sqrt(2(1 - phr));

and line 542:

lambda = (1 + (1 + m + 0.24*(dth - 1/2)) / (1 + 0.15m)x03(W/2 - 0.03x03*sqrt(W))).

Whereas, a same variable THR2 is used in all these three equations according to Gooding's paper. To be honest, I have no idea about the physical meaning of these three equations, but I'm just wondering if a same variable dth or phr need to be employed here for consistency?

Thanks.

Best wishes
Han Cai

On 17 March 2017 at 01:36, Rody Oldenhuis [email protected] wrote:

Hi Han Cai,

Thanks a lot for your email, know that it is very much appreciated.

I don’t think you are correct though. I’m looking at R.H. Gooding’s paper:

“A procedure for the solution of Lambert's orbital boundary-value problem” from 1990.

On page 162, the routine XLAMB is printed (Appendix B). There, he defines

THR2 = DATAN2(QSQFMI, 2D0*Q)/PI

Which is (eventually) equal to my definition of phr:

phr = mod(2atan2(1 - q^2, 2q), 2*pi);

Several lines below the definition of THR2, he computes:

W = X + C0DSQRT(2D0(ID0 - THR2))

Compare my line 535:

W = x00 + 1.7sqrt(2(1 - phr));

Now, while reading my code again, I kind of lost a few factors of 2 or π here and there, so I’ll be reviewing my code in the nearby future.

However, since his 1990 paper is newer than the one you referenced, I’m inclined to believe that my code is accurate.

…or do you have a test case as counterexample?

Thanks again! Best regards,
Rody Oldenhuis

From: Han Cai [mailto:[email protected]]
Sent: 09 March 2017 09:53
To: Rody Oldenhuis [email protected]
Subject: A bug about the Lambert code

Hi Rody:

I'm a PhD student in RMIT, Australia. I'm very appreciate for your Matlab code of Lambert solver. This code is fantastic, it helps me a lot in my study.

However, there is a small bug sometimes could result in program error. So I go back to check the reference and find that Gooding attached the fortran code in his paper: ON THE SOLUTION OF LAMBERT'S ORBITAL BOUNDARY-VALUE PROBLEM, 1988, page 44. According to his code, I think the bug located in line 535, where the variable 'phr' should be replaced by another variable 'dth' in this equation. When using 'phr', the quantity under the square root sign could be negative. Hope this can be helpful. Thanks.

Best wishes!
Han Cai

Content of attached glambert.m:

function [vi, vf] = glambert(cbmu, sv1, sv2, tof, nrev)

% Gooding's solution of Lambert's problem

% input

%  cbmu = central body gravitational constant
%  sv1  = initial 6-element state vector (position + velocity)
%  sv2  = final 6-element state vector (position + velocity)
%  tof  = time of flight (+ posigrade, - retrograde)
%  nrev = number of full revolutions
%         (positive for long period orbit,
%          negative for short period orbit)

% output

%  vi = initial velocity vector of the transfer orbit
%  vf = final velocity vector of the transfer orbit

% References

%  R. H. Gooding, Technical Report 88027
%  On the Solution of Lambert's Orbital Boundary-Value Problem,
%  Royal Aerospace Establishment, April 1988

%  R. H. Gooding, Technical Memo SPACE 378
%  A Procedure for the Solution of Lambert's Orbital Boundary-Value Problem
%  Royal Aerospace Establishment, March 1991

% Orbital Mechanics with Matlab

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

r1mag = norm(sv1(1:3));

r2mag = norm(sv2(1:3));

ur1xv1 = cross(sv1(1:3), sv1(4:6));

ur1xv1 = ur1xv1 / norm(ur1xv1);

ux1 = sv1(1:3) / r1mag;

ux2 = sv2(1:3) / r2mag;

uz1 = cross(ux1, ux2);

uz1 = uz1 / norm(uz1);

% calculate the minimum transfer angle (radians)

theta = dot(ux1, ux2);

if (theta > 1.0)
    theta = 1.0;
end

if (theta < -1.0)
    theta = -1.0;
end

theta = acos(theta);

% calculate the angle between the orbit normal of the initial orbit
% and the fundamental reference plane

angle_to_on = dot(ur1xv1, uz1);

if (angle_to_on > 1.0)
    angle_to_on = 1.0;
end

if (angle_to_on < -1.0)
    angle_to_on = -1.0;
end

angle_to_on = acos(angle_to_on);

% if angle to orbit normal is greater than 90 degrees
% and posigrade orbit, then flip the orbit normal
% and the transfer angle

if ((angle_to_on > 0.5 * pi) && (tof > 0.0))
    theta = 2.0 * pi - theta;

    uz1 = -uz1;
end

if ((angle_to_on < 0.5 * pi) && (tof < 0.0))
    theta = 2.0 * pi - theta;

    uz1 = -uz1;
end

uz2 = uz1;

uy1 = cross(uz1, ux1);

uy1 = uy1 / norm(uy1);

uy2 = cross(uz2, ux2);

uy2 = uy2 / norm(uy2);

theta = theta + 2.0 * pi * abs(nrev);

[vr11, vr12, vr21, vr22, vt11, vt12, vt21, vt22, n] = vlamb(cbmu, r1mag, r2mag, theta, tof);

if (nrev > 0)
    if (n == -1)
        disp ('no tminium')

        vi = 0.0;

        vf = 0.0;
    elseif (n == 0)
        disp ('no solution time')

        vi = 0.0;

        vf = 0.0;
    elseif (n == 1)
        disp ('one solution')
    else
        disp ('two solutions')
    end
end

% compute transfer orbit initial and final velocity vectors

if ((nrev > 0) && (n > 1))
    vi = vr21 * ux1 + vt21 * uy1;

    vf = vr22 * ux2 + vt22 * uy2;
else
    vi = vr11 * ux1 + vt11 * uy1;

    vf = vr12 * ux2 + vt12 * uy2;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [dt, d2t, d3t, t] = tlamb(m, q, qsqfm1, x, n)

% Gooding lambert support function

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

lm1 = false;

l1 = false;

l2 = false;

l3 = false;

sw = 0.4;

lm1 = n == -1;

l1 = n >= 1;

l2 = n >= 2;

l3 = n == 3;

qsq = q * q;

xsq = x * x;

u = (1.0 - x) * (1.0 + x);

% needed if series, and otherwise useful when z = 0

if (~lm1)
    dt = 0.0;

    d2t = 0.0;

    d3t = 0.0;
end

if (lm1 || m > 0 || x < 0.0 || abs(u) > sw)
    % direct computation, not series
    
    y = sqrt(abs(u));

    z = sqrt(qsqfm1 + qsq * xsq);

    qx = q * x;

    if (qx <= 0.0)
        a = z - qx;

        b = q * z - x;
    end

    if ((qx < 0) && lm1)
        aa = qsqfm1 / a;

        bb = qsqfm1 * (qsq * u - xsq) / b;
    end

    if (qx == 0.0 && lm1 || qx > 0.0)
        aa = z + qx;

        bb = q * z + x;
    end

    if (qx > 0)
        a = qsqfm1 / aa;

        b = qsqfm1 * (qsq * u - xsq) / bb;
    end

    if (lm1)
        dt = b;

        d2t = bb;

        d3t = aa;
    else
        if (qx * u >= 0.0)
            g = x * z + q * u;
        else
            g = (xsq - qsq * u) / (x * z - q * u);
        end

        f = a * y;

        if (x <= 1)
            t = m * pi + atan2(f, g);
        else
            if (f > sw)
                t = log(f + g);
            else
                fg1 = f / (g + 1.0);

                term = 2.0 * fg1;

                fg1sq = fg1 * fg1;

                t = term;

                twoi1 = 1.0;

                told = 0.0;

                while (t ~= told)
                    twoi1 = twoi1 + 2.0;

                    term = term * fg1sq;

                    told = t;

                    t = t + term / twoi1;
                end
            end
        end

        t = 2.0 * (t / y + b) / u;

        if (l1 && z ~= 0.0)
            qz = q / z;

            qz2 = qz * qz;

            qz = qz * qz2;

            dt = (3.0 * x * t - 4.0 * (a + qx * qsqfm1) / z) / u;

            if (l2)
                d2t = (3.0 * t + 5.0 * x * dt + 4.0 * qz * qsqfm1) / u;
            end

            if (l3)
                d3t = (8.0 * dt + 7.0 * x * d2t ...
                    - 12.0 * qz * qz2 * x * qsqfm1) / u;
            end
        end
    end
else
    % compute by series
    
    u0i = 1.0;

    if (l1)
        u1i = 1.0;
    end

    if (l2)
        u2i = 1.0;
    end

    if (l3)
        u3i = 1.0;
    end

    term = 4.0;

    tq = q * qsqfm1;

    if (q < 0.5)
        tqsum = 1.0 - q * qsq;
    end

    if (q >= 0.5)
        tqsum = (1.0 / (1.0 + q) + q) * qsqfm1;
    end

    ttmold = term / 3.0;

    t = ttmold * tqsum;

    % start of loop
    
    icounter = 0;

    while (icounter < n || t ~= told)
        icounter = icounter + 1;

        p = icounter;

        u0i = u0i * u;

        if (l1 && icounter > 1)
            u1i = u1i * u;
        end

        if (l2 && icounter > 2)
            u2i = u2i * u;
        end

        if (l3 && icounter > 3)
            u3i = u3i * u;
        end

        term = term * (p - 0.5) / p;

        tq = tq * qsq;

        tqsum = tqsum + tq;

        told = t;

        tterm = term / (2.0 * p + 3.0);

        tqterm = tterm * tqsum;

        t = t - u0i * ((1.5 * p + 0.25) * tqterm / (p * p - 0.25) ...
            - ttmold * tq);

        ttmold = tterm;

        tqterm = tqterm * p;

        if (l1)
            dt = dt + tqterm * u1i;
        end

        if (l2)
            d2t = d2t + tqterm * u2i * (p - 1.0);
        end

        if (l3)
            d3t = d3t + tqterm * u3i * (p - 1.0) * (p - 2.0);
        end

    end

    if (l3)
        d3t = 8.0 * x * (1.5 * d2t - xsq * d3t);
    end

    if(l2)
        d2t = 2.0 * (2.0 * xsq * d2t - dt);
    end

    if (l1)
        dt = -2.0 * x * dt;
    end

    t = t / xsq;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [vr11, vr12, vr21, vr22, vt11, vt12, vt21, vt22, n] = vlamb(cbmu, r1, r2, thr2, tdelt)

% Gooding lambert support function

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

vr21 = 0;

vr22 = 0;

vt21 = 0;

vt22 = 0;

m = 0;

while (thr2 > (2.0 * pi))
    thr2 = thr2 - 2.0 * pi;

    m = m + 1;
end

thr2 = thr2 / 2.0;

dr = r1 - r2;

r1r2 = r1 * r2;

r1r2th = 4.0 * r1r2 * sin(thr2)^2;

csq = dr^2 + r1r2th;

c = sqrt(csq);

s = (r1 + r2 + c) / 2.0;

gms = sqrt(cbmu * s / 2.0);

qsqfm1 = c/s;

q = sqrt(r1 * r2) * cos(thr2) / s;

if (c ~= 0.0)
    rho = dr / c;

    sig = r1r2th /csq;
else
    rho = 0.0;

    sig = 1.0;
end

t = 4.0 * gms * tdelt / s^2;

[x1, x2, n] = xlamb(m, q, qsqfm1, t);

% proceed for single solution, or a pair

for i = 1:1:n
    if (i == 1)
        x = x1;
    else
        x = x2;
    end

    [qzminx, qzplx, zplqx] = tlamb(m, q, qsqfm1, x, -1);

    vt2 = gms * zplqx * sqrt(sig);

    vr1 = gms * (qzminx - qzplx * rho) / r1;

    vt1 = vt2 / r1;

    vr2 = -gms * (qzminx + qzplx * rho) / r2;

    vt2 = vt2 / r2;

    if (i == 1)
        vr11 = vr1;

        vt11 = vt1;

        vr12 = vr2;

        vt12 = vt2;
    else
        vr21 = vr1;

        vt21 = vt1;

        vr22 = vr2;

        vt22 = vt2;
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [x, xpl, n] = xlamb(m, q, qsqfm1, tin)

% Gooding lambert support function

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

tol = 3.0e-7;

xpl = 0.0;

c0 = 1.7;

c1 = 0.5;

c2 = 0.03;

c3 = 0.15;

c41 = 1.0;

c42 = 0.24;

termflag = false;

thr2 = atan2(qsqfm1, 2.0 * q) / pi;

if (m == 0)
    % single rev starter from t (at x = 0) & bilinear (usually)
    
    n = 1;

    [dt, d2t, d3t, t0] = tlamb(m, q, qsqfm1, 0.0, 0.0);

    tdiff = tin - t0;

    if (tdiff <= 0.0)
        x = t0 * tdiff / (-4.0 * tin);
    else
        x = -tdiff / (tdiff + 4.0);

        w = x + c0 * sqrt(2.0 * (1.0 - thr2));

        if (w < 0.0)
            x = x - sqrt(d8rt(-w)) * (x + sqrt(tdiff / (tdiff + 1.5 * t0)));
        end

        w = 4.0 /(4.0 + tdiff);

        x = x * (1.0 + x * (c1 * w - c2 * x * sqrt(w)));
    end
else
    % with multirevs, first get t(min) as basis for starter
    
    xm = 1.0 / (1.5 * (m + 0.5) * pi);

    if (thr2 < 0.5)
        xm = d8rt(2.0 * thr2) * xm;
    end

    if (thr2 > 0.5)
        xm = (2.0 - d8rt(2.0 - 2.0 * thr2)) * xm;
    end;

    % starter for tmin
    
    for i = 1:1:12
        [dt, d2t, d3t, tmin] = tlamb(m, q, qsqfm1, xm, 3);

        if (d2t == 0.0)
            solnflag = true;
            break
        end

        xmold = xm;

        xm = xm - dt * d2t / (d2t * d2t - dt * d3t / 2.0);

        xtest = abs(xmold / xm - 1.0);

        if (xtest <= tol)
            solnflag = true;
            break
        end
    end

    % break off & exit if tmin not located - should never happen
    
    % now proceed from t(min) to full starter
    
    if (solnflag)
        tdiffm = tin - tmin;

        if (tdiffm < 0.0)
            n = 0;

            termflag = true;
            
            % exit if no solution with this m
        elseif (tdiffm == 0)
            x = xm;

            n = 1;

            termflag = true;
            
            % exit if unique solution already from x(tmin)
        else
            n = 3;

            if (d2t == 0)
                d2t = 6.0 * m * pi;
            end

            x = sqrt(tdiffm / (d2t / 2.0 + tdiffm / (1.0 - xm)^2));

            w = xm + x;

            w = w * 4.0 / (4.0 + tdiffm) + (1.0 - w)^2;

            x = x * (1.0 - (1.0 + m + c41 * (thr2 - 0.5)) / (1.0 + c3 * m) ...
                * x * (c1 * w + c2 * x * sqrt(w))) + xm;

            d2t2 = d2t / 2.0;

            if (x >= 1.0)
                n = 1;

                [dt, d2t, d3t, t0] = tlamb(m, q, qsqfm1, 0, 0);


                tdiff0 = t0 - tmin;

                tdiff = tin - t0;

                if (tdiff <= 0.0)
                    x = xm - sqrt(tdiffm / (d2t2 - tdiffm ...
                        * (d2t2 / tdiff0 - 1.0 / xm^2)));
                else
                    x = -tdiff / (tdiff + 4.0);

                    w = x + c0 * sqrt(2.0 * (1.0 - thr2));

                    if (w < 0.0)
                        x = x - sqrt(d8rt(-w)) * (x + sqrt(tdiff / (tdiff + 1.5 * t0)));
                    end

                    w = 4.0 / (4.0 + tdiff);

                    x = x * (1.0 + (1.0 + m + c42 * (thr2 - 0.5)) ...
                        / (1.0 + c3 * m) * x * (c1 * w - c2 * x * sqrt(w)));

                    if (x <= -1.0)
                        n = n - 1;

                        if (n == 1)
                            x = xpl;
                        end
                    end
                end
            end
        end
    else
        n = -1;

        termflag = true;
    end
end

% now have a starter, so proceed by halley

if (termflag ~= true)
    for i = 1:1:3
        [dt, d2t, d3t, t] = tlamb(m, q, qsqfm1, x, 2);

        t = tin - t;

        if (dt ~= 0.0)
            x = x + t * dt / (dt * dt + t * d2t / 2.0);
        end
    end

    if (n ~= 3)
        % exit if only one solution, normally when m = 0
        return
    end

    n = 2;

    xpl = x;

    % second multi-rev starter
    
    [dt, d2t, d3t, t0] = tlamb(m, q, qsqfm1, 0, 0);

    tdiff0 = t0 - tmin;

    tdiff = tin - t0;

    if (tdiff <= 0.0)
        x = xm - sqrt(tdiffm / (d2t2 - tdiffm * (d2t2 / tdiff0 - 1.0 / xm^2)));
    else
        x = -tdiff / (tdiff + 4.0);

        w = x + c0 * sqrt(2.0 * (1.0 - thr2));

        if (w < 0.0)
            x = x - sqrt(d8rt(-w)) * (x + sqrt(tdiff / (tdiff + 1.5 * t0)));
        end

        w = 4.0 / (4.0 + tdiff);

        x = x * (1.0 + (1.0 + m + c42 * (thr2 - 0.5)) / (1.0 + c3 * m) ...
            * x *(c1 * w - c2 * x * sqrt(w)));

        if (x <= -1.0)
            n = n - 1;

            if (n == 1)
                % no finite solution for x < xm
                x = xpl;
            end
        end
    end
end

%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%

function d8rt = d8rt(x)

% Gooding lambert support function

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

d8rt = sqrt(sqrt(sqrt(x)));