KenanZhu/KNZ_GeoTrackLab
Archived
1
0 Fork 0
Code Issues Actions Packages Projects Releases Wiki Activity

See update notes

Browse Source
This commit is contained in:
KenanZhu111 committed 2024-10-31 14:20:09 +08:00
1 parent 8d22576a73
commit b28dd3e28d
9 files changed
+1732 -1738
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

src/brdm2pos.h
+285 -353
View File
@@ -8,398 +8,330 @@
#include <time.h>
#endif // _BRDM2POS_H_
/* CALCU CONST*/
#define C_V 299792458
#define GM 398600500000000
#define math_e 2.718281828459
#define PI 3.141592653589793
#define Earth_e 7.2921151467e-5
#define C20 1.0826257e-3
#define C_V 299792458 /* Speed of light (m/s) */
#define GM 398600500000000 /* */
#define math_e 2.718281828459 /* Natural constant e */
#define PI 3.141592653589793 /* Ratio of circumference to diameter */
#define Earth_e 7.2921151467e-5 /* Rotational angular velocity of the earths (omega/s) */
#define C20 1.0826257e-3 /* */
#define DAYSEC 86400 /* The total second of a day */
/*WGS-84 CONST*/
#define a 6378137.0
#define e2 (0.0033528106647475*(2 - 0.0033528106647475))
/*PZ-90 CONST*/
#define TSTEP 30.0
#define TSTEP 30.0 /* RK-4 step */
/*GNSS OBSERV TYPE*/
#define FREQ1 1.57542E9 /* L1/E1/B1C frequency (Hz) */
#define FREQ2 1.22760E9 /* L2 frequency (Hz) */
#define FREQ5 1.17645E9 /* L5/E5a/B2a frequency (Hz) */
#define FREQ6 1.27875E9 /* E6/L6 frequency (Hz) */
#define FREQ7 1.20714E9 /* E5b frequency (Hz) */
#define FREQ8 1.191795E9 /* E5a+b frequency (Hz) */
#define FREQ9 2.492028E9 /* S frequency (Hz) */
#define FREQ1_GLO 1.60200E9 /* GLONASS G1 base frequency (Hz) */
#define DFRQ1_GLO 0.56250E6 /* GLONASS G1 bias frequency (Hz/n) */
#define FREQ2_GLO 1.24600E9 /* GLONASS G2 base frequency (Hz) */
#define DFRQ2_GLO 0.43750E6 /* GLONASS G2 bias frequency (Hz/n) */
#define FREQ3_GLO 1.202025E9 /* GLONASS G3 frequency (Hz) */
#define FREQ1a_GLO 1.600995E9 /* GLONASS G1a frequency (Hz) */
#define FREQ2a_GLO 1.248060E9 /* GLONASS G2a frequency (Hz) */
#define FREQ1_BDS 1.561098E9 /* BDS B1I frequency (Hz) */
#define FREQ2_BDS 1.20714E9 /* BDS B2I/B2b frequency (Hz) */
#define FREQ3_BDS 1.26852E9 /* BDS B3 frequency (Hz) */
//frequency 1
#define C1 100
#define C1A 100
#define C1B 101
#define C1C 102
#define C1D 103
#define C1E
#define C1F
#define C1G
#define C1H
#define C1I
#define C1J
#define C1K
#define C1L 104
#define C1M 105
#define C1N
#define C1O
#define C1P 106
#define C1Q
#define C1R
#define C1S 107
#define C1T
#define C1U
#define C1V
#define C1W
#define C1X 108
#define C1Y 109
#define C1Z 110
#define C1 100
#define C1A 100
#define C1B 101
#define C1C 102
#define C1D 103
#define C1L 104
#define C1M 105
#define C1P 106
#define C1S 107
#define C1X 108
#define C1Y 109
#define C1Z 110
//frequency 2
#define C2 200
#define C2A
#define C2B
#define C2C 200
#define C2D 201
#define C2E
#define C2F
#define C2G
#define C2H
#define C2I 202
#define C2J
#define C2K
#define C2L 203
#define C2M 204
#define C2N
#define C2O
#define C2P 205
#define C2Q 206
#define C2 200
#define C2C 200
#define C2D 201
#define C2I 202
#define C2L 203
#define C2M 204
#define C2P 205
#define C2Q 206
#define C2R
#define C2S 207
#define C2T
#define C2U
#define C2V
#define C2W
#define C2X 208
#define C2Y 209
#define C2Z 210
#define C2S 207
#define C2X 208
#define C2Y 209
#define C2Z 210
//frequency 3
#define C3 300
#define C3I 300
#define C3Q 301
#define C3X 302
#define C3 300
#define C3I 300
#define C3Q 301
#define C3X 302
//frequency 4
#define C4 400
#define C4A 400
#define C4B 401
#define C4X 402
#define C4 400
#define C4A 400
#define C4B 401
#define C4X 402
//frequency 5
#define C5 500
#define C5A
#define C5B
#define C5C
#define C5D
#define C5E
#define C5F
#define C5G
#define C5H
#define C5I 500
#define C5J
#define C5K
#define C5L
#define C5M
#define C5N
#define C5O
#define C5P
#define C5Q 501
#define C5R
#define C5S
#define C5T
#define C5U
#define C5V
#define C5W
#define C5X 502
#define C5Y
#define C5Z
#define C5 500
#define C5I 500
#define C5Q 501
#define C5X 502
//frequency 6
#define C6 600
#define C6A 600
#define C6B 601
#define C6C 602
#define C6D
#define C6E
#define C6F
#define C6G
#define C6H
#define C6I
#define C6J
#define C6K
#define C6L
#define C6M
#define C6N
#define C6O
#define C6P
#define C6Q
#define C6R
#define C6S
#define C6T
#define C6U
#define C6V
#define C6W
#define C6X 603
#define C6Y
#define C6Z 604
#define C6 600
#define C6A 600
#define C6B 601
#define C6C 602
#define C6X 603
#define C6Z 604
//frequency 7
#define C7 700
#define C7A
#define C7B
#define C7C
#define C7D
#define C7E
#define C7F
#define C7G
#define C7H
#define C7I 700
#define C7J
#define C7K
#define C7L
#define C7M
#define C7N
#define C7O
#define C7P
#define C7Q 701
#define C7R
#define C7S
#define C7T
#define C7U
#define C7V
#define C7W
#define C7X 702
#define C7Y
#define C7Z
#define C7 700
#define C7I 700
#define C7Q 701
#define C7X 702
//frequency 8
#define C8 800
#define C8I 800
#define C8Q 801
#define C8X 802
#define C8 800
#define C8I 800
#define C8Q 801
#define C8X 802
/*GNSS CODE*/
#define GPS 01
#define BDS 02
#define GAL 03
#define GLO 04
#define SBAS 05
#define GPS 01
#define BDS 02
#define GAL 03
#define GLO 04
#define SBAS 05
#define MAXRINEX 1000
#define DAYSEC 86400
//satellite position & clock bias structure
typedef struct
{
double X;
double Y;
double Z;
double deltat;
double delta_t;
double delta_clk;
#define MAXRINEX 4096 /* Max length of line of rinex file */
#define MAXPATH 260 /* Max length of file path */
typedef struct pos_ts{//Satellite position
double X;
double Y;
double Z;
double deltat;
double delta_t;
double delta_clk;
}pos_ts;
//receiver station position & clock bias structure
typedef struct
{
double X;
double Y;
double Z;
double B;
double L;
double H;
double delta_TR;
typedef struct stations{//
double X;
double Y;
double Z;
double B;
double L;
double H;
double delta_TR;
}stations;
//longitude & latitude structure
typedef struct
{
double B;
int B_d;
int B_m;
int B_s;
double L;
int L_d;
int L_m;
int L_s;
double H;
}blhs;
//satellite azimuth angle structure
typedef struct
{
double R;
double A;
double H;
}rahcal;
//local Cartesian coordinates coordinate system structure
typedef struct
{
double E;
double N;
double U;
}blh2enu;
typedef struct
{
double B;
double L;
double H;
}xyz2blh;
//header data of file of nav
typedef struct nav_head
{
double ver;//rinex
char type[20];
double ION_GPSA[4];
double ION_GPSB[4];
double ION_BDSA[4];
double ION_BDSB[4];
double ION_QZSA[4];
double ION_QZSB[4];
double ION_IRNA[4];
double ION_IRNB[4];
double GPUT[4];
double GLUT[4];
double GAUT[4];
double BDUT[4];
double QZUT[4];
double IRUT[4];
double SBUT[4];
int leap;
typedef struct rah{//
double R;
double A;
double H;
}rah;
typedef struct enu{//East/North/Up coordinate system
double E;
double N;
double U;
}enu;
typedef struct blh{//Longitude/Latitude/Altitude coordinate system
double B;
double L;
double H;
}blh;
typedef struct blhdms{//Longitude/Latitude/Altitude coordinate system
//With Degree/Minute/Second
double B;
int B_d;
int B_m;
int B_s;
double L;
int L_d;
int L_m;
int L_s;
double H;
}blhdms;
typedef struct nav_head{//Header data of file of nav
double ver; /* RINEX version */
char type[20]; /* RINEX file format */
double ION_GPSA[4]; /* Ionosphere correction parameters alpha GPS */
double ION_GPSB[4]; /* Ionosphere correction parameters belta GPS */
double ION_BDSA[4]; /* Ionosphere correction parameters alpha BeiDou */
double ION_BDSB[4]; /* Ionosphere correction parameters belta BeiDou */
double ION_QZSA[4]; /* Ionosphere correction parameters alpha QZSS */
double ION_QZSB[4]; /* Ionosphere correction parameters belta QZSS */
double ION_IRNA[4]; /* Ionosphere correction parameters alpha */
double ION_IRNB[4]; /* Ionosphere correction parameters belta */
double GPUT[4]; /* Time system correction of GPST */
double GLUT[4]; /* Time system correction of GLONASST */
double GAUT[4]; /* Time system correction of GALT */
double BDUT[4]; /* Time system correction of BDT */
double QZUT[4]; /* Time system correction of QZST */
double IRUT[4]; /* Time system correction of */
double SBUT[4]; /* Time system correction of SBAST */
int leap; /* Leap second */
}nav_head, * pnav_head;
//body data of file of nav: GPS BeiDou Galileo
typedef struct nav_body
{
int sPRN_GPS; int sPRN_BDS; int sPRN_GAL; int sPRN_GLO;
int TOC_Y;
int TOC_M;
int TOC_D;
int TOC_H;
int TOC_Min;
int TOC_Sec;
double sa0;
double sa1;
double sa2; double Dos;
double IODE; double AODE; double SatX;
double Crs; double SatXv;
double deltan; double SatXa;
double M0;
typedef struct nav_body{//Body data of file of nav: GPS BeiDou Galileo GLONASS
double Cuc; double SatY;
double e; double SatYv;
double Cus; double SatYa;
double sqrtA; int FreN;
/* GPS BeiDou Galileo GLONASS*/
int sPRN_GPS; int sPRN_BDS; int sPRN_GAL; int sPRN_GLO;/* System */
int TOC_Y;
int TOC_M;
int TOC_D;
int TOC_H;
int TOC_Min;
int TOC_Sec;
double sa0;
double sa1;
double sa2; double Dos;
double TOE; double SatZ;
double Cic; double SatZv;
double OMEGA; double SatZa;
double Cis; int AOO;
double IODE; double AODE; double SatX;
double Crs; double SatXv;
double deltan; double SatXa;
double M0;
double i0;
double Crc;
double omega;
double deltaomega;
double Cuc; double SatY;
double e; double SatYv;
double Cus; double SatYa;
double sqrtA; int FreN;
double IDOT;
double L2code; int Datasource;
double GPSweek; double BDTweek; double GALweek;
double L2Pflag;
double TOE; double SatZ;
double Cic; double SatZv;
double OMEGA; double SatZa;
double Cis; int AOO;
double sACC;
double sHEA;
double TGD; double TGD1; double BGDa;
double IODC; double TGD2; double BGDb;
double i0;
double Crc;
double omega;
double deltaomega;
double TTN;
double fit; double AODC;
double spare1;
double spare2;
double IDOT;
double L2code; int Datasource;
double GPSweek; double BDTweek; double GALweek;
double L2Pflag;
double sACC;
double sHEA;
double TGD; double TGD1; double BGDa;
double IODC; double TGD2; double BGDb;
double TTN;
double fit; double AODC;
double spare1;
double spare2;
}nav_body, * pnav_body;
//header data of file of obs
typedef struct obs_head
{
double ver;
char type[30];
double apX;
double apY;
double apZ;
double ANTH;
double ANTdeltaE;
double ANTdeltaN;
int obstypenum_gps;
int obstypenum_bds;
int obstypenum_gal;
int obstypenum_glo;
int obscode_gps[40];
int obscode_bds[40];
int obscode_gal[40];
int obscode_glo[40];
double interval;
int f_y;
int f_m;
int f_d;
int f_h;
int f_min;
double f_sec;
int l_y;
int l_m;
int l_d;
int l_h;
int l_min;
double l_sec;
char tsys[5];//time system
typedef struct obs_head{//Header data of file of obs
double ver;
char type[30];
double apX;
double apY;
double apZ;
double apB;
double apL;
double apH;
double ANTH;
double ANTdeltaE;
double ANTdeltaN;
int obstypenum_gps; /* Observation amount of GPS */
int obstypenum_bds; /* Observation amount of BeiDou */
int obstypenum_gal; /* Observation amount of Galilro */
int obstypenum_glo; /* Observation amount of GLONASS */
int obscode_gps[40];/* Observation code of GPS */
int obscode_bds[40];/* Observation code of BeiDou */
int obscode_gal[40];/* Observation code of Galileo */
int obscode_glo[40];/* Observation code of GLONASS */
double interval; /* */
int f_y; /* Year of first observe */
int f_m; /* Month of first observe */
int f_d; /* Day of first observe */
int f_h; /* Hour of first observe */
int f_min; /* Minute of first observe */
double f_sec; /* Second of first observe */
int l_y; /* Year of last observe */
int l_m; /* Month of last observe */
int l_d; /* Day of last observe */
int l_h; /* Hour of last observe */
int l_min; /* Minute of last observe */
double l_sec; /* Second of last observe */
char tsys[5]; /* Time system of obs */
}obs_head, * pobs_head;
//epoch data of file of obs
typedef struct obs_epoch
{
int y;
int m;
int d;
int h;
int min;
double sec;//epoch time
int p_flag;//epoch flag
int sat_num;//the total number of obsable sat
int gps_num;//the total gps sat
int bds_num;//the total bds sat
int gal_num;//the total galileo sat
int glo_num;//the total glonass sat
int sPRN[84];//total sprn of obs sat
int sPRN_GPS[32];
int sPRN_BDS[60];
int sPRN_GAL[30];
int sPRN_GLO[24];
typedef struct obs_epoch{//Epoch data of file of obs
int y; /* Epoch year */
int m; /* Epoch month */
int d; /* Epoch day */
int h; /* Epoch hour */
int min; /* Epoch minute */
double sec; /* Epoch second */
int p_flag; /* Epoch flag */
int sat_num; /* The total number of obsable sat */
int gps_num; /* The total GPS satellite */
int bds_num; /* The total BeiDou satellite */
int gal_num; /* The total Galileo satellite */
int glo_num; /* The total GLONASS satellite */
int sPRN[84]; /* Total sprn of obs satellite */
int sPRN_GPS[32];/* Total sprn of GPS satellite in this epoch */
int sPRN_BDS[60];/* Total sprn of BeiDou satellite in this epoch */
int sPRN_GAL[30];/* Total sprn of Galileo satellite in this epoch */
int sPRN_GLO[24];/* Total sprn of GLONASS satellite in this epoch */
}obs_epoch, * pobs_epoch;
//body data of file of obs
typedef struct obs_body
{
double obs_gps[20][26];//GPS obs data
double obs_bds[30][26];//BDS obs data
double obs_gal[20][26];//GALILEO obs data
double obs_glo[20][26];//GLONASS obs data
typedef struct obs_body{//Body data of file of obs
double obs_gps[30][26];/* GPS obs data */
double obs_bds[40][26];/* BeiDou obs data */
double obs_gal[30][26];/* Galileo obs data */
double obs_glo[30][26];/* GLONASS obs data */
}obs_body, * pobs_body;
//
//Contain in timesys.cpp
extern double JDUTC2GPST(double);
extern double Time2GPST(int, int, int, double, int, double);
extern double UTCTime2JD(int, int, int, double, int, double);
//
extern double deg2rad(double);
extern double rad2deg(double);
//
extern rahcal RAHCAL(rahcal, double, double, double);
extern blh2enu BLH2ENU(blh2enu, double, double, double, double, double);
extern xyz2blh XYZ2BLH(xyz2blh, double, double, double);
//
extern int getsatnum(FILE*);
extern int get_epochnum(FILE*);
extern void initobs_e(pobs_epoch, pobs_body);
//Contain in coordtrans.cpp
extern double deg2rad(double);
extern double rad2deg(double);
extern rah rahcal(rah, double, double, double);
extern enu blh2enu(enu, double, double, double, double, double);
extern blh xyz2blh(blh, double, double, double);
//Contain in readrnx.cpp
extern int getsatnum(FILE*);
extern int get_epochnum(FILE*);
extern void freeobs_e(pobs_epoch, pobs_body);
extern int Type2Code(int, char buff[MAXRINEX]);
extern int Code2Type(int, int, int typearr[36]);
extern int select_epoch(double, int, pnav_body, int, int);
//
extern int type2code(int, char buff[MAXRINEX]);
extern int code2type(int, int, int typearr[36]);
extern int select_epoch(double, int, pnav_body, int, int);
//Contain in readrnx.cpp/read
extern void read_n_h(FILE*, pnav_head);
extern void read_n_b(FILE*, pnav_body);
extern void read_o_h(FILE*, pobs_head);
extern void read_o_eb(FILE*, pobs_head, pobs_epoch, pobs_body);
//
extern "C" _declspec(dllexport) double brdm2pos(char nav_path[260], char obs_path[260], char res_path[260], int gnsscode);
//Contain in solutionout.cpp
extern FILE* outputheader(pobs_head, char obs_path[MAXPATH], char nav_path[MAXPATH], char res_path[MAXPATH]);
extern void outputbodydata(pobs_head, pobs_body, pos_ts, rah, FILE*, int, int, int);
//Entry function
extern "C" _declspec(dllexport) double brdm2pos(char nav_path[MAXPATH], char obs_path[MAXPATH], char res_path[MAXPATH],
int GNSS, int Hangle);
src/coordtrans.cpp
+60 -57
View File
@@ -1,57 +1,61 @@
#include "brdm2pos.h"
using namespace std;
/* -------------------------------------------------------------------------- */
/// @brief degree to radian
/// @param deg
/// @return return radian
/* -------------------------------------------------------------------------- */
/**
* Degree to radian.
*
* \param deg: Degree
* \return Radian
*/
double deg2rad(double deg)
{
double rad = (PI / 180) * deg;
return rad;
}
/* -------------------------------------------------------------------------- */
/// @brief radian to degree
/// @param rad
/// @return return degree
/* -------------------------------------------------------------------------- */
/**
* Radian to degree .
*
* \param rad: Radian
* \return Degree
*/
double rad2deg(double rad)
{
double deg = (180 / PI) * rad;
return deg;
}
/* -------------------------------------------------------------------------- */
/// @brief satellite azimuth angle calculation
/// @param rahcal parameter transfer structure
/// @param E east
/// @param N north
/// @param U up
/// @return return structure contain R, A, H members
/* -------------------------------------------------------------------------- */
rahcal RAHCAL(rahcal rahcal,
/**
* Azimuth/Calculate.
*
* \param rah: Parameter transfer structure
* \param E: East
* \param N: North
* \param U: Up
* \return structure contain R, A, H members
*/
rah rahcal(rah rah,
double E, double N, double U)
{
rahcal.H = atan2(U, sqrt(E * E + N * N));
rahcal.A = atan2(E, U);
if (rahcal.A < 0)
rahcal.A += 2 * PI;
if (rahcal.A > 2 * PI)
rahcal.A -= 2 * PI;
rahcal.R = sqrt(E * E + N * N + U * U);
rah.H = atan2(U, sqrt(E * E + N * N));
rah.A = atan2(E, U);
if (rah.A < 0)
rah.A += 2 * PI;
if (rah.A > 2 * PI)
rah.A -= 2 * PI;
rah.R = sqrt(E * E + N * N + U * U);
return rahcal;
return rah;
}
/* -------------------------------------------------------------------------- */
/// @brief convert BLH coordinates to ENU coordinates
/// @param blh2enu parameter transfer structure
/// @param stationB latitude of receiver station
/// @param stationL longitude of receiver station
/// @param deltax X-coordinate difference of satellite arrival center
/// @param deltay Y-coordinate difference of satellite arrival center
/// @param deltaz Z-coordinate difference of satellite arrival center
/// @return return type of structure contain E, N, U members
/* -------------------------------------------------------------------------- */
blh2enu BLH2ENU(blh2enu blh2enu,
/**
* Convert BLH coordinates to ENU coordinates.
*
* \param enu: Parameter transfer structure
* \param stationB: Latitude of receiver station
* \param stationL: Longitude of receiver station
* \param deltax: Deltax X-coordinate difference of satellite arrival center
* \param deltay: Deltax Y-coordinate difference of satellite arrival center
* \param deltaz: Deltax Z-coordinate difference of satellite arrival center
* \return
*/
enu blh2enu(enu enu,
double stationB, double stationL,
double deltax, double deltay, double deltaz)
{
@@ -59,29 +63,28 @@ blh2enu BLH2ENU(blh2enu blh2enu,
double cosB = cos(stationB);
double sinL = sin(stationL);
double cosL = cos(stationL);
blh2enu.E = -sinL * (deltax)+cosL * (deltay);
blh2enu.N = -sinB * cosL * (deltax)-sinB * sinL * (deltay)+cosB * (deltaz);
blh2enu.U = cosB * cosL * (deltax)+cosB * sinL * (deltay)+sinB * (deltaz);
enu.E = -sinL * (deltax) +cosL * (deltay);
enu.N = -sinB * cosL * (deltax)-sinB * sinL * (deltay)+cosB * (deltaz);
enu.U = cosB * cosL * (deltax)+cosB * sinL * (deltay)+sinB * (deltaz);
return blh2enu;
return enu;
}
/* -------------------------------------------------------------------------- */
/// @brief Convert ECEF geostationary coordinates to BLH coordinates
/// @param xyz2blh parameter transfer structure
/// @param X X coordinates of ECEF
/// @param Y Y coordinates of ECEF
/// @param Z Z coordinates of ECEF
/// @param a major semiaxis of WGS-84 ellipsoid
/// @param e2 the square of the first eccentricity of WGS-84 ellipsoid
/// @return return type of structure contain B, L, H members
/* -------------------------------------------------------------------------- */
xyz2blh XYZ2BLH(xyz2blh xyz2blh,
/**
* Convert ECEF geostationary coordinates to BLH coordinates.
*
* \param blh: Parameter transfer structure
* \param X: X coordinates of ECEF
* \param Y; Y coordinates of ECEF
* \param Z: Z coordinates of ECEF
* \return type of structure contain B, L, H members
*/
blh xyz2blh(blh blh,
double X, double Y, double Z)
{
double B = 0.0, N = 0.0, H = 0.0, R0, R1, deltaH, deltaB;
R0 = sqrt(pow(X, 2) + pow(Y, 2));
R1 = sqrt(pow(X, 2) + pow(Y, 2) + pow(Z, 2));
xyz2blh.L = atan2(Y, X);
blh.L = atan2(Y, X);
N = a;
H = R1 - a;
B = atan2(Z * (N + H), R0 * (N * (1 - e2) + H));
@@ -92,8 +95,8 @@ xyz2blh XYZ2BLH(xyz2blh xyz2blh,
H = R0 / cos(B) - N;
B = atan2(Z * (N + H), R0 * (N * (1 - e2) + H));
} while (fabs(deltaH - H) > 1.0e-3 && fabs(deltaB - B) > 1.0e-9);
xyz2blh.B = B;
xyz2blh.H = H;
blh.B = B;
blh.H = H;
return xyz2blh;
return blh;
}
src/readrnx.cpp
+733 -792
View File
File diff suppressed because it is too large. Load diff
src/rnx2pos.cpp
+330 -347
View File
@@ -3,411 +3,394 @@ using namespace std;
/*Obatain the intergral term of acceleraction of each component*/
void pz90deq(double X[6], double K[6], double acc[3])
{
double r = sqrt(pow(X[0], 2) + pow(X[1], 2) + pow(X[2], 2));
double coef0 = -GM / pow(r, 3);
double coef1 = 1.5 * C20 * (GM * pow(a, 2) / pow(r, 5));
double r = sqrt(pow(X[0], 2) + pow(X[1], 2) + pow(X[2], 2));
double coef0 = -GM / pow(r, 3);
double coef1 = 1.5 * C20 * (GM * pow(a, 2) / pow(r, 5));
K[0] = X[3]; K[1] = X[4]; K[2] = X[5];
K[3] =
coef0 * X[0] +
coef1 * (1 - 5 * pow((X[2] / r), 2)) * X[0] +
pow(Earth_e, 2) * X[0] +
2 * Earth_e * X[4] +
acc[0];
K[4] =
coef0 * X[1] +
coef1 * (1 - 5 * pow((X[2] / r), 2)) * X[1] +
pow(Earth_e, 2) * X[1] -
2 * Earth_e * X[3] +
acc[1];
K[5] =
coef0 * X[2] +
coef1 * (3 - 5 * pow((X[2] / r), 2)) * X[2] +
acc[2];
K[0] = X[3]; K[1] = X[4]; K[2] = X[5];
K[3] =
coef0 * X[0] +
coef1 * (1 - 5 * pow((X[2] / r), 2)) * X[0] +
pow(Earth_e, 2) * X[0] +
2 * Earth_e * X[4] +
acc[0];
K[4] =
coef0 * X[1] +
coef1 * (1 - 5 * pow((X[2] / r), 2)) * X[1] +
pow(Earth_e, 2) * X[1] -
2 * Earth_e * X[3] +
acc[1];
K[5] =
coef0 * X[2] +
coef1 * (3 - 5 * pow((X[2] / r), 2)) * X[2] +
acc[2];
}
/*Construct the RK-4 method*/
void pz90pos(double tstep, double X[6], double acc[3])
{
double K1[6], K2[6], K3[6], K4[6], W[6];
double K1[6], K2[6], K3[6], K4[6], W[6];
pz90deq(X, K1, acc); for (int i = 0; i < 6; i++) W[i] = X[i] + K1[i] * tstep / 2.0;
pz90deq(W, K2, acc); for (int i = 0; i < 6; i++) W[i] = X[i] + K2[i] * tstep / 2.0;
pz90deq(W, K3, acc); for (int i = 0; i < 6; i++) W[i] = X[i] + K3[i] * tstep;
pz90deq(W, K4, acc);
for (int i = 0; i < 6; i++) X[i] += (K1[i] + 2.0 * K2[i] + 2.0 * K3[i] + K4[i]) * tstep / 6.0;
pz90deq(X, K1, acc); for (int i = 0; i < 6; i++) W[i] = X[i] + K1[i] * tstep / 2.0;
pz90deq(W, K2, acc); for (int i = 0; i < 6; i++) W[i] = X[i] + K2[i] * tstep / 2.0;
pz90deq(W, K3, acc); for (int i = 0; i < 6; i++) W[i] = X[i] + K3[i] * tstep;
pz90deq(W, K4, acc);
for (int i = 0; i < 6; i++) X[i] += (K1[i] + 2.0 * K2[i] + 2.0 * K3[i] + K4[i]) * tstep / 6.0;
}
/*Calculate the satellite position of GLONASS on this epoch*/
pos_ts glo_pos(int best_epoch, double Weeksec,
pnav_body nav_b, pobs_head obs_h, pos_ts pos_t)
pnav_body nav_b, pobs_head obs_h, pos_ts pos_t)
{
double X[6], acc[3]; double R = 0.0;
double tdist = Weeksec - pos_t.delta_t - nav_b[best_epoch].TOE, tstep = 0.0;
pos_t.delta_clk = -nav_b[best_epoch].sa0 + nav_b[best_epoch].sa1 * tdist;
double X[6], acc[3]; double R = 0.0;
double tdist = Weeksec - pos_t.delta_t - nav_b[best_epoch].TOE, tstep = 0.0;
pos_t.delta_clk = -nav_b[best_epoch].sa0 + nav_b[best_epoch].sa1 * tdist;
X[0] = nav_b[best_epoch].SatX * 1e3;
X[1] = nav_b[best_epoch].SatY * 1e3;
X[2] = nav_b[best_epoch].SatZ * 1e3;//Position
X[0] = nav_b[best_epoch].SatX * 1e3;
X[1] = nav_b[best_epoch].SatY * 1e3;
X[2] = nav_b[best_epoch].SatZ * 1e3;//Position
X[3] = nav_b[best_epoch].SatXv * 1e3;
X[4] = nav_b[best_epoch].SatYv * 1e3;
X[5] = nav_b[best_epoch].SatZv * 1e3;//Velocity
X[3] = nav_b[best_epoch].SatXv * 1e3;
X[4] = nav_b[best_epoch].SatYv * 1e3;
X[5] = nav_b[best_epoch].SatZv * 1e3;//Velocity
acc[0] = nav_b[best_epoch].SatXa * 1e3;
acc[1] = nav_b[best_epoch].SatYa * 1e3;
acc[2] = nav_b[best_epoch].SatZa * 1e3;//Acclerate
acc[0] = nav_b[best_epoch].SatXa * 1e3;
acc[1] = nav_b[best_epoch].SatYa * 1e3;
acc[2] = nav_b[best_epoch].SatZa * 1e3;//Acclerate
for (tstep = tdist < 0.0 ? TSTEP : -TSTEP; fabs(tdist) > 1.0e-9; tdist += tstep) {
if (fabs(tdist) < TSTEP) tstep = -tdist;
pz90pos(tstep, X, acc);
}
for (tstep = tdist < 0.0 ? TSTEP : -TSTEP; fabs(tdist) > 1.0e-9; tdist += tstep) {
if (fabs(tdist) < TSTEP) tstep = -tdist;
pz90pos(tstep, X, acc);
}
X[0] = -0.47 + (X[0] * 1 + X[1] * 1.728e-6 + X[2] * -0.017e-6) * (1 + 22e-9);
X[1] = -0.51 + (X[0] * 1.728e-6 + X[1] * 1 + X[2] * -0.076e-6) * (1 + 22e-9);
X[2] = -1.56 + (X[0] * 0.0178e-6 + X[1] * -0.076e-6 + X[2] * 1) * (1 + 22e-9);//PZ90 Convert to WGS-84
X[0] = -0.47 + (X[0] * 1 + X[1] * 1.728e-6 + X[2] * -0.017e-6) * (1 + 22e-9);
X[1] = -0.51 + (X[0] * 1.728e-6 + X[1] * 1 + X[2] * -0.076e-6) * (1 + 22e-9);
X[2] = -1.56 + (X[0] * 0.0178e-6 + X[1] * -0.076e-6 + X[2] * 1) * (1 + 22e-9);//PZ90 Convert to WGS-84
pos_t.X = X[0];
pos_t.Y = X[1];
pos_t.Z = X[2];
pos_t.X = X[0];
pos_t.Y = X[1];
pos_t.Z = X[2];
R = sqrt(pow(pos_t.X - obs_h->apX, 2) + pow(pos_t.Y - obs_h->apY, 2) + pow(pos_t.Z - obs_h->apZ, 2));
pos_t.deltat = pos_t.delta_t;
pos_t.delta_t = R / C_V + pos_t.delta_clk;
R = sqrt(pow(pos_t.X - obs_h->apX, 2) + pow(pos_t.Y - obs_h->apY, 2) + pow(pos_t.Z - obs_h->apZ, 2));
pos_t.deltat = pos_t.delta_t;
pos_t.delta_t = R / C_V + pos_t.delta_clk;
return pos_t;
return pos_t;
}
/*Calculate the satellite position of other systems on this epoch*/
pos_ts sat_pos(int sPRN, int best_epoch, double Weeksec, int gnsscode,
pnav_body nav_b, pobs_head obs_h, pos_ts pos_t)
pos_ts sat_pos(int sPRN, int best_epoch, double Weeksec, int GNSS,
pnav_body nav_b, pobs_head obs_h, pos_ts pos_t)
{
double T_S = Weeksec - pos_t.delta_t;
double n_0 = sqrt(GM) / pow(nav_b[best_epoch].sqrtA, 3);
double n = n_0 + nav_b[best_epoch].deltan;
double tk;
if (gnsscode == BDS) tk = T_S - nav_b[best_epoch].TOE - 14;
else tk = T_S - nav_b[best_epoch].TOE;
double T_S = Weeksec - pos_t.delta_t;
double n_0 = sqrt(GM) / pow(nav_b[best_epoch].sqrtA, 3);
double n = n_0 + nav_b[best_epoch].deltan;
double tk;
if (tk > 32400)tk -= 604800;
else if (tk < -32400)tk += 604800;
if (GNSS == BDS) tk = T_S - nav_b[best_epoch].TOE - 14;//BDST = GPST + 14
else tk = T_S - nav_b[best_epoch].TOE;
double Ms = nav_b[best_epoch].M0 + n * tk;
if (tk > 32400)tk -= 604800;
else if (tk < -32400)tk += 604800;
double Es = Ms, E0;
do
{
E0 = Es;
Es = Ms + nav_b[best_epoch].e * sin(Es);
} while (fabs(Es - E0) > 1.0e-10);
double Ms = nav_b[best_epoch].M0 + n * tk;
double cosfs = (cos(Es) - nav_b[best_epoch].e) / (1 - nav_b[best_epoch].e * cos(Es));
double sinfs = (sqrt(1 - pow(nav_b[best_epoch].e, 2)) * sin(Es)) / (1 - nav_b[best_epoch].e * cos(Es));
double fs = atan2(sinfs, cosfs);
double u0 = fs + nav_b[best_epoch].omega;
double Es = Ms, E0;
do
{
E0 = Es;
Es = Ms + nav_b[best_epoch].e * sin(Es);
} while (fabs(Es - E0) > 1.0e-10);
double epsilon_u = nav_b[best_epoch].Cus * sin(2 * u0) + nav_b[best_epoch].Cuc * cos(2 * u0);
double epsilon_r = nav_b[best_epoch].Crs * sin(2 * u0) + nav_b[best_epoch].Crc * cos(2 * u0);
double epsilon_i = nav_b[best_epoch].Cis * sin(2 * u0) + nav_b[best_epoch].Cic * cos(2 * u0);
double cosfs = (cos(Es) - nav_b[best_epoch].e) / (1 - nav_b[best_epoch].e * cos(Es));
double sinfs = (sqrt(1 - pow(nav_b[best_epoch].e, 2)) * sin(Es)) / (1 - nav_b[best_epoch].e * cos(Es));
double fs = atan2(sinfs, cosfs);
double u0 = fs + nav_b[best_epoch].omega;
double u = u0 + epsilon_u;
double r = pow(nav_b[best_epoch].sqrtA, 2) * (1 - nav_b[best_epoch].e * cos(Es)) + epsilon_r;
double i = nav_b[best_epoch].i0 + epsilon_i + nav_b[best_epoch].IDOT * tk;
double epsilon_u = nav_b[best_epoch].Cus * sin(2 * u0) + nav_b[best_epoch].Cuc * cos(2 * u0);
double epsilon_r = nav_b[best_epoch].Crs * sin(2 * u0) + nav_b[best_epoch].Crc * cos(2 * u0);
double epsilon_i = nav_b[best_epoch].Cis * sin(2 * u0) + nav_b[best_epoch].Cic * cos(2 * u0);
double x = r * cos(u);
double y = r * sin(u);
double u = u0 + epsilon_u;
double r = pow(nav_b[best_epoch].sqrtA, 2) * (1 - nav_b[best_epoch].e * cos(Es)) + epsilon_r;
double i = nav_b[best_epoch].i0 + epsilon_i + nav_b[best_epoch].IDOT * tk;
double l, X, Y, Z;
if (gnsscode == BDS) {
if (sPRN > 5 && sPRN < 59) //BDS IGSO & MEO Satellites
{
l = nav_b[best_epoch].OMEGA + (nav_b[best_epoch].deltaomega - Earth_e) * tk - Earth_e * nav_b[best_epoch].TOE;
X = x * cos(l) - y * cos(i) * sin(l);
Y = x * sin(l) + y * cos(i) * cos(l);
Z = y * sin(i);
}
else //BDS GEO Satellites
{
l = nav_b[best_epoch].OMEGA + nav_b[best_epoch].deltaomega * tk - Earth_e * nav_b[best_epoch].TOE;
X = x * cos(l) - y * cos(i) * sin(l);
Y = x * sin(l) + y * cos(i) * cos(l);
Z = y * sin(i);
double x = r * cos(u);
double y = r * sin(u);
double f = deg2rad(-5);
double p = Earth_e * tk;
double l, X, Y, Z;
if (GNSS == BDS && (sPRN <= 5 || sPRN >= 59)) {//BDS GEO Satellites
l = nav_b[best_epoch].OMEGA + nav_b[best_epoch].deltaomega * tk - Earth_e * nav_b[best_epoch].TOE;
X = x * cos(l) - y * cos(i) * sin(l);
Y = x * sin(l) + y * cos(i) * cos(l);
Z = y * sin(i);
X = X * cos(p) + Y * sin(p) * cos(f) + Z * sin(p) * sin(f);
Y = X * -sin(p) + Y * cos(p) * cos(f) + Z * cos(p) * sin(f);
Z = Y * -sin(f) + Z * cos(f);
}
}
else {
l = nav_b[best_epoch].OMEGA + (nav_b[best_epoch].deltaomega - Earth_e) * tk - Earth_e * nav_b[best_epoch].TOE;
X = x * cos(l) - y * cos(i) * sin(l);
Y = x * sin(l) + y * cos(i) * cos(l);
Z = y * sin(i);
}
pos_t.X = cos(Earth_e * pos_t.delta_t) * X + sin(Earth_e * pos_t.delta_t) * Y;
pos_t.Y = -sin(Earth_e * pos_t.delta_t) * X + cos(Earth_e * pos_t.delta_t) * Y;
pos_t.Z = Z;//Correction of earth rotation
double f = deg2rad(-5);
double p = Earth_e * tk;
double TGD;
if (gnsscode == GPS) TGD = nav_b[best_epoch].TGD;
else if (gnsscode == GAL) TGD = nav_b[best_epoch].BGDa;
else if (gnsscode == BDS) TGD = 0;
X = X * cos(p) + Y * sin(p) * cos(f) + Z * sin(p) * sin(f);
Y = X * -sin(p) + Y * cos(p) * cos(f) + Z * cos(p) * sin(f);
Z = Y * -sin(f) + Z * cos(f);
}
else {
l = nav_b[best_epoch].OMEGA + (nav_b[best_epoch].deltaomega - Earth_e) * tk - Earth_e * nav_b[best_epoch].TOE;
X = x * cos(l) - y * cos(i) * sin(l);
Y = x * sin(l) + y * cos(i) * cos(l);
Z = y * sin(i);
}
pos_t.X = cos(Earth_e * pos_t.delta_t) * X + sin(Earth_e * pos_t.delta_t) * Y;
pos_t.Y = -sin(Earth_e * pos_t.delta_t) * X + cos(Earth_e * pos_t.delta_t) * Y;
pos_t.Z = Z;//Correction of earth rotation
double R = sqrt(pow(pos_t.X - obs_h->apX, 2) + pow(pos_t.Y - obs_h->apY, 2) + pow(pos_t.Z - obs_h->apZ, 2));
double rela = 2 * sqrt(GM) * nav_b[best_epoch].e * nav_b[best_epoch].sqrtA * sin(Es) / pow(C_V, 2);//Relativistic effect
pos_t.delta_clk = nav_b[best_epoch].sa0 + nav_b[best_epoch].sa1 * tk + nav_b[best_epoch].sa2 * pow(tk, 2) - rela - TGD;
pos_t.deltat = pos_t.delta_t;
pos_t.delta_t = R / C_V + pos_t.delta_clk;
double TGD=0.0;
if (GNSS == GPS) TGD = nav_b[best_epoch].TGD;
else if (GNSS == GAL) TGD = nav_b[best_epoch].BGDa;
else if (GNSS == BDS) TGD = nav_b[best_epoch].TGD1;
return pos_t;
//Recalculate the diatance of satellite-receiver
double R = sqrt(pow(pos_t.X - obs_h->apX, 2) + pow(pos_t.Y - obs_h->apY, 2) + pow(pos_t.Z - obs_h->apZ, 2));
//Get relativistic effect correction
double rela = 2 * sqrt(GM) * nav_b[best_epoch].e * nav_b[best_epoch].sqrtA * sin(Es) / pow(C_V, 2);
pos_t.delta_clk = nav_b[best_epoch].sa0 + nav_b[best_epoch].sa1 * tk + nav_b[best_epoch].sa2 * pow(tk, 2) - rela + TGD;
pos_t.deltat = pos_t.delta_t;
pos_t.delta_t = R / C_V + pos_t.delta_clk;
return pos_t;
}
/* -------------------------------------------------------------------------- */
/// @brief Calculate the position of satellite in the ECEF coordinate system by the broadcast ephemeris and epoch information
/// @param obs_h Observation header data
/// @param obs_e Observation epoch data
/// @param obs_b Observation data
/// @param nav_b Navigation data
/// @param res_file The file path of result file of satellite position
/// @param station Prior information about station location
/// @param gnsscode GNSS system code
/// @param satnum The total number of satellite of designated satellite system
/// @return 1 refer OK
/* -------------------------------------------------------------------------- */
/**
* .
*
* \param obs_h: Observation header data
* \param obs_e: Observation epoch data
* \param obs_b: Observation data
* \param nav_b: Navigation data
* \param res_file: The file path of result file of satellite position
* \param GNSS: GNSS system code
* \param satnum: The total number of satellite of designated satellite system
* \return
*/
int sat_pos_cal(pobs_head obs_h, pobs_epoch obs_e, pobs_body obs_b,
pnav_body nav_b,
FILE* res_file, stations station,
int gnsscode, int satnum)
pnav_body nav_b,
FILE* res_file,
int GNSS, int satnum, int Hangle)
{
//the beginning second of observation
double beginsec = Time2GPST(obs_h->f_y, obs_h->f_m, obs_h->f_d, obs_h->f_h, obs_h->f_min, obs_h->f_sec);
//the ending second of observation
double endinsec = Time2GPST(obs_h->l_y, obs_h->l_m, obs_h->l_d, obs_h->l_h, obs_h->l_min, obs_h->l_sec);
//the beginning second of observation
double beginsec = Time2GPST(obs_h->f_y, obs_h->f_m, obs_h->f_d, obs_h->f_h, obs_h->f_min, obs_h->f_sec);
blhs blh = { 0 }; blh2enu enu = { 0 };
rahcal rah = { 0 }; pos_ts pos_t = { 0 };
xyz2blh tem1 = { 0 }; blh2enu tem2 = { 0 }; rahcal tem3 = { 0 };
//the ending second of observation
double endinsec = Time2GPST(obs_h->l_y, obs_h->l_m, obs_h->l_d, obs_h->l_h, obs_h->l_min, obs_h->l_sec);
int y = obs_e->y; int m = obs_e->m; int d = obs_e->d;
double h = obs_e->h; int min = obs_e->min; double sec = obs_e->sec;
rah in_rah = { 0 }; rah tem_rah = { 0 };
enu in_enu = { 0 }; enu tem_enu = { 0 };
blh in_blh = { 0 }; blh tem_blh = { 0 };
pos_ts pos_t = { 0 };
int y = obs_e->y; int m = obs_e->m; int d = obs_e->d;
double h = obs_e->h; int min = obs_e->min; double sec = obs_e->sec;
double Weeksec = Time2GPST(y, m, d, h, min, sec);
int epochcount = round((Weeksec - beginsec) / obs_h->interval) + 1;
fprintf(res_file, "\n>%04d %04d %02d %02d %02d %02d %07.04f"
, epochcount, obs_e->y, obs_e->m, obs_e->d, obs_e->h, obs_e->min, obs_e->sec);
int sat_num = 0, sPRN = 0, SIGN = 0, PERS = 0;
double detat_toc;
//Check the GNSS and assign the satellite number
if (gnsscode == GPS)sat_num = obs_e->gps_num;
else if (gnsscode == GAL)sat_num = obs_e->gal_num;
else if (gnsscode == BDS)sat_num = obs_e->bds_num;
else if (gnsscode == GLO)sat_num = obs_e->glo_num;
double Weeksec = Time2GPST(y, m, d, h, min, sec);
int epochcount = round((Weeksec - beginsec) / obs_h->interval) + 1;
fprintf(res_file, "\n>%04d %04d %02d %02d %02d %02d %07.04f"
, epochcount, obs_e->y, obs_e->m, obs_e->d, obs_e->h, obs_e->min, obs_e->sec);
int sat_num = 0, sPRN = 0, PERS = 0;
double detat_toc;
for (int j = 0; j < sat_num; j++) {
if (gnsscode == GPS) {
sPRN = obs_e->sPRN_GPS[j]; SIGN = GPS;
PERS = obs_b->obs_gps[j][Code2Type(C1, obs_h->obstypenum_gps, obs_h->obscode_gps)] / C_V;
}
else if (gnsscode == GAL) {
sPRN = obs_e->sPRN_GAL[j]; SIGN = GAL;
PERS = obs_b->obs_gal[j][Code2Type(C1, obs_h->obstypenum_gal, obs_h->obscode_gal)] / C_V;
}
else if (gnsscode == BDS) {
sPRN = obs_e->sPRN_BDS[j]; SIGN = BDS;
PERS = obs_b->obs_bds[j][Code2Type(C2, obs_h->obstypenum_bds, obs_h->obscode_bds)] / C_V;
}
else if (gnsscode == GLO) {
sPRN = obs_e->sPRN_GLO[j]; SIGN = GLO;
PERS = obs_b->obs_glo[j][Code2Type(C1, obs_h->obstypenum_glo, obs_h->obscode_glo)] / C_V;
}
//Match the system and assign the satellite number
if (GNSS == GPS)sat_num = obs_e->gps_num;
else if (GNSS == GAL)sat_num = obs_e->gal_num;
else if (GNSS == BDS)sat_num = obs_e->bds_num;
else if (GNSS == GLO)sat_num = obs_e->glo_num;
int best_epoch = select_epoch(Weeksec, sPRN, nav_b, satnum, SIGN);
if (best_epoch == -1)//when the sPRN is not exist function will return -1
break;
if (SIGN == BDS) detat_toc = Weeksec - nav_b[best_epoch].TOE - 14;
else detat_toc = Weeksec - nav_b[best_epoch].TOE;
for (int j = 0; j < sat_num; j++) {
//Iterative approximate propagation time
//Exclude GLONASS
if (SIGN == GLO) {
pos_t.delta_t = PERS -
station.delta_TR +
nav_b[best_epoch].sa0 +
nav_b[best_epoch].sa1 * detat_toc;
pos_t.deltat = 0.0;
//Match system and assign the sPRN
if (GNSS == GPS) {
sPRN = obs_e->sPRN_GPS[j];
PERS = obs_b->obs_gps[j][code2type(C1, obs_h->obstypenum_gps, obs_h->obscode_gps)] / C_V;
}
else if (GNSS == GAL) {
sPRN = obs_e->sPRN_GAL[j];
PERS = obs_b->obs_gal[j][code2type(C1, obs_h->obstypenum_gal, obs_h->obscode_gal)] / C_V;
}
else if (GNSS == BDS) {
sPRN = obs_e->sPRN_BDS[j];
PERS = obs_b->obs_bds[j][code2type(C2, obs_h->obstypenum_bds, obs_h->obscode_bds)] / C_V;
}
else if (GNSS == GLO) {
sPRN = obs_e->sPRN_GLO[j];
PERS = obs_b->obs_glo[j][code2type(C1, obs_h->obstypenum_glo, obs_h->obscode_glo)] / C_V;
}
while (fabs(pos_t.delta_t - pos_t.deltat) > 1.0e-9) {
pos_t = glo_pos(best_epoch, Weeksec, nav_b, obs_h, pos_t);
}
}
else {
pos_t.delta_t = PERS -
station.delta_TR +
nav_b[best_epoch].sa0 +
nav_b[best_epoch].sa1 * detat_toc +
nav_b[best_epoch].sa2 * pow(detat_toc, 2);
pos_t.deltat = 0.0;
while (fabs(pos_t.delta_t - pos_t.deltat) > 1.0e-9) {
pos_t = sat_pos(sPRN, best_epoch, Weeksec, gnsscode, nav_b, obs_h, pos_t);
}
}
//Satellite blh position calculation
tem1 = XYZ2BLH(tem1, pos_t.X, pos_t.Y, pos_t.Z);
blh.B = rad2deg(tem1.B);
blh.L = rad2deg(tem1.L);
blh.H = tem1.H;
//Match the nearest satellite of nav with this epoch
int best_epoch = select_epoch(Weeksec, sPRN, nav_b, satnum, GNSS);
double deltax = pos_t.X - obs_h->apX;
double deltay = pos_t.Y - obs_h->apY;
double deltaz = pos_t.Z - obs_h->apZ;
if (best_epoch == -1)//when the sPRN is not exist function will return -1
break;
tem2 = BLH2ENU(tem2, station.B, station.L, deltax, deltay, deltaz);
enu.E = tem2.E;
enu.N = tem2.N;
enu.U = tem2.U;
//Satellite elevation Angle calculation
tem3 = RAHCAL(tem3, enu.E, enu.N, enu.U);
rah.R = tem3.R;
rah.A = rad2deg(tem3.A);
rah.H = rad2deg(tem3.H);
if (blh.H < 0 || rah.H < 10 || nav_b[best_epoch].sHEA != 0) {
continue;
}
else {
if (SIGN == GPS) {
fprintf(res_file, "\nG %02d| %15.05f %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
, sPRN, pos_t.X, pos_t.Y, pos_t.Z
, obs_b->obs_gps[j][Code2Type(C1, obs_h->obstypenum_gps, obs_h->obscode_gps)]
, obs_b->obs_gps[j][Code2Type(C2, obs_h->obstypenum_gps, obs_h->obscode_gps)]
, rah.H, pos_t.delta_clk);//GPS satellite pos output
}
else if (SIGN == GAL) {
fprintf(res_file, "\nE %02d| %15.05f %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
, sPRN, pos_t.X, pos_t.Y, pos_t.Z
, obs_b->obs_gal[j][Code2Type(C1, obs_h->obstypenum_gal, obs_h->obscode_gal)]
, obs_b->obs_gal[j][Code2Type(C6, obs_h->obstypenum_gal, obs_h->obscode_gal)]
, rah.H, pos_t.delta_clk);//Galileo satellite pos output
}
else if (SIGN == BDS) {
if (sPRN >= 19) {
fprintf(res_file, "\nCC%02d| %15.05f %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
, sPRN, pos_t.X, pos_t.Y, pos_t.Z
, obs_b->obs_bds[j][Code2Type(C2, obs_h->obstypenum_bds, obs_h->obscode_bds)]
, obs_b->obs_bds[j][Code2Type(C7, obs_h->obstypenum_bds, obs_h->obscode_bds)]
, rah.H, pos_t.delta_clk);//BDS-3 satellite pos output
}
else {
fprintf(res_file, "\nCB%02d| %15.05f %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
, sPRN, pos_t.X, pos_t.Y, pos_t.Z
, obs_b->obs_bds[j][Code2Type(C2, obs_h->obstypenum_bds, obs_h->obscode_bds)]
, obs_b->obs_bds[j][Code2Type(C7, obs_h->obstypenum_bds, obs_h->obscode_bds)]
, rah.H, pos_t.delta_clk);//BDS-2 satellite pos output
}
}
else if (SIGN == GLO) {
fprintf(res_file, "\nR %02d| %15.05f %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
, sPRN, pos_t.X, pos_t.Y, pos_t.Z
, obs_b->obs_glo[j][Code2Type(C1, obs_h->obstypenum_glo, obs_h->obscode_glo)]
, obs_b->obs_glo[j][Code2Type(C2, obs_h->obstypenum_glo, obs_h->obscode_glo)]
, rah.H, pos_t.delta_clk);//Galileo satellite pos output
}
}
}return 1;
if (GNSS == BDS) detat_toc = Weeksec - nav_b[best_epoch].TOE - 14;//BDST = GPST + 14s
else detat_toc = Weeksec - nav_b[best_epoch].TOE;
//Iterative approximate propagation time
if (GNSS == GLO) {
//Get the approximately transit of signal
pos_t.delta_t = PERS - 0 +
nav_b[best_epoch].sa0 +
nav_b[best_epoch].sa1 * detat_toc;
pos_t.deltat = 0.0;
//Satellite position calculation
while (fabs(pos_t.delta_t - pos_t.deltat) > 1.0e-9) {
pos_t = glo_pos(best_epoch, Weeksec, nav_b, obs_h, pos_t);
}
}
//Exclude GLONASS
else {
//Get the approximately transit of signal
pos_t.delta_t = PERS - 0 +
nav_b[best_epoch].sa0 +
nav_b[best_epoch].sa1 * detat_toc +
nav_b[best_epoch].sa2 * pow(detat_toc, 2);
pos_t.deltat = 0.0;
//Satellite position calculation
while (fabs(pos_t.delta_t - pos_t.deltat) > 1.0e-9) {
pos_t = sat_pos(sPRN, best_epoch, Weeksec, GNSS, nav_b, obs_h, pos_t);
}
}
//Satellite blh position calculation
tem_blh = xyz2blh(tem_blh, pos_t.X, pos_t.Y, pos_t.Z);
in_blh.B = rad2deg(tem_blh.B);
in_blh.L = rad2deg(tem_blh.L);
in_blh.H = tem_blh.H;
double deltax = pos_t.X - obs_h->apX;
double deltay = pos_t.Y - obs_h->apY;
double deltaz = pos_t.Z - obs_h->apZ;
tem_enu = blh2enu(tem_enu, obs_h->apB, obs_h->apL, deltax, deltay, deltaz);
in_enu.E = tem_enu.E;
in_enu.N = tem_enu.N;
in_enu.U = tem_enu.U;
//Satellite elevation Angle calculation
tem_rah = rahcal(tem_rah, in_enu.E, in_enu.N, in_enu.U);
in_rah.R = tem_rah.R;
in_rah.A = rad2deg(tem_rah.A);
in_rah.H = rad2deg(tem_rah.H);
if (in_blh.H < 0 || in_rah.H < Hangle || nav_b[best_epoch].sHEA != 0) {
continue;
}
else outputbodydata(obs_h, obs_b, pos_t, in_rah, res_file, sPRN, j, GNSS);
}return 1;
}
/* -------------------------------------------------------------------------- */
/// @brief The entry function of full .dll package,
/// @param nav_path The RINEX navigation file's path
/// @param obs_path The RINEX observation file's path
/// @param res_path The Satellite output file's path
/// @param gnsscode The GNSS position you want to calculate
/// @return 0 refer complete solution; -1 refer fail to complete solution
/// @return > 0 refer partlt complete solution
/* -------------------------------------------------------------------------- */
double brdm2pos(char nav_path[260], char obs_path[260], char res_path[260], int gnsscode)
/**
* The entrance of function.
*
* \param nav_path: The RINEX navigation file's path
* \param obs_path: The RINEX observation file's path
* \param res_path: The satellite position solution output path
* \param GNSS: GNSS code
* \return
* -1 ,Can not open the nav file
* -2 ,Can not open the obs file
* -3 ,Mismatch version or format of nav
* -4 ,No data of nav
* -5 ,Mismatch version or format of obs
* -6 ,No data of obs
* 0 ,Soulution generated completely
* >0 ,Some epoch cant be calculate
*/
double brdm2pos(char nav_path[MAXPATH], char obs_path[MAXPATH], char res_path[MAXPATH],
int GNSS, int Hangle)
{
int satnum, epochunum, result = 0;
pos_ts pos_t = { 0 };
xyz2blh tem2 = { 0 };
stations station = { 0 };
time_t gen_time; time(&gen_time);
int satnum, epochnum, result = 0;
pos_ts pos_t = { 0 };
blh apblh = { 0 };
pnav_head nav_h = NULL;
pnav_body nav_b = NULL;
pobs_head obs_h = NULL;
pobs_body obs_b = NULL;
pobs_epoch obs_e = NULL;
pnav_head nav_h = NULL;
pnav_body nav_b = NULL;
pobs_head obs_h = NULL;
pobs_body obs_b = NULL;
pobs_epoch obs_e = NULL;
FILE* fp_nav = fopen(nav_path, "r");
if (fp_nav == NULL) return -1;//cant read nav file
FILE* fp_nav = fopen(nav_path, "r");
if (fp_nav == NULL) return -1;
satnum = getsatnum(fp_nav);
//Get the amount of satellite
satnum = getsatnum(fp_nav);
switch (satnum) {
case -1: return -3;//mismatch version or format
case 0: return -4;//no data
default:
break;
}
switch (satnum) {
case -1: return -3;//mismatch version or format
case 0: return -4;//no data
default: break;
}
rewind(fp_nav);
nav_h = (pnav_head)malloc(sizeof(nav_head));
nav_b = (pnav_body)malloc(sizeof(nav_body) * (satnum));
if (nav_h && nav_b) {
read_n_h(fp_nav, nav_h); read_n_b(fp_nav, nav_b);
}
fclose(fp_nav);
rewind(fp_nav);
nav_h = (pnav_head)malloc(sizeof(nav_head));
nav_b = (pnav_body)malloc(sizeof(nav_body) * (satnum));
FILE* fp_obs = fopen(obs_path, "r");
if (fp_obs == NULL) return -2;
epochunum = get_epochnum(fp_obs);
if (nav_h && nav_b) { read_n_h(fp_nav, nav_h); read_n_b(fp_nav, nav_b); }
fclose(fp_nav);
switch (epochunum) {
case -1: return -5;//mismatch version or format
case 0: return -6;//no data
default:
break;
}
FILE* fp_obs = fopen(obs_path, "r");
if (fp_obs == NULL) return -2;//cant read obs file
rewind(fp_obs);
obs_h = (pobs_head)malloc(sizeof(obs_head));
if (obs_h) {
read_o_h(fp_obs, obs_h);
//Get the amonut of obs epoch
epochnum = get_epochnum(fp_obs);
tem2 = XYZ2BLH(tem2, obs_h->apX, obs_h->apY, obs_h->apZ);
station.B = tem2.B; station.L = tem2.L; station.H = tem2.H;
switch (epochnum) {
case -1: return -5;//mismatch version or format
case 0: return -6;//no data
default: break;
}
rewind(fp_obs); obs_h = (pobs_head)malloc(sizeof(obs_head));
//Out put the header of solution file
FILE* result_file_clear = fopen(res_path, "w");
fclose(result_file_clear);
FILE* result_file = fopen(res_path, "a+");
fprintf(result_file, "@ GENERATE PROGRAM : KNZ_GeoTrackLab ver1.5.3.1\n");
fprintf(result_file, "@ GENERATE TYPE : Satellite Position\n");
fprintf(result_file, "@ GENERATE TIME : %s", ctime(&gen_time));
fprintf(result_file, "@ OBS FILE PATH : %s\n", obs_path);
fprintf(result_file, "@ NAV FILE PATH : %s\n", nav_path);
fprintf(result_file, "@ TIME OF FIRST OBS : %4d %02d %02d %02d %02d %07.4f\n"
, obs_h->f_y, obs_h->f_m, obs_h->f_d, obs_h->f_h, obs_h->f_min, obs_h->f_sec);
fprintf(result_file, "@ TIME OF LAST OBS : %4d %02d %02d %02d %02d %07.4f\n"
, obs_h->l_y, obs_h->l_m, obs_h->l_d, obs_h->l_h, obs_h->l_min, obs_h->l_sec);
fprintf(result_file, "@ APPROX POSITION XYZ: %13.04f%13.04f%13.04f\n@ APPROX POSITION BLH: %12.07f %12.07f %12.07f\n"
, obs_h->apX, obs_h->apY, obs_h->apZ
, station.B, station.L, station.H);
fprintf(result_file, "END OF HEADER\n");
if (obs_h) {
//Begin to calculate
for (int i = 0; i < epochunum; i++) {
//Allocate memory for obs epoch and body data
obs_e = (pobs_epoch)malloc(sizeof(obs_epoch));
obs_b = (pobs_body)malloc(sizeof(obs_body));
//Read the header of obs
read_o_h(fp_obs, obs_h);
read_o_eb(fp_obs, obs_h, obs_e, obs_b);
result += sat_pos_cal(obs_h, obs_e, obs_b, nav_b,
result_file, station,
gnsscode, satnum);
//Convert the approximately position (ECEF > LLA)
apblh = xyz2blh(apblh, obs_h->apX, obs_h->apY, obs_h->apZ);
obs_h->apB = apblh.B;
obs_h->apL = apblh.L;
obs_h->apH = apblh.H;
fprintf(result_file, "\n");
freeobs_e(obs_e, obs_b);
}
free(obs_h); free(nav_h); free(nav_b);
fprintf(result_file, "\nEND"); fclose(result_file);
if (epochunum == result) return 0;
}
//Output header of solution
FILE* result_file = outputheader(obs_h, obs_path, nav_path, res_path);
//Begin to calculate
for (int i = 0; i < epochnum; i++) {
//Allocate memory for obs epoch and body data
obs_e = (pobs_epoch)malloc(sizeof(obs_epoch));
obs_b = (pobs_body)malloc(sizeof(obs_body));
//Read epoch data
read_o_eb(fp_obs, obs_h, obs_e, obs_b);
//Calculate the position of satellite
result += sat_pos_cal(obs_h, obs_e, obs_b, nav_b,
result_file,
GNSS, satnum, Hangle);
fprintf(result_file, "\n");
//Free epoch data
freeobs_e(obs_e, obs_b);
}
//Output the signal of ending
fprintf(result_file, "\nEND"); fclose(result_file);
//free nav & obs memory of data
free(obs_h); free(nav_h); free(nav_b);
if (epochnum == result) return 0;
else return epochnum - result;
}
}
src/solutionout.cpp
+103
View File
@@ -0,0 +1,103 @@
#include "brdm2pos.h"
using namespace std;
/*Output the header data of satllite position solution*/
FILE* outputheader(pobs_head obs_h,
char obs_path[MAXPATH], char nav_path[MAXPATH], char res_path[MAXPATH])
{
time_t gen_time;
time(&gen_time);
FILE* result_file_clear = fopen(res_path, "w");
fclose(result_file_clear);
FILE* result_file = fopen(res_path, "a+");
fprintf(result_file, "@ GENERATE PROGRAM : KNZ_GeoTrackLab ver1.5.4.1\n");
fprintf(result_file, "@ GENERATE TYPE : Satellite Position\n");
fprintf(result_file, "@ GENERATE TIME : %s", ctime(&gen_time));
fprintf(result_file, "@ OBS FILE PATH : %s\n", obs_path);
fprintf(result_file, "@ NAV FILE PATH : %s\n", nav_path);
fprintf(result_file, "@ TIME OF FIRST OBS : %4d %02d %02d %02d %02d %07.4f\n"
, obs_h->f_y, obs_h->f_m, obs_h->f_d, obs_h->f_h, obs_h->f_min, obs_h->f_sec);
fprintf(result_file, "@ TIME OF LAST OBS : %4d %02d %02d %02d %02d %07.4f\n"
, obs_h->l_y, obs_h->l_m, obs_h->l_d, obs_h->l_h, obs_h->l_min, obs_h->l_sec);
fprintf(result_file, "@ APPROX POSITION XYZ: %13.04f%13.04f%13.04f\n@ APPROX POSITION BLH: %12.07f %12.07f %12.07f\n"
, obs_h->apX, obs_h->apY, obs_h->apZ
, obs_h->apB, obs_h->apL, obs_h->apH);
fprintf(result_file, "END OF HEADER\n");
return result_file;
}
/*Ion correction by IF-IC*/
static double ionofree(pobs_body obs_b, pobs_head obs_h, int j, int GNSS)
{
double P1=0.0, P2=0.0, freq1=0.0, freq2=0.0;
//Match the code with system
switch (GNSS) {
case GPS:
P1 = obs_b->obs_gps[j][code2type(C1, obs_h->obstypenum_gps, obs_h->obscode_gps)];
P2 = obs_b->obs_gps[j][code2type(C2, obs_h->obstypenum_gps, obs_h->obscode_gps)];
freq1 = FREQ1; freq2 = FREQ2;
break;
case BDS:
P1 = obs_b->obs_bds[j][code2type(C2, obs_h->obstypenum_bds, obs_h->obscode_bds)];
P2 = obs_b->obs_bds[j][code2type(C7, obs_h->obstypenum_bds, obs_h->obscode_bds)];
freq1 = FREQ1_BDS; freq2 = FREQ2_BDS;
break;
case GAL:
P1 = obs_b->obs_gal[j][code2type(C1, obs_h->obstypenum_gal, obs_h->obscode_gal)];
P2 = obs_b->obs_gal[j][code2type(C5, obs_h->obstypenum_gal, obs_h->obscode_gal)];
freq1 = FREQ1; freq2 = FREQ6;
break;
case GLO:
P1 = obs_b->obs_glo[j][code2type(C1, obs_h->obstypenum_glo, obs_h->obscode_glo)];
P2 = obs_b->obs_glo[j][code2type(C2, obs_h->obstypenum_glo, obs_h->obscode_glo)];
freq1 = FREQ1_GLO; freq2 = FREQ2_GLO;
break;
}
//IF-IC
if (P1 != 0.0 && P2 != 0.0) {
P1 = (pow(freq1, 2) * P1) / (pow(freq1, 2) - pow(freq2, 2))
- (pow(freq2, 2) * P2) / (pow(freq1, 2) - pow(freq2, 2));
return P1;
}
if (P1 == 0.0 || P2 == 0.0) {
if (P1 == 0.0 && P2 != 0.0) return P2;
if (P1 != 0.0 && P2 == 0.0) return P1;
return 0.0;
}
}
/*Output the satellite solution data*/
void outputbodydata(pobs_head obs_h, pobs_body obs_b,
pos_ts pos_t, rah in_rah, FILE* res_file,
int sPRN, int j, int GNSS)
{
double P = ionofree(obs_b, obs_h, j, GNSS);
if (P == 0) return;
switch (GNSS) {
case GPS:
fprintf(res_file, "\nG %02d| %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
, sPRN, pos_t.X, pos_t.Y, pos_t.Z
, P, in_rah.H, pos_t.delta_clk);
break;
case BDS:
if (sPRN <= 5 || sPRN >= 59) break;
else;
fprintf(res_file, "\nC %02d| %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
, sPRN, pos_t.X, pos_t.Y, pos_t.Z
, P, in_rah.H, pos_t.delta_clk);
break;
case GAL:
fprintf(res_file, "\nE %02d| %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
, sPRN, pos_t.X, pos_t.Y, pos_t.Z
, P, in_rah.H, pos_t.delta_clk);
break;
case GLO:
fprintf(res_file, "\nR %02d| %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
, sPRN, pos_t.X, pos_t.Y, pos_t.Z
, P, in_rah.H, pos_t.delta_clk);
break;
}
}
src/timesys.cpp
+69 -51
View File
@@ -1,68 +1,86 @@
#include "brdm2pos.h"
using namespace std;
/* -------------------------------------------------------------------------- */
/// @brief convert UTC to GPS Second of week
/// @param y year
/// @param m month
/// @param d day
/// @param h hour
/// @param min minute
/// @param sec second
/// @return return GPS Second of week
/* -------------------------------------------------------------------------- */
/**
* Convert Time format to GPS second of week.
*
* \param y: Year of
* \param m: Month of
* \param d: Day of
* \param h: Hour of
* \param min: Minute of
* \param sec: Second of
* \return GPS: Second of week
*/
double Time2GPST(int y, int m, int d, double h, int min, double sec)
{
if (m > 2)
{
y = y;
m = m;
}
if (m > 2)
{
y = y;
m = m;
}
if (m <= 2)
{
y = y - 1;
m = m + 12;
}
if (m <= 2)
{
y = y - 1;
m = m + 12;
}
h = h + min / 60.0 + sec / 3600.0;
double JD = (int)(365.25 * y) + (int)(30.6001 * (m + 1)) + d + h / 24.0 + 1720981.5;
double MJD = JD - 2400000.5;
int gpsweek = (int)((MJD - 44244) / 7);
double secofweek = (MJD - 44244.0 - gpsweek * 7.0) * 86400.0;
return secofweek;
h = h + min / 60.0 + sec / 3600.0;
double JD = (int)(365.25 * y) + (int)(30.6001 * (m + 1)) + d + h / 24.0 + 1720981.5;
double MJD = JD - 2400000.5;
int gpsweek = (int)((MJD - 44244) / 7);
double secofweek = (MJD - 44244.0 - gpsweek * 7.0) * 86400.0;
return secofweek;
}
/**
* Convert UTC(SU) time to GPS second of week.
*
* \param JD_UTCSU: JD of UTC(SU) time
* \return GPS: Second of week
*/
double JDUTC2GPST(double JD_UTCSU)
{
double n;//Leap second
double n;//Leap second
if (2451179.5000000 < JD_UTCSU && JD_UTCSU < 2453736.5000000) { n = 32.0; }
else if (2453736.5000000 < JD_UTCSU && JD_UTCSU < 2454832.5000000) { n = 33.0; }
else if (2454832.5000000 < JD_UTCSU && JD_UTCSU < 2455927.5000000) { n = 34.0; }
else if (2455927.5000000 < JD_UTCSU && JD_UTCSU < 2457023.5000000) { n = 35.0; }
else if (2457023.5000000 < JD_UTCSU && JD_UTCSU < 2457754.5000000) { n = 36.0; }
else if (2457754.5000000 < JD_UTCSU) { n = 37.0; }
if (2451179.5000000 < JD_UTCSU && JD_UTCSU < 2453736.5000000) { n = 32.0; }
else if (2453736.5000000 < JD_UTCSU && JD_UTCSU < 2454832.5000000) { n = 33.0; }
else if (2454832.5000000 < JD_UTCSU && JD_UTCSU < 2455927.5000000) { n = 34.0; }
else if (2455927.5000000 < JD_UTCSU && JD_UTCSU < 2457023.5000000) { n = 35.0; }
else if (2457023.5000000 < JD_UTCSU && JD_UTCSU < 2457754.5000000) { n = 36.0; }
else if (2457754.5000000 < JD_UTCSU) { n = 37.0; }
JD_UTCSU += ((n * 1 - 19) / 3600.0) / 24.0;
double MJD = JD_UTCSU - 2400000.5;
int gpsweek = (int)((MJD - 44244) / 7);
double secofweek = (MJD - 44244.0 - gpsweek * 7.0) * 86400.0;
return secofweek;
JD_UTCSU += ((n * 1 - 19) / 3600.0) / 24.0;
double MJD = JD_UTCSU - 2400000.5;
int gpsweek = (int)((MJD - 44244) / 7);
double secofweek = (MJD - 44244.0 - gpsweek * 7.0) * 86400.0;
return secofweek;
}
/**
* Convert UTC time to JD format.
*
* \param y: Year of UTC time
* \param m: Month of UTC time
* \param d: Day of UTC time
* \param h: Hour of UTC time
* \param min: Minute of UTC time
* \param sec: Second of UTC time
* \return JD:
*/
double UTCTime2JD(int y, int m, int d, double h, int min, double sec)
{
if (m > 2)
{
y = y;
m = m;
}
if (m > 2)
{
y = y;
m = m;
}
if (m <= 2)
{
y = y - 1;
m = m + 12;
}
if (m <= 2)
{
y = y - 1;
m = m + 12;
}
h = h + min / 60.0 + sec / 3600.0;
double JD = (int)(365.25 * y) + (int)(30.6001 * (m + 1)) + d + h / 24.0 + 1720981.5;
return JD;
h = h + min / 60.0 + sec / 3600.0;
double JD = (int)(365.25 * y) + (int)(30.6001 * (m + 1)) + d + h / 24.0 + 1720981.5;
return JD;
}