Update: 2024/11/03:

1.  KNZ_GeoTrackLab: Fix some bug, Modify some UI logic.
2.  KNZ_GeoTrackLab: Remove 3D plot function.
3.  KNZ_GeoTrackLab: Merge some functions and concentrated.
4.  KNZ_GeoTrackLab: New added:
                                Sat track projection ui(not ok on function).

src/rnx2pos.cpp

@@ -42,7 +42,7 @@ pos_ts glo_pos(int best_epoch, double Weeksec,
 {
     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;
+    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;
@@ -56,22 +56,22 @@ pos_ts glo_pos(int best_epoch, double Weeksec,
     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;
+    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
+    pz90towgs84(X[0], X[1], 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));
+    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;
+    pos_t.delta_t = R / C_V - pos_t.delta_clk;
 
     return pos_t;
 }
@@ -103,11 +103,9 @@ pos_ts sat_pos(int sPRN, int best_epoch, double	Weeksec, int GNSS,
     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 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 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;
@@ -129,7 +127,6 @@ pos_ts sat_pos(int sPRN, int best_epoch, double	Weeksec, int GNSS,
         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;
@@ -147,13 +144,18 @@ pos_ts sat_pos(int sPRN, int best_epoch, double	Weeksec, int GNSS,
     else if (GNSS == BDS) TGD = nav_b[best_epoch].TGD1;
 
     //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));
+    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.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;
+    pos_t.delta_t = R / C_V - pos_t.delta_clk;
 
     return pos_t;
 }
@@ -171,29 +173,31 @@ pos_ts sat_pos(int sPRN, int best_epoch, double	Weeksec, int GNSS,
  */
 int sat_pos_cal(pobs_head obs_h, pobs_epoch obs_e, pobs_body obs_b,
     pnav_body nav_b,
-    FILE* res_file,
-    int GNSS, int satnum, int Hangle)
+    FILE* solu,
+    int GNSS, int satnum, int Hangle, int ionopt, int troopt)
 {
+    int y, m, d, min, epochcount, sat_num = 0, sPRN = 0;
+    double h, sec, detat_toc, beginsec, endinsec, Weeksec, PERS = 0;
+    double apX, apY, apZ, ap_X, ap_Y, ap_Z;
+
+    stations sta = { 0 };rah in_rah = { 0 };
+    enu in_enu = { 0 };blh in_blh = { 0 };
+    lsq_part in_lsq = { 0 };pos_ts pos_t = { 0 };
+    acct_obs obs = { 0 };
+
     //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);
-
+    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);
+    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);
 
-    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;
+    y = obs_e->y;m = obs_e->m;d = obs_e->d;
+    h = obs_e->h;min = obs_e->min;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, PERS = 0;
-    double detat_toc;
+    Weeksec = time2gpst(y, m, d, h, min, sec);
+    epochcount = round((Weeksec - beginsec) / obs_h->interval) + 1;
+    fprintf(solu, "\n>%05d  lat/r     lon/r           alt/m", epochcount);
 
     //Match the system and assign the satellite number
     if (GNSS == GPS)sat_num = obs_e->gps_num;
@@ -201,39 +205,39 @@ int sat_pos_cal(pobs_head obs_h, pobs_epoch obs_e, pobs_body obs_b,
     else if (GNSS == BDS)sat_num = obs_e->bds_num;
     else if (GNSS == GLO)sat_num = obs_e->glo_num;
 
-    for (int j = 0; j < sat_num; j++) {
+    in_lsq.l = mat(sat_num, 1);
+    in_lsq.m = mat(sat_num, 1);
+    in_lsq.n = mat(sat_num, 1);
+    in_lsq.o = mat(sat_num, 1); int k = 0;
 
+    for (int j = 0; j < sat_num; j++) {
         //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;
-        }
+            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;
-        }
+            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;
-        }
+            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;
-        }
+            PERS = obs_b->obs_glo[j][
+                code2type(C1, obs_h->obstypenum_glo, obs_h->obscode_glo)] / C_V;}
 
         //Match the nearest satellite of nav with this epoch
         int best_epoch = select_epoch(Weeksec, sPRN, nav_b, satnum, GNSS);
 
