Skip to content

Commit 41deb90

Browse files
YibinWuYibinWu
committed
first commit
0 parents  commit 41deb90

25 files changed

+2822
-0
lines changed

‎README.md

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
# Pseudorange Differential GPS
2+
3+
An C version implementation of the real-time pseudorange differential GPS algorithm based on the iterative least square. The navigation data, including observations, ephemeris, and etc., are obtained from the OEMSTAR board by serial communication. The differential correction information is also obatined from another OEMSTAR board by serial communication. Refering to the reference *OEMSTAR-Reference Manual* for the data formation. Please feel free to contact me if you want a version with detailed Chinese comments or have any questions. [email protected]
4+
5+
一个基于迭代最小二乘的实时伪距差分GPS定位的C语言版本的实现。导航数据，包括观测值，星历等均通过串口通信从NoVatel OEMSTAR板卡获取。差分改正信息同样通过串口从另一个板卡中获取。参考reference文件夹中的 OEMSTAR-Reference Manual 文件获取数据格式的相关信息。参考其他文件获得代码实现的相关信息。如果你想要一个有详细中文注释的版本，或者有任何问题，欢迎联系我。

‎reference/GPS应用程序设计.pdf

4.02 MB
Binary file not shown.

‎reference/OEMSTAR-Reference Manual.pdf

2.75 MB
Binary file not shown.

‎reference/卫星导航定位_常用参数和公式.pdf

653 KB
Binary file not shown.

‎src/CoorTran.cpp

Lines changed: 120 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,120 @@
1+
/****************************************************************************
2+
Coordinates Trasformation
3+
****************************************************************************/
4+
#include "stdafx.h"
5+
#include "stdio.h"
6+
#include "math.h"
7+
#include "RTDConst.h"
8+
#include "Matrix.h"
9+
10+
bool BLH2XYZ(double BLH[], double XYZ[])
11+
{
12+
double a, b, e, B, L, H, N;
13+
a = 6378137.0;
14+
b = 6356752.3;
15+
e = (sqrt(a*a - b*b)) / a;
16+
17+
B = BLH[0] * Rad;
18+
L = BLH[1] * Rad;
19+
H = BLH[2];
20+
if (B<-pi || B>pi || L<-pi || L>pi || H < 0)
21+
{
22+
printf("error:The radian of latitude or longitude or height");
23+
return 0;
24+
}
25+
else
26+
{
27+
N = a / (sqrt(1 - e*e*sin(B)*sin(B)));
28+
XYZ[0] = (N + H)*cos(B)*cos(L);
29+
XYZ[1] = (N + H)*cos(B)*sin(L);
30+
XYZ[2] = ((1 - e*e)*N + H)*sin(B);
31+
return 1;
32+
}
33+
}
34+
35+
bool XYZ2BLH(double XYZ[], double BLH[])
36+
{
37+
/*if (XYZ[0] == 0 && XYZ[1] == 0 && XYZ[2] == 0)
38+
{
39+
BLH[0] = 30.0; BLH[1] = 114.0; BLH[2] = 200.0;
40+
return true;
41+
}*/
42+
double dz0, dz, a, b, e, sinB, N;
43+
a = 6378137.0;
44+
b = 6356752.3;
45+
e = (sqrt(a*a - b*b)) / a;
46+
dz0 = 0;
47+
dz = e*e*XYZ[2];
48+
while (1)
49+
{
50+
dz0 = dz;
51+
BLH[0] = atan((XYZ[2] + dz0) / sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]));
52+
sinB = (XYZ[2] + dz0) / sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1] + (XYZ[2] + dz0)*(XYZ[2] + dz0));
53+
N = a / sqrt(1 - e*e*sinB*sinB);
54+
dz = N*e*e*sinB;
55+
if (dz0 - dz < 0.0000001)
56+
{
57+
break;
58+
}
59+
}
60+
BLH[0] = atan2((XYZ[2] + dz), sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]))*Deg;
61+
BLH[1] = atan2(XYZ[1], XYZ[0])*Deg;
62+
BLH[2] = sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1] + (XYZ[2] + dz)*(XYZ[2] + dz)) - N;
63+
return 1;
64+
}
65+
66+
67+
68+
bool XYZ2ENU(double XYZ[], double XYZ1[], double ENU[])
69+
{
70+
double BLH[3] = { 0.0 };
71+
double B = 0.0, L = 0.0;
72+
double arrTemp[3] = { 0.0 };
73+
double spin[9] = { 0.0 };
74+
75+
XYZ2BLH(XYZ, BLH);
76+
B = BLH[0] * Rad; L = BLH[1] * Rad;
77+
spin[0] = -sin(L);
78+
spin[1] = cos(L);
79+
spin[2] = 0.0;
80+
spin[3] = -sin(B)*cos(L);
81+
spin[4] = -sin(B)*sin(L);
82+
spin[5] = cos(B);
83+
spin[6] = cos(B)*cos(L);
84+
spin[7] = cos(B)*sin(L);
85+
spin[8] = sin(B);
86+
87+
88+
//double spin[9] = { -sin(L), cos(L), 0.0,
89+
// -sin(B)*cos(L), -sin(B)*sin(L), cos(B),
90+
// cos(B)*cos(L), cos(B)*sin(L), sin(B)};
91+
92+
arrTemp[0] = XYZ1[0] - XYZ[0]; arrTemp[1] = XYZ1[1] - XYZ[1]; arrTemp[2] = XYZ1[2] - XYZ[2];
93+
MatrixMul(spin, 3, 3, arrTemp, 3, 1, ENU);
94+
return 1;
95+
}
96+
97+
//Calculate the elevation of the satellite
98+
bool CalElevation(double RecPos[], double SatPos[], double *El)
99+
{
100+
double arrENU[3] = { 0.0 };
101+
double dE = 0.0, dN = 0.0, dU = 0.0;
102+
103+
XYZ2ENU(RecPos, SatPos, arrENU);
104+
dE = arrENU[0]; dN = arrENU[1]; dU = arrENU[2];
105+
*El = atan2(dU, sqrt(dE*dE + dN*dN)); //atan(U,sqrt(E^2 + N^2))
106+
return true;
107+
}
108+
109+
//Calculate the azimuth of the satellite
110+
bool CalAzimuth(double RecPos[], double SatPos[], double *Az)
111+
{
112+
double arrENU[3] = { 0.0 };
113+
double dE = 0.0, dN = 0.0, dU = 0.0;
114+
115+
XYZ2ENU(RecPos, SatPos, arrENU);
116+
dN = arrENU[0]; dE = arrENU[1]; dU = arrENU[2];
117+
*Az = atan2(dN, dE);//atan(N/E)
118+
119+
return true;
120+
}

