696 lines
23 KiB
C++
696 lines
23 KiB
C++
#include "OribtModelOperator.h"
|
||
#include <Eigen/Core>
|
||
#include <Eigen/Dense>
|
||
#include <QMatrix>
|
||
#include <QtMath>
|
||
#include <QDate>
|
||
#include <QTime>
|
||
#include <QDateTime>
|
||
#include <BaseTool.h>
|
||
|
||
OrbitElements::OrbitElements() : semiMajorAxis(0), eccentricity(0), inclination(0),
|
||
longitudeOfAscendingNode(0), argumentOfPeriapsis(0), trueAnomaly(0) {}
|
||
|
||
|
||
/** 六根数轨道拟合模型************************************************************************************/
|
||
|
||
OrbitEstimator::OrbitEstimator() {}
|
||
|
||
void OrbitEstimator::estimateOrbit(const SatellitePos& node) {
|
||
// 根据轨道节点估算轨道六根数的具体算法
|
||
elements = estimateKeplerianElements(node);
|
||
}
|
||
|
||
void OrbitEstimator::saveToXml(const QString& filename) const {
|
||
QDomDocument doc("OrbitElements");
|
||
QDomElement root = doc.createElement("OrbitElements");
|
||
doc.appendChild(root);
|
||
|
||
QDomElement elem;
|
||
|
||
elem = doc.createElement("SemiMajorAxis");
|
||
elem.appendChild(doc.createTextNode(QString::number(elements.semiMajorAxis)));
|
||
root.appendChild(elem);
|
||
|
||
elem = doc.createElement("Eccentricity");
|
||
elem.appendChild(doc.createTextNode(QString::number(elements.eccentricity)));
|
||
root.appendChild(elem);
|
||
|
||
elem = doc.createElement("Inclination");
|
||
elem.appendChild(doc.createTextNode(QString::number(elements.inclination)));
|
||
root.appendChild(elem);
|
||
|
||
elem = doc.createElement("LongitudeOfAscendingNode");
|
||
elem.appendChild(doc.createTextNode(QString::number(elements.longitudeOfAscendingNode)));
|
||
root.appendChild(elem);
|
||
|
||
elem = doc.createElement("ArgumentOfPeriapsis");
|
||
elem.appendChild(doc.createTextNode(QString::number(elements.argumentOfPeriapsis)));
|
||
root.appendChild(elem);
|
||
|
||
elem = doc.createElement("TrueAnomaly");
|
||
elem.appendChild(doc.createTextNode(QString::number(elements.trueAnomaly)));
|
||
root.appendChild(elem);
|
||
|
||
QFile file(filename);
|
||
if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
|
||
qDebug() << "Cannot open file for writing:" << filename;
|
||
return;
|
||
}
|
||
QTextStream out(&file);
|
||
doc.save(out, 4); // Indent by 4 spaces
|
||
file.close();
|
||
}
|
||
|
||
bool OrbitEstimator::loadFromXml(const QString& filename) {
|
||
QFile file(filename);
|
||
if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
|
||
qDebug() << "Cannot open file for reading:" << filename;
|
||
return false;
|
||
}
|
||
|
||
QDomDocument doc;
|
||
if (!doc.setContent(&file)) {
|
||
file.close();
|
||
return false;
|
||
}
|
||
file.close();
|
||
|
||
QDomElement root = doc.documentElement();
|
||
QDomNodeList nodeList = root.elementsByTagName("*");
|
||
|
||
for (int i = 0; i < nodeList.count(); ++i) {
|
||
QDomElement element = nodeList.at(i).toElement();
|
||
if (element.tagName() == "SemiMajorAxis") {
|
||
elements.semiMajorAxis = element.text().toDouble();
|
||
}
|
||
else if (element.tagName() == "Eccentricity") {
|
||
elements.eccentricity = element.text().toDouble();
|
||
}
|
||
else if (element.tagName() == "Inclination") {
|
||
elements.inclination = element.text().toDouble();
|
||
}
|
||
else if (element.tagName() == "LongitudeOfAscendingNode") {
|
||
elements.longitudeOfAscendingNode = element.text().toDouble();
|
||
}
|
||
else if (element.tagName() == "ArgumentOfPeriapsis") {
|
||
elements.argumentOfPeriapsis = element.text().toDouble();
|
||
}
|
||
else if (element.tagName() == "TrueAnomaly") {
|
||
elements.trueAnomaly = element.text().toDouble();
|
||
}
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
SatellitePos OrbitEstimator::calculateNode(double time) const {
|
||
// 根据轨道六根数计算轨道节点的具体算法
|
||
return calculateKeplerianNode(elements, time);
|
||
}
|
||
|
||
SatellitePos OrbitEstimator::predictNode(double time)
|
||
{
|
||
return calculateNode(time);
|
||
}
|
||
|
||
OrbitElements OrbitEstimator::estimateKeplerianElements(const SatellitePos& node) {
|
||
// 示例：简单地将所有值设为0
|
||
OrbitElements elements;
|
||
elements.semiMajorAxis = 0;
|
||
elements.eccentricity = 0;
|
||
elements.inclination = 0;
|
||
elements.longitudeOfAscendingNode = 0;
|
||
elements.argumentOfPeriapsis = 0;
|
||
elements.trueAnomaly = 0;
|
||
return elements;
|
||
}
|
||
|
||
SatellitePos OrbitEstimator::calculateKeplerianNode(const OrbitElements& elements, double time) const {
|
||
const double G = 6.67430e-11; // 引力常数 m^3 kg^-1 s^-2
|
||
const double M_Earth = 5.972e24; // 地球质量 kg
|
||
const double mu = G * M_Earth; // 地球引力参数 m^3 s^-2
|
||
|
||
double a = elements.semiMajorAxis;
|
||
double e = elements.eccentricity;
|
||
double i = elements.inclination;
|
||
double Omega = elements.longitudeOfAscendingNode;
|
||
double omega = elements.argumentOfPeriapsis;
|
||
|
||
// 计算平均运动
|
||
double n = std::sqrt(mu / (a * a * a));
|
||
|
||
// 计算从参考时刻到给定时间的平均异常
|
||
double t_ref = 0.0; // 参考时刻，假设为0
|
||
double M = n * (time - t_ref);
|
||
|
||
// 求解开普勒方程 E - e*sin(E) = M 使用牛顿迭代法
|
||
double E = M; // 初始猜测
|
||
double tolerance = 1e-8;
|
||
int maxIterations = 100;
|
||
for (int iter = 0; iter < maxIterations; ++iter) {
|
||
double f = E - e * std::sin(E) - M;
|
||
double df = 1 - e * std::cos(E);
|
||
double delta = f / df;
|
||
E -= delta;
|
||
if (std::abs(delta) < tolerance) {
|
||
break;
|
||
}
|
||
}
|
||
|
||
// 计算偏近点角 ν
|
||
double sin_nu = std::sqrt(1 - e * e) * std::sin(E) / (1 - e * std::cos(E));
|
||
double cos_nu = (std::cos(E) - e) / (1 - e * std::cos(E));
|
||
double nu = std::atan2(sin_nu, cos_nu);
|
||
|
||
// 计算半径 r
|
||
double r = a * (1 - e * std::cos(E));
|
||
|
||
// 计算位置向量在轨道平面上的分量
|
||
double Px_prime = r * std::cos(nu);
|
||
double Py_prime = r * std::sin(nu);
|
||
|
||
// 计算速度向量在轨道平面上的分量
|
||
double h = std::sqrt(mu * a * (1 - e * e)); // 角动量
|
||
double Vx_prime = -(h / r) * std::sin(nu);
|
||
double Vy_prime = (h / r) * (std::cos(nu) + e);
|
||
|
||
// 将轨道平面坐标系转换到惯性坐标系
|
||
double cos_Omega = std::cos(Omega);
|
||
double sin_Omega = std::sin(Omega);
|
||
double cos_omega = std::cos(omega);
|
||
double sin_omega = std::sin(omega);
|
||
double cos_i = std::cos(i);
|
||
double sin_i = std::sin(i);
|
||
|
||
double Px = Px_prime * (cos_Omega * cos_omega - sin_Omega * sin_omega * cos_i) +
|
||
Py_prime * (-cos_Omega * sin_omega - sin_Omega * cos_omega * cos_i);
|
||
double Py = Px_prime * (sin_Omega * cos_omega + cos_Omega * sin_omega * cos_i) +
|
||
Py_prime * (-sin_Omega * sin_omega + cos_Omega * cos_omega * cos_i);
|
||
double Pz = Px_prime * sin_omega * sin_i + Py_prime * cos_omega * sin_i;
|
||
|
||
double Vx = Vx_prime * (cos_Omega * cos_omega - sin_Omega * sin_omega * cos_i) +
|
||
Vy_prime * (-cos_Omega * sin_omega - sin_Omega * cos_omega * cos_i);
|
||
double Vy = Vx_prime * (sin_Omega * cos_omega + cos_Omega * sin_omega * cos_i) +
|
||
Vy_prime * (-sin_Omega * sin_omega + cos_Omega * cos_omega * cos_i);
|
||
double Vz = Vx_prime * sin_omega * sin_i + Vy_prime * cos_omega * sin_i;
|
||
|
||
SatellitePos node;
|
||
node.time = time;
|
||
node.Px = Px;
|
||
node.Py = Py;
|
||
node.Pz = Pz;
|
||
node.Vx = Vx;
|
||
node.Vy = Vy;
|
||
node.Vz = Vz;
|
||
return node;
|
||
}
|
||
|
||
double OrbitEstimator::solveKeplerEquation(double M, double e) const {
|
||
double E = M; // 初始猜测
|
||
double tolerance = 1e-8;
|
||
int maxIterations = 100;
|
||
for (int iter = 0; iter < maxIterations; ++iter) {
|
||
double f = E - e * std::sin(E) - M;
|
||
double df = 1 - e * std::cos(E);
|
||
double delta = f / df;
|
||
E -= delta;
|
||
if (std::abs(delta) < tolerance) {
|
||
break;
|
||
}
|
||
}
|
||
return E;
|
||
}
|
||
|
||
/** 多项式轨道拟合模型************************************************************************************/
|
||
|
||
void PolynomialOrbitModel::fitPolynomial(const QList<SatellitePos>& nodes, int degree) {
|
||
|
||
QList<double> times;
|
||
QList<double> positionsX;
|
||
QList<double> positionsY;
|
||
QList<double> positionsZ;
|
||
QList<double> velocitiesX;
|
||
QList<double> velocitiesY;
|
||
QList<double> velocitiesZ;
|
||
|
||
for (const auto& node : nodes) {
|
||
times.append(node.time);
|
||
positionsX.append(node.Px);
|
||
positionsY.append(node.Py);
|
||
positionsZ.append(node.Pz);
|
||
velocitiesX.append(node.Vx);
|
||
velocitiesY.append(node.Vy);
|
||
velocitiesZ.append(node.Vz);
|
||
}
|
||
|
||
qDebug() << "fit degree " << degree;
|
||
|
||
coefficientsX = leastSquaresFit(times, positionsX, degree);
|
||
coefficientsY = leastSquaresFit(times, positionsY, degree);
|
||
coefficientsZ = leastSquaresFit(times, positionsZ, degree);
|
||
coefficientsVx = leastSquaresFit(times, velocitiesX, degree);
|
||
coefficientsVy = leastSquaresFit(times, velocitiesY, degree);
|
||
coefficientsVz = leastSquaresFit(times, velocitiesZ, degree);
|
||
}
|
||
|
||
SatellitePos PolynomialOrbitModel::predictPosition(double time) {
|
||
double Px = 0.0;
|
||
double Py = 0.0;
|
||
double Pz = 0.0;
|
||
double Vx = 0.0;
|
||
double Vy = 0.0;
|
||
double Vz = 0.0;
|
||
|
||
for (int i = 0; i < coefficientsX.size(); ++i) {
|
||
Px += coefficientsX(i) * std::pow(time, i);
|
||
Py += coefficientsY(i) * std::pow(time, i);
|
||
Pz += coefficientsZ(i) * std::pow(time, i);
|
||
Vx += coefficientsVx(i) * std::pow(time, i);
|
||
Vy += coefficientsVy(i) * std::pow(time, i);
|
||
Vz += coefficientsVz(i) * std::pow(time, i);
|
||
}
|
||
|
||
SatellitePos predictedNode;
|
||
predictedNode.time = time;
|
||
predictedNode.Px = Px;
|
||
predictedNode.Py = Py;
|
||
predictedNode.Pz = Pz;
|
||
predictedNode.Vx = Vx;
|
||
predictedNode.Vy = Vy;
|
||
predictedNode.Vz = Vz;
|
||
return predictedNode;
|
||
}
|
||
|
||
Eigen::VectorXd PolynomialOrbitModel::leastSquaresFit(const QList<double>& x, const QList<double>& y, int degree) {
|
||
int n = x.size();
|
||
Eigen::MatrixXd A=Eigen::MatrixXd::Zero(n, degree + 1);
|
||
|
||
for (int i = 0; i < n; ++i) {
|
||
for (int j = 0; j <= degree; ++j) {
|
||
A(i, j) = std::pow(x[i], j);
|
||
}
|
||
}
|
||
|
||
Eigen::VectorXd b(y.count());
|
||
|
||
for (long i = 0; i < y.count(); i++) {
|
||
b(i) = y[i];
|
||
}
|
||
|
||
|
||
|
||
Eigen::VectorXd coefficients = A.colPivHouseholderQr().solve(b);
|
||
//Eigen::Vector2d coefficientsVector = Eigen::Vector2d::Zero(degree);
|
||
//coefficientsVector = coefficients.array();
|
||
return coefficients;
|
||
}
|
||
|
||
void PolynomialOrbitModel::saveCoefficientsToXml(QDomElement& parent, const Eigen::VectorXd& coefficients, const QString& tagName) const {
|
||
QDomDocument doc = parent.ownerDocument();
|
||
QDomElement coeffElement = doc.createElement(tagName);
|
||
for (int i = 0; i < coefficients.size(); ++i) {
|
||
QDomElement valueElement = doc.createElement("Value");
|
||
valueElement.appendChild(doc.createTextNode(QString::number(coefficients(i))));
|
||
coeffElement.appendChild(valueElement);
|
||
}
|
||
parent.appendChild(coeffElement);
|
||
}
|
||
|
||
bool PolynomialOrbitModel::loadCoefficientsFromXml(const QDomElement& parent, Eigen::VectorXd& coefficients, const QString& tagName) {
|
||
QDomNodeList coeffNodes = parent.elementsByTagName(tagName);
|
||
if (coeffNodes.isEmpty()) {
|
||
qWarning() << "Tag" << tagName << "not found in XML.";
|
||
return false;
|
||
}
|
||
|
||
QDomElement coeffElement = coeffNodes.at(0).toElement();
|
||
QDomNodeList valueNodes = coeffElement.elementsByTagName("Value");
|
||
|
||
int size = valueNodes.count();
|
||
coefficients.resize(size);
|
||
|
||
for (int i = 0; i < size; ++i) {
|
||
QDomElement valueElement = valueNodes.at(i).toElement();
|
||
coefficients(i) = valueElement.text().toDouble();
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
void PolynomialOrbitModel::saveToXml(const QString& filename) const {
|
||
QDomDocument doc("PolynomialOrbitModel");
|
||
QDomElement root = doc.createElement("PolynomialOrbitModel");
|
||
doc.appendChild(root);
|
||
|
||
saveCoefficientsToXml(root, coefficientsX, "CoefficientsX");
|
||
saveCoefficientsToXml(root, coefficientsY, "CoefficientsY");
|
||
saveCoefficientsToXml(root, coefficientsZ, "CoefficientsZ");
|
||
saveCoefficientsToXml(root, coefficientsVx, "CoefficientsVx");
|
||
saveCoefficientsToXml(root, coefficientsVy, "CoefficientsVy");
|
||
saveCoefficientsToXml(root, coefficientsVz, "CoefficientsVz");
|
||
|
||
QFile file(filename);
|
||
if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
|
||
qDebug() << "Cannot open file for writing:" << filename;
|
||
return;
|
||
}
|
||
QTextStream out(&file);
|
||
doc.save(out, 4); // Indent by 4 spaces
|
||
file.close();
|
||
}
|
||
|
||
bool PolynomialOrbitModel::loadFromXml(const QString& filename) {
|
||
QFile file(filename);
|
||
if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
|
||
qDebug() << "Cannot open file for reading:" << filename;
|
||
return false;
|
||
}
|
||
|
||
QDomDocument doc;
|
||
if (!doc.setContent(&file)) {
|
||
file.close();
|
||
return false;
|
||
}
|
||
file.close();
|
||
|
||
QDomElement root = doc.documentElement();
|
||
|
||
if (!loadCoefficientsFromXml(root, coefficientsX, "CoefficientsX")) return false;
|
||
if (!loadCoefficientsFromXml(root, coefficientsY, "CoefficientsY")) return false;
|
||
if (!loadCoefficientsFromXml(root, coefficientsZ, "CoefficientsZ")) return false;
|
||
if (!loadCoefficientsFromXml(root, coefficientsVx, "CoefficientsVx")) return false;
|
||
if (!loadCoefficientsFromXml(root, coefficientsVy, "CoefficientsVy")) return false;
|
||
if (!loadCoefficientsFromXml(root, coefficientsVz, "CoefficientsVz")) return false;
|
||
|
||
return true;
|
||
}
|
||
|
||
SatellitePos PolynomialOrbitModel::predictNode(double time)
|
||
{
|
||
return predictPosition(time);
|
||
}
|
||
|
||
|
||
|
||
|
||
/** 两行根数************************************************************************************/
|
||
|
||
TwoLineElements::TwoLineElements() {}
|
||
|
||
bool TwoLineElements::loadFromTleFile(const QString& filename) {
|
||
QFile file(filename);
|
||
if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
|
||
qDebug() << "Cannot open file for reading:" << filename;
|
||
return false;
|
||
}
|
||
|
||
QTextStream in(&file);
|
||
line1 = in.readLine().toStdString();
|
||
line2 = in.readLine().toStdString();
|
||
|
||
if (line1.empty() || line2.empty()) {
|
||
qDebug() << "Invalid TLE data in file:" << filename;
|
||
return false;
|
||
}
|
||
|
||
file.close();
|
||
|
||
parseTleLines();
|
||
return true;
|
||
}
|
||
|
||
void TwoLineElements::parseTleLines() {
|
||
// 解析第一行
|
||
// 示例：1 25544U 98067A 23044.15434167 .00001971 00000-0 39389-4 0 9994
|
||
// 第二行
|
||
// 示例：2 25544 51.6416 243.5887 0001672 130.5360 325.0288 15.49472743275958
|
||
|
||
// 这里仅做基本解析，实际应用中需要更严格的验证和错误处理
|
||
}
|
||
|
||
double TwoLineElements::meanMotion() const {
|
||
// 第二行第53-63位是平均运动（每分钟转数）
|
||
return std::stod(line2.substr(52, 11));
|
||
}
|
||
|
||
double TwoLineElements::semiMajorAxis() const {
|
||
const double G = 6.67430e-11; // 引力常数 m^3 kg^-1 s^-2
|
||
const double M_Earth = 5.972e24; // 地球质量 kg
|
||
const double mu = G * M_Earth; // 地球引力参数 m^3 s^-2
|
||
double n = meanMotion() * 2 * M_PI / 60; // 转换为弧度/秒
|
||
return std::cbrt(mu / (n * n));
|
||
}
|
||
|
||
double TwoLineElements::eccentricity() const {
|
||
// 第二行第9-16位是离心率（小数点省略）
|
||
return std::stod("0." + line2.substr(8, 7));
|
||
}
|
||
|
||
double TwoLineElements::inclination() const {
|
||
// 第二行第20-25位是倾斜角（度）
|
||
return std::stod(line2.substr(19, 5));
|
||
}
|
||
|
||
double TwoLineElements::longitudeOfAscendingNode() const {
|
||
// 第二行第34-42位是升交点赤经（度）
|
||
return std::stod(line2.substr(33, 9));
|
||
}
|
||
|
||
double TwoLineElements::argumentOfPeriapsis() const {
|
||
// 第二行第45-52位是近地点幅角（度）
|
||
return std::stod(line2.substr(44, 8));
|
||
}
|
||
|
||
double TwoLineElements::meanAnomaly() const {
|
||
// 第二行第64-72位是平均近点角（度）
|
||
return std::stod(line2.substr(63, 9));
|
||
}
|
||
|
||
double TwoLineElements::revolutionNumberAtEpoch() const {
|
||
// 第二行第64-72位是绕地次数
|
||
return std::stod(line2.substr(63, 5));
|
||
}
|
||
|
||
double TwoLineElements::epochTime() const {
|
||
// 第一行第20-32位是历元时间
|
||
int year = std::stoi(line1.substr(18, 2)) + 2000;
|
||
double dayOfYear = std::stod(line1.substr(20, 12));
|
||
int month = 1, day = 1;
|
||
while (dayOfYear > 0) {
|
||
int daysInMonth[] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
|
||
if ((year % 4 == 0 && year % 100 != 0) || (year % 400 == 0)) {
|
||
daysInMonth[1] = 29;
|
||
}
|
||
if (dayOfYear <= daysInMonth[month - 1]) {
|
||
day = static_cast<int>(dayOfYear);
|
||
break;
|
||
}
|
||
dayOfYear -= daysInMonth[month - 1];
|
||
month++;
|
||
}
|
||
QDate date(year, month, day);
|
||
QTime time(0, 0, 0);
|
||
QDateTime dateTime(date, time);
|
||
return dateTime.toSecsSinceEpoch() + (dayOfYear - day) * 86400;
|
||
}
|
||
|
||
SatellitePos TwoLineElements::calculateNode(double time) const {
|
||
return calculateKeplerianNode(time);
|
||
}
|
||
|
||
SatellitePos TwoLineElements::predictNode(double time)
|
||
{
|
||
return calculateNode(time);
|
||
}
|
||
|
||
SatellitePos TwoLineElements::calculateKeplerianNode(double time) const {
|
||
const double G = 6.67430e-11; // 引力常数 m^3 kg^-1 s^-2
|
||
const double M_Earth = 5.972e24; // 地球质量 kg
|
||
const double mu = G * M_Earth; // 地球引力参数 m^3 s^-2
|
||
|
||
double a = semiMajorAxis();
|
||
double e = eccentricity();
|
||
double i = inclination() * M_PI / 180.0;
|
||
double Omega = longitudeOfAscendingNode() * M_PI / 180.0;
|
||
double omega = argumentOfPeriapsis() * M_PI / 180.0;
|
||
double M0 = meanAnomaly() * M_PI / 180.0;
|
||
|
||
// 计算平均运动
|
||
double n = std::sqrt(mu / (a * a * a));
|
||
|
||
// 计算从参考时刻到给定时间的平均异常
|
||
double t_ref = epochTime(); // 参考时刻
|
||
double M = M0 + n * (time - t_ref);
|
||
|
||
// 求解开普勒方程 E - e*sin(E) = M 使用牛顿迭代法
|
||
double E = solveKeplerEquation(M, e);
|
||
|
||
// 计算偏近点角 ν
|
||
double sin_nu = std::sqrt(1 - e * e) * std::sin(E) / (1 - e * std::cos(E));
|
||
double cos_nu = (std::cos(E) - e) / (1 - e * std::cos(E));
|
||
double nu = std::atan2(sin_nu, cos_nu);
|
||
|
||
// 计算半径 r
|
||
double r = a * (1 - e * std::cos(E));
|
||
|
||
// 计算位置向量在轨道平面上的分量
|
||
double Px_prime = r * std::cos(nu);
|
||
double Py_prime = r * std::sin(nu);
|
||
|
||
// 计算速度向量在轨道平面上的分量
|
||
double h = std::sqrt(mu * a * (1 - e * e)); // 角动量
|
||
double Vx_prime = -(h / r) * std::sin(nu);
|
||
double Vy_prime = (h / r) * (std::cos(nu) + e);
|
||
|
||
// 将轨道平面坐标系转换到惯性坐标系
|
||
double cos_Omega = std::cos(Omega);
|
||
double sin_Omega = std::sin(Omega);
|
||
double cos_omega = std::cos(omega);
|
||
double sin_omega = std::sin(omega);
|
||
double cos_i = std::cos(i);
|
||
double sin_i = std::sin(i);
|
||
|
||
double Px = Px_prime * (cos_Omega * cos_omega - sin_Omega * sin_omega * cos_i) +
|
||
Py_prime * (-cos_Omega * sin_omega - sin_Omega * cos_omega * cos_i);
|
||
double Py = Px_prime * (sin_Omega * cos_omega + cos_Omega * sin_omega * cos_i) +
|
||
Py_prime * (-sin_Omega * sin_omega + cos_Omega * cos_omega * cos_i);
|
||
double Pz = Px_prime * sin_omega * sin_i + Py_prime * cos_omega * sin_i;
|
||
|
||
double Vx = Vx_prime * (cos_Omega * cos_omega - sin_Omega * sin_omega * cos_i) +
|
||
Vy_prime * (-cos_Omega * sin_omega - sin_Omega * cos_omega * cos_i);
|
||
double Vy = Vx_prime * (sin_Omega * cos_omega + cos_Omega * sin_omega * cos_i) +
|
||
Vy_prime * (-sin_Omega * sin_omega + cos_Omega * cos_omega * cos_i);
|
||
double Vz = Vx_prime * sin_omega * sin_i + Vy_prime * cos_omega * sin_i;
|
||
|
||
SatellitePos node;
|
||
node.time = time;
|
||
node.Px = Px;
|
||
node.Py = Py;
|
||
node.Pz = Pz;
|
||
node.Vx = Vx;
|
||
node.Vy = Vy;
|
||
node.Vz = Vz;
|
||
return node;
|
||
}
|
||
|
||
double TwoLineElements::solveKeplerEquation(double M, double e) const {
|
||
double E = M; // 初始猜测
|
||
double tolerance = 1e-8;
|
||
int maxIterations = 100;
|
||
for (int iter = 0; iter < maxIterations; ++iter) {
|
||
double f = E - e * std::sin(E) - M;
|
||
double df = 1 - e * std::cos(E);
|
||
double delta = f / df;
|
||
E -= delta;
|
||
if (std::abs(delta) < tolerance) {
|
||
break;
|
||
}
|
||
}
|
||
return E;
|
||
}
|
||
|
||
|
||
|
||
|
||
/** 轨道节点文件读写************************************************************************************/
|
||
|
||
QList<SatellitePos> loadNodesFromXml(const QString& filename) {
|
||
QFile file(filename);
|
||
if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
|
||
qDebug() << "Cannot open file for reading:" << filename;
|
||
return {};
|
||
}
|
||
|
||
QDomDocument doc;
|
||
if (!doc.setContent(&file)) {
|
||
file.close();
|
||
return {};
|
||
}
|
||
file.close();
|
||
|
||
QDomElement root = doc.documentElement();
|
||
QDomNodeList gpsParamNodes = root.elementsByTagName("GPSParam");
|
||
|
||
QList<SatellitePos> loadedNodes;
|
||
|
||
for (int i = 0; i < gpsParamNodes.count(); ++i) {
|
||
QDomElement paramElement = gpsParamNodes.at(i).toElement();
|
||
|
||
SatellitePos node;
|
||
node.time = convertToMilliseconds(paramElement.firstChildElement("TimeStamp").text().toStdString());
|
||
node.Px = paramElement.firstChildElement("xPosition").text().toDouble();
|
||
node.Py = paramElement.firstChildElement("yPosition").text().toDouble();
|
||
node.Pz = paramElement.firstChildElement("zPosition").text().toDouble();
|
||
node.Vx = paramElement.firstChildElement("xVelocity").text().toDouble();
|
||
node.Vy = paramElement.firstChildElement("yVelocity").text().toDouble();
|
||
node.Vz = paramElement.firstChildElement("zVelocity").text().toDouble();
|
||
|
||
loadedNodes.append(node);
|
||
}
|
||
|
||
return loadedNodes;
|
||
}
|
||
|
||
void saveNodesToXml(const QString& filename, const QList<SatellitePos>& inposes) {
|
||
QDomDocument doc("GPSData");
|
||
QDomElement root = doc.createElement("GPS");
|
||
doc.appendChild(root);
|
||
|
||
for (const auto& node : inposes) {
|
||
QDomElement paramElement = doc.createElement("GPSParam");
|
||
|
||
QDomElement timeStampElement = doc.createElement("TimeStamp");
|
||
timeStampElement.appendChild(doc.createTextNode(QString::number(node.time)));
|
||
paramElement.appendChild(timeStampElement);
|
||
|
||
QDomElement xPositionElement = doc.createElement("xPosition");
|
||
xPositionElement.appendChild(doc.createTextNode(QString::number(node.Px)));
|
||
paramElement.appendChild(xPositionElement);
|
||
|
||
QDomElement yPositionElement = doc.createElement("yPosition");
|
||
yPositionElement.appendChild(doc.createTextNode(QString::number(node.Py)));
|
||
paramElement.appendChild(yPositionElement);
|
||
|
||
QDomElement zPositionElement = doc.createElement("zPosition");
|
||
zPositionElement.appendChild(doc.createTextNode(QString::number(node.Pz)));
|
||
paramElement.appendChild(zPositionElement);
|
||
|
||
QDomElement xVelocityElement = doc.createElement("xVelocity");
|
||
xVelocityElement.appendChild(doc.createTextNode(QString::number(node.Vx)));
|
||
paramElement.appendChild(xVelocityElement);
|
||
|
||
QDomElement yVelocityElement = doc.createElement("yVelocity");
|
||
yVelocityElement.appendChild(doc.createTextNode(QString::number(node.Vy)));
|
||
paramElement.appendChild(yVelocityElement);
|
||
|
||
QDomElement zVelocityElement = doc.createElement("zVelocity");
|
||
zVelocityElement.appendChild(doc.createTextNode(QString::number(node.Vz)));
|
||
paramElement.appendChild(zVelocityElement);
|
||
|
||
root.appendChild(paramElement);
|
||
}
|
||
|
||
QFile file(filename);
|
||
if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
|
||
qDebug() << "Cannot open file for writing:" << filename;
|
||
return;
|
||
}
|
||
QTextStream out(&file);
|
||
doc.save(out, 4); // Indent by 4 spaces
|
||
file.close();
|
||
}
|
||
|
||
OrbitModelAbstract::OrbitModelAbstract()
|
||
{
|
||
}
|
||
|
||
OrbitModelAbstract::~OrbitModelAbstract()
|
||
{
|
||
}
|
||
|
||
SatellitePos OrbitModelAbstract::predictNode(double time)
|
||
{
|
||
return SatellitePos();
|
||
}
|