-        if (best_epoch == -1)//when the sPRN is not exist function will return -1
-            break;
-
+        if (best_epoch == -1) continue;//when the sPRN is not exist function will 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 +
@@ -247,7 +251,6 @@ int sat_pos_cal(pobs_head obs_h, pobs_epoch obs_e, pobs_body obs_b,
         }
         //Exclude GLONASS
         else {
-
             //Get the approximately transit of signal
             pos_t.delta_t = PERS - 0 +
                 nav_b[best_epoch].sa0 +
@@ -260,31 +263,72 @@ int sat_pos_cal(pobs_head obs_h, pobs_epoch obs_e, pobs_body obs_b,
                 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;
+        //Satellite blh & rah position calculation
+        in_rah = xyz2rah(pos_t, obs_h);
+        in_blh = xyz2blh(in_blh, pos_t.X, pos_t.Y, pos_t.Z);
+        
+        if (in_blh.H < 0 || rad2deg(in_rah.H) < Hangle)continue;
+        else {
+            //Store the accept observation data
+            in_lsq.l[k] = pos_t.X, in_lsq.m[k] = pos_t.Y;
+            in_lsq.n[k] = pos_t.Z, in_lsq.o[k] = deg2rad(in_rah.H);
+            obs.sclk[k] = pos_t.delta_clk;
 
-        double deltax = pos_t.X - obs_h->apX;
-        double deltay = pos_t.Y - obs_h->apY;
-        double deltaz = pos_t.Z - obs_h->apZ;
+            switch (GNSS) {
+            case GPS:
+                obs.Per1[k] = obs_b->obs_gps[j][code2type(C1, obs_h->obstypenum_gps, obs_h->obscode_gps)];
+                obs.Per2[k] = obs_b->obs_gps[j][code2type(C2, obs_h->obstypenum_gps, obs_h->obscode_gps)];
+                obs.freq1 = FREQ1; obs.freq2 = FREQ2; break;
+            case BDS:
+                obs.Per1[k] = obs_b->obs_bds[j][code2type(C2, obs_h->obstypenum_bds, obs_h->obscode_bds)];
+                obs.Per2[k] = obs_b->obs_bds[j][code2type(C7, obs_h->obstypenum_bds, obs_h->obscode_bds)];
+                obs.freq1 = FREQ1_BDS; obs.freq2 = FREQ2_BDS;
+                break;
+            case GAL:
+                obs.Per1[k] = obs_b->obs_gal[j][code2type(C1, obs_h->obstypenum_gal, obs_h->obscode_gal)];
+                obs.Per2[k] = obs_b->obs_gal[j][code2type(C2, obs_h->obstypenum_gal, obs_h->obscode_gal)];
+                obs.freq1 = FREQ1; obs.freq2 = FREQ6;
+                break;
+            case GLO:
+                obs.Per1[k] = obs_b->obs_glo[j][code2type(C1, obs_h->obstypenum_glo, obs_h->obscode_glo)];
+                obs.Per2[k] = obs_b->obs_glo[j][code2type(C2, obs_h->obstypenum_glo, obs_h->obscode_glo)];
+                obs.freq1 = FREQ1_GLO; obs.freq2 = FREQ2_GLO;
+                break;}
+            
+            outdata(in_blh, solu, sPRN, GNSS); k++;
+        };
+    }
+    //Solve the position
+    if (k >= 4) {
+        in_lsq.x = mat(4, 1);
+        in_lsq.B = mat(k, 4);
+        in_lsq.L = mat(k, 1);
+        in_lsq.P = zeros(k, k);
+        apX = 0; ap_X = obs_h->apX;
+        apY = 0; ap_Y = obs_h->apY;
+        apZ = 0; ap_Z = obs_h->apZ;
+        int stop = 0;
+        while (fabs(ap_X - apX) > 1e-6) {
 
-        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;
+            if (stop == 3) break;
+            apX = ap_X;
+            apY = ap_Y;
+            apZ = ap_Z;
+            sta = designmat(obs_h, obs,
+                in_lsq.l, in_lsq.m, in_lsq.n, in_lsq.o,
+                in_lsq.B, in_lsq.P, in_lsq.L,
+                ap_X, ap_Y, ap_Z, k,
+                ionopt, troopt, GNSS);
+            ap_X += sta.X;
+            ap_Y += sta.Y;
+            ap_Z += sta.Z;
+            stop++;
+        }solution(solu, obs_h, ap_X, ap_Y, ap_Z);
+    }
+    else;
+    free(in_lsq.l); free(in_lsq.m); free(in_lsq.n); free(in_lsq.o);
+    free(in_lsq.B); free(in_lsq.P); free(in_lsq.L);
+    return 1;
 }
 /**
  * The entrance of function.
@@ -303,8 +347,15 @@ int sat_pos_cal(pobs_head obs_h, pobs_epoch obs_e, pobs_body obs_b,
  *			 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)
+double brdm2pos(
+    char nav_path[MAXPATH],
+    char obs_path[MAXPATH],
+    char res_path[MAXPATH],
+    int GNSS,
+    int Hangle,
+    int ionopt,
+    int tropopt
+)
 {
     int satnum, epochnum, result = 0;
     pos_ts pos_t = { 0 };
@@ -354,14 +405,13 @@ double brdm2pos(char nav_path[MAXPATH], char obs_path[MAXPATH], char res_path[MA
         //Read the header of obs
         read_o_h(fp_obs, obs_h);
 
-        //Convert the approximately position (ECEF > LLA)
+        //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;
 
-        //Output header of solution
-        FILE* result_file = outputheader(obs_h, obs_path, nav_path, res_path);
+        FILE* solu = headerout(obs_h, obs_path, nav_path, res_path);
 
         //Begin to calculate
         for (int i = 0; i < epochnum; i++) {
@@ -375,17 +425,15 @@ double brdm2pos(char nav_path[MAXPATH], char obs_path[MAXPATH], char res_path[MA
 
             //Calculate the position of satellite
             result += sat_pos_cal(obs_h, obs_e, obs_b, nav_b,
-                result_file,
-                GNSS, satnum, Hangle);
+                solu, GNSS, satnum, Hangle, ionopt,tropopt);
 
-
-            fprintf(result_file, "\n");
+            fprintf(solu, "\n");
 
             //Free epoch data
             freeobs_e(obs_e, obs_b);
         }
         //Output the signal of ending
-        fprintf(result_file, "\nEND"); fclose(result_file);
+        fprintf(solu, "\nEND"); fclose(solu);
 
         //free nav & obs memory of data
         free(obs_h); free(nav_h); free(nav_b);