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).

README.md

@@ -17,9 +17,6 @@ Support method of orientation
 * Pseudorange positioning
 * (OINP)
 
-Features ：
-* Realize 3D display of satellite orbit
-
 # MAIN INTRO
 ## Program intro
   ### About KNZ_GeoTrackLab
@@ -36,6 +33,13 @@ Features ：
   
 ## Project Update
 
+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).
+
 Update: 2024/10/31:
 1.  KNZ_GeoTrackLab: Fix some bug, Modify some UI logic.
 2.  KNZ_GeoTrackLab: Optimize code structure.

READMECN.md

@@ -10,16 +10,13 @@
 支持GNSS：
 * GPS：卫星位置解算&接收机定位
 * Galileo：卫星位置解算&接收机定位
-* GLONASS：卫星位置解算
+* GLONASS：卫星位置解算(OINP)
-* 北斗/Compass：卫星位置解算
+* 北斗/Compass：卫星位置解算(OINP)
 
 支持定位方式
 * 伪距定位
 * (OINP)
 
-特色功能：
-* 实现卫星轨道3D显示
-
 # 主要介绍
 ## 程序介绍
 ### 关于KNZ_GeoTrackLab
@@ -36,6 +33,13 @@
 
 ## 项目更新
 
+更新: 2024/10/31:
+1. KNZ_GeoTrackLab: 修复一些bug, 优化UI逻辑.
+2. KNZ_GeoTrackLab: 移除3D绘图功能。
+2. KNZ_GeoTrackLab: 合并功能并专一化.
+3. KNZ_GeoTrackLab: 新增: 
+                        卫星轨迹投影图（还未实现功能化）。
+
 更新: 2024/10/31:
 1. KNZ_GeoTrackLab: 修复一些bug, 优化UI逻辑.
 2. KNZ_GeoTrackLab: 优化代码结构.

app/Config_file.py

@@ -49,7 +49,7 @@ class Configinit:
         # -------------------------------------------------------------------------------
         ### Config for software info
         self.config.add_section('INFO')
-        self.config.set('INFO', 'Version','1.5.4.1')
+        self.config.set('INFO', 'Version','1.5.5.0')
         self.config.add_section('Options_Calc')
         ### Config for calculate param
         self.config.set('Options_Calc', 'Iono_model', '0')

app/Func_callback.py

@@ -8,7 +8,6 @@
 #
 #
 #-----------------------------------------------------------------#
-
 ### Std
 import threading
 
@@ -58,6 +57,10 @@ class CALLBACK:
         for i in self.outdir:
             if self.outdir.get(i):
                 self.outvar += (self.outdir.get(i),)
+        ### Const value
+        global blank, C_V
+        blank = ' '
+        C_V = 299792458
 
     @staticmethod
     def Move_center(hwnd, win_x, win_y):
@@ -158,7 +161,8 @@ class CALLBACK:
                 self.cfg_get.set('Options_Draw', 'U_Line', str(entry2.get()))
                 self.cfg.save_config()
 
-    def Dir_or_name_get(self, path, mode):
+    @staticmethod
+    def Dir_or_name_get(path, mode):
         # -------------------------------------------------------------------------------
         # >
         # Method:
@@ -259,8 +263,8 @@ class CALLBACK:
                              path + '/*.sp')
             self.cfg.save_config()
 
-    def AskOrNotCheck(self,
+    @staticmethod
-                      AskOrNotCheckVar,
+    def AskOrNotCheck(AskOrNotCheckVar,
                       AskDirectorySelectBox,
                       AskDirectorySelectButton):
         # -------------------------------------------------------------------------------
@@ -316,7 +320,7 @@ class CALLBACK:
 
         directory = self.Dir_or_name_get(directory, 1)
         syssign = self.system.get(int(self.cfg_get['Options_Calc']['sat_system']))
-        resname = syssign + '-' + self.Dir_or_name_get(obspath,0) + 'sp'
+        resname = syssign + '-' + self.Dir_or_name_get(obspath,0) + 'ksln'
         respath = (directory + resname)
 
         ExecuteState.config(text='Processing...... file : %s' %resname)
@@ -327,8 +331,10 @@ class CALLBACK:
             navpath.encode(),
             obspath.encode(),
             respath.encode(),
-            int(self.cfg_get['Options_Calc']['sat_system']),
+            int(self.cfg_get['Options_Calc']['Sat_system']),
-            int(self.cfg_get['Options_Calc']['elev_angle'])
+            int(self.cfg_get['Options_Calc']['Elev_angle']),
+            int(self.cfg_get['Options_Calc']['Iono_Model']),
+            int(self.cfg_get['Options_Calc']['Trop_Model'])
             ):
             case  0.0:
                 ExecuteState.config(text='Generated successfully ! file : %s' %resname)
@@ -357,3 +363,7 @@ class CALLBACK:
                                                              AskOrNotCheckVar,
                                                              DirectorySelectBoxVar))
         T.start()
+
+
+
+

app/Trocor.py