‎src/CoorTran.h

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
#ifndef _COORTRAN_H_
2+
#define _COORTRAN_H_
3+
#include "stdio.h"
4+
5+
bool BLH2XYZ(double BLH[], double XYZ[]);
6+
bool XYZ2BLH(double XYZ[], double BLH[]);
7+
bool XYZ2ENU(double XYZ[], double XYZ1[], double ENU[]);
8+
bool CalElevation(double RecPos[], double SatPos[], double* El);
9+
bool CalAzimuth(double RecPos[], double SatPos[], double* Az);
10+
11+
#endif

‎src/ErrorCorr.cpp

Lines changed: 103 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,103 @@
1+
/****************************************************************************
2+
Correction of the Ionospheric and Tropospheric delay, earth rotation
3+
****************************************************************************/
4+
#include "stdafx.h"
5+
#include "stdio.h"
6+
#include "RTDConst.h"
7+
#include "CoorTran.h"
8+
#include "math.h"
9+
#include "RTDStruct.h"
10+
11+
#define R2SC (1.0/pi) /*Rad to Semi-Circles*/
12+
#define D2SC (1/180.0) /*Degree to Semi-Circles*/
13+
#define SC2R (pi) /*Semi-Circles to Rad*/
14+
15+
//Klobuchar model for the Ionospheric delay correction
16+
bool klobuchar(IONOPARA *IONO, GPSTIME t, double RecPos[], double SatPos[], double *IonoCorr, double *El)
17+
{
18+
double EA = 0.0, BLH[3] = { 0.0 };
19+
double B = 0.0, L = 0.0, BM = 0.0;
20+
double localT = 0.0;
21+
double A = 0.0, P = 0.0;
22+
double X = 0.0, F = 0.0; //phase of ionospheric delay & slant factor
23+
double Az = 0.0;
24+
double *q = &Az;
25+
26+
CalElevation(RecPos, SatPos, El);
27+
CalAzimuth(RecPos, SatPos, q);
28+
29+
EA = 0.0137 / (*El*R2SC + 0.11) - 0.022;
30+
XYZ2BLH(RecPos, BLH);
31+
32+
B = BLH[0] * D2SC + EA*cos(Az);
33+
if (B > 0.416) B = 0.416;
34+
if (B < -0.416) B = -0.416;
35+
36+
L = BLH[0] * D2SC + EA*sin(Az) / cos(B*SC2R);
37+
38+
localT = 43200 * L + t.SecOfWeek;
39+
if (localT >= 86400) localT = localT - 86400;
40+
if (localT < 0) localT = localT + 86400;
41+
42+
BM = B + 0.064*cos(L*SC2R - 1.617*SC2R);
43+
44+
A = IONO->alpha[0] + IONO->alpha[1] * BM + IONO->alpha[2] * BM*BM + IONO->alpha[3] * BM*BM*BM;
45+
if (A < 0) A = 0;
46+
47+
P = IONO->beta[0] + IONO->beta[1] * BM + IONO->beta[2] * BM*BM + IONO->beta[3] * BM*BM*BM;
48+
if (P < 72000) P = 72000;
49+
50+
X = 2 * pi*(localT - 50400) / P; //Rad
51+
F = 1.0 + 16.0*(0.53 - *El*R2SC)*(0.53 - *El*R2SC)*(0.53 - *El*R2SC);
52+
53+
if (abs(X) <= 1.57)
54+
{
55+
*IonoCorr = (5e-9 + A*(1 - X*X / 2 + X*X*X*X / 24))*F;
56+
}
57+
else
58+
*IonoCorr = 5e-9*F;
59+
60+
*IonoCorr = *IonoCorr*C_Light;
61+
return true;
62+
}
63+
64+
//Hopfield model for the Tropospheric delay correction
65+
bool Hopfield(double RecPos[], double SatPos[], double * TropCorr)
66+
{
67+
double BLH[3];
68+
XYZ2BLH(RecPos, BLH);
69+
70+
double El = 0.0;
71+
double H0 = 0.0;
72+
double T0 = 15 + 273.15;
73+
double P0 = 1013.25;
74+
double RH0 = 0.5;
75+
double *p = &El;
76+
77+
double Kd = 0.0, Kw = 0.0, hd = 0.0, hw = 0.0;
78+
double e = 0.0, T = 0.0, P = 0.0, RH = 0.0;
79+
80+
CalElevation(RecPos, SatPos, p);
81+
82+
RH = RH0*exp(-0.0006396*(BLH[2] - H0));
83+
P = P0*pow(abs(1 - 0.0000226*(BLH[2] - H0)), 5.225);
84+
T = T0 - 0.0065*(BLH[2] - H0);
85+
e = RH*exp(-37.2465 + 0.213166*T - 0.000256908*T*T);
86+
hw = 11000.0;
87+
hd = 40136 + 148.72*(T0 - 273.16);
88+
Kw = (155.2e-7) * 4810 * e*(hw - BLH[2]) / T / T;
89+
Kd = (155.2e-7)*P*(hd - BLH[2]) / T;
90+
91+
*TropCorr = Kd / sin(sqrt(El*El + 6.25*Rad*Rad)) + Kw / sin(sqrt(El*El + 2.25*Rad*Rad));
92+
return true;
93+
}
94+
95+
//EarthRotation correction
96+
97+
bool EarthRotation(double SatPos[], double t)
98+
{
99+
*SatPos = *SatPos*cos(Omega_WGS*t) + (*SatPos + 1)*cos(Omega_WGS*t);
100+
*(SatPos + 1) = *(SatPos + 1)*cos(Omega_WGS*t) - (*SatPos)*cos(Omega_WGS*t);
101+
//SatPos[2] = SatPos[2];
102+
return true;
103+
}

‎src/ErrorCorr.h

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
#ifndef _ERRORCORR_H_
2+
#define _ERRORCORR_H_
3+
#include "RTDStruct.h"
4+
5+
bool klobuchar(IONOPARA *IONO, GPSTIME t, double RecPos[], double SatPos[], double *IonoCorr, double *El);
6+
bool Hopfield(double RecPos[], double SatPos[], double *TropCorr);
7+
bool EarthRotation(double SatPos[], double t);
8+
9+
#endif // !_ERRORCORR_H_

0 commit comments

Comments
 (0)