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

Update: 2024/11/03:

Browse Source 
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).
This commit is contained in:
KenanZhu111 committed 2024-11-03 22:38:54 +08:00
1 parent d4db845cb3
commit a3fb64a657
20 files changed
+860 -1986
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

README.md
+7 -3
View File
@@ -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
+9 -5
View File
@@ -10,16 +10,13 @@
支持GNSS：
* GPS：卫星位置解算&接收机定位
* Galileo：卫星位置解算&接收机定位
* GLONASS：卫星位置解算
* 北斗/Compass：卫星位置解算
* GLONASS：卫星位置解算(OINP)
* 北斗/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
+1 -1
View File
@@ -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
+18 -8
View File
@@ -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,
AskOrNotCheckVar,
@staticmethod
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']['elev_angle'])
int(self.cfg_get['Options_Calc']['Sat_system']),
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)
@@ -356,4 +362,8 @@ class CALLBACK:
NavSelectBoxVar,
AskOrNotCheckVar,
DirectorySelectBoxVar))
T.start()
T.start()
app/Mainui.py
-1601
View File
File diff suppressed because it is too large. Load diff
app/Trocor.py
-36
View File
@@ -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
+2 -1
View File
@@ -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
+23 -3
View File
@@ -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_):
abo.ABODUI(hwndparemt, cfg_get_)
def instanceABO(hwndparemt, cfg_get_, func_):
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
+2
View File
@@ -16,6 +16,8 @@ from base64 import b64decode
import tkinter as tk
# noinspection PyProtectedMember
class OPTGUI:
def __init__(
self,
app/Ui_processor.py
+1
View File
@@ -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
+2
View File
@@ -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
+74 -20
View File
@@ -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 Time2GPST(int, int, int, double, int, double);
extern double UTCTime2JD(int, int, int, double, int, double);
extern double jdutc2gpst(double JD_UTCSU);
extern double time2gpst(int y, int m, int d, double h, int min, double sec);
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 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);
extern double deg2rad(double deg);
extern double rad2deg(double rad);
extern rah rahcal(rah rah, double E, double N, double U);
extern enu blh2enu(enu enu, double stationB, double stationL,
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 get_epochnum(FILE*);
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);
//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);
extern FILE* headerout(pobs_head obs_h, char obs_path[MAXPATH], char nav_path[MAXPATH], char res_path[MAXPATH]);
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],
int GNSS, int Hangle);
extern "C" _declspec(dllexport) double brdm2pos(
char nav_path[MAXPATH],
char obs_path[MAXPATH],
char res_path[MAXPATH],
int GNSS,
int Hangle,
int ionopt,
int tropopt);
src/coords.cpp
+134
View File
@@ -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
-102
View File
@@ -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
+231
View File
@@ -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
+11 -20
View File
@@ -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(
UTCTime2JD(nav_b[i_g].TOC_Y, nav_b[i_g].TOC_M, nav_b[i_g].TOC_D,
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,
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
+128 -80
View File
@@ -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);
src/solutionout.cpp
-103
View File
@@ -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
+214
View File
@@ -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
+3 -3
View File
@@ -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)
{