@@ -1,36 +0,0 @@
-##Tro correct-Hopfield
-import math
-def Hop_tro_cor(H, ma):
-    global e
-    P0 = 1013.25
-    e0 = 11.69
-    T0 = 288.15
-    p = P0 * pow((1.0 - 0.0068 / T0 / H) , 5)
-    T = 288.15 - 0.0068 * H
-    if H > 11000:
-        e = 0
-    if H <= 11000:
-        e = e0 * pow( (1.0 - 0.0068 * H / T0) , 4)
-    deltaSd = 1.552e-5 * p * (40136 + 148.72 * (T - 273.16) - H) / T
-    deltaSw = 1.552e-5 * 4810 * e * (11000 - H) / pow(T , 2)
-    #Dry
-    dtropd = deltaSd / math.sqrt( math.sin( math.radians( math.degrees( pow(math.radians(ma) , 2) ) + 6.25) ) )
-    #Wet
-    dtropw = deltaSw / math.sqrt( math.sin( math.radians( math.degrees( pow(math.radians(ma) , 2) ) + 6.25) ) )
-    dtrop = dtropw + dtropd
-    return dtrop
-
-#Tro corrct-Saastamoinen
-def Sas_tro_cor(B, H, ma):
-    global e
-    P0 = 1013.25 * pow( (1 - 2.2557 * 10e-5 * H), 5.2568)
-    e0 = 11.69
-    T0 = 15 - 6.5 * 10e-3 * H + 273.15
-    deltaSd = 2.2768e-3 * P0 / (1 - 2.66e-3 * math.cos(2 * B) - 2.8e-4 * H * 1e3)
-    deltaSw = 2.2768e-3 * (1255 / T0 + 0.05) * e0
-    # Dry
-    dtropd = deltaSd / math.sqrt( math.sin( math.radians( math.degrees( pow(math.radians(ma), 2) ) + 6.25) ) )
-    # Wet
-    dtropw = deltaSw / math.sqrt( math.sin( math.radians( math.degrees( pow(math.radians(ma), 2) ) + 6.25) ) )
-    dtrop = dtropw + dtropd
-    return dtrop

app/Ui_about.py

@@ -13,9 +13,10 @@
 import tkinter as tk
 
 class ABODUI:
-    def __init__(self, hwndparent, cfg_get):
+    def __init__(self, hwndparent, cfg_get, func):
         # init
         # -------------------------------------------------------------------------------
+        self.func = func
         self.cfg_get = cfg_get
         self.hwndparent = hwndparent
 

app/Ui_main.py

@@ -18,6 +18,8 @@ import Config_file as config
 import Func_callback as func
 
 ### Std
+import threading
+
 from tkinter import ttk
 from PIL import ImageTk
 from base64 import b64decode
@@ -39,9 +41,27 @@ def instancePRO(hwndparent, cfg_get_, func_):
 def instanceVIE(hwndparent, func_):
     vie.VIEWGUI(hwndparent, func_)
 
-def instanceABO(hwndparemt, cfg_get_):
+def instanceABO(hwndparemt, cfg_get_, func_):
-    abo.ABODUI(hwndparemt, cfg_get_)
+    abo.ABODUI(hwndparemt, cfg_get_, func_)
 
+def _instanceOPT(hwndparemt, cfg_get_, func_):
+    T = threading.Thread(target=lambda :instanceOPT(hwndparemt, cfg_get_, func_))
+    T.start()
+
+def _instancePRO(hwndparemt, cfg_get_, func_):
+    T = threading.Thread(target=lambda :instancePRO(hwndparemt, cfg_get_, func_))
+    T.start()
+
+def _instanceVIE(hwndparemt, func_):
+    T = threading.Thread(target=lambda :instanceVIE(hwndparemt, func_))
+    T.start()
+
+def _instanceABO(hwndparemt, cfg_get_, func_):
+    T = threading.Thread(target=lambda :instanceABO(hwndparemt, cfg_get_, func_))
+    T.start()
+
+
+# noinspection PyUnusedLocal,PyProtectedMember
 class MAINGUI:
     def  __init__(self, hwnd_, func_):
         # init
@@ -230,7 +250,7 @@ class MAINGUI:
         ### Help > About
         helpmenu.add_command(
             label="About",
-            command=lambda :instanceABO(self.hwnd, cfg_get)
+            command=lambda :instanceABO(self.hwnd, cfg_get, self.func)
         )
         topmenu.add_cascade(label="Help", menu=helpmenu)
 

app/Ui_options.py

@@ -16,6 +16,8 @@ from base64 import b64decode
 
 import tkinter as tk
 
+
+# noinspection PyProtectedMember
 class OPTGUI:
     def __init__(
             self,

app/Ui_processor.py

@@ -17,6 +17,7 @@ from base64 import b64decode
 import tkinter as tk
 
 
+# noinspection PyProtectedMember
 class EXECUDUI:
     def __init__(self, hwndparent, cfg_get, func):
         # init

app/Ui_view.py

@@ -16,6 +16,8 @@ from tkinter import filedialog
 
 import tkinter as tk
 
+
+# noinspection PyProtectedMember
 class VIEWGUI:
     def __init__(self, hwndparent, func):
         # init

src/brdm2pos.h

@@ -8,6 +8,9 @@
 #include <time.h>
 
 #endif // _BRDM2POS_H_
+
+#define KNZ_GTL_VER   "1.5.5.0"
+
 /* CALCU CONST*/
 #define C_V     299792458               /* Speed of light (m/s) */
 #define GM      398600500000000         /*  */
@@ -121,9 +124,6 @@ typedef struct stations{//
     double X;
     double Y;
     double Z;
-    double B;
-    double L;
-    double H;
     double delta_TR;
 }stations;
 
@@ -306,32 +306,86 @@ typedef struct obs_body{//Body data of file of obs
     double obs_glo[30][26];/* GLONASS obs data */
 }obs_body, * pobs_body;
 
+typedef struct lsq_part {
+    double* l;
+    double* m;
+    double* n;
+    double* o;
+    double* B;
+    double* P;
+    double* L;
+    double* x;
+}lsq_part;
+
+typedef struct acct_obs {
+    double Per1[30];
+    double Per2[30];
+    double sclk[30];
+    int freq1;
+    int freq2;
+
+    acct_obs() = default;
+}acct_obs;
+
 //Contain in timesys.cpp
-extern double JDUTC2GPST(double);
+extern double jdutc2gpst(double JD_UTCSU);
-extern double Time2GPST(int, int, int, double, int, double);
+extern double time2gpst(int y, int m, int d, double h, int min, double sec);
-extern double UTCTime2JD(int, int, int, double, int, double);
+extern double utctime2jd(int y, int m, int d, double h, int min, double sec);
+//Contain in martix.cpp
+extern double* mat(int n, int m);
+extern int* imat(int n, int m);
+extern double* zeros(int n, int m);
+extern double* eye(int n);
+extern double dot(const double* va, const double* vb, int n);
+extern double norm(const double* va, int n);
+extern void cross3(const double* va, const double* vb, double* vc);
+extern int normv3(const double* va, double* vb);
+extern void matcpy(double* A, const double* B, int n, int m);
+extern void matmul(const char* tr, int n, int k, int m, double alpha,
+    const double* A, const double* B, double beta, double* C);
+extern int matinv(double* A, int n);
+extern int solve(const char* tr, const double* A, const double* Y, int n,
+    int m, double* X);
+extern stations designmat(pobs_head obs_h, acct_obs obs,
+    double* l, double* m, double* n, double* o,
+    double* B, double* P, double* L,
+    double X, double Y, double Z, int satnum, int ionopt, int tropopt,
+    int GNSS);
+extern int lsq(const double* BtP, const double* L, const double* B, int n, int m, double* x,
+    double* Q);
+extern void matprint(const double A[], int n, int m, int p, int q);
 //Contain in coordtrans.cpp
-extern double  deg2rad(double);
+extern double deg2rad(double deg);
-extern double  rad2deg(double);
+extern double rad2deg(double rad);
-extern rah  rahcal(rah, double, double, double);
+extern rah rahcal(rah rah, double E, double N, double U);
-extern enu blh2enu(enu, double, double, double, double, double);
+extern enu blh2enu(enu enu, double stationB, double stationL,
-extern blh xyz2blh(blh, double, double, double);
+    double deltax, double deltay, double deltaz);
+extern blh xyz2blh(blh blh, double X, double Y, double Z);
+extern rah xyz2rah(pos_ts pos_t, pobs_head obs_h);
+extern void pz90towgs84(double X, double Y, double Z);
 //Contain in readrnx.cpp
-extern int  getsatnum(FILE*);
+extern int getsatnum(FILE*);
-extern int  get_epochnum(FILE*);
+extern int get_epochnum(FILE*);
 extern void freeobs_e(pobs_epoch, pobs_body);
-extern int  type2code(int, char buff[MAXRINEX]);
+extern int type2code(int, char buff[MAXRINEX]);
-extern int  code2type(int, int, int typearr[36]);
+extern int code2type(int, int, int typearr[36]);
-extern int  select_epoch(double, int, pnav_body, int, int);
+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);
 //Contain in solutionout.cpp
-extern FILE* outputheader(pobs_head, char obs_path[MAXPATH], char nav_path[MAXPATH], char res_path[MAXPATH]);
+extern FILE* headerout(pobs_head obs_h, 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);
+extern void solution(FILE* res_file, pobs_head obs_h, double X, double Y, double Z);
+extern void outdata(blh blh, FILE* res_file, int sPRN, int GNSS);
 //Entry function
-extern "C" _declspec(dllexport) double brdm2pos(char nav_path[MAXPATH], char obs_path[MAXPATH], char res_path[MAXPATH],
+extern "C" _declspec(dllexport) double brdm2pos(
-    int GNSS, int Hangle);
+    char nav_path[MAXPATH],
+    char obs_path[MAXPATH],
+    char res_path[MAXPATH],
+    int GNSS,
+    int Hangle,
+    int ionopt,
+    int tropopt);
 

src/coords.cpp

@@ -0,0 +1,134 @@
+#include "brdm2pos.h"
+using namespace std;
+/**
+ * Degree to radian.
+ * 
+ * \param deg: Degree
+ * \return Radian
+ */
+double deg2rad(double deg)
+{
+    double rad = (PI / 180) * deg;
+    return rad;
+}
+/**
+ * Radian to degree .
+ * 
+ * \param rad: Radian
+ * \return Degree
+ */
+double rad2deg(double rad)
+{
+    double deg = (180 / PI) * rad;
+    return deg;
+}
+/**
+ * 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)
+{
+    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 rah;
+}
+/**
+ * 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)
+{
+    double sinB = sin(stationB);
+    double cosB = cos(stationB);
+    double sinL = sin(stationL);
+    double cosL = cos(stationL);
+    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 enu;
+}
+/**
+ * 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));
+    blh.L = atan2(Y, X);
+    N = a;
+    H = R1 - a;
+    B = atan2(Z * (N + H), R0 * (N * (1 - e2) + H));
+    do{
+        deltaH = N;
+        deltaB = B;
+        N = a / sqrt(1 - e2 * pow(sin(B), 2));
+        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);
+    blh.B = B;
+    blh.H = H;
+
+    return blh;
+}
+rah xyz2rah(pos_ts pos_t, pobs_head obs_h)
+{
+    double
+        deltax,
+        deltay,
+        deltaz;
+    enu enu = { 0 };
+    rah rah = { 0 };
+    deltax = pos_t.X - obs_h->apX;
+    deltay = pos_t.Y - obs_h->apY;
+    deltaz = pos_t.Z - obs_h->apZ;
+    enu = blh2enu(enu, obs_h->apB, obs_h->apL,
+        deltax, deltay, deltaz);
+    //Satellite elevation Angle calculation
+    rah = rahcal(rah, enu.E, enu.N, enu.U);
+
+    return rah;
+}
+/**
+ * Convert from PZ-90 to WGS-84.
+ * 
+ * \param X: Coordinate in PZ-90
+ */
+void pz90towgs84(double X, double Y, double Z)
+{
+    while (X || Y || Z) {
+        X = -0.47 + (X * 1         + Y * 1.728e-6  + Z * -0.017e-6) * (1 + 22e-9);
+        Y = -0.51 + (X * 1.728e-6  + Y * 1         + Z * -0.076e-6) * (1 + 22e-9);
+        Z = -1.56 + (X * 0.0178e-6 + Y * -0.076e-6 + Z * 1) * (1 + 22e-9);//PZ90 Convert to WGS-84
+        break;
+    }return;
+}

src/coordtrans.cpp

@@ -1,102 +0,0 @@
-#include "brdm2pos.h"
-using namespace std;
-/**
- * Degree to radian.
- * 
- * \param deg: Degree
- * \return Radian
- */
-double deg2rad(double deg)
-{
-	double rad = (PI / 180) * deg;
-	return rad;
-}
-/**
- * Radian to degree .
- * 
- * \param rad: Radian
- * \return Degree
- */
-double rad2deg(double rad)
-{
-	double deg = (180 / PI) * rad;
-	return deg;
-}
-/**
- * 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)
-{
-	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 rah;
-}
-/**
- * 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)
-{
-	double sinB = sin(stationB);
-	double cosB = cos(stationB);
-	double sinL = sin(stationL);
-	double cosL = cos(stationL);
-	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 enu;
-}
-/**
- * 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));
-	blh.L = atan2(Y, X);
-	N = a;
-	H = R1 - a;
-	B = atan2(Z * (N + H), R0 * (N * (1 - e2) + H));
-	do{
-		deltaH = N;
-		deltaB = B;
-		N = a / sqrt(1 - e2 * pow(sin(B), 2));
-		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);
-	blh.B = B;
-	blh.H = H;
-
-	return blh;
-}

src/matrix.cpp

@@ -0,0 +1,231 @@
+#include "brdm2pos.h"
+/* new matrix ------------------------------------------------------------------
+* allocate memory of matrix
+* args   : int    n,m       I   number of rows and columns of matrix
+* return : matrix pointer (if n<=0 or m<=0, return NULL)
+*-----------------------------------------------------------------------------*/
+double* mat(int n, int m)
+{
+    double* p;
+
+    if (n <= 0 || m <= 0) return NULL;
+    if (!(p = (double*)malloc(sizeof(double) * n * m))) {
+        return NULL;
+    }
+    return p;
+}
+/* new integer matrix ----------------------------------------------------------
+* allocate memory of integer matrix
+* args   : int    n,m       I   number of rows and columns of matrix
+* return : matrix pointer (if n<=0 or m<=0, return NULL)
+*-----------------------------------------------------------------------------*/
+int* imat(int n, int m)
+{
+    int* p;
+
+    if (n <= 0 || m <= 0) return NULL;
+    if (!(p = (int*)malloc(sizeof(int) * n * m))) {
+        return NULL;
+    }
+    return p;
+}
+/* zero matrix -----------------------------------------------------------------
+* generate new zero matrix
+* args   : int    n,m       I   number of rows and columns of matrix
+* return : matrix pointer (if n<=0 or m<=0, return NULL)
+*-----------------------------------------------------------------------------*/
+double* zeros(int n, int m)
+{
+    double* p;
+
+#if NOCALLOC
+    if ((p = mat(n, m))) for (n = n * m - 1; n >= 0; n--) p[n] = 0.0;
+#else
+    if (n <= 0 || m <= 0) return NULL;
+    if (!(p = (double*)calloc(sizeof(double), n * m))) {
+        return NULL;
+    }
+#endif
+    return p;
+}
+/* identity matrix -------------------------------------------------------------
+* generate new identity matrix
+* args   : int    n         I   number of rows and columns of matrix
+* return : matrix pointer (if n<=0, return NULL)
+*-----------------------------------------------------------------------------*/
+double* eye(int n)
+{
+    double* p;
+    int i;
+
+    if ((p = zeros(n, n))) for (i = 0; i < n; i++) p[i + i * n] = 1.0;
+    return p;
+}
+/* inner product ---------------------------------------------------------------
+* inner product of vectors
+* args   : double *a,*b     I   vector a,b (n x 1)
+*          int    n         I   size of vector a,b
+* return : a'*b
+*-----------------------------------------------------------------------------*/
+double dot(const double* va, const double* vb, int n)
+{
+    double c = 0.0;
+
+    while (--n >= 0) c += va[n] * vb[n];
+    return c;
+}
+/* euclid norm -----------------------------------------------------------------
+* euclid norm of vector
+* args   : double *a        I   vector a (n x 1)
+*          int    n         I   size of vector a
+* return : || a ||
+*-----------------------------------------------------------------------------*/
+double norm(const double* va, int n)
+{
+    return sqrt(dot(va, va, n));
+}
+/* outer product of 3d vectors -------------------------------------------------
+* outer product of 3d vectors
+* args   : double *a,*b     I   vector a,b (3 x 1)
+*          double *c        O   outer product (a x b) (3 x 1)
+* return : none
+*-----------------------------------------------------------------------------*/
+void cross3(const double* va, const double* vb, double* vc)
+{
+    vc[0] = va[1] * vb[2] - va[2] * vb[1];
+    vc[1] = va[2] * vb[0] - va[0] * vb[2];
+    vc[2] = va[0] * vb[1] - va[1] * vb[0];
+}
+/* normalize 3d vector ---------------------------------------------------------
+* normalize 3d vector
+* args   : double *a        I   vector a (3 x 1)
+*          double *b        O   normlized vector (3 x 1) || b || = 1
+* return : status (1:ok,0:error)
+*-----------------------------------------------------------------------------*/
+int normv3(const double* va, double* vb)
+{
+    double r;
+    if ((r = norm(va, 3)) <= 0.0) return 0;
+    vb[0] = va[0] / r;
+    vb[1] = va[1] / r;
+    vb[2] = va[2] / r;
+    return 1;
+}
+/* copy matrix -----------------------------------------------------------------
+* copy matrix
+* args   : double *A        O   destination matrix A (n x m)
+*          double *B        I   source matrix B (n x m)
+*          int    n,m       I   number of rows and columns of matrix
+* return : none
+*-----------------------------------------------------------------------------*/
+void matcpy(double* A, const double* B, int n, int m)
+{
+    memcpy(A, B, sizeof(double) * n * m);
+}
+/*multiply matrix*/
+void matmul(const char* tr, int n, int k, int m, double alpha,
+    const double* A, const double* B, double beta, double* C)
+{
+    double d;
+    int i, j, x, f = tr[0] == 'N' ? (tr[1] == 'N' ? 1 : 2) : (tr[1] == 'N' ? 3 : 4);
+
+    for (i = 0; i < n; i++) for (j = 0; j < k; j++) {
+        d = 0.0;
+        switch (f) {
+        case 1: for (x = 0; x < m; x++) d += A[i + x * n] * B[x + j * m]; break;
+        case 2: for (x = 0; x < m; x++) d += A[i + x * n] * B[j + x * k]; break;
+        case 3: for (x = 0; x < m; x++) d += A[x + i * m] * B[x + j * m]; break;
+        case 4: for (x = 0; x < m; x++) d += A[x + i * m] * B[j + x * k]; break;
+        }
+        if (beta == 0.0) C[i + j * n] = alpha * d; else C[i + j * n] = alpha * d + beta * C[i + j * n];
+    }
+}
+/*LU decomposition*/
+static int ludcmp(double* A, int n, int* indx, double* d)
+{
+    double big, s, tmp, * vv = mat(n, 1);
+    int i, imax = 0, j, k;
+
+    *d = 1.0;
+    for (i = 0; i < n; i++) {
+        big = 0.0; for (j = 0; j < n; j++) if ((tmp = fabs(A[i + j * n])) > big) big = tmp;
+        if (big > 0.0) vv[i] = 1.0 / big; else { free(vv); return -1; }
+    }
+    for (j = 0; j < n; j++) {
+        for (i = 0; i < j; i++) {
+            s = A[i + j * n]; for (k = 0; k < i; k++) s -= A[i + k * n] * A[k + j * n]; A[i + j * n] = s;
+        }
+        big = 0.0;
+        for (i = j; i < n; i++) {
+            s = A[i + j * n]; for (k = 0; k < j; k++) s -= A[i + k * n] * A[k + j * n]; A[i + j * n] = s;
+            if ((tmp = vv[i] * fabs(s)) >= big) { big = tmp; imax = i; }
+        }
+        if (j != imax) {
+            for (k = 0; k < n; k++) {
+                tmp = A[imax + k * n]; A[imax + k * n] = A[j + k * n]; A[j + k * n] = tmp;
+            }
+            *d = -(*d); vv[imax] = vv[j];
+        }
+        indx[j] = imax;
+        if (A[j + j * n] == 0.0) { free(vv); return -1; }
+        if (j != n - 1) {
+            tmp = 1.0 / A[j + j * n]; for (i = j + 1; i < n; i++) A[i + j * n] *= tmp;
+        }
+    }
+    free(vv);
+    return 0;
+}
+/*LU back-substitution*/
+static void lubksb(const double* A, int n, const int* indx, double* b)
+{
+    double s;
+    int i, ii = -1, ip, j;
+
+    for (i = 0; i < n; i++) {
+        ip = indx[i]; s = b[ip]; b[ip] = b[i];
+        if (ii >= 0) for (j = ii; j < i; j++) s -= A[i + j * n] * b[j]; else if (s) ii = i;
+        b[i] = s;
+    }
+    for (i = n - 1; i >= 0; i--) {
+        s = b[i]; for (j = i + 1; j < n; j++) s -= A[i + j * n] * b[j]; b[i] = s / A[i + i * n];
+    }
+}
+/*inverse of matrix*/
+int matinv(double* A, int n)
+{
+    double d, * B;
+    int i, j, * indx;
+
+    indx = imat(n, 1); B = mat(n, n); matcpy(B, A, n, n);
+    if (ludcmp(B, n, indx, &d)) { free(indx); free(B); return -1; }
+    for (j = 0; j < n; j++) {
+        for (i = 0; i < n; i++) A[i + j * n] = 0.0;
+        A[j + j * n] = 1.0;
+        lubksb(B, n, indx, A + j * n);
+    }
+    free(indx); free(B);
+    return 0;
+}
+/*solve linear equation*/
+int solve(const char* tr, const double* A, const double* Y, int n,
+    int m, double* X)
+{
+    double* B = mat(n, n);
+    int info;
+
+    matcpy(B, A, n, n);
+    if (!(info = matinv(B, n))) matmul(tr[0] == 'N' ? "NN" : "TN", n, m, n, 1.0, B, Y, 0.0, X);
+    free(B);
+    return info;
+}
+void matprint(const double A[], int n, int m, int p, int q)
+{
+    int i, j;
+
+    for (i = 0; i < n; i++) {
+        for (j = 0; j < m; j++) printf(" %*.*f", p, q, A[i + j * n]);
+        printf("\n");
+    }
+}
+
+

src/readrnx.cpp

@@ -64,8 +64,7 @@ int select_epoch(double SecofWeek, int sPRN, pnav_body nav_b, int satnum, int sy
         for (int i = 0; i < satnum; i++){
             if (sPRN == nav_b[i].sPRN_GLO){
                 Min = fabs(SecofWeek - nav_b[i].TOE);
-                if (Min <= min)
+                if (Min <= min){
-                {
                     best_epoch = i;
                     min = Min;
                 }
@@ -235,53 +234,45 @@ void read_n_h(FILE* fp_nav, pnav_head nav_h)
         if (strstr(lable, "RINEX VERSION / TYPE")) {
             nav_h->ver = strtonum(buff, 0, 9);
             strncpy((nav_h->type), buff + 20, 15);
-            continue;
+            continue;}
-        }
         else if (strstr(lable, "GPSA")) {
             nav_h->ION_GPSA[0] = strtonum(buff, 6, 12);
             nav_h->ION_GPSA[1] = strtonum(buff, 6 + 12, 12);
             nav_h->ION_GPSA[2] = strtonum(buff, 6 + 12 + 12, 12);
             nav_h->ION_GPSA[3] = strtonum(buff, 6 + 12 + 12 + 12, 12);
-            continue;
+            continue;}
-        }
         else if (strstr(lable, "GPSB")) {
             nav_h->ION_GPSB[0] = strtonum(buff, 6, 12);
             nav_h->ION_GPSB[1] = strtonum(buff, 6 + 12, 12);
             nav_h->ION_GPSB[2] = strtonum(buff, 6 + 12 + 12, 12);
             nav_h->ION_GPSB[3] = strtonum(buff, 6 + 12 + 12 + 12, 12);
-            continue;
+            continue;}
-        }
         else if (strstr(lable, "BDSA")) {
             nav_h->ION_BDSA[0] = strtonum(buff, 6, 12);
             nav_h->ION_BDSA[1] = strtonum(buff, 6 + 12, 12);
             nav_h->ION_BDSA[2] = strtonum(buff, 6 + 12 + 12, 12);
             nav_h->ION_BDSA[3] = strtonum(buff, 6 + 12 + 12 + 12, 12);
-            continue;
+            continue;}
-        }
         else if (strstr(lable, "BDSB")) {
             nav_h->ION_BDSB[0] = strtonum(buff, 6, 12);
             nav_h->ION_BDSB[1] = strtonum(buff, 6 + 12, 12);
             nav_h->ION_BDSB[2] = strtonum(buff, 6 + 12 + 12, 12);
             nav_h->ION_BDSB[3] = strtonum(buff, 6 + 12 + 12 + 12, 12);
-            continue;
+            continue;}
-        }
         else if (strstr(lable, "BDUT")) {
             nav_h->BDUT[0] = strtonum(buff, 5, 17);
             nav_h->BDUT[1] = strtonum(buff, 5 + 17, 16);
             nav_h->BDUT[2] = strtonum(buff, 5 + 17 + 16, 7);
             nav_h->BDUT[3] = strtonum(buff, 5 + 17 + 16 + 7, 5);
-            continue;
+            continue;}
-        }
         else if (strstr(lable, "GPUT")) {
             nav_h->GPUT[0] = strtonum(buff, 5, 17);
             nav_h->GPUT[1] = strtonum(buff, 5 + 17, 16);
             nav_h->GPUT[2] = strtonum(buff, 5 + 17 + 16, 7);
             nav_h->GPUT[3] = strtonum(buff, 5 + 17 + 16 + 7, 5);
-            continue;
+            continue;}
-        }
         else if (strstr(lable, "LEAP SECONDS")) {
-            nav_h->leap = (int)strtonum(buff, 4, 2);
+            nav_h->leap = (int)strtonum(buff, 4, 2);}
-        }
         else if (strstr(lable, "END OF HEADER"))
             break;
     }
@@ -533,8 +524,8 @@ void read_n_b(FILE* fp_nav, pnav_body nav_b)
                     nav_b[i_g].sa1 = strtonum(buff, 23 + 19, 19);
                     nav_b[i_g].Dos = strtonum(buff, 23 + 19 + 19, 19);
 
-                    nav_b[i_g].TOE = JDUTC2GPST(
+                    nav_b[i_g].TOE = jdutc2gpst(
-                        UTCTime2JD(nav_b[i_g].TOC_Y, nav_b[i_g].TOC_M, nav_b[i_g].TOC_D,
+                        utctime2jd(nav_b[i_g].TOC_Y, nav_b[i_g].TOC_M, nav_b[i_g].TOC_D,
                             nav_b[i_g].TOC_H, nav_b[i_g].TOC_Min, nav_b[i_g].TOC_Sec)
                     );
                     fgets(buff, MAXRINEX, fp_nav);

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) {
+    for (tstep = tdist < 0.0 ? -TSTEP : TSTEP; fabs(tdist) > 1.0e-9; tdist -= tstep) {
-        if (fabs(tdist) < TSTEP) tstep = -tdist;
+        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);
+    pz90towgs84(X[0], X[1], X[2]);
-    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];
 
-    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,
+    FILE* solu,
-    int GNSS, int satnum, int Hangle)
+    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 };
+    y = obs_e->y;m = obs_e->m;d = obs_e->d;
-    enu in_enu = { 0 }; enu tem_enu = { 0 };
+    h = obs_e->h;min = obs_e->min;sec = obs_e->sec;
-    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;
+    Weeksec = time2gpst(y, m, d, h, min, sec);
-    double h = obs_e->h; int min = obs_e->min; double sec = obs_e->sec;
+    epochcount = round((Weeksec - beginsec) / obs_h->interval) + 1;
-
+    fprintf(solu, "\n>%05d  lat/r     lon/r           alt/m", epochcount);
-    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;
 
     //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
+        if (best_epoch == -1) continue;//when the sPRN is not exist function will return -1
-            break;
-
         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
