454 lines
17 KiB
C++
454 lines
17 KiB
C++
#include "stdafx.h"
|
||
#include "GF3PSTNClass.h"
|
||
#include "GeoOperator.h"
|
||
#include <boost/math/tools/polynomial.hpp> // 多项式
|
||
#include "BaseTool.h"
|
||
#include <QDebug>
|
||
|
||
|
||
|
||
GF3PolyfitSatelliteOribtModel::GF3PolyfitSatelliteOribtModel()
|
||
{
|
||
this->oribtStartTime = 0;
|
||
this->beamAngle = 0;
|
||
this->RightLook = true;
|
||
this->cycletime = 0;
|
||
this->minAzAngle = 0;
|
||
this->maxAzAngle = 0;
|
||
this->referenceAzAngle = 0;
|
||
this->referenceTimeFromStartTime = 0;
|
||
this->AntnnaAxisX = Point3{ 1,0,0 };
|
||
this->AntnnaAxisY = Point3{ 0,1,0 };
|
||
this->AntnnaAxisZ = Point3{ 0,0,1 };
|
||
this->Pxchisq = 0;
|
||
this->Pychisq = 0;
|
||
this->Pzchisq = 0;
|
||
this->Vxchisq = 0;
|
||
this->Vychisq = 0;
|
||
this->Vzchisq = 0;
|
||
this->Pt = 1;
|
||
this->Gri = 1;
|
||
}
|
||
|
||
GF3PolyfitSatelliteOribtModel::~GF3PolyfitSatelliteOribtModel()
|
||
{
|
||
//TODO: 析构函数
|
||
|
||
|
||
}
|
||
|
||
QString GF3PolyfitSatelliteOribtModel::getSatelliteOribtModelParamsString()
|
||
{
|
||
QString result = "";
|
||
result += this->polyfitPx.size() - 1 + "\n";
|
||
result += "----------- poly Position X -------------------------\n";
|
||
for (long i = 0; i < this->polyfitPx.size(); i++) { // 求解平方
|
||
result += QString::number(this->polyfitPx[i], 'e', 6) + "\n";
|
||
}
|
||
result += "----------- poly Position Y -------------------------\n";
|
||
for (long i = 0; i < this->polyfitPx.size(); i++) { // 求解平方
|
||
result += QString::number(this->polyfitPy[i], 'e', 6) + "\n";
|
||
}
|
||
result += "----------- poly Position Z -------------------------\n";
|
||
for (long i = 0; i < this->polyfitPx.size(); i++) { // 求解平方
|
||
result += QString::number(this->polyfitPz[i], 'e', 6) + "\n";
|
||
}
|
||
result += "----------- poly Position Vector X ------------------\n";
|
||
for (long i = 0; i < this->polyfitPx.size(); i++) { // 求解平方
|
||
result += QString::number(this->polyfitVx[i], 'e', 6) + "\n";
|
||
}
|
||
result += "----------- poly Position Vector Y ------------------\n";
|
||
for (long i = 0; i < this->polyfitPx.size(); i++) { // 求解平方
|
||
result += QString::number(this->polyfitVy[i], 'e', 6) + "\n";
|
||
}
|
||
result += "----------- poly Position Vector Z ------------------\n";
|
||
for (long i = 0; i < this->polyfitPx.size(); i++) { // 求解平方
|
||
result += QString::number(this->polyfitVz[i], 'e', 6) + "\n";
|
||
}
|
||
result += "------------------------------------------------------\n";
|
||
printf("%s\n", result.toUtf8().constData());
|
||
return result;
|
||
}
|
||
|
||
SatelliteOribtNode GF3PolyfitSatelliteOribtModel::getSatelliteOribtNode(double& timeFromStartTime, bool& antAzAngleFlag)
|
||
{
|
||
// 位置、速度
|
||
SatelliteOribtNode node;
|
||
ErrorCode state = getSatelliteOribtNode(timeFromStartTime, node, antAzAngleFlag);
|
||
return node;
|
||
}
|
||
|
||
ErrorCode GF3PolyfitSatelliteOribtModel::getSatelliteOribtNode(double& timeFromStartTime, SatelliteOribtNode& node, bool& antAzAngleFlag)
|
||
{
|
||
// 卫星坐标，速度方向
|
||
node.time = timeFromStartTime;
|
||
node.Px = 0;
|
||
node.Py = 0;
|
||
node.Pz = 0;
|
||
node.Vx = 0;
|
||
node.Vy = 0;
|
||
node.Vz = 0;
|
||
|
||
for (long i = 0; i < this->polyfitPx.size(); i++) { // 求解平方
|
||
node.Px += this->polyfitPx[i] * pow(timeFromStartTime, i);
|
||
node.Py += this->polyfitPy[i] * pow(timeFromStartTime, i);
|
||
node.Pz += this->polyfitPz[i] * pow(timeFromStartTime, i);
|
||
node.Vx += this->polyfitVx[i] * pow(timeFromStartTime, i);
|
||
node.Vy += this->polyfitVy[i] * pow(timeFromStartTime, i);
|
||
node.Vz += this->polyfitVz[i] * pow(timeFromStartTime, i);
|
||
}
|
||
|
||
|
||
node.beamAngle = this->beamAngle; // 波位角
|
||
ErrorCode Azstatecode = this->getAzAngleInCurrentTimeFromStartTime(timeFromStartTime, node.AzAngle); // 摆动角
|
||
if (Azstatecode != ErrorCode::SUCCESS) {
|
||
return Azstatecode;
|
||
}
|
||
else {}
|
||
|
||
|
||
if (!antAzAngleFlag) {
|
||
return ErrorCode::SUCCESS;
|
||
}
|
||
else {
|
||
}
|
||
// 计算卫星天线指向
|
||
ErrorCode state = getAntnnaDirection(node);
|
||
|
||
if (state != ErrorCode::SUCCESS) {
|
||
return state;
|
||
}
|
||
else {}
|
||
|
||
state = getZeroDopplerAntDirect(node);
|
||
if (state != ErrorCode::SUCCESS) {
|
||
return state;
|
||
}
|
||
else {}
|
||
return ErrorCode::SUCCESS;
|
||
}
|
||
|
||
|
||
ErrorCode GF3PolyfitSatelliteOribtModel::getSatelliteAntDirectNormal(SatelliteOribtNode& Rs, Vector3D& Rt, SatelliteAntDirect& antNode)
|
||
{
|
||
//Vector3D Rts = Vector3D{Rt.x-Rs.Px,Rt.y- Rs.Py,Rt.z-Rs.Pz}; // Rts t-->s
|
||
antNode.Xst = (Rt.x - Rs.Px); // 卫星 --> 地面
|
||
antNode.Yst = (Rt.y - Rs.Py);
|
||
antNode.Zst = (Rt.z - Rs.Pz);
|
||
antNode.Vxs = Rs.Vx; // 卫星速度
|
||
antNode.Vys = Rs.Vy;
|
||
antNode.Vzs = Rs.Vz;
|
||
// 天线指向在天线坐标系下的值
|
||
antNode.Xant = (antNode.Xst * (Rs.AntYaxisY * Rs.AntZaxisZ - Rs.AntYaxisZ * Rs.AntZaxisY) + antNode.Xst * (Rs.AntXaxisZ * Rs.AntZaxisY - Rs.AntXaxisY * Rs.AntZaxisZ) + antNode.Xst * (Rs.AntXaxisY * Rs.AntYaxisZ - Rs.AntXaxisZ * Rs.AntYaxisY)) / (Rs.AntXaxisX * (Rs.AntYaxisY * Rs.AntZaxisZ - Rs.AntZaxisY * Rs.AntYaxisZ) - Rs.AntYaxisX * (Rs.AntXaxisY * Rs.AntZaxisZ - Rs.AntXaxisZ * Rs.AntZaxisY) + Rs.AntZaxisX * (Rs.AntXaxisY * Rs.AntYaxisZ - Rs.AntXaxisZ * Rs.AntYaxisY));
|
||
antNode.Yant = (antNode.Yst * (Rs.AntYaxisZ * Rs.AntZaxisX - Rs.AntYaxisX * Rs.AntZaxisZ) + antNode.Yst * (Rs.AntXaxisX * Rs.AntZaxisZ - Rs.AntXaxisZ * Rs.AntZaxisX) + antNode.Yst * (Rs.AntYaxisX * Rs.AntXaxisZ - Rs.AntXaxisX * Rs.AntYaxisZ)) / (Rs.AntXaxisX * (Rs.AntYaxisY * Rs.AntZaxisZ - Rs.AntZaxisY * Rs.AntYaxisZ) - Rs.AntYaxisX * (Rs.AntXaxisY * Rs.AntZaxisZ - Rs.AntXaxisZ * Rs.AntZaxisY) + Rs.AntZaxisX * (Rs.AntXaxisY * Rs.AntYaxisZ - Rs.AntXaxisZ * Rs.AntYaxisY));
|
||
antNode.Zant = (antNode.Zst * (Rs.AntYaxisX * Rs.AntZaxisY - Rs.AntYaxisY * Rs.AntZaxisX) + antNode.Zst * (Rs.AntXaxisY * Rs.AntZaxisX - Rs.AntXaxisX * Rs.AntZaxisY) + antNode.Zst * (Rs.AntXaxisX * Rs.AntYaxisY - Rs.AntYaxisX * Rs.AntXaxisY)) / (Rs.AntXaxisX * (Rs.AntYaxisY * Rs.AntZaxisZ - Rs.AntZaxisY * Rs.AntYaxisZ) - Rs.AntYaxisX * (Rs.AntXaxisY * Rs.AntZaxisZ - Rs.AntXaxisZ * Rs.AntZaxisY) + Rs.AntZaxisX * (Rs.AntXaxisY * Rs.AntYaxisZ - Rs.AntXaxisZ * Rs.AntYaxisY));
|
||
// 计算theta 与 phi
|
||
antNode.Norm = std::sqrt(antNode.Xant * antNode.Xant + antNode.Yant * antNode.Yant + antNode.Zant * antNode.Zant); // 计算 pho
|
||
antNode.ThetaAnt = std::acos(antNode.Zant / antNode.Norm); // theta 与 Z轴的夹角
|
||
antNode.PhiAnt = (antNode.Yant * std::sin(antNode.ThetaAnt) / std::abs(antNode.Yant * std::sin(antNode.ThetaAnt))) * std::acos(antNode.Xant / (antNode.Norm * std::sin(antNode.ThetaAnt)));
|
||
return ErrorCode::SUCCESS;
|
||
}
|
||
|
||
void GF3PolyfitSatelliteOribtModel::setSatelliteOribtStartTime(long double oribtStartTime)
|
||
{
|
||
this->oribtStartTime = oribtStartTime;
|
||
}
|
||
|
||
long double GF3PolyfitSatelliteOribtModel::getSatelliteOribtStartTime()
|
||
{
|
||
return this->oribtStartTime;
|
||
}
|
||
|
||
void GF3PolyfitSatelliteOribtModel::setbeamAngle(double beamAngle, bool RightLook)
|
||
{
|
||
this->beamAngle = beamAngle;
|
||
this->RightLook = RightLook;
|
||
}
|
||
|
||
void GF3PolyfitSatelliteOribtModel::setAzAngleRange(double cycletime, double minAzAngle, double maxAzAngle, double referenceAzAngle, double referenceTimeFromStartTime)
|
||
{
|
||
this->cycletime = cycletime;
|
||
this->minAzAngle = minAzAngle;
|
||
this->maxAzAngle = maxAzAngle;
|
||
this->referenceAzAngle = referenceAzAngle;
|
||
this->referenceTimeFromStartTime = referenceTimeFromStartTime;
|
||
}
|
||
|
||
double GF3PolyfitSatelliteOribtModel::getAzAngleInCurrentTimeFromStartTime(double& currentTime)
|
||
{
|
||
// 计算角度
|
||
double Azangle = 0;
|
||
ErrorCode state = getAzAngleInCurrentTimeFromStartTime(currentTime, Azangle);
|
||
return Azangle;
|
||
}
|
||
|
||
///
|
||
/// X=Y x Z 摆动角(-） 摆动角（+）
|
||
/// Z Z Z
|
||
/// ^ \ /
|
||
/// | \ /
|
||
/// y<--X--- y<--X--- y<--X---
|
||
/// | |\ /|
|
||
/// | \ /
|
||
///
|
||
|
||
ErrorCode GF3PolyfitSatelliteOribtModel::getAzAngleInCurrentTimeFromStartTime(double& currentTime, double& AzAngle)
|
||
{
|
||
if (std::abs(this->maxAzAngle - this->minAzAngle) < 1e-7 || std::abs(this->cycletime) < 1e-7) {
|
||
AzAngle = this->referenceAzAngle;
|
||
return ErrorCode::SUCCESS;
|
||
}
|
||
else {
|
||
AzAngle = this->referenceAzAngle + (currentTime - this->referenceTimeFromStartTime) / this->cycletime * (this->maxAzAngle - this->minAzAngle);
|
||
return ErrorCode::SUCCESS;
|
||
}
|
||
}
|
||
|
||
void GF3PolyfitSatelliteOribtModel::setAntnnaAxisX(double X, double Y, double Z)
|
||
{
|
||
this->AntnnaAxisX = Point3{ X, Y, Z };
|
||
}
|
||
|
||
void GF3PolyfitSatelliteOribtModel::setAntnnaAxisY(double X, double Y, double Z)
|
||
{
|
||
this->AntnnaAxisY = Point3{ X, Y, Z };
|
||
}
|
||
|
||
void GF3PolyfitSatelliteOribtModel::setAntnnaAxisZ(double X, double Y, double Z)
|
||
{
|
||
this->AntnnaAxisZ = Point3{ X, Y, Z };
|
||
}
|
||
|
||
/// 左视(+） 右视（-）
|
||
/// Z Z Z
|
||
/// ^ \ /
|
||
/// | \ /
|
||
/// x<--O--- x<--O--- x<--O---
|
||
/// | |\ /|
|
||
/// \ /
|
||
///
|
||
ErrorCode GF3PolyfitSatelliteOribtModel::getAntnnaDirection(SatelliteOribtNode& node)
|
||
{
|
||
bool flag = false;
|
||
double nexttime = node.time + 1e-6;
|
||
SatelliteOribtNode node1 = this->getSatelliteOribtNode(nexttime, flag);
|
||
|
||
//qDebug() << "getAntnnaDirection corrdination " << std::endl;
|
||
|
||
double Vx = (node1.Px - node.Px);
|
||
double Vy = (node1.Py - node.Py);
|
||
double Vz = (node1.Pz - node.Pz);
|
||
|
||
// 代码测试部分
|
||
//node.Px = 0;
|
||
//node.Py = 0;
|
||
//node.Pz = 1;
|
||
|
||
//Vx = 1, Vy = 0, Vz = 0;
|
||
|
||
|
||
// 1. 计算天线指向
|
||
Eigen::Vector3d axisZ0 = { -1 * node.Px ,-1 * node.Py,-1 * node.Pz }; // z 轴 --波位角为0时，天线指向的反方向
|
||
Eigen::Vector3d axisX0 = { Vx,Vy,Vz }; // x 轴 --飞行方向
|
||
Eigen::Vector3d axisY0 = axisZ0.cross(axisX0); // y 轴 --右手定则 -- 初始坐标系
|
||
|
||
//qDebug() << "axis_X0=[ " << axisX0.x() << "," << axisX0.y() << "," << axisX0.z() << "]" << std::endl;
|
||
//qDebug() << "axis_Y0=[ " << axisY0.x() << "," << axisY0.y() << "," << axisY0.z() << "]" << std::endl;
|
||
//qDebug() << "axis_Z0=[ " << axisZ0.x() << "," << axisZ0.y() << "," << axisZ0.z() << "]" << std::endl;
|
||
|
||
double rotateAngle = this->RightLook ? -this->beamAngle : this->beamAngle; // 旋转角度 左（逆时针）：theta , 右（顺时针）： -theta
|
||
//qDebug() << "rotateAngle=" << rotateAngle << std::endl;
|
||
//qDebug() << "Look side:\t" << (this->RightLook ? "right" : "left") << std::endl;
|
||
// 1.2. 根据波位角，确定卫星绕X轴-飞行轴
|
||
Eigen::Matrix3d rotateMatrixBeam = rotationMatrix(axisX0, rotateAngle * d2r); // 旋转矩阵
|
||
axisZ0 = rotateMatrixBeam * axisZ0; // 旋转矩阵
|
||
axisY0 = rotateMatrixBeam * axisY0;
|
||
axisX0 = rotateMatrixBeam * axisX0;
|
||
// 1.3. 根据方位角，确定卫星绕Y轴旋转
|
||
double azangle = node.AzAngle;
|
||
Eigen::Matrix3d rotateMatrixAzAngle = rotationMatrix(axisY0, azangle * d2r); // 旋转矩阵
|
||
axisZ0 = rotateMatrixAzAngle * axisZ0; // 旋转矩阵
|
||
axisY0 = rotateMatrixAzAngle * axisY0;
|
||
axisX0 = rotateMatrixAzAngle * axisX0;
|
||
|
||
// 1.4. 计算实际指向
|
||
node.AntDirecX = axisZ0[0];
|
||
node.AntDirecY = axisZ0[1];
|
||
node.AntDirecZ = axisZ0[2];
|
||
|
||
// 2. 计算天线坐标系,方便计算天线方向图
|
||
|
||
node.AntXaxisX = axisX0[0]; // 实际天线坐标系 在 WGS84 坐标系
|
||
node.AntXaxisY = axisX0[1];
|
||
node.AntXaxisZ = axisX0[2];
|
||
|
||
node.AntYaxisX = axisY0[0];
|
||
node.AntYaxisY = axisY0[1];
|
||
node.AntYaxisZ = axisY0[2];
|
||
|
||
node.AntZaxisX = axisZ0[0];
|
||
node.AntZaxisY = axisZ0[1];
|
||
node.AntZaxisZ = axisZ0[2];
|
||
|
||
|
||
//qDebug() << "axis_X=[" << axisX0.x() << "," << axisX0.y() << "," << axisX0.z() << "]" << std::endl;
|
||
//qDebug() << "axis_Y=[" << axisY0.x() << "," << axisY0.y() << "," << axisY0.z() << "]" << std::endl;
|
||
//qDebug() << "axis_Z=[" << axisZ0.x() << "," << axisZ0.y() << "," << axisZ0.z() << "]" << std::endl;
|
||
//qDebug() << "------------------------------------" << std::endl;
|
||
return ErrorCode::SUCCESS;
|
||
}
|
||
|
||
ErrorCode GF3PolyfitSatelliteOribtModel::getZeroDopplerAntDirect(SatelliteOribtNode& node)
|
||
{
|
||
bool flag = false;
|
||
double nexttime = node.time + 1e-6;
|
||
SatelliteOribtNode node1 = this->getSatelliteOribtNode(nexttime, flag);
|
||
|
||
// 1. 计算天线指向
|
||
Eigen::Vector3d axisZ0 = { -1 * node.Px ,-1 * node.Py,-1 * node.Pz }; // z 轴 --波位角为0时，天线指向的反方向
|
||
Eigen::Vector3d axisX0 = { (node1.Px - node.Px) , (node1.Py - node.Py) , (node1.Pz - node.Pz) }; // x 轴 --飞行方向
|
||
Eigen::Vector3d axisY0 = axisZ0.cross(axisX0); // y 轴 --右手定则 -- 初始坐标系
|
||
|
||
double rotateAngle = this->RightLook ? -this->beamAngle : this->beamAngle; // 旋转角度 左（逆时针）：theta , 右（顺时针）： -theta
|
||
|
||
// 1.2. 根据波位角，确定卫星绕X轴-飞行轴
|
||
Eigen::Matrix3d rotateMatrixBeam = rotationMatrix(axisX0, rotateAngle * d2r); // 旋转矩阵
|
||
axisZ0 = rotateMatrixBeam * axisZ0; // 旋转矩阵
|
||
axisY0 = rotateMatrixBeam * axisY0;
|
||
axisX0 = rotateMatrixBeam * axisX0;
|
||
// 1.3. 根据方位角，确定卫星绕Y轴旋转
|
||
double azangle = 0;
|
||
Eigen::Matrix3d rotateMatrixAzAngle = rotationMatrix(axisY0, azangle * d2r); // 旋转矩阵
|
||
axisZ0 = rotateMatrixAzAngle * axisZ0; // 旋转矩阵
|
||
axisY0 = rotateMatrixAzAngle * axisY0;
|
||
axisX0 = rotateMatrixAzAngle * axisX0;
|
||
|
||
// 1.4. 计算实际指向
|
||
node.zeroDopplerDirectX = axisZ0[0];
|
||
node.zeroDopplerDirectY = axisZ0[1];
|
||
node.zeroDopplerDirectZ = axisZ0[2];
|
||
|
||
return ErrorCode::SUCCESS;
|
||
}
|
||
|
||
void GF3PolyfitSatelliteOribtModel::addOribtNode(SatelliteOribtNode node)
|
||
{
|
||
this->oribtNodes.push_back(node);
|
||
}
|
||
|
||
|
||
|
||
ErrorCode GF3PolyfitSatelliteOribtModel::polyFit(int polynum/*=3*/, bool input_timeFromReferenceTime)
|
||
{
|
||
if (polynum > this->oribtNodes.size() - 1) {
|
||
return ErrorCode::OrbitNodeNotEnough;
|
||
}
|
||
else {}
|
||
|
||
this->polyfitPx = std::vector<double>(polynum + 1, 0);
|
||
this->polyfitPy = std::vector<double>(polynum + 1, 0);
|
||
this->polyfitPz = std::vector<double>(polynum + 1, 0);
|
||
this->polyfitVx = std::vector<double>(polynum + 1, 0);
|
||
this->polyfitVy = std::vector<double>(polynum + 1, 0);
|
||
this->polyfitVz = std::vector<double>(polynum + 1, 0);
|
||
|
||
std::vector<double> timeArr = std::vector<double>(this->oribtNodes.size(), 0);
|
||
std::vector<double> PxArr = std::vector<double>(this->oribtNodes.size(), 0);
|
||
std::vector<double> PyArr = std::vector<double>(this->oribtNodes.size(), 0);
|
||
std::vector<double> PzArr = std::vector<double>(this->oribtNodes.size(), 0);
|
||
std::vector<double> VxArr = std::vector<double>(this->oribtNodes.size(), 0);
|
||
std::vector<double> VyArr = std::vector<double>(this->oribtNodes.size(), 0);
|
||
std::vector<double> VzArr = std::vector<double>(this->oribtNodes.size(), 0);
|
||
|
||
// 数据准备
|
||
for (long i = 0; i < this->oribtNodes.size(); i++) {
|
||
if (input_timeFromReferenceTime) {
|
||
timeArr[i] = this->oribtNodes[i].time;
|
||
}
|
||
else {
|
||
timeArr[i] = this->oribtNodes[i].time - this->oribtStartTime; // 以参考时间为基准
|
||
qDebug() << this->oribtNodes[i].time << "\t--->\t" << timeArr[i];
|
||
}
|
||
PxArr[i] = this->oribtNodes[i].Px;
|
||
PyArr[i] = this->oribtNodes[i].Py;
|
||
PzArr[i] = this->oribtNodes[i].Pz;
|
||
VxArr[i] = this->oribtNodes[i].Vx;
|
||
VyArr[i] = this->oribtNodes[i].Vy;
|
||
VzArr[i] = this->oribtNodes[i].Vz;
|
||
}
|
||
|
||
// 拟合Px Py Pz Vx Vy Vz
|
||
ErrorCode codePx = polynomial_fit(timeArr, PxArr, polynum, this->polyfitPx, this->Pxchisq);
|
||
ErrorCode codePy = polynomial_fit(timeArr, PyArr, polynum, this->polyfitPy, this->Pychisq);
|
||
ErrorCode codePz = polynomial_fit(timeArr, PzArr, polynum, this->polyfitPz, this->Pzchisq);
|
||
ErrorCode codeVx = polynomial_fit(timeArr, VxArr, polynum, this->polyfitVx, this->Vxchisq);
|
||
ErrorCode codeVy = polynomial_fit(timeArr, VyArr, polynum, this->polyfitVy, this->Vychisq);
|
||
ErrorCode codeVz = polynomial_fit(timeArr, VzArr, polynum, this->polyfitVz, this->Vzchisq);
|
||
|
||
|
||
// 打印 this->polyfitPx
|
||
qDebug() << "polyfit value:";
|
||
qDebug() << "Px" << "\t" << "Py" << "\t" << "Pz" << "\t" << "Vx" << "\t" << "Vy" << "\t" << "Vz";
|
||
for (long i = 0; i < this->polyfitPx.size(); i++) { // 求解平方
|
||
qDebug() << this->polyfitPx[i] << "\t" << this->polyfitPy[i] << "\t" << this->polyfitPz[i] << "\t" << this->polyfitVx[i] << "\t" << this->polyfitVy[i] << "\t" << this->polyfitVz[i];
|
||
}
|
||
|
||
|
||
// 评价拟合情况
|
||
double starttime = this->oribtStartTime;
|
||
qDebug() << "oribt refrence time: " << starttime;
|
||
qDebug() << "time" << "\t" << "dPx" << "\t" << "dPy" << "\t" << "dPz" << "\t" << "dVx" << "\t" << "dVy" << "\t" << "dVz";
|
||
for (long i = 0; i < timeArr.size(); i++) {
|
||
double time_temp = timeArr[i];
|
||
bool flag = false;
|
||
SatelliteOribtNode tempnode;
|
||
this->getSatelliteOribtNode(time_temp, tempnode, flag);
|
||
qDebug() << timeArr[i] << "\t" << PxArr[i] - tempnode.Px << "\t" << PyArr[i] - tempnode.Py << "\t" << PzArr[i] - tempnode.Pz << "\t" << VxArr[i] - tempnode.Vx << "\t" << VyArr[i] - tempnode.Vy << "\t" << VzArr[i] - tempnode.Vz;
|
||
}
|
||
return ErrorCode::SUCCESS;
|
||
}
|
||
|
||
double GF3PolyfitSatelliteOribtModel::getPt()
|
||
{
|
||
return this->Pt;
|
||
}
|
||
|
||
double GF3PolyfitSatelliteOribtModel::getGri()
|
||
{
|
||
return this->Gri;
|
||
}
|
||
|
||
void GF3PolyfitSatelliteOribtModel::setPt(double pt)
|
||
{
|
||
this->Pt = pt;
|
||
}
|
||
|
||
void GF3PolyfitSatelliteOribtModel::setGri(double gri)
|
||
{
|
||
this->Gri = gri;
|
||
}
|
||
|
||
std::shared_ptr<GF3PolyfitSatelliteOribtModel> CreataGF3PolyfitSatelliteOribtModel(std::vector<SatelliteOribtNode>& nodes, long double startTime, int polynum)
|
||
{
|
||
qDebug() << "CreataGF3PolyfitSatelliteOribtModel \t" << (double)startTime << "\t" << polynum;
|
||
|
||
|
||
std::shared_ptr<GF3PolyfitSatelliteOribtModel> ployfitOribtModel = std::make_shared< GF3PolyfitSatelliteOribtModel>();
|
||
for (long i = 0; i < nodes.size(); i++) {
|
||
ployfitOribtModel->addOribtNode(nodes[i]);
|
||
}
|
||
ployfitOribtModel->setSatelliteOribtStartTime(startTime);
|
||
ErrorCode stateCode = ployfitOribtModel->polyFit(polynum, false);
|
||
if (stateCode != ErrorCode::SUCCESS) {
|
||
qDebug() << QString::fromStdString(errorCode2errInfo(stateCode));
|
||
return nullptr;
|
||
}
|
||
else {
|
||
return ployfitOribtModel;
|
||
}
|
||
return ployfitOribtModel;
|
||
}
|
||
|
||
|
||
|