根据轨道根数解算位置速度

已知轨道根数：半长轴、偏心率、倾角、近地点幅角、近地点时刻、生交点赤经，求解目标（卫星）的位置矢量

clc
clear all
close all

miu = 3.9860047e14;  %地球引力常数(m^3/s^2)
M = @(t,tao,a)(t-tao)*(sqrt(miu/a^3));
v = @(M,a,e)M+e*(2-e^2/4+5*e^4/96)*sin(M)+e^2*(5/4-11*e^2/24)*sin(2*M) ...
    +e^3*(13/12-43*e^2/64)*sin(3*M)+103*e^4*sin(4*M)/96+1097*e^5*sin(5*M)/960;
r = @(v,a,e)a*(1-e.^2)./(1+e.*cos(v));

%三轴转换矩阵
Rx = @(i)[1      0      0
          0  cos(i) sin(i)
          0 -sin(i) cos(i)];  %轨道倾角


Ry = @(theta)[cos(theta)  0  sin(theta)
       0          1      0 
      -sin(theta) 0  cos(theta)];

Rz = @(w)[cos(w)  sin(w) 0
          -sin(w) cos(w) -2
           0       0     1];   %升交点赤经或者近地点幅角


% p0 = [r*cos(v),r*sin(v),0]; 
% p = Rz(-Omiga)*Rx(-i)*Rz(-w)*p0; %各量在J2000惯性坐标系下的矢量坐标


%% 目标卫星轨道
a_o = 6862.8 ; %轨道半长轴 (km)
e_o = 0.001884; %偏心率 
i_o = 98.79*pi/180 ;