kopia lustrzana https://github.com/rs1729/RS
RS92: Lsg iterieren
Zilog80
rodzic
3791f72feb
commit
d10efcca25
|
|
@ -1708,8 +1708,8 @@ double NAV_relVel(LOC_t loc, VEL_t vel) {
|
|||
return d;
|
||||
}
|
||||
|
||||
int NAV_LinP(int N, SAT_t satv[], double pos0_ecef[3], double dt,
|
||||
double pos_ecef[3], double *cc) {
|
||||
int NAV_LinP(int N, SAT_t satv[], double pos_ecef[3], double dt,
|
||||
double dpos_ecef[3], double *cc) {
|
||||
|
||||
int i, j, k;
|
||||
double B[N][4], Binv[4][N], BtB[4][4], BBinv[4][4];
|
||||
|
|
@ -1717,17 +1717,21 @@ int NAV_LinP(int N, SAT_t satv[], double pos0_ecef[3], double dt,
|
|||
|
||||
double X, Y, Z;
|
||||
double norm[N];
|
||||
double obs_range, prox_range;
|
||||
double range, obs_range, prox_range;
|
||||
|
||||
if (N < 4 || N > 12) return -1;
|
||||
|
||||
for (i = 0; i < N; i++) {
|
||||
rotZ(satv[i].X, satv[i].Y, satv[i].Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE, B[i], B[i]+1, B[i]+2);
|
||||
|
||||
range = dist( pos_ecef[0], pos_ecef[3], pos_ecef[2], satv[i].X, satv[i].Y, satv[i].Z);
|
||||
range /= LIGHTSPEED;
|
||||
if (range < 0.06 || range > 0.1) range = RANGE_ESTIMATE;
|
||||
rotZ(satv[i].X, satv[i].Y, satv[i].Z, EARTH_ROTATION_RATE*range, B[i], B[i]+1, B[i]+2);
|
||||
//rotZ(satv[i].X, satv[i].Y, satv[i].Z, 0.0, B[i], B[i]+1, B[i]+2); // est. RANGE_RATE = 0.0
|
||||
|
||||
X = B[i][0]-pos0_ecef[0];
|
||||
Y = B[i][1]-pos0_ecef[1];
|
||||
Z = B[i][2]-pos0_ecef[2];
|
||||
X = B[i][0]-pos_ecef[0];
|
||||
Y = B[i][1]-pos_ecef[1];
|
||||
Z = B[i][2]-pos_ecef[2];
|
||||
norm[i] = sqrt(X*X+Y*Y+Z*Z);
|
||||
|
||||
B[i][0] = X/norm[i];
|
||||
|
|
@ -1776,20 +1780,18 @@ int NAV_LinP(int N, SAT_t satv[], double pos0_ecef[3], double dt,
|
|||
}
|
||||
}
|
||||
|
||||
pos_ecef[0] = Ba[0];
|
||||
pos_ecef[1] = Ba[1];
|
||||
pos_ecef[2] = Ba[2];
|
||||
dpos_ecef[0] = Ba[0];
|
||||
dpos_ecef[1] = Ba[1];
|
||||
dpos_ecef[2] = Ba[2];
|
||||
|
||||
//rotZ(Ba[0], Ba[1], Ba[2], 0.0/*EARTH_ROTATION_RATE*RANGE_ESTIMATE*/, pos_ecef, pos_ecef+1, pos_ecef+2);
|
||||
|
||||
*cc = Ba[3];
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int NAV_LinV(int N, SAT_t satv[], double pos0_ecef[3],
|
||||
double vel0_ecef[3],double dt,
|
||||
double vel_ecef[3], double *cc) {
|
||||
int NAV_LinV(int N, SAT_t satv[], double pos_ecef[3],
|
||||
double vel_ecef[3], double dt,
|
||||
double dvel_ecef[3], double *cc) {
|
||||
|
||||
int i, j, k;
|
||||
double B[N][4], Binv[4][N], BtB[4][4], BBinv[4][4];
|
||||
|
|
@ -1804,9 +1806,9 @@ int NAV_LinV(int N, SAT_t satv[], double pos0_ecef[3],
|
|||
|
||||
if (N < 4 || N > 12) return -1;
|
||||
|
||||
loc.X = pos0_ecef[0];
|
||||
loc.Y = pos0_ecef[1];
|
||||
loc.Z = pos0_ecef[2];
|
||||
loc.X = pos_ecef[0];
|
||||
loc.Y = pos_ecef[1];
|
||||
loc.Z = pos_ecef[2];
|
||||
|
||||
if (N < 4 || N > 12) return -1;
|
||||
|
||||
|
|
@ -1814,9 +1816,9 @@ int NAV_LinV(int N, SAT_t satv[], double pos0_ecef[3],
|
|||
rotZ(satv[i].X, satv[i].Y, satv[i].Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE, B[i], B[i]+1, B[i]+2);
|
||||
//rotZ(satv[i].X, satv[i].Y, satv[i].Z, 0.0, B[i], B[i]+1, B[i]+2); // est. RANGE_RATE = 0.0
|
||||
|
||||
X = B[i][0]-pos0_ecef[0];
|
||||
Y = B[i][1]-pos0_ecef[1];
|
||||
Z = B[i][2]-pos0_ecef[2];
|
||||
X = B[i][0]-pos_ecef[0];
|
||||
Y = B[i][1]-pos_ecef[1];
|
||||
Z = B[i][2]-pos_ecef[2];
|
||||
norm[i] = sqrt(X*X+Y*Y+Z*Z);
|
||||
B[i][0] = X/norm[i];
|
||||
B[i][1] = Y/norm[i];
|
||||
|
|
@ -1858,9 +1860,9 @@ int NAV_LinV(int N, SAT_t satv[], double pos0_ecef[3],
|
|||
vel.X = satv[i].X;
|
||||
vel.Y = satv[i].Y;
|
||||
vel.Z = satv[i].Z;
|
||||
vel.vX = satv[i].vX - vel0_ecef[0];
|
||||
vel.vY = satv[i].vY - vel0_ecef[1];
|
||||
vel.vZ = satv[i].vZ - vel0_ecef[2];
|
||||
vel.vX = satv[i].vX - vel_ecef[0];
|
||||
vel.vY = satv[i].vY - vel_ecef[1];
|
||||
vel.vZ = satv[i].vZ - vel_ecef[2];
|
||||
v_proj = NAV_relVel(loc, vel);
|
||||
prox_rate = v_proj - dt;
|
||||
a[i] = prox_rate - obs_rate;
|
||||
|
|
@ -1873,12 +1875,10 @@ int NAV_LinV(int N, SAT_t satv[], double pos0_ecef[3],
|
|||
}
|
||||
}
|
||||
|
||||
vel_ecef[0] = Ba[0];
|
||||
vel_ecef[1] = Ba[1];
|
||||
vel_ecef[2] = Ba[2];
|
||||
dvel_ecef[0] = Ba[0];
|
||||
dvel_ecef[1] = Ba[1];
|
||||
dvel_ecef[2] = Ba[2];
|
||||
|
||||
//rotZ(Ba[0], Ba[1], Ba[2], 0.0/*EARTH_ROTATION_RATE*RANGE_ESTIMATE*/, vel_ecef, vel_ecef+1, vel_ecef+2);
|
||||
|
||||
*cc = Ba[3];
|
||||
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -66,6 +66,7 @@ typedef struct {
|
|||
int sats[4];
|
||||
double dop;
|
||||
unsigned short aux[4];
|
||||
double diter;
|
||||
} gpx_t;
|
||||
|
||||
gpx_t gpx;
|
||||
|
|
@ -79,6 +80,7 @@ int option_verbose = 0, // ausfuehrliche Anzeige
|
|||
option_b = 0,
|
||||
fileloaded = 0,
|
||||
option_vergps = 0,
|
||||
option_iter = 0,
|
||||
option_vel = 0, // velocity
|
||||
option_aux = 0, // Aux/Ozon
|
||||
rawin = 0;
|
||||
|
|
@ -985,7 +987,7 @@ int get_GPSkoord(int N) {
|
|||
double lat, lon, alt, rx_cl_bias;
|
||||
double vH, vD, vU;
|
||||
double lat1s, lon1s, alt1s;
|
||||
double pos_ecef[3], pos1s_ecef[3],
|
||||
double pos_ecef[3], pos1s_ecef[3], dpos_ecef[3],
|
||||
vel_ecef[3], dvel_ecef[3];
|
||||
double gdop, gdop0 = 1000.0;
|
||||
double hdop, vdop, pdop;
|
||||
|
|
@ -994,7 +996,7 @@ int get_GPSkoord(int N) {
|
|||
int num = 0;
|
||||
SAT_t Sat_A[4];
|
||||
SAT_t Sat_B[12]; // N <= 12
|
||||
SAT_t Sat_V[12];
|
||||
SAT_t Sat_B1s[12];
|
||||
|
||||
if (option_vergps == 8) {
|
||||
fprintf(stdout, " sats: ");
|
||||
|
|
@ -1053,7 +1055,7 @@ int get_GPSkoord(int N) {
|
|||
if (option_vergps == 8 || option_vergps == 2) {
|
||||
|
||||
for (j = 0; j < N; j++) Sat_B[j] = sat[prn[j]];
|
||||
for (j = 0; j < N; j++) Sat_V[j] = sat1s[prn[j]];
|
||||
for (j = 0; j < N; j++) Sat_B1s[j] = sat1s[prn[j]];
|
||||
|
||||
NAV_bancroft1(N, Sat_B, pos_ecef, &rx_cl_bias);
|
||||
ecef2elli(pos_ecef[0], pos_ecef[1], pos_ecef[2], &lat, &lon, &alt);
|
||||
|
|
@ -1062,8 +1064,19 @@ int get_GPSkoord(int N) {
|
|||
gdop = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]);
|
||||
}
|
||||
|
||||
if (option_iter) {
|
||||
NAV_LinP(N, Sat_B, pos_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias);
|
||||
for (j = 0; j < 3; j++) pos_ecef[j] += dpos_ecef[j];
|
||||
ecef2elli(pos_ecef[0], pos_ecef[1], pos_ecef[2], &lat, &lon, &alt);
|
||||
gpx.diter = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[0],dpos_ecef[0]);
|
||||
}
|
||||
|
||||
if (option_vel == 1) {
|
||||
NAV_bancroft1(N, Sat_V, pos1s_ecef, &rx_cl_bias);
|
||||
NAV_bancroft1(N, Sat_B1s, pos1s_ecef, &rx_cl_bias);
|
||||
if (option_iter) {
|
||||
NAV_LinP(N, Sat_B1s, pos1s_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias);
|
||||
for (j = 0; j < 3; j++) pos1s_ecef[j] += dpos_ecef[j];
|
||||
}
|
||||
for (j = 0; j < 3; j++) vel_ecef[j] = pos_ecef[j] - pos1s_ecef[j];
|
||||
get_GPSvel(lat, lon, vel_ecef, &vH, &vD, &vU);
|
||||
ecef2elli(pos1s_ecef[0], pos1s_ecef[1], pos1s_ecef[2], &lat1s, &lon1s, &alt1s);
|
||||
|
|
@ -1154,8 +1167,13 @@ int print_position() { // GPS-Hoehe ueber Ellipsoid
|
|||
if (k >= 4) {
|
||||
if (get_GPSkoord(k) > 0) {
|
||||
fprintf(stdout, " ");
|
||||
|
||||
if (almanac) fprintf(stdout, " lat: %.4f lon: %.4f alt: %.1f ", gpx.lat, gpx.lon, gpx.h);
|
||||
else fprintf(stdout, " lat: %.5f lon: %.5f alt: %.1f ", gpx.lat, gpx.lon, gpx.h);
|
||||
|
||||
if (option_iter && (option_vergps == 8 || option_vergps == 2)) {
|
||||
fprintf(stdout, " (d:%.1f) ", gpx.diter);
|
||||
}
|
||||
if (option_vel && option_vergps >= 2) {
|
||||
fprintf(stdout," vH: %4.1f D: %5.1f° vV: %3.1f ", gpx.vH, gpx.vD, gpx.vU);
|
||||
}
|
||||
|
|
@ -1266,6 +1284,9 @@ int main(int argc, char *argv[]) {
|
|||
option_vel = 2;
|
||||
if (option_vergps < 1) option_vergps = 2;
|
||||
}
|
||||
else if ( (strcmp(*argv, "--iter") == 0) ) {
|
||||
option_iter = 1;
|
||||
}
|
||||
else if ( (strcmp(*argv, "-v") == 0) ) {
|
||||
option_verbose = 1;
|
||||
}
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue