KJ135
/
RasterProcessTool
mirror of http://172.16.0.12:5000/LAMPSARToolSoftware/RasterProcessTool.git
4
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
RasterProcessTool/Toolbox/SimulationSARTool/PowerSimulationIncoherent/OribtModelOperator.cpp

694 lines
23 KiB
C++
Raw Normal View History

调整主窗口大小
2025-02-12 01:34:03 +00:00
#include "OribtModelOperator.h"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <QMatrix>
#include <QtMath>
#include <QDate>
#include <QTime>
#include <QDateTime>
OrbitElements::OrbitElements() : semiMajorAxis(0), eccentricity(0), inclination(0),
longitudeOfAscendingNode(0), argumentOfPeriapsis(0), trueAnomaly(0) {}
/** <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3>************************************************************************************/
OrbitEstimator::OrbitEstimator() {}
void OrbitEstimator::estimateOrbit(const SatellitePos& node) {
// <20><><EFBFBD>ݹ<EFBFBD><DDB9><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD><EFBFBD>㷨
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 {
// <20><><EFBFBD>ݹ<EFBFBD><DDB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5>ľ<EFBFBD><C4BE><EFBFBD><EFBFBD>㷨
return calculateKeplerianNode(elements, time);
}
SatellitePos OrbitEstimator::predictNode(double time)
{
return calculateNode(time);
}
OrbitElements OrbitEstimator::estimateKeplerianElements(const SatellitePos& node) {
// ʾ<><CABE><EFBFBD><EFBFBD><EFBFBD>򵥵ؽ<F2B5A5B5><D8BD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>Ϊ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; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> m^3 kg^-1 s^-2
const double M_Earth = 5.972e24; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> kg
const double mu = G * M_Earth; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> m^3 s^-2
double a = elements.semiMajorAxis;
double e = elements.eccentricity;
double i = elements.inclination;
double Omega = elements.longitudeOfAscendingNode;
double omega = elements.argumentOfPeriapsis;
// <20><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>˶<EFBFBD>
double n = std::sqrt(mu / (a * a * a));
// <20><><EFBFBD><EFBFBD><EFBFBD>Ӳο<D3B2>ʱ<EFBFBD>̵<EFBFBD><CCB5><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>쳣
double t_ref = 0.0; // <20>ο<EFBFBD>ʱ<EFBFBD>̣<EFBFBD><CCA3><EFBFBD><EFBFBD><EFBFBD>Ϊ0
double M = n * (time - t_ref);
// <20><><EFBFBD>⿪<EFBFBD><E2BFAA><EFBFBD>շ<EFBFBD><D5B7><EFBFBD> E - e*sin(E) = M ʹ<><CAB9>ţ<EFBFBD>ٵ<EFBFBD><D9B5><EFBFBD><EFBFBD><EFBFBD>
double E = M; // <20><>ʼ<EFBFBD>²<EFBFBD>
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;
}
}
// <20><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>
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);
// <20><><EFBFBD><EFBFBD><EFBFBD>뾶 r
double r = a * (1 - e * std::cos(E));
// <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڹ<EFBFBD><DAB9><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>ϵķ<CFB5><C4B7><EFBFBD>
double Px_prime = r * std::cos(nu);
double Py_prime = r * std::sin(nu);
// <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڹ<EFBFBD><DAB9><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>ϵķ<CFB5><C4B7><EFBFBD>
double h = std::sqrt(mu * a * (1 - e * e)); // <20>Ƕ<EFBFBD><C7B6><EFBFBD>
double Vx_prime = -(h / r) * std::sin(nu);
double Vy_prime = (h / r) * (std::cos(nu) + e);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵת<CFB5><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
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; // <20><>ʼ<EFBFBD>²<EFBFBD>
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;
}
/** <20><><EFBFBD><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3>************************************************************************************/
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);
}
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::Vector2d 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);
}
/** <20><><EFBFBD>и<EFBFBD><D0B8><EFBFBD>************************************************************************************/
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() {
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
// ʾ<><CABE><EFBFBD><EFBFBD>1 25544U 98067A 23044.15434167 .00001971 00000-0 39389-4 0 9994
// <20>ڶ<EFBFBD><DAB6><EFBFBD>
// ʾ<><CABE><EFBFBD><EFBFBD>2 25544 51.6416 243.5887 0001672 130.5360 325.0288 15.49472743275958
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD>ϸ<EFBFBD><CFB8><EFBFBD><EFBFBD><EFBFBD>֤<EFBFBD>ʹ<EFBFBD><CDB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
double TwoLineElements::meanMotion() const {
// <20>ڶ<EFBFBD><DAB6>е<EFBFBD>53-63λ<33><CEBB>ƽ<EFBFBD><C6BD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
return std::stod(line2.substr(52, 11));
}
double TwoLineElements::semiMajorAxis() const {
const double G = 6.67430e-11; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> m^3 kg^-1 s^-2
const double M_Earth = 5.972e24; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> kg
const double mu = G * M_Earth; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> m^3 s^-2
double n = meanMotion() * 2 * M_PI / 60; // ת<><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>/<2F><>
return std::cbrt(mu / (n * n));
}
double TwoLineElements::eccentricity() const {
// <20>ڶ<EFBFBD><DAB6>е<EFBFBD>9-16λ<36><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʣ<EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD>ʡ<EFBFBD>ԣ<EFBFBD>
return std::stod("0." + line2.substr(8, 7));
}
double TwoLineElements::inclination() const {
// <20>ڶ<EFBFBD><DAB6>е<EFBFBD>20-25λ<35><CEBB><EFBFBD><EFBFBD>б<EFBFBD>ǣ<EFBFBD><C7A3>ȣ<EFBFBD>
return std::stod(line2.substr(19, 5));
}
double TwoLineElements::longitudeOfAscendingNode() const {
// <20>ڶ<EFBFBD><DAB6>е<EFBFBD>34-42λ<32><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ྭ<EFBFBD><E0BEAD><EFBFBD>ȣ<EFBFBD>
return std::stod(line2.substr(33, 9));
}
double TwoLineElements::argumentOfPeriapsis() const {
// <20>ڶ<EFBFBD><DAB6>е<EFBFBD>45-52λ<32>ǽ<EFBFBD><C7BD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD>ǣ<EFBFBD><C7A3>ȣ<EFBFBD>
return std::stod(line2.substr(44, 8));
}
double TwoLineElements::meanAnomaly() const {
// <20>ڶ<EFBFBD><DAB6>е<EFBFBD>64-72λ<32><CEBB>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD><C7A3>ȣ<EFBFBD>
return std::stod(line2.substr(63, 9));
}
double TwoLineElements::revolutionNumberAtEpoch() const {
// <20>ڶ<EFBFBD><DAB6>е<EFBFBD>64-72λ<32><CEBB><EFBFBD>Ƶش<C6B5><D8B4><EFBFBD>
return std::stod(line2.substr(63, 5));
}
double TwoLineElements::epochTime() const {
// <20><>һ<EFBFBD>е<EFBFBD>20-32λ<32><CEBB><EFBFBD><EFBFBD>Ԫʱ<D4AA><CAB1>
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; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> m^3 kg^-1 s^-2
const double M_Earth = 5.972e24; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> kg
const double mu = G * M_Earth; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 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;
// <20><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>˶<EFBFBD>
double n = std::sqrt(mu / (a * a * a));
// <20><><EFBFBD><EFBFBD><EFBFBD>Ӳο<D3B2>ʱ<EFBFBD>̵<EFBFBD><CCB5><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>쳣
double t_ref = epochTime(); // <20>ο<EFBFBD>ʱ<EFBFBD><CAB1>
double M = M0 + n * (time - t_ref);
// <20><><EFBFBD>⿪<EFBFBD><E2BFAA><EFBFBD>շ<EFBFBD><D5B7><EFBFBD> E - e*sin(E) = M ʹ<><CAB9>ţ<EFBFBD>ٵ<EFBFBD><D9B5><EFBFBD><EFBFBD><EFBFBD>
double E = solveKeplerEquation(M, e);
// <20><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>
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);
// <20><><EFBFBD><EFBFBD><EFBFBD>뾶 r
double r = a * (1 - e * std::cos(E));
// <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڹ<EFBFBD><DAB9><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>ϵķ<CFB5><C4B7><EFBFBD>
double Px_prime = r * std::cos(nu);
double Py_prime = r * std::sin(nu);
// <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڹ<EFBFBD><DAB9><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>ϵķ<CFB5><C4B7><EFBFBD>
double h = std::sqrt(mu * a * (1 - e * e)); // <20>Ƕ<EFBFBD><C7B6><EFBFBD>
double Vx_prime = -(h / r) * std::sin(nu);
double Vy_prime = (h / r) * (std::cos(nu) + e);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵת<CFB5><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
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; // <20><>ʼ<EFBFBD>²<EFBFBD>
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;
}
/** <20><><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5>ļ<EFBFBD><C4BC><EFBFBD>д************************************************************************************/
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 = paramElement.firstChildElement("TimeStamp").text().toDouble();
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();
}