#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <sstream>
#include <iomanip>
#include <cctype>
#include <cmath>
#include <algorithm>
#include <ctime>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
// ========== 常量定义 ==========
const double PI = 3.14159265358979323846;
const double SPEED_OF_LIGHT = 299792458.0;
const double GM = 3.986005e14;
const double EARTH_ROTATION_RATE = 7.2921151467e-5;
// ========== 结构体定义 ==========
struct GPSEphemeris {
string satellite;
int year, month, day, hour, min;
double sec;
double a0;
double a1;
double a2;
double IODE;
double Crs;
double Delta_n;
double M0;
double Cuc;
double e;
double Cus;
double sqrtA;
double Toe;
double Cic;
double Omega0;
double Cis;
double i0;
double Crc;
double omega;
double Omega_dot;
double IDOT;
};
struct Observation {
struct EpochTime {
int year, month, day;
int hour, minute;
double second;
} time;
string satellite;
double c1cValue;
};
// ========== 函数声明 ==========
Vector3d calculateSatellitePosition(const GPSEphemeris& eph, double transmitTime, double& Ek);
double calculateSatelliteClockCorrection(const GPSEphemeris& eph, double transmitTime, double Ek);
double gpsTimeToSec(int year, int month, int day, int hour, int min, double sec);
double julianday(int year, int month, int day, int hour, int min, double sec);
GPSEphemeris findEphemeris(const vector<GPSEphemeris>& ephemerides, const string& sat, double obsTime);
void parseGPSEphemeris(const vector<string>& lines, vector<GPSEphemeris>& ephemerides);
string formatSatID(const string& satID);
string trim(const string& str);
// ========== 主函数 ==========
int main() {
// 读取星历数据
vector<GPSEphemeris> ephemerides;
{
string filename = "C:/Users/giaoming/Desktop/1111/brdm2460.22p";
ifstream file(filename);
if (!file.is_open()) {
cerr << "Error opening ephemeris file: " << filename << endl;
return 1;
}
vector<string> currentRecord;
bool headerEnded = false;
string line;
while (getline(file, line)) {
if (!headerEnded) {
if (line.find("END OF HEADER") != string::npos) {
headerEnded = true;
}
continue;
}
if (line.empty()) continue;
if (line[0] == 'G') {
if (!currentRecord.empty()) {
parseGPSEphemeris(currentRecord, ephemerides);
currentRecord.clear();
}
currentRecord.push_back(line);
}
else if (!currentRecord.empty() && currentRecord.size() < 8) {
currentRecord.push_back(line);
}
}
if (!currentRecord.empty()) {
parseGPSEphemeris(currentRecord, ephemerides);
}
cout << "Loaded " << ephemerides.size() << " GPS ephemeris records." << endl;
}
// 读取观测数据
vector<Observation> observations;
{
string filename = "C:/Users/giaoming/Desktop/1111/brst2460.22o";
ifstream file(filename);
if (!file.is_open()) {
cerr << "Error opening observation file: " << filename << endl;
return 1;
}
bool headerEnded = false;
vector<string> gpsObsTypes;
int gpsObsCount = 0;
int c1cIndex = -1;
Observation::EpochTime currentEpoch;
string line;
while (getline(file, line)) {
if (!line.empty() && line.back() == '\r') {
line.pop_back();
}
if (!headerEnded) {
if (line.find("END OF HEADER") != string::npos) {
headerEnded = true;
}
if (line.find("G ") == 0 || line.find("G ") == 0) {
stringstream ss(line);
string system, countStr;
ss >> system >> countStr;
try {
gpsObsCount = stoi(countStr);
for (int i = 0; i < gpsObsCount; i++) {
string obsType;
ss >> obsType;
gpsObsTypes.push_back(obsType);
if (obsType == "C1C") {
c1cIndex = i;
}
}
}
catch (...) {
cerr << "Error parsing GPS observation types" << endl;
}
}
continue;
}
if (line.empty()) continue;
if (line[0] == '>') {
stringstream ss(line.substr(1));
ss >> currentEpoch.year >> currentEpoch.month >> currentEpoch.day
>> currentEpoch.hour >> currentEpoch.minute >> currentEpoch.second;
continue;
}
if (line.length() < 4) continue;
string satID = line.substr(0, 3);
if (satID[0] != 'G') continue;
string formattedSatID = formatSatID(satID);
const int obsWidth = 16;
int startPos = 3 + c1cIndex * obsWidth;
if (startPos + obsWidth > line.length()) {
cerr << "Warning: Line too short for satellite " << formattedSatID << endl;
continue;
}
string c1cStr = trim(line.substr(startPos, obsWidth));
if (c1cStr.empty()) continue;
try {
double c1cValue = stod(c1cStr);
Observation obs;
obs.time = currentEpoch;
obs.satellite = formattedSatID;
obs.c1cValue = c1cValue;
observations.push_back(obs);
}
catch (...) {
cerr << "Error parsing C1C value for satellite " << formattedSatID << endl;
}
}
cout << "Loaded " << observations.size() << " GPS C1C observations." << endl;
}
// 伪距单点定位处理
cout << "\nStarting single point positioning...\n";
for (const auto& obs : observations) {
// 关键修复：确保时间转换正确处理闰年问题
double obsTime = gpsTimeToSec(obs.time.year, obs.time.month, obs.time.day,
obs.time.hour, obs.time.minute, obs.time.second);
// 查找对应卫星的星历
GPSEphemeris eph = findEphemeris(ephemerides, obs.satellite, obsTime);
if (eph.satellite.empty()) {
// 增强错误日志输出
cerr << "未找到卫星 " << obs.satellite
<< " 在时间 " << obs.time.year << "-" << setfill('0') << setw(2) << obs.time.month << "-" << setw(2) << obs.time.day
<< " " << setw(2) << obs.time.hour << ":" << setw(2) << obs.time.minute << ":" << fixed << setprecision(6) << obs.time.second
<< " 的有效星历" << endl;
continue;
}
double transmitTime = obsTime - obs.c1cValue / SPEED_OF_LIGHT;
Vector3d satPos;
double Ek = 0.0;
satPos = calculateSatellitePosition(eph, transmitTime, Ek);
double satClockCorrection = calculateSatelliteClockCorrection(eph, transmitTime, Ek);
transmitTime -= satClockCorrection;
// 重新计算卫星位置（使用修正后的发射时间）
satPos = calculateSatellitePosition(eph, transmitTime, Ek);
// 输出结果
cout << "卫星: " << obs.satellite
<< " 时间: " << obs.time.year << "-" << setfill('0') << setw(2) << obs.time.month << "-" << setw(2) << obs.time.day
<< " " << setw(2) << obs.time.hour << ":" << setw(2) << obs.time.minute << ":" << fixed << setprecision(6) << obs.time.second
<< " 位置: X=" << setprecision(3) << satPos[0]
<< " Y=" << satPos[1]
<< " Z=" << satPos[2] << " (m)" << endl;
}
return 0;
}
// ========== 功能函数实现 ==========
Vector3d calculateSatellitePosition(const GPSEphemeris& eph, double transmitTime, double& Ek) {
// 关键修复：确保时间差在有效范围内
double t_k = transmitTime - eph.Toe;
// 调整时间差到[-302400, 302400]秒范围内
if (t_k > 302400) t_k -= 604800;
else if (t_k < -302400) t_k += 604800;
double a = pow(eph.sqrtA, 2);
double n_0 = sqrt(GM / pow(a, 3));
double n = n_0 + eph.Delta_n;
double M_k = eph.M0 + n * t_k;
// 迭代计算偏近点角
Ek = M_k;
double E_prev;
const double tolerance = 1e-12;
int max_iterations = 100;
int iter_count = 0;
do {
E_prev = Ek;
Ek = M_k + eph.e * sin(Ek);
iter_count++;
if (iter_count > max_iterations) {
cerr << "警告：偏近点角迭代未收敛" << endl;
break;
}
} while (fabs(Ek - E_prev) > tolerance);
double sin_v_k = sqrt(1 - pow(eph.e, 2)) * sin(Ek) / (1 - eph.e * cos(Ek));
double cos_v_k = (cos(Ek) - eph.e) / (1 - eph.e * cos(Ek));
double v_k = atan2(sin_v_k, cos_v_k);
double Phi_k = v_k + eph.omega;
double delta_u_k = eph.Cuc * cos(2 * Phi_k) + eph.Cus * sin(2 * Phi_k);
double delta_r_k = eph.Crc * cos(2 * Phi_k) + eph.Crs * sin(2 * Phi_k);
double delta_i_k = eph.Cic * cos(2 * Phi_k) + eph.Cis * sin(2 * Phi_k);
double u_k = Phi_k + delta_u_k;
double r_k = a * (1 - eph.e * cos(Ek)) + delta_r_k;
double i_k = eph.i0 + delta_i_k + eph.IDOT * t_k;
double x_k_prime = r_k * cos(u_k);
double y_k_prime = r_k * sin(u_k);
// 关键修复：正确的升交点赤经计算
double Omega_k = eph.Omega0 + (eph.Omega_dot - EARTH_ROTATION_RATE) * t_k;
double x_k = x_k_prime * cos(Omega_k) - y_k_prime * cos(i_k) * sin(Omega_k);
double y_k = x_k_prime * sin(Omega_k) + y_k_prime * cos(i_k) * cos(Omega_k);
double z_k = y_k_prime * sin(i_k);
return Vector3d(x_k, y_k, z_k);
}
double calculateSatelliteClockCorrection(const GPSEphemeris& eph, double transmitTime, double Ek) {
double t = transmitTime - eph.Toe;
// 调整时间差到有效范围
if (t > 302400) t -= 604800;
else if (t < -302400) t += 604800;
double F = -2.0 * sqrt(GM) / pow(SPEED_OF_LIGHT, 2);
double dt_rel = F * eph.e * eph.sqrtA * sin(Ek);
double dt_sv = eph.a0 + eph.a1 * t + eph.a2 * t * t + dt_rel;
return dt_sv;
}
// 关键修复：完整的时间转换函数（包含闰年处理）
double gpsTimeToSec(int year, int month, int day, int hour, int min, double sec) {
// GPS起始时间：1980年1月6日0时
const double GPS_EPOCH_JD = julianday(1980, 1, 6, 0, 0, 0.0);
double current_jd = julianday(year, month, day, hour, min, sec);
// 计算天数差
double days = current_jd - GPS_EPOCH_JD;
// 转换为秒数
return days * 86400.0;
}
// 关键修复：完整的儒略日计算（包含闰年处理）
double julianday(int year, int month, int day, int hour, int min, double sec) {
// 处理1月和2月（视为前一年的13月和14月）
if (month <= 2) {
year -= 1;
month += 12;
}
int A = year / 100;
int B = 2 - A + A / 4;
// 完整的儒略日计算公式
double JD = floor(365.25 * (year + 4716)) + floor(30.6001 * (month + 1)) +
day + B - 1524.5;
// 加上时间部分
double fraction = (hour + min / 60.0 + sec / 3600.0) / 24.0;
return JD + fraction;
}
// 星历查找函数
GPSEphemeris findEphemeris(const vector<GPSEphemeris>& ephemerides, const string& sat, double obsTime) {
GPSEphemeris bestEph;
double minTimeDiff = 1e15; // 初始化为一个很大的值
for (const auto& eph : ephemerides) {
if (eph.satellite == sat) {
// 计算星历参考时间的GPS秒
double toeTime = gpsTimeToSec(eph.year, eph.month, eph.day,
eph.hour, eph.min, eph.sec);
// 计算时间差绝对值
double timeDiff = fabs(obsTime - toeTime);
// 寻找时间最接近的星历
if (timeDiff < minTimeDiff) {
minTimeDiff = timeDiff;
bestEph = eph;
}
}
}
// 检查找到的星历是否在有效期内（4小时）
if (minTimeDiff < 3600 && !bestEph.satellite.empty()) {
return bestEph;
}
return GPSEphemeris(); // 返回空结构体表示未找到
}
void parseGPSEphemeris(const vector<string>& lines, vector<GPSEphemeris>& ephemerides) {
if (lines.size() < 8) return;
GPSEphemeris eph;
stringstream ss(lines[0]);
ss >> eph.satellite;
ss >> eph.year >> eph.month >> eph.day >> eph.hour >> eph.min >> eph.sec;
ss >> eph.a0 >> eph.a1 >> eph.a2;
// 关键修复：确保年份格式正确
if (eph.year < 100) {
if (eph.year >= 80) eph.year += 1900;
else eph.year += 2000;
}
ss = stringstream(lines[1]);
ss >> eph.IODE >> eph.Crs >> eph.Delta_n >> eph.M0;
ss = stringstream(lines[2]);
ss >> eph.Cuc >> eph.e >> eph.Cus >> eph.sqrtA;
ss = stringstream(lines[3]);
ss >> eph.Toe >> eph.Cic >> eph.Omega0;
ss = stringstream(lines[4]);
ss >> eph.Cis >> eph.i0 >> eph.Crc >> eph.omega;
ss = stringstream(lines[5]);
ss >> eph.Omega_dot >> eph.IDOT;
ephemerides.push_back(eph);
}
string formatSatID(const string& satID) {
if (satID.length() < 3) return satID;
string system = satID.substr(0, 1);
string number = satID.substr(1);
number.erase(remove(number.begin(), number.end(), ' '), number.end());
if (number.length() == 1) {
number = "0" + number;
}
return system + number;
}
string trim(const string& str) {
size_t first = str.find_first_not_of(' ');
if (string::npos == first) return "";
size_t last = str.find_last_not_of(' ');
return str.substr(first, (last - first + 1));
}详细讲解该代码
文章讲述了在LeetCode上解题的经验，包括找树左下角节点的问题，需关注最左边的节点，以及使用回溯法解决路径总和问题时注意全局变量的使用。对于从中序与后序遍历序列构造二叉树的复杂问题，作者认为需要进一步练习和理解数学算法逻辑。
扫一扫
1057
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
