卫星六根数参数预测卫星轨迹

#include <iostream>  
#include <cmath>

using namespace std;

// 地球引力常数 (m³/s²)  
const double MU = 3.986004418e14;

// 三维向量结构体  
struct Vector3D {  
double x, y, z;  
Vector3D(double x_ = 0, double y_ = 0, double z_ = 0) : x(x_), y(y_), z(z_) {}  
};

// 轨道六根数结构体  
struct KeplerianElements {  
double a; // 半长轴 (m)  
double e; // 偏心率  
double i; // 轨道倾角 (rad)  
double Omega; // 升交点赤经 (rad)  
double omega; // 近地点幅角 (rad)  
double M0; // 历元平近点角 (rad)  
double t0; // 历元时间 (s)  
};

// 绕Z轴旋转  
Vector3D rotate_z(const Vector3D& v, double theta) {  
double cos_t = cos(theta);  
double sin_t = sin(theta);  
return Vector3D(  
v.x * cos_t - v.y * sin_t,  
v.x * sin_t + v.y * cos_t,  
v.z  
);  
}

// 绕X轴旋转  
Vector3D rotate_x(const Vector3D& v, double theta) {  
double cos_t = cos(theta);  
double sin_t = sin(theta);  
return Vector3D(  
v.x,  
v.y * cos_t - v.z * sin_t,  
v.y * sin_t + v.z * cos_t  
);  
}

// 解开普勒方程（牛顿迭代法）  
double solveKeplerEquation(double M, double e, double tol = 1e-12, int max_iter = 1000) {  
double E = M; // 初始猜测  
for (int i = 0; i < max_iter; ++i) {  
double delta = E - e * sin(E) - M;  
if (abs(delta) < tol) break;  
E -= delta / (1 - e * cos(E));  
}  
return E;  
}

// 计算卫星在给定时间的位置（ECI坐标系）  
Vector3D calculatePosition(const KeplerianElements& elements, double t) {  
// 计算时间差  
double delta_t = t - elements.t0;  
  
// 计算平均运动  
double n = sqrt(MU / (elements.a * elements.a * elements.a));  
  
// 当前平近点角  
double M = elements.M0 + n * delta_t;  
M = fmod(M, 2 * M_PI);  
if (M < 0) M += 2 * M_PI;  
  
// 解开普勒方程求偏近点角  
double E = solveKeplerEquation(M, elements.e);  
  
// 计算perifocal坐标系坐标  
double x_p = elements.a * (cos(E) - elements.e);  
double y_p = elements.a * sqrt(1 - elements.e*elements.e) * sin(E);  
Vector3D perifocal(x_p, y_p, 0);  
  
// 应用三次旋转变换  
Vector3D v = rotate_z(perifocal, elements.omega); // 近地点幅角旋转  
v = rotate_x(v, elements.i); // 轨道倾角旋转  
v = rotate_z(v, elements.Omega); // 升交点赤经旋转  
  
return v;  
}

int main() {  
// 示例轨道参数（GPS卫星近似值）  
KeplerianElements kepler;  
kepler.a = 26560000; // 半长轴 26,560 km  
kepler.e = 0.01; // 偏心率  
kepler.i = 0.3; // 倾角 ~17.2度（0.3 rad）  
kepler.Omega = 0.5; // 升交点赤经  
kepler.omega = 1.2; // 近地点幅角  
kepler.M0 = 0; // 历元平近点角  
kepler.t0 = 0; // 历元时间  
  
// 计算未来1小时后的位置  
double t = kepler.t0 + 3600;  
Vector3D pos = calculatePosition(kepler, t);  
  
// 输出结果  
cout << "ECI坐标系中的位置 (m):" << endl;  
cout << "X: " << pos.x << endl;  
cout << "Y: " << pos.y << endl;  
cout << "Z: " << pos.z << endl;  
  
return 0;  
}