преобразование вектора состояния в классические орбитальные элементы в орбитальной механике: матричные манипуляции

Question

У меня есть некоторые ошибки, связанные с фрагментом кода из файла большего размера. Существует функциональный файл, который преобразует набор из 10x6 элементов в вектор состояния M = [xyz ydot ydot zdot], где R = [xyz] и V = [xdot udot zdot], каждый из которых имеет 10 значений, в классические орбитальные элементы, то есть [a , е, я, Раан, ш, тета, ч]. Проблема, с которой я сейчас сталкиваюсь, заключается в перезаписи результатов, и в результате я получаю неправильные значения для набора классических орбитальных элементов соответствующего вектора состояния. Вот что у меня есть. Любая помощь приветствуется. Спасибо.

function [a,e,inclination,RA,w,trueanomaly,h]=coe_from_cartesianfinal(r,v)
mu_of_moon=4904.8695;
number=length(r);
vradial=zeros(number,3);
h=zeros(number,1);
H=zeros(number,3);
RA=zeros(number,1);
inclination=zeros(number,1);
w=zeros(number,1);
trueanomaly=zeros(number,1);
a=zeros(number,1);
N=zeros(number,3);
n=zeros(number,1);
E=zeros(number,3);
e=zeros(number,1);
rmagnitude=zeros(number,1);
vmagnitude=zeros(number,1);
K=repmat([0 0 1],number,1);
for k=1:number
rmagnitude(k)=norm(r(k)); %in km
vmagnitude(k)=norm(v(k)); % in km/s
vradial(k)=dot(r(k),v(k))/rmagnitude(k);
end
for k=1:number
H=cross(r,v); %%angular momentum vector
h(k)=norm(H);

inclination(k)=acos(H(k,3)/h(k));
N=cross(K,H);
n(k)=norm(N);
cp=cross(N,r);

if n(k)~=0
    RA(k)=acos(N(k,1)/n(k));
    if N(k,2)<0
        RA(k)=2*pi-RA(k);
    end
else
    RA=0;
end

E=(1/mu_of_moon)*(((vmagnitude(k).^2-mu_of_moon/rmagnitude(k)).*r)-(rmagnitude(k)*vradial(k)*v(k)));
e(k)=sqrt(dot(E(k),E(k)));
eps=1.e-10;

if n(k)~=0
    if e(k)>eps
        w(k)=acos(dot(N(k),E(k))/(n(k)*e(k)));
    if E(k,3)>=0
        w(k)=0+w(k);
    else
        w(k)=2*pi-w(k);
    end
    else 
        w(k)=0;
    end
else
    w(k)=0;
end

if e(k)>eps
    trueanomaly(k)=acos(dot(E(k),r(k))/(e(k)*rmagnitude(k)));
    if vradial(k)<0
    trueanomaly(k)=2*pi - trueanomaly(k);
    end
else

    if cp(k,3)>=0
        trueanomaly(k)=acos(dot(N(k),r(k))/(n(k)*rmagnitude(k)));
    else
        trueanomaly(k)=2*pi- acos(dot(N(k),r(k))/(n(k)*rmagnitude(k)));
    end
end

%%for hyperbola
a(k)=(h(k).^2/mu_of_moon)*(1/(1-e(k).^2));

end
return

%%main script
%%x and y precalculated
R=[x y z];
V=[xdot ydot zdot];
R =

   1.0e+03 *

  -1.539543931038617  -1.071744082094234  -0.126366771509053
  -1.330687701629566  -1.342081271655471  -0.415930037786897
  -1.388204041889298  -1.338783059251117   0.064643782753795
  -1.746536513511584  -0.696456811628313  -0.345306735225741
  -1.035542939142647  -1.546765050831709  -0.137246669037133
  -0.876727514529091  -1.717106335359355  -0.181664490600635
  -0.786828482429288  -1.696549340718617  -0.322180648909427
  -1.353803403176800  -1.358459700082971   0.220334514950041
  -0.624050627308063  -1.768814312117858  -0.433907846555942
  -1.078978929041128  -1.491445175045869   0.065068333728124


V =

  -0.834262318050923   1.264469899064419  -0.560310608663483
  -1.155428432334938   1.031548890177763   0.368071417806823
  -0.765554902854875   0.847528122161641   1.112402132026975
   0.647354488449319  -1.381539397296574  -0.487814775382694
   1.289469931929601  -0.814550810565715  -0.549250177549768
   0.734875876906693  -0.227824624723748  -1.393155006501252
   1.462969211723908  -0.653493221799611  -0.131672557730817
   1.134138427903162  -1.085461076386547   0.276149812946353
   1.504643018131369  -0.468793144378925  -0.252969373590829
  -1.322452638720385   0.955760603071847  -0.022038245050036

[a,e,inclination,RA,w,trueanomaly,h]=coe_from_cartesianfinal(R,V);