// 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