编辑代码

using System;


    public static void Main()
    {
     //定义未知数
Pos_t sat_pos( double deltan,double sqrtA, double M0,double e,double omega,double OMEGA,double deltaomega,double Cuc,double Crc, double Cic, double Cus,double Crs,double Cis,double i0,double IDOT,double delta_t, double deltat, double X_R,double Y_R, double Z_R,)
{
//计算卫星平均角速度
double n0 = sqrt(GM) / pow(sqrtA, 3);
	double n = n0 + deltan;
//计算平近点角M
	double M = M0 + n * tk;
//计算偏近点角N
double E = M, E0;
	do
	{
		E0 = E;
		E = M + e * sin(E);
	} while (fabs(E - E0) > 1.0e-5);
//计算真近点角f
double cosf = (cos(E) - e) / (1 - e * cos(E));//cosf
	double sinf = sqrt(1 - pow(e, 2)) * sin(E) / (1 - e * cos(E));//sinf
	double fk = atan2(sinf, cosf);
//计算升交距角
double u = fk + omega;
//计算卫星向径
double r = A*(1-e*cos(E));
//计算摄动改正项
double Epsilon_u = Cuc * cos(2 * u) + Cus * sin(2 * u);
double Epsilon_r = Crc * cos(2 * u) + Crs * sin(2 * u);
double Epsilon_i = Cic * cos(2 * u) + Cis * sin(2 * u);
//进行摄动改正
double uk = u + Epsilon_u;
double rk = pow(sqrtA, 2) * (1 - e * cos(E)) + Epsilon_r;
double ik = i0 + Epsilon_i + IDOT * tk;
	//计算卫星在升交点轨道直角坐标系的坐标
	double xk = rk * cos(uk);
	double yk = rk * sin(uk);
	//计算升交点经度
	double L = OMEGA + (deltaomega - Earth_e) * tk - Earth_e * TOE; 
	//计算卫星在地固系下的直角坐标
	double X = xk * cos(L) - yk * cos(ik) * sin(L);
	double Y = xk * sin(L) + yk * cos(ik) * cos(L);
	double Z = yk * sin(ik);
	//通过 Z 轴的旋转变换，对该坐标进行地球自转改正，得到新坐标
	pos_t.X = cos(Earth_e * delta_t) * X + sin(Earth_e * delta_t) * Y;
	pos_t.Y = -sin(Earth_e * delta_t) * X + cos(Earth_e * delta_t) * Y;
	pos_t.Z = Z;
}

    }