+        //Satellite blh & rah position calculation
-        tem_blh = xyz2blh(tem_blh, pos_t.X, pos_t.Y, pos_t.Z);
+        in_rah = xyz2rah(pos_t, obs_h);
-        in_blh.B = rad2deg(tem_blh.B);
+        in_blh = xyz2blh(in_blh, pos_t.X, pos_t.Y, pos_t.Z);
-        in_blh.L = rad2deg(tem_blh.L);
-        in_blh.H = tem_blh.H;
         
-        double deltax = pos_t.X - obs_h->apX;
+        if (in_blh.H < 0 || rad2deg(in_rah.H) < Hangle)continue;
-        double deltay = pos_t.Y - obs_h->apY;
+        else {
-        double deltaz = pos_t.Z - obs_h->apZ;
+            //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;
 
-        tem_enu = blh2enu(tem_enu, obs_h->apB, obs_h->apL, deltax, deltay, deltaz);
+            switch (GNSS) {
-        in_enu.E = tem_enu.E;
+            case GPS:
-        in_enu.N = tem_enu.N;
+                obs.Per1[k] = obs_b->obs_gps[j][code2type(C1, obs_h->obstypenum_gps, obs_h->obscode_gps)];
-        in_enu.U = tem_enu.U;
+                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;}
             
-        //Satellite elevation Angle calculation
+            outdata(in_blh, solu, sPRN, GNSS); k++;
-        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);
+    //Solve the position
-        in_rah.H = rad2deg(tem_rah.H);
+    if (k >= 4) {
-        if (in_blh.H < 0 || in_rah.H < Hangle || nav_b[best_epoch].sHEA != 0) {
+        in_lsq.x = mat(4, 1);
-            continue;
+        in_lsq.B = mat(k, 4);
-        }
+        in_lsq.L = mat(k, 1);
-        else outputbodydata(obs_h, obs_b, pos_t, in_rah, res_file, sPRN, j, GNSS);
+        in_lsq.P = zeros(k, k);
-    }return 1;
+        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) {
+
+            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],
+double brdm2pos(
-    int GNSS, int Hangle)
+    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* solu = headerout(obs_h, obs_path, nav_path, res_path);
-        FILE* result_file = outputheader(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,
+                solu, GNSS, satnum, Hangle, ionopt,tropopt);
-                GNSS, satnum, Hangle);
 
-
+            fprintf(solu, "\n");
-            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);
+        fprintf(solu, "\nEND"); fclose(solu);
 
         //free nav & obs memory of data
         free(obs_h); free(nav_h); free(nav_b);

src/solutionout.cpp

@@ -1,103 +0,0 @@
-#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/solutions.cpp

