// Copyright (c) CNES 2008 // // This software is part of CelestLab, a CNES toolbox for Scilab // // This software is governed by the CeCILL license under French law and // abiding by the rules of distribution of free software. You can use, // modify and/ or redistribute the software under the terms of the CeCILL // license as circulated by CEA, CNRS and INRIA at the following URL // 'http://www.cecill.info'. function [equin,jacob] = CL__oe_car2equin(pos,vel, mu) // Cartesian to equinoctial orbital elements // // Calling Sequence // [equin, jacob] = CL__oe_car2equin(pos, vel [, mu]) // // Description // //

Converts cartesian orbital elements to equinoctial orbital elements.

//

The transformation jacobian is optionally computed.

//

See Orbital elements for more details.

//
//
// // Parameters // pos: Position vector [x;y;z] [m] (3xN) // vel: Velocity vector [vx;vy;vz] [m/s] (3xN) // mu : (optional) Gravitational constant. [m^3/s^2] (default value is %CL_mu) // equin: Orbital elements adapted to near-circular orbits [sma;ex;ey;hx,hy;L] [m,rad] (6xN) // jacob: (optional) Transformation jacobian (See Orbital elements for more details) (6x6xN) // // Authors // CNES - DCT/SB // // Examples // // Example 1 // pos = [7000.e3; 1000.e3; -500.e3]; // vel = [1.e3; 2.e3; 7e3]; // equin = CL__oe_car2equin(pos, vel); // // // Example 2 // pos = [7000.e3; 1000.e3; -500.e3]; // vel = [1.e3; 2.e3; 7e3]; // [equin, jacob1] = CL__oe_car2equin(pos, vel); // [pos2, vel2, jacob2] = CL__oe_equin2car(equin); // pos2 - pos // => zero // vel2 - vel // => zero // jacob2 * jacob1 // => identity // Declarations: EPS_ORB = CL__dataGetEnv("epsOrb", internal=%t); // Code: if (~exists("mu", "local")); mu = CL__dataGetEnv("mu"); end // Handle [] cases if (pos == [] | vel == []) equin = []; jacob = []; return; end // Check input sizes [pos,vel,N] = CL__checkInputs(pos,3, vel,3); // Check validity of inputs [isvalid,orbit_type] = CL__oe_isValid("car",[pos;vel],mu); if (~isvalid); CL__error("Invalid position velocity (zero norm or colinear)"); end; if (find(orbit_type <> 1) <> []); CL__error("Invalid position velocity (parabolic or hyperbolic orbit)"); end; // Semi major axis r = CL_norm(pos); V = CL_norm(vel); W = CL_cross(pos,vel); a = r ./ (2 - r .* V.^2/mu); esinE = CL_dot(pos,vel) ./ sqrt(mu.*a); // ecosE = r .* V.^2/mu - 1 // Inclination vector Wu = CL_unitVector(W); hx = -Wu(2,:) ./ (1+Wu(3,:)); hy = Wu(1,:) ./ (1+Wu(3,:)); // Eccentricity vector // Coordinates in the "natural frame" // P = first column of matrix R // Q = second column of matrix R h = 1 ./ (1+hx.^2+hy.^2); P = [(1+hx.^2-hy.^2).*h; 2*hx.*hy.*h; -2*hy.*h]; Q = [2*hx.*hy.*h; (1-hx.^2+hy.^2).*h; 2*hx.*h]; X = CL_dot(pos, P); Y = CL_dot(pos, Q); VX = CL_dot(vel, P); VY = CL_dot(vel, Q); ex = CL_norm(W) .* VY ./ mu - X ./ r; ey = -CL_norm(W) .* VX ./ mu - Y ./ r; // L = mean longitude // LE = eccentric longitude nu = 1 ./ (1 + sqrt(1 - ex.^2 - ey.^2)); cosLE = X ./ a + ex - esinE .* nu .* ey; sinLE = Y ./ a + ey + esinE .* nu .* ex; L = atan(sinLE,cosLE) - ex .* sinLE + ey .* cosLE; // Reduce L to [0,2pi[ equin = [a;ex;ey;hx;hy;CL_rMod(L,2*%pi)]; // Jacobian computation (dequin/dcar) if (argn(1) == 2) // jacob(i,j) = d(equin_i)/d(car_j) jacob = zeros(6,6,N); eta = sqrt(1-ex.^2-ey.^2); nu = 1 ./ (1+eta); n = sqrt(mu ./ a.^3); na2 = sqrt(mu.*a); // Partial derivatives of X,Y,VX,VY with respect to ex: dXdex = ey ./ (n .* (1+eta)) .* VX + Y.*VX./(n.*a.*eta) - a; dYdex = ey ./ (n .* (1+eta)) .* VY - X.*VX./(n.*a.*eta); dVXdex = VX.*VY./(n.*a.*eta) - na2 ./ r.^3 .* (a.*ey./(1+eta).*X + X.*Y./eta); dVYdex = -VX.*VX./(n.*a.*eta) - na2 ./ r.^3 .* (a.*ey./(1+eta).*Y - X.*X./eta); // Partial derivatives of X,Y,VX,VY with respect to ey: dXdey = -ex ./ (n .* (1+eta)) .* VX + Y.*VY./(n.*a.*eta); dYdey = -ex ./ (n .* (1+eta)) .* VY - X.*VY./(n.*a.*eta) - a; dVXdey = VY.*VY./(n.*a.*eta) + na2 ./ r.^3 .* (a.*ex./(1+eta).*X - Y.*Y./eta); dVYdey = -VX.*VY./(n.*a.*eta) + na2 ./ r.^3 .* (a.*ex./(1+eta).*Y + X.*Y./eta); for j = 1 : 3 // d(vx,vy,vz)/dex dvdex = dVXdex .* P(j,:) + dVYdex .* Q(j,:); // d(vx,vy,vz)/dey dvdey = dVXdey .* P(j,:) + dVYdey .* Q(j,:); // da/d(x,y,z) and da/d(vx,vy,vz) dadr = 2 * a.^2 ./ r.^3 .* pos(j,:); dadv = 2 ./ (n.^2 .* a) .* vel(j,:); // dex/d(x,y,z) and dex/d(vx,vy,vz) dexdr = -a.*nu.*ex.*eta./r.^3 .* pos(j,:) - ey.*(hy.*VX-hx.*VY)./(na2.*eta) .* Wu(j,:) + eta./na2 .* dvdey; dexdv = 1 ./ mu .* ((2*X.*VY - VX.*Y) .* Q(j,:) - Y.*VY.*P(j,:)) - ey.*(hx.*Y-hy.*X)./(na2.*eta) .* Wu(j,:); // dey/d(x,y,z) and dey/d(vx,vy,vz) deydr = -a.*nu.*ey.*eta./r.^3 .* pos(j,:) + ex.*(hy.*VX-hx.*VY)./(na2.*eta) .* Wu(j,:) - eta./na2 .* dvdex; deydv = 1 ./ mu .* ((2*VX.*Y - X.*VY) .* P(j,:) - X.*VX.*Q(j,:)) + ex.*(hx.*Y-hy.*X)./(na2.*eta) .* Wu(j,:); // dhx/d(x,y,z) and dhx/d(vx,vy,vz) dhxdr = - VX ./ (2*na2.*eta.*h) .* Wu(j,:); dhxdv = X ./ (2*na2.*eta.*h) .* Wu(j,:); // Wyp = -vel(3,:); // normWp = -vel(3,:) .* Wu(2,:) + vel(2,:) .* Wu(3,:); // dhxdx = -Wyp ./ (CL_norm(W)+W(3,:)) + W(2,:) .* (normWp + vel(2,:)) ./ (CL_norm(W)+W(3,:)).^2 // pause // dhy/d(x,y,z) and dhy/d(vx,vy,vz) dhydr = - VY ./ (2*na2.*eta.*h) .* Wu(j,:); dhydv = Y ./ (2*na2.*eta.*h) .* Wu(j,:); // dL/d(x,y,z) and dL/d(vx,vy,vz) dLdr = - vel(j,:) ./ na2 + (hy.*VX - hx.*VY) ./ (na2.*eta) .* Wu(j,:) - nu.*eta./na2 .* (ey.*dvdey + ex.*dvdex); dLdv = -2 ./ na2 .* pos(j,:) + nu .* (ex.*deydv - ey.*dexdv) + 1 ./ na2 .* (hx.*Y - hy.*X) .* Wu(j,:); // Assign results to the output jacobian: jacob(1,j,:) = dadr; jacob(2,j,:) = dexdr; jacob(3,j,:) = deydr; jacob(4,j,:) = dhxdr; jacob(5,j,:) = dhydr; jacob(6,j,:) = dLdr; jacob(1,j+3,:) = dadv; jacob(2,j+3,:) = dexdv; jacob(3,j+3,:) = deydv; jacob(4,j+3,:) = dhxdv; jacob(5,j+3,:) = dhydv; jacob(6,j+3,:) = dLdv; end end endfunction