#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <time.h>
#include <Eigen/Dense>
using Eigen::MatrixXd;
using Eigen::VectorXd;
#define MAXGPSSATNUM 32 //最大GPS卫星数
#define MAXEPHNUM 24 //单颗卫星最大星历数
#define GPSEPHLINE 8 //GPS卫星星历所占行数
#define MAXPSEUDORANGE 100000000.0 //伪距最大值
#define LINEMAXDATANUM 50 //一行的最大数据量
#define LEAPSECS 18 //自1980-1-6以来的跳秒数
#define LEN_STRING 1024 //字符最大长度
/*WGS84椭球*/
#define FLATTENING_WGS84 (1.0/298.2572236) //椭球扁率
#define MAJOR_SEMI_AXIS 6378137.0 //地球长半轴
#define GM_WGS84 3.986005e14 //GM（GPS）
#define OeDOT_WGS84 7.2921151467e-5 //地球自转角速度（GPS）,rad/s
#define PI 3.141592653589793
#define CLIGHT 299792458.0 //光速
#define NATURAL_CONSTANT 2.718281828459045 //自然常数
// 提前声明结构体
typedef struct GPSTime;
typedef struct EPH_ONE;
typedef struct EPH_ALL;
typedef struct OBS_ONE;
typedef struct OBS_ALL;
typedef struct Satelite_Result;
typedef struct SPP_Result;
// 提前声明函数
int SeekEph(const EPH_ALL sateph[MAXGPSSATNUM], unsigned short prn, const GPSTime& gt);
int GPS_compute(EPH_ONE eph, GPSTime gt, Satelite_Result* sr);
void Compute_error_correct(double pos[3], Satelite_Result* sres);
void Build_model_pos(double pos0[3], double clo_correct0, Satelite_Result* sres, OBS_ONE* obs, int obs_num, Eigen::MatrixXd& B, Eigen::VectorXd& L, Eigen::MatrixXd& P);
void Hopefield(double E, double* tro_correct);
int XYZ2BLH(double XYZ[3], double BLH[3]);
int XYZ2ENU(int type, double center[3], double X[3], double N[3]);
void Count_OBS(OBS_ONE* obs, EPH_ALL* eph, Satelite_Result* sres, double pos[3], int* obs_num);
void Sat_compute(EPH_ALL sateph[MAXGPSSATNUM], OBS_ONE* obs, Satelite_Result sres[MAXGPSSATNUM]);
int SPP_compute_pos(OBS_ONE* obs, EPH_ALL* eph, SPP_Result* res);
void Display_res(SPP_Result* res);
void Display_res_file(FILE* fp, SPP_Result* res);
char filename_eph[LEN_STRING];
char filename_obs[LEN_STRING];
char filename_res[LEN_STRING];
/*GPS时间结构体*/
struct GPSTime {
int Week;
double Sec;
};
double Compute_range(double a1[3], double a2[3]) {
double a = 0;
int i;
for (i = 0; i < 3; i++) a += (a1[i] - a2[i]) * (a1[i] - a2[i]);
return sqrt(a);
}
int LeastSquare_filter(
const MatrixXd& B,
const VectorXd& L,
const MatrixXd& P,
VectorXd& v,
VectorXd& x,
MatrixXd& Q)
{
Q = B.transpose() * P * B;
Eigen::FullPivLU<MatrixXd> lu(Q);
if (!lu.isInvertible()) return -1;
x = Q.inverse() * (B.transpose() * P * L);
v = B * x - L;
return 1;
};
/*通用时结构体定义*/
typedef struct CommonTime {
unsigned short Year;
unsigned short Month;
unsigned short Day;
unsigned short Hour;
unsigned short Minute;
double Sec;
};
/*简化儒略日结构体定义*/
typedef struct MJDTime {
int Days;
double FracDay;
};
/*通用时转简化儒略日*/
void CT2MJD(CommonTime ct, MJDTime* mt) {
int m, y;
double UT;
double JD;
UT = ct.Hour + ct.Minute / 60.0 + ct.Sec / 3600.0;
if (ct.Month <= 2) {
y = ct.Year - 1;
m = ct.Month + 12;
}
else {
y = ct.Year;
m = ct.Month;
}
JD = (int)(365.25 * y) + (int)(30.6001 * (m + 1)) + ct.Day + UT / 24.0 + 1720981.5;
mt->Days = (int)(JD - 2400000.5);
mt->FracDay = JD - 2400000.5 - mt->Days;
}
/*简化儒略日转GPS时*/
void MJD2GT(MJDTime mt, GPSTime* gt, int leapsecs) {
gt->Week = (int)((mt.Days + mt.FracDay - 44244) / 7);
gt->Sec = (mt.Days + mt.FracDay - 44244 - gt->Week * 7) * 86400 + leapsecs;
if (gt->Sec > 604800.0) {
gt->Week = gt->Week + 1;
gt->Sec = gt->Sec - 604800.0;
}
}
/*通用时转GPS时（GPS周+周内秒）*/
void CT2GT(CommonTime ct, GPSTime* gt, int leapsecs) {
MJDTime mt;
CT2MJD(ct, &mt);
MJD2GT(mt, gt, leapsecs);
}
/*从GPS历元起算的连续秒数转为GPS时*/
void Sec2GT(double sec, GPSTime* gt) {
gt->Week = (int)(sec / (7.0 * 24.0 * 3600.0));
gt->Sec = sec - gt->Week * 7 * 24 * 3600;
}
/*地心地固坐标系（ECFC）转大地坐标（BLH）*/
int XYZ2BLH(double XYZ[3], double BLH[3]) {
double e2, N, sin_B;
double dZ1, dZ2;
int n = 0;
e2 = 2 * FLATTENING_WGS84 - FLATTENING_WGS84 * FLATTENING_WGS84;
dZ1 = e2 * XYZ[2], dZ2 = e2 * XYZ[2];
if (fabs(XYZ[0]) < pow(10, -6)) {
if (XYZ[1] > 0) BLH[1] = 90 * PI / 180.0;
else if (XYZ[1] < 0) BLH[1] = -90 * PI / 180.0;
else return -1;
}
else {
BLH[1] = atan(fabs(XYZ[1] / XYZ[0]));
if (XYZ[0] < 0 && XYZ[1] > 0) BLH[1] = PI - BLH[1];
else if (XYZ[0] < 0 && XYZ[1] <= 0) BLH[1] = -PI + BLH[1];
else if (XYZ[0] > 0 && XYZ[1] < 0) BLH[1] = -BLH[1];
}
do {
dZ1 = dZ2;
sin_B = (XYZ[2] + dZ1) / sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1] + (XYZ[2] + dZ1) * (XYZ[2] + dZ1));
N = MAJOR_SEMI_AXIS / sqrt(1 - e2 * sin_B * sin_B);
BLH[0] = atan2(XYZ[2] + dZ1, sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]));
BLH[2] = sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1] + (XYZ[2] + dZ1) * (XYZ[2] + dZ1)) - N;
dZ2 = N * e2 * sin_B;
n++;
} while (n < 3 && fabs(dZ1 - dZ2) > 1e-3);
return 1;
}
/*地心地固坐标系（ECFC）转站心坐标（ENU）*/
int XYZ2ENU(int type, double center[3], double X[3], double N[3]) {
double BLH[3], XYZ[3];
int i = 0;
if (XYZ2BLH(center, BLH) != 1) return -1;
if (type == 1) for (i = 0; i < 3; i++) XYZ[i] = X[i] - center[i];
else if (type == 2) for (i = 0; i < 3; i++) XYZ[i] = X[i];
else return -2;
N[0] = -sin(BLH[0]) * cos(BLH[1]) * XYZ[0] - sin(BLH[0]) * sin(BLH[1]) * XYZ[1] + cos(BLH[0]) * XYZ[2];
N[1] = -sin(BLH[1]) * XYZ[0] + cos(BLH[1]) * XYZ[1];
N[2] = cos(BLH[0]) * cos(BLH[1]) * XYZ[0] + cos(BLH[0]) * sin(BLH[1]) * XYZ[1] + sin(BLH[0]) * XYZ[2];
return 1;
}
//单颗卫星观测数据结构体
struct OBS_ALL {
unsigned short Prn;
double Psr;
double Adr;
double Dop;
double SNR;
short status;
};
//单历元观测数据结构体
struct OBS_ONE {
GPSTime GT;
int Epochflag;
unsigned short SatNum;
double Rclockoffset;
OBS_ALL SatObs[MAXGPSSATNUM];
short GPSObsType;
short status;
};
//单颗卫星-单个时刻星历结构体（广播星历）
struct EPH_ONE {
unsigned short Prn;
GPSTime GT;
double ClkBias, ClkDrift, ClkDriftRate;
double Cuc, Cus, Crc, Crs, Cic, Cis;
double AODE, AODC;
double IODE, IODC;
double Delta_n, M0;
double E, SqrtA;
double Toe, OMEGA0;
double I0, omega, ODOT;
double IDOT;
double Tgd;
double Toc;
short ClockJump;
int Health;
short status;
};
//单颗卫星多个时刻星历结构体（广播星历）
struct EPH_ALL {
EPH_ONE EPH_ONE[MAXEPHNUM];
unsigned short Ephnum;
};
//卫星计算结果结构体
struct Satelite_Result {
unsigned short prn;
GPSTime gt;
double psr;
double doppler;
double pos[6], vel[3];
double clo_correct, clo_rate;
double tro_correct;
double E;
short status;
};
//SPP结果结构体
struct SPP_Result {
GPSTime gt;
int sat_num;
int obs_num;
Satelite_Result sat_res[MAXGPSSATNUM];
double pos[3];
double clo_correct;
double vel[3];
double clo_rate;
double PDOP_pos;
double PDOP_vel;
double sigP;
double sigV;
double D_pos[3 * 3];
double D_vel[3 * 3];
double v[MAXGPSSATNUM];
short status;
};
void Initial_EPH_ALL(EPH_ALL* eph) {
int i, j;
for (i = 0; i < MAXGPSSATNUM; i++) {
eph[i].Ephnum = 0;
for (j = 0; j < MAXEPHNUM; j++) {
eph[i].EPH_ONE[j].status = 0;
}
}
}
void Initial_OBS_ONE(OBS_ONE* obs) {
int i;
obs->GT.Week = 0; obs->GT.Sec = 0.0;
obs->SatNum = 0;
for (i = 0; i < MAXGPSSATNUM; i++) {
obs->SatObs[i].Prn = 99;
obs->SatObs[i].Psr = 0.0;
obs->SatObs[i].status = 0;
}
obs->status = 0;
}
void Initial_SPP_Result(SPP_Result* res) {
int i;
for (i = 0; i < MAXGPSSATNUM; i++) res->sat_res[i].status = 0;
res->clo_correct = 0.0;
res->clo_rate = 0.0;
for (i = 0; i < 3; i++) {
res->pos[i] = 0.0;
res->vel[i] = 0.0;
}
res->status = 0;
}
int ObsHead_Reader(FILE* fp, OBS_ONE* obs)
{
char line[128];
while (fgets(line, sizeof(line), fp))
{
if (line[0] == 'G' && strncmp(line + 60, "SYS / # / OBS TYPES", 19) == 0)
{
const char* obs_str = line + 6;
const char* p = strstr(obs_str, "C1C");
if (!p) return -1;
obs->GPSObsType = (int)((p - obs_str) / 4);
break;
}
if (strncmp(line + 60, "END OF HEADER", 13) == 0) break;
}
puts("观测数据头文件读取完毕...");
return 1;
}
void ObsData_Reader(FILE* fp, OBS_ONE* obs) {
CommonTime ctime;
char line[LEN_STRING], data[10][20];
unsigned short satPrn;
unsigned short obsType;
int sNum, i = 0, j = 0, k = 0, m = 0;
for (i = 0; i < 10; i++) memset(data[i], '\0', sizeof(data[i]));
while (!feof(fp)) {
for (i = 0; i < MAXGPSSATNUM; i++) obs->SatObs[i].status = 0;
obs->status = 0;
memset(line, '\0', sizeof(line));
fgets(line, sizeof(line), fp);
if (line[0] == '>') {
sscanf(line, ">%5s%3s%3s%3s%3s%11s%3s%3s", data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7]);
ctime.Year = atoi(data[0]);
ctime.Month = atoi(data[1]);
ctime.Day = atoi(data[2]);
ctime.Hour = atoi(data[3]);
ctime.Minute = atoi(data[4]);
ctime.Sec = atof(data[5]);
//打印读取时间
printf("%04d/%02d/%02d %02d:%02d:%06.3f:\n",
ctime.Year, ctime.Month, ctime.Day,
ctime.Hour, ctime.Minute, ctime.Sec);
CT2GT(ctime, &(obs->GT), 0);
obs->Epochflag = atoi(data[6]);
obs->SatNum = atoi(data[7]);
for (i = 0; i < obs->SatNum; i++) {
memset(line, '\0', sizeof(line));
fgets(line, sizeof(line), fp);
memset(data[0], '\0', sizeof(data[0]));
strncpy(data[0], line + 1, 2);
satPrn = atoi(data[0]);
if (line[0] == 'G') {
sNum = satPrn - 1;
obsType = obs->GPSObsType;
m = 3 + obsType * 16;
memset(data[0], '\0', sizeof(data[0]));
if (strlen(line) < m + 14) strncpy(data[0], "0", 2);
else strncpy(data[0], line + m, 14);
obs->SatObs[sNum].Psr = atof(data[0]);
obs->SatObs[sNum].Prn = satPrn;
obs->SatObs[sNum].status = 1;
}
}
obs->status = 1;
break;
}
}
}
void EphHead_Reader(FILE* fp, EPH_ALL* eph) {
char line[LEN_STRING];
while (!feof(fp)) {
memset(line, '\0', sizeof(line));
fgets(line, sizeof(line), fp);
if (strncmp(line + 60, "END OF HEADER", 13) == 0) break;
}
}
void EphData_Reader(FILE* fp, EPH_ALL* eph) {
CommonTime ctime;
char line[LEN_STRING], data[10][25];
unsigned short satprn;
short snum, ephnum;
int i = 0, j = 0, m = 0;
for (i = 0; i < MAXGPSSATNUM; i++) {
eph[i].Ephnum = 0;
for (j = 0; j < MAXEPHNUM; j++) eph[i].EPH_ONE[j].status = 0;
}
for (i = 0; i < 10; i++) memset(data[i], '\0', sizeof(data[i]));
while (!feof(fp)) {
memset(line, '\0', sizeof(line));
fgets(line, sizeof(line), fp);
if (line[0] == 'G') {
sscanf(line + 1, "%2s%5s%3s%3s%3s%3s%3s", data[0], data[1], data[2], data[3], data[4], data[5], data[6]);
satprn = atoi(data[0]);
ctime.Year = atoi(data[1]);
ctime.Month = atoi(data[2]);
ctime.Day = atoi(data[3]);
ctime.Hour = atoi(data[4]);
ctime.Minute = atoi(data[5]);
ctime.Sec = atof(data[6]);
snum = satprn - 1;
ephnum = eph[snum].Ephnum;
eph[snum].EPH_ONE[ephnum].Prn = satprn;
CT2GT(ctime, &(eph[snum].EPH_ONE[ephnum].GT), 0);
for (i = 0; i < 3; i++) {
memset(data[i], '\0', sizeof(data[i]));
strncpy(data[i], line + 23 + i * 19, 19);
}
eph[snum].EPH_ONE[ephnum].ClkBias = atof(data[0]);
eph[snum].EPH_ONE[ephnum].ClkDrift = atof(data[1]);
eph[snum].EPH_ONE[ephnum].ClkDriftRate = atof(data[2]);
memset(line, '\0', sizeof(line));
fgets(line, sizeof(line), fp);
for (i = 0; i < 4; i++) {
memset(data[i], '\0', sizeof(data[i]));
strncpy(data[i], line + 4 + i * 19, 19);
}
eph[snum].EPH_ONE[ephnum].IODE = atof(data[0]);
eph[snum].EPH_ONE[ephnum].Crs = atof(data[1]);
eph[snum].EPH_ONE[ephnum].Delta_n = atof(data[2]);
eph[snum].EPH_ONE[ephnum].M0 = atof(data[3]);
memset(line, '\0', sizeof(line));
fgets(line, sizeof(line), fp);
for (i = 0; i < 4; i++) {
memset(data[i], '\0', sizeof(data[i]));
strncpy(data[i], line + 4 + i * 19, 19);
}
eph[snum].EPH_ONE[ephnum].Cuc = atof(data[0]);
eph[snum].EPH_ONE[ephnum].E = atof(data[1]);
eph[snum].EPH_ONE[ephnum].Cus = atof(data[2]);
eph[snum].EPH_ONE[ephnum].SqrtA = atof(data[3]);
memset(line, '\0', sizeof(line));
fgets(line, sizeof(line), fp);
for (i = 0; i < 4; i++) {
memset(data[i], '\0', sizeof(data[i]));
strncpy(data[i], line + 4 + i * 19, 19);
}
eph[snum].EPH_ONE[ephnum].Toe = atof(data[0]);
eph[snum].EPH_ONE[ephnum].Cic = atof(data[1]);
eph[snum].EPH_ONE[ephnum].OMEGA0 = atof(data[2]);
eph[snum].EPH_ONE[ephnum].Cis = atof(data[3]);
memset(line, '\0', sizeof(line));
fgets(line, sizeof(line), fp);
for (i = 0; i < 4; i++) {
memset(data[i], '\0', sizeof(data[i]));
strncpy(data[i], line + 4 + i * 19, 19);
}
eph[snum].EPH_ONE[ephnum].I0 = atof(data[0]);
eph[snum].EPH_ONE[ephnum].Crc = atof(data[1]);
eph[snum].EPH_ONE[ephnum].omega = atof(data[2]);
eph[snum].EPH_ONE[ephnum].ODOT = atof(data[3]);
memset(line, '\0', sizeof(line));
fgets(line, sizeof(line), fp);
for (i = 0; i < 4; i++) {
memset(data[i], '\0', sizeof(data[i]));
strncpy(data[i], line + 4 + i * 19, 19);
}
eph[snum].EPH_ONE[ephnum].IDOT = atof(data[0]);
memset(line, '\0', sizeof(line));
fgets(line, sizeof(line), fp);
for (i = 0; i < 4; i++) {
memset(data[i], '\0', sizeof(data[i]));
strncpy(data[i], line + 4 + i * 19, 19);
}
eph[snum].EPH_ONE[ephnum].Tgd = atof(data[2]);
eph[snum].EPH_ONE[ephnum].IODC = atof(data[3]);
fgets(line, sizeof(line), fp);
eph[snum].EPH_ONE[ephnum].status = 1;
ephnum++;
eph[snum].Ephnum = ephnum;
}
}
printf("星历数据读取完毕...\n");
}
void Count_OBS(OBS_ONE* obs, EPH_ALL* eph, Satelite_Result* sres, double pos[3], int* obs_num)
{
*obs_num = 0;
for (int i = 0; i < MAXGPSSATNUM; ++i) {
if (obs->SatObs[i].status == 1 && obs->SatObs[i].Psr != 0.0)
++(*obs_num);
}
}
/*SeekEph:寻找最近卫星
p 文件 → 提供星历（Toe）
o 文件 → 提供观测历元时间
两者在 SeekEph() 里做时间差比较，选出离观测历元最近的那条星历*/
int SeekEph(const EPH_ALL sateph[MAXGPSSATNUM], unsigned short prn, const GPSTime& gt)
{
const double limit2h = 3600.0;
const double obsSec = gt.Week * 604800.0 + gt.Sec;
unsigned short idx = prn - 1;
unsigned short ephCnt = sateph[idx].Ephnum;
if (ephCnt == 0) return -1;
int bestIdx = -1;
double bestDelta = limit2h;
for (int i = 0; i < ephCnt; ++i)
{
const EPH_ONE& eph = sateph[idx].EPH_ONE[i];
double toeSec = eph.GT.Week * 604800.0 + eph.Toe; // 用 Toe
double delta = fabs(obsSec - toeSec);
if (delta > limit2h) continue; // 过期直接丢弃
if (delta < bestDelta)
{
bestDelta = delta;
bestIdx = i;
}
}
return bestIdx; // 可能返回 -1 → 无有效星历
}
int GPS_compute(EPH_ONE eph, GPSTime gt, Satelite_Result* sr) {
double A, n0, tk;
double n, Mk, Ek, Ek1;
int i = 0;
double vk, FI, duk, drk, dik, uk, rk, ik, xk, yk, Omegak;
double EkDOT, FIDOT, ukDOT, rkDOT, ikDOT, xkDOT, ykDOT;
double OkDOT;
double F, Delta_tr, t1;
A = eph.SqrtA * eph.SqrtA;
n0 = sqrt(GM_WGS84 / (A * A * A));
tk = gt.Week * 604800.0 + gt.Sec - eph.GT.Week * 604800.0 - eph.Toe;
n = n0 + eph.Delta_n;
Mk = eph.M0 + n * tk;
Ek = Mk;
do {
Ek1 = Ek;
Ek = eph.E * sin(Ek1) + Mk;
i++;
} while (fabs(Ek - Ek1) > 1e-7 && i < 3);
vk = atan2(sqrt(1 - eph.E * eph.E) * sin(Ek) / (1 - eph.E * cos(Ek)), (cos(Ek) - eph.E) / (1 - eph.E * cos(Ek)));
FI = vk + eph.omega;
duk = eph.Cus * sin(2 * FI) + eph.Cuc * cos(2 * FI);
drk = eph.Crs * sin(2 * FI) + eph.Crc * cos(2 * FI);
dik = eph.Cis * sin(2 * FI) + eph.Cic * cos(2 * FI);
uk = FI + duk;
rk = A * (1 - eph.E * cos(Ek)) + drk;
ik = eph.I0 + dik + eph.IDOT * tk;
xk = rk * cos(uk);
yk = rk * sin(uk);
Omegak = eph.OMEGA0 + (eph.ODOT - OeDOT_WGS84) * tk - OeDOT_WGS84 * eph.Toe;
sr->pos[0] = xk * cos(Omegak) - yk * cos(ik) * sin(Omegak);
sr->pos[1] = xk * sin(Omegak) + yk * cos(ik) * cos(Omegak);
sr->pos[2] = yk * sin(ik);
EkDOT = n / (1 - eph.E * cos(Ek));
FIDOT = sqrt((1 + eph.E) / (1 - eph.E)) * pow(cos(vk / 2) / cos(Ek / 2), 2) * EkDOT;
ukDOT = 2 * (eph.Cus * cos(2 * FI) - eph.Cuc * sin(2 * FI)) * FIDOT + FIDOT;
rkDOT = A * eph.E * sin(Ek) * EkDOT + 2 * (eph.Crs * cos(2 * FI) - eph.Crc * sin(2 * FI)) * FIDOT;
ikDOT = eph.IDOT + 2 * (eph.Cis * cos(2 * FI) - eph.Cic * sin(2 * FI)) * FIDOT;
OkDOT = eph.ODOT - OeDOT_WGS84;
xkDOT = rkDOT * cos(uk) - rk * ukDOT * sin(uk);
ykDOT = rkDOT * sin(uk) + rk * ukDOT * cos(uk);
MatrixXd m1(3, 4), m2(4, 1), m3(3, 1);
m1 << cos(Omegak), -sin(Omegak) * cos(ik), -(xk * sin(Omegak) + yk * cos(Omegak) * cos(ik)), yk* sin(Omegak)* sin(ik),
sin(Omegak), cos(Omegak)* cos(ik), (xk * cos(Omegak) - yk * sin(Omegak) * cos(ik)), yk* cos(Omegak)* sin(ik),
0.0, sin(ik), 0.0, yk* cos(ik);
m2 << xkDOT, ykDOT, OkDOT, ikDOT;
m3 = m1 * m2;
for (int i = 0; i < 3; ++i) sr->vel[i] = m3(i);
F = -2 * sqrt(GM_WGS84) / (CLIGHT * CLIGHT);
Delta_tr = F * eph.E * eph.SqrtA * sin(Ek);
t1 = gt.Week * 604800.0 + gt.Sec - eph.GT.Week * 604800.0 - eph.Toe;
sr->clo_correct = eph.ClkBias + eph.ClkDrift * t1 + eph.ClkDriftRate * t1 * t1 + Delta_tr;
sr->clo_correct = sr->clo_correct - eph.Tgd;
sr->clo_rate = eph.ClkDrift + 2 * eph.ClkDriftRate * t1 + F * eph.E * eph.SqrtA * cos(Ek) * EkDOT;
sr->prn = eph.Prn;
sr->gt = gt;
sr->status = 1;
return 1;
}
void Sat_compute(EPH_ALL sateph[MAXGPSSATNUM], OBS_ONE* obs, Satelite_Result sres[MAXGPSSATNUM])
{
double CloCor0, CloCor;
int i, k, s;
double rt, st;
double psr;
GPSTime gt1;
EPH_ONE eph;
for (i = 0; i < MAXGPSSATNUM; ++i)
{
if (obs->SatObs[i].status != 1)
{
sres[i].status = 0;
continue;
}
sres[i].prn = i + 1;
rt = obs->GT.Week * 604800.0 + obs->GT.Sec;
psr = obs->SatObs[i].Psr;
if (psr == 0.0)
{
obs->SatObs[i].status = 0;
continue;
}
CloCor = sateph[i].EPH_ONE[0].ClkBias;
s = 0;
do
{
CloCor0 = CloCor;
st = rt - psr / CLIGHT - CloCor0;
Sec2GT(st, >1);
k = SeekEph(sateph, i + 1, gt1);
eph = sateph[i].EPH_ONE[k];
if (!GPS_compute(eph, gt1, &sres[i])) break;
CloCor = sres[i].clo_correct;
++s;
} while (fabs(CloCor0 - CloCor) > 1e-7 && s < 3);
//信号传播时间迭代
if (k < 0 || s == 0)
{
obs->SatObs[i].status = 0;
continue;
}
sres[i].pos[3] = sres[i].pos[0];
sres[i].pos[4] = sres[i].pos[1];
sres[i].pos[5] = sres[i].pos[2];
sres[i].gt.Week = gt1.Week;
sres[i].gt.Sec = gt1.Sec;
sres[i].status = 1;
}
}
void Hopefield(double E, double* tro_correct) {
// 简化Hopfield模型公式: d_{Top} = 2.47 / (sin(E) + 0.0121)
// 其中 E 是卫星高度角（单位：弧度）
// 计算对流层延迟
double sinE = sin(E);
*tro_correct = 2.47 / (sinE + 0.0121);
}
void Compute_error_correct(double pos[3], Satelite_Result* sres) {
double p, signal_time, angle, sat_pos[3];
double sat_neu[3], BLH[3];
int i;
for (i = 0; i < MAXGPSSATNUM; i++) {
if (sres[i].status != 1) continue;
p = Compute_range(pos, sres[i].pos);
signal_time = p / CLIGHT;
angle = OeDOT_WGS84 * signal_time;
// 保存原始卫星位置
double X = sres[i].pos[0];
double Y = sres[i].pos[1];
double Z = sres[i].pos[2];
// 应用地球自转改正（绕Z轴旋转）
sres[i].pos[3] = X * cos(angle) + Y * sin(angle);
sres[i].pos[4] = -X * sin(angle) + Y * cos(angle);
sres[i].pos[5] = Z;
sat_pos[0] = sres[i].pos[3];
sat_pos[1] = sres[i].pos[4];
sat_pos[2] = sres[i].pos[5];
if (XYZ2BLH(pos, BLH) != 1 || XYZ2ENU(1, pos, sat_pos, sat_neu) != 1) {
sres[i].tro_correct = 0.0;
sres[i].E = PI / 2.0;
continue;
}
sres[i].E = atan(sat_neu[2] / sqrt(sat_neu[0] * sat_neu[0] + sat_neu[1] * sat_neu[1]));
Hopefield(sres[i].E, &sres[i].tro_correct);
}
}
void Build_model_pos(
double pos0[3],
double clo_correct0,
Satelite_Result* sres,
OBS_ONE* obs,
int obs_num,
Eigen::MatrixXd& B,
Eigen::VectorXd& L,
Eigen::MatrixXd& P)
{
const double a = 0.004, b = 0.003;
int j = 0;
// 提前 resize，避免越界
B.resize(obs_num, 4);
L.resize(obs_num);
P.setZero(obs_num, obs_num);
for (int i = 0; i < MAXGPSSATNUM; ++i) {
if (!obs->SatObs[i].status) continue;
double sat_pos[3];
memcpy(sat_pos, &sres[i].pos[3], 3 * sizeof(double));
double range = Compute_range(pos0, sat_pos);
// 设计矩阵 B (行 j)
B(j, 0) = (pos0[0] - sat_pos[0]) / range;
B(j, 1) = (pos0[1] - sat_pos[1]) / range;
B(j, 2) = (pos0[2] - sat_pos[2]) / range;
B(j, 3) = 1.0;
// 观测残差 L
L(j) = obs->SatObs[i].Psr
- range
- CLIGHT * clo_correct0
- sres[i].tro_correct
+ CLIGHT * sres[i].clo_correct;
P(j, j) = 1;
++j;
}
}
int SPP_compute_pos(OBS_ONE* obs,
EPH_ALL* eph,
SPP_Result* res)
{
// 1. 卫星计算 + 计数
Sat_compute(eph, obs, res->sat_res);
int obs_num = 0;
Count_OBS(obs, eph, res->sat_res, res->pos, &obs_num);
if (obs_num < 4) return -2; // 卫星不足
// 2. 初值
double pos0[3] = { res->pos[0], res->pos[1], res->pos[2] };
double clk0 = res->clo_correct;
// 3. Eigen 矩阵/向量
Eigen::MatrixXd B(obs_num, 4);
Eigen::VectorXd L(obs_num);
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(obs_num, obs_num);
Eigen::VectorXd v, dx;
Eigen::MatrixXd Q;
// 4. 迭代
int iter = 0;
do {
Compute_error_correct(pos0, res->sat_res);
Build_model_pos(pos0, clk0, res->sat_res, obs, obs_num, B, L, P);
Q = B.transpose() * P * B;
Eigen::FullPivLU<Eigen::MatrixXd> lu(Q);
if (!lu.isInvertible()) return -1;
dx = Q.inverse() * (B.transpose() * P * L);
pos0[0] += dx(0);
pos0[1] += dx(1);
pos0[2] += dx(2);
clk0 += dx(3) / CLIGHT;
++iter;
} while (dx.head<3>().norm() > 1e-3 && iter < 3);
// 5. 结果回填
for (int i = 0; i < 3; ++i) res->pos[i] = pos0[i];
res->clo_correct = clk0;
res->obs_num = obs_num;
res->sat_num = obs_num;
res->status = 1;
// 6. 精度评估（可选）
Eigen::VectorXd v_final = B * dx - L;
Eigen::MatrixXd Q_final = Q;
Eigen::FullPivLU<Eigen::MatrixXd> lu2(Q_final);
if (lu2.isInvertible()) {
Eigen::MatrixXd cov = lu2.inverse();
res->PDOP_pos = std::sqrt(cov(0, 0) + cov(1, 1) + cov(2, 2));
}
return 1;
}
void Display_res(SPP_Result* res) {
printf("SPP Result: X=%.3f, Y=%.3f, Z=%.3f, dt=%.9f\n",
res->pos[0], res->pos[1], res->pos[2], res->clo_correct);
}
void Display_res_file(FILE* fp, SPP_Result* res) {
fprintf(fp, "SPP Result: X=%.3f, Y=%.3f, Z=%.3f, dt=%.9f\n",
res->pos[0], res->pos[1], res->pos[2], res->clo_correct);
}
int main() {
FILE* obs_fp, * eph_fp, * display_fp;
OBS_ONE* obs = (OBS_ONE*)malloc(sizeof(OBS_ONE));
EPH_ALL* eph = (EPH_ALL*)malloc(sizeof(EPH_ALL) * MAXGPSSATNUM);
SPP_Result* res = (SPP_Result*)malloc(sizeof(SPP_Result));
int err1, err2;
strcpy(filename_obs, "C:/Users/giaoming/Desktop/1111/brst2460.22o");
strcpy(filename_eph, "C:/Users/giaoming/Desktop/1111/brdm2460.22p");
strcpy(filename_res, "./result.txt");
fopen_s(&display_fp, filename_res, "w");
Initial_EPH_ALL(eph);
err1 = fopen_s(&eph_fp, filename_eph, "r");
err2 = fopen_s(&obs_fp, filename_obs, "r");
if (err1 != 0 || err2 != 0) {
printf("Error opening files!\n");
return -1;
}
EphHead_Reader(eph_fp, eph);
EphData_Reader(eph_fp, eph);
ObsHead_Reader(obs_fp, obs);
Initial_SPP_Result(res);
while (!feof(obs_fp)) {
Initial_OBS_ONE(obs);
ObsData_Reader(obs_fp, obs);
if (obs->status == 1) {
SPP_compute_pos(obs, eph, res);
Display_res(res);
Display_res_file(display_fp, res);
}
}
fclose(eph_fp);
fclose(obs_fp);
fclose(display_fp);
free(eph);
free(obs);
free(res);
return 0;
}详细逐行讲解该代码
最新发布
扫一扫
1927
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