@@ -0,0 +1,214 @@
+#include "brdm2pos.h"
+using namespace std;
+/*Tropshere correction with saastamoinen model*/
+static double trosaastamoinen(pobs_head obs_h, double elev)
+{
+    double P0, e0, T0, deltaSd, deltaSw, dtropd, dtropw, dtrop;
+    double B = obs_h->apB, L = obs_h->apL, H = obs_h->apH;
+
+    P0 = 1013.25 * pow((1 - 2.2557 * 10e-5 * H), 5.2568);
+    e0 = 11.69;
+    T0 = 15 - 6.5 * 10e-3 * H + 273.15;
+    deltaSd = 2.2768e-3 * P0 / (1 - 2.66e-3 * cos(2 * B) - 2.8e-4 * H * 1e3);
+    deltaSw = 2.2768e-3 * (1255 / T0 + 0.05) * e0;
+    // Dry
+    dtropd = deltaSd / sqrt(
+                            sin(
+                                deg2rad(
+                                        rad2deg(
+                                                pow(elev,2) 
+                                               ) + 6.25
+                                       )
+                               )
+                           );
+    // Wet
+    dtropw = deltaSw / sqrt(
+                            sin(
+                                deg2rad(
+                                        rad2deg(
+                                                pow(elev,2)
+                                               ) + 6.25
+                                       )
+                               )
+                           );
+    dtrop = dtropw + dtropd;
+    return dtrop;
+}
+/*Tropshere correction with hopfiled model*/
+static double trohopfiled(pobs_head obs_h, double elev)
+{
+    double P0, e0, T0, deltaSd, deltaSw, dtropd, dtropw, dtrop;
+    double B = obs_h->apB, L = obs_h->apL, H = obs_h->apH;
+
+    P0 = 1013.25 * pow((1 - 2.2557 * 10e-5 * H), 5.2568);
+    e0 = 11.69;
+    T0 = 15 - 6.5 * 10e-3 * H + 273.15;
+    deltaSd = 1.552e-5 * P0 * (40136 + 148.72 * (T0 - 273.16) - H) / T0;
+        deltaSw = 1.552e-5 * 4810 * e0 * (11000 - H) / pow(T0, 2);
+    // Dry delay component
+    dtropd = deltaSd / sqrt(
+        sin(
+            deg2rad(
+                rad2deg(
+                    pow(deg2rad(elev), 2)
+                ) + 6.25
+            )
+        )
+    );
+    // Wet delay component
+    dtropw = deltaSw / sqrt(
+        sin(
+            deg2rad(
+                rad2deg(
+                    pow(deg2rad(elev), 2)
+                ) + 6.25
+            )
+        )
+    );
+    dtrop = dtropw + dtropd;
+    return dtrop;
+}
+/*Iono correction by IF-IC*/
+static double ionofree(acct_obs obs, pobs_head obs_h, int k, int GNSS)
+{
+    double P1 = obs.Per1[k], P2 = obs.Per2[k],
+           freq1 = obs.freq1, freq2 = obs.freq2;
+    //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;
+    }
+}
+/*Rest correction of observation*/
+static double restcorr(pobs_head obs_h, acct_obs obs, int ionopt, int tropopt,
+    int k, int GNSS, double elev) {
+
+    double P = 0.0, diono = 0.0, dtrop = 0.0;
+
+    switch (ionopt) {
+    case 0:P = obs.Per1[k]; break;
+    case 1:P = ionofree(obs, obs_h, k, GNSS); break;
+    case 2:break;
+    }
+    switch (tropopt) {
+    case 0:break;
+    case 1:dtrop = trohopfiled(obs_h, elev); break;
+    case 2:dtrop = trosaastamoinen(obs_h, elev); break;
+    }
+    P = P - diono - dtrop;
+    return P;
+}
+/*(Weighted)Least square estimate*/
+int lsq(const double* BtP, const double* L, const double* B, int n, int m, double* x,
+    double* Q)
+{
+    double* BtPL;
+    int info;
+
+    if (m < n) return -1;
+    BtPL = mat(n, 1);
+
+    matmul("NN", n, 1, m, 1.0, BtP, L, 0.0, BtPL); /* =B'*P*L */
+    matmul("NN", n, n, m, 1.0, BtP, B, 0.0, Q);  /* =B'*P*B */
+    if (!(info = matinv(Q, n)))
+    matmul("NN", n, 1, n, 1.0, Q, BtPL, 0.0, x); /* x=Q^-1*(B'*P*L) */
+    free(BtPL);
+    return info;
+}
+/*Construct into desgin matrix and solve the lsq*/
+stations designmat(pobs_head obs_h, acct_obs obs,
+    double *l, double* m, double* n, double* o,
+    double* B, double* P, double* L,
+    double X, double Y, double Z,
+    int satnum, int ionopt, int tropopt, int GNSS)
+{
+    stations sta;
+    double Q[16], dx[4], *BP, *PL;
+    BP = mat(satnum, 4);
+    PL = mat(satnum, 1);
+
+    double R;
+    for (int i = 0; i < satnum; i++) {
+        R = sqrt(
+            pow(l[i] - X, 2) +
+            pow(m[i] - Y, 2) +
+            pow(n[i] - Z, 2));
+        B[i + satnum * 0] = -(l[i] - X) / R;
+        B[i + satnum * 1] = -(m[i] - Y) / R;
+        B[i + satnum * 2] = -(n[i] - Z) / R;
+        B[i + satnum * 3] = 1;
+        P[i * (satnum) + i] = pow(sin(o[i]),2);
+        L[i] = restcorr(obs_h, obs, ionopt, tropopt, i, GNSS, o[i]) -
+            R + obs.sclk[i] * C_V;
+    }
+    matmul("TN", 4, satnum, satnum, 1.0, B, P, 0.0, BP);//B'*P
+    if (!lsq(BP, L, B, 4, satnum, dx, Q))
+        sta.X = dx[0]; sta.Y = dx[1]; sta.Z = dx[2]; sta.delta_TR = dx[3];
+
+    free(BP); free(PL);
+
+    return sta;
+}
+/*Output the header data of satllite position solution*/
+FILE* headerout(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 ver%s\n", KNZ_GTL_VER);
+    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, " 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, " 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, " INTERVAL           : %5.02f\n",obs_h->interval);
+    fprintf(result_file, "<END OF HEADER\n");
+
+    return result_file;
+}
+/*Output the satellite solution data*/
+void outdata(blh blh, FILE* res_file, int sPRN, int GNSS)
+{
+    switch (GNSS) {
+    case GPS:
+        fprintf(res_file, "\nG%02d %9.06f %9.06f %15.05f"
+            , sPRN, blh.B, blh.L, blh.H);break;
+    case BDS:
+        fprintf(res_file, "\nC%02d %9.06f %9.06f %15.05f"
+            , sPRN, blh.B, blh.L, blh.H);break;
+    case GAL:
+        fprintf(res_file, "\nE%02d %9.06f %9.06f %15.05f"
+            , sPRN, blh.B, blh.L, blh.H);break;
+    case GLO:
+        fprintf(res_file, "\nR%02d %9.06f %9.06f %15.05f"
+            , sPRN, blh.B, blh.L, blh.H);break;
+    }
+}
+/*Output the satellite solution data*/
+void solution(FILE* res_file,pobs_head obs_h, double X, double Y, double Z)
+{
+    double deltax, deltay, deltaz;
+    deltax = X - obs_h->apX;
+    deltay = Y - obs_h->apY;
+    deltaz = Z - obs_h->apZ;
+    fprintf(res_file, "\nRec:%9.4f %9.4f %9.4f",
+    deltax, deltay, deltaz);
+}

src/timesys.cpp

@@ -11,7 +11,7 @@ using namespace std;
  * \param sec: Second of 
  * \return GPS: Second of week
  */
-double Time2GPST(int y, int m, int d, double h, int min, double sec)
+double time2gpst(int y, int m, int d, double h, int min, double sec)
 {
     if (m > 2)
     {
@@ -38,7 +38,7 @@ double Time2GPST(int y, int m, int d, double h, int min, double sec)
  * \param JD_UTCSU: JD of UTC(SU) time
  * \return GPS: Second of week
  */
-double JDUTC2GPST(double JD_UTCSU)
+double jdutc2gpst(double JD_UTCSU)
 {
     double n;//Leap second
 
@@ -66,7 +66,7 @@ double JDUTC2GPST(double JD_UTCSU)
  * \param sec: Second of UTC time
  * \return JD:
  */
-double UTCTime2JD(int y, int m, int d, double h, int min, double sec)
+double utctime2jd(int y, int m, int d, double h, int min, double sec)
 {
     if (m > 2)
     {