orb=rv2orb(r,v)

function orb=rv2orb(r,v)

% Calculation of orbital elements from position end velocity of satellite according to Keplerian lows.
% Calculation of six Keplerian elements wich represent orbit.
%
% INPUT: r=[x;y;z]- position v=[vx;vy;vy] - velocity
% UNITS: km and km/s (r=[25976.458;-5253.797;-3663.962] %km v=[-0.5047;0.4692;-3.0407] %km/s)
%
% OUTPUT: orb = [a;ex; i; Wg; wp; M]
% a  - semi major axis [km]
% ex - eccentricity   []
% i  - inclination    [radians]
% Wg - argument of ascending node [radians]
% wp - argument of perigee [radians]
% M -  mean anomaly [radians]
%
% Author:
% Milan KILIBARDA, Faculty of Civil Engineering, Department of Geodesy and
% Geoinformatics, University of Belgrade, Serbia
% [e-mail]
% May, 1, 2008

% Do some input checking

mi=398600.8; %km/s^2
h=cross(r,v);
hh=sqrt(h'*h);
rr=sqrt(r'*r);
vv=sqrt(v'*v);
ve=cross(v,h)/mi-(r./rr);
ex=sqrt(ve'*ve);
a=1/(2/rr-vv^2/mi);
Wg=atan2(h(1),-h(2));
 if Wg<0
    Wg=Wg+2*pi;
 end
i=acos(h(3)/hh);

wp=atan2(ve(3)/sin(i),ve(1)/cos(Wg)+ve(3)*tan(Wg)/tan(i));
if wp<0
    wp=wp+2*pi;
end
p=hh^2/mi;
teta=acos((1/ex)*(p/rr-1));
E=2*atan2(sqrt(1-ex)*sin(teta/2),sqrt(1+ex)*cos(teta/2));
if E<0
    E=E+2*pi;
end
M=E-ex*sin(E);

orb=[a ;ex ;i ;Wg; wp; M];