1221 lines
45 KiB
C
1221 lines
45 KiB
C
|
/*------------------------------------------------------------------------------
|
|||
|
|
* pntpos.c : standard positioning
|
|||
|
|
*
|
|||
|
|
* Copyright (C) 2007-2020 by T.TAKASU, All rights reserved.
|
|||
|
|
*
|
|||
|
|
* version : $Revision:$ $Date:$
|
|||
|
|
* history : 2010/07/28 1.0 moved from rtkcmn.c
|
|||
|
|
* changed api:
|
|||
|
|
* pntpos()
|
|||
|
|
* deleted api:
|
|||
|
|
* pntvel()
|
|||
|
|
* 2011/01/12 1.1 add option to include unhealthy satellite
|
|||
|
|
* reject duplicated observation data
|
|||
|
|
* changed api: ionocorr()
|
|||
|
|
* 2011/11/08 1.2 enable snr mask for single-mode (rtklib_2.4.1_p3)
|
|||
|
|
* 2012/12/25 1.3 add variable snr mask
|
|||
|
|
* 2014/05/26 1.4 support galileo and beidou
|
|||
|
|
* 2015/03/19 1.5 fix bug on ionosphere correction for GLO and BDS
|
|||
|
|
* 2018/10/10 1.6 support api change of satexclude()
|
|||
|
|
* 2020/11/30 1.7 support NavIC/IRNSS in pntpos()
|
|||
|
|
* no support IONOOPT_LEX option in ioncorr()
|
|||
|
|
* improve handling of TGD correction for each system
|
|||
|
|
* use E1-E5b for Galileo dual-freq iono-correction
|
|||
|
|
* use API sat2freq() to get carrier frequency
|
|||
|
|
* add output of velocity estimation error in estvel()
|
|||
|
|
*-----------------------------------------------------------------------------*/
|
|||
|
|
#include "rtklib.h"
|
|||
|
|
|
|||
|
|
/* constants/macros ----------------------------------------------------------*/
|
|||
|
|
|
|||
|
|
#define SQR(x) ((x) * (x))
|
|||
|
|
#define MAX(x, y) ((x) >= (y) ? (x) : (y))
|
|||
|
|
|
|||
|
|
#if 0 /* enable GPS-QZS time offset estimation */
|
|||
|
|
#define NX (4 + 5) /* # of estimated parameters */
|
|||
|
|
#else
|
|||
|
|
#define NX (4 + 3) /* # of estimated parameters */
|
|||
|
|
#endif
|
|||
|
|
#define MAXITR 10 /* max number of iteration for point pos */
|
|||
|
|
#define ERR_ION 5.0 /* ionospheric delay Std (m) */
|
|||
|
|
#define ERR_TROP 3.0 /* tropspheric delay Std (m) */
|
|||
|
|
#define ERR_SAAS 0.3 /* Saastamoinen model error Std (m) */
|
|||
|
|
#define ERR_BRDCI 0.5 /* broadcast ionosphere model error factor */
|
|||
|
|
#define ERR_CBIAS 0.3 /* code bias error Std (m) */
|
|||
|
|
#define REL_HUMI 0.7 /* relative humidity for Saastamoinen model */
|
|||
|
|
#define MIN_EL (5.0 * D2R) /* min elevation for measurement error (rad) */
|
|||
|
|
#define RESCODE_MIN_EL (15.0 * D2R) /* min elevation for second calculation (rad) */
|
|||
|
|
|
|||
|
|
/* pseudorange measurement error variance ------------------------------------*/
|
|||
|
|
static double varerr(const prcopt_t *opt, const ssat_t *ssat, const obsd_t *obs, double el, int sys)
|
|||
|
|
{
|
|||
|
|
double fact = 1.0, varr, snr_rover;
|
|||
|
|
|
|||
|
|
switch (sys)
|
|||
|
|
{
|
|||
|
|
case SYS_GPS:
|
|||
|
|
fact *= EFACT_GPS;
|
|||
|
|
break;
|
|||
|
|
case SYS_GLO:
|
|||
|
|
fact *= EFACT_GLO;
|
|||
|
|
break;
|
|||
|
|
case SYS_SBS:
|
|||
|
|
fact *= EFACT_SBS;
|
|||
|
|
break;
|
|||
|
|
case SYS_CMP:
|
|||
|
|
fact *= EFACT_CMP;
|
|||
|
|
break;
|
|||
|
|
case SYS_QZS:
|
|||
|
|
fact *= EFACT_QZS;
|
|||
|
|
break;
|
|||
|
|
case SYS_IRN:
|
|||
|
|
fact *= EFACT_IRN;
|
|||
|
|
break;
|
|||
|
|
default:
|
|||
|
|
fact *= EFACT_GPS;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
if (el < MIN_EL)
|
|||
|
|
el = MIN_EL;
|
|||
|
|
/* var = R^2*(a^2 + (b^2/sin(el) + c^2*(10^(0.1*(snr_max-snr_rover)))) + (d*rcv_std)^2) */
|
|||
|
|
varr = SQR(opt->err[1]) + SQR(opt->err[2]) / sin(el);
|
|||
|
|
if (opt->err[6] > 0.0)
|
|||
|
|
{ /* if snr term not zero */
|
|||
|
|
snr_rover = (ssat) ? SNR_UNIT * ssat->snr_rover[0] : opt->err[5];
|
|||
|
|
varr += SQR(opt->err[6]) * pow(10, 0.1 * MAX(opt->err[5] - snr_rover, 0));
|
|||
|
|
}
|
|||
|
|
varr *= SQR(opt->eratio[0]);
|
|||
|
|
if (opt->err[7] > 0.0)
|
|||
|
|
{
|
|||
|
|
varr += SQR(opt->err[7] * 0.01 * (1 << (obs->Pstd[0] + 5))); /* 0.01*2^(n+5) m */
|
|||
|
|
}
|
|||
|
|
if (opt->ionoopt == IONOOPT_IFLC)
|
|||
|
|
varr *= SQR(3.0); /* iono-free */
|
|||
|
|
return SQR(fact) * varr;
|
|||
|
|
}
|
|||
|
|
/* get group delay parameter (m) ---------------------------------------------*/
|
|||
|
|
static double gettgd(int sat, const nav_t *nav, int type)
|
|||
|
|
{
|
|||
|
|
int i, sys = satsys(sat, NULL);
|
|||
|
|
|
|||
|
|
if (sys == SYS_GLO)
|
|||
|
|
{
|
|||
|
|
for (i = 0; i < nav->ng; i++)
|
|||
|
|
{
|
|||
|
|
if (nav->geph[i].sat == sat)
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
return (i >= nav->ng) ? 0.0 : -nav->geph[i].dtaun * CLIGHT;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
for (i = 0; i < nav->n; i++)
|
|||
|
|
{
|
|||
|
|
if (nav->eph[i].sat == sat)
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
return (i >= nav->n) ? 0.0 : nav->eph[i].tgd[type] * CLIGHT;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
/* test SNR mask -------------------------------------------------------------*/
|
|||
|
|
static int snrmask(const obsd_t *obs, const double *azel, const prcopt_t *opt)
|
|||
|
|
{
|
|||
|
|
if (testsnr(0, 0, azel[1], obs->SNR[0] * SNR_UNIT, &opt->snrmask))
|
|||
|
|
{
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
if (opt->ionoopt == IONOOPT_IFLC)
|
|||
|
|
{
|
|||
|
|
if (testsnr(0, 1, azel[1], obs->SNR[1] * SNR_UNIT, &opt->snrmask))
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
/* iono-free or "pseudo iono-free" pseudorange with code bias correction -----*/
|
|||
|
|
static double prange(const obsd_t *obs, const nav_t *nav, const prcopt_t *opt, double *var)
|
|||
|
|
{
|
|||
|
|
double P1, P2, gamma, b1, b2;
|
|||
|
|
int sat, sys;
|
|||
|
|
|
|||
|
|
sat = obs->sat;
|
|||
|
|
sys = satsys(sat, NULL);
|
|||
|
|
P1 = obs->P[0];
|
|||
|
|
P2 = obs->P[1];
|
|||
|
|
*var = 0.0;
|
|||
|
|
|
|||
|
|
if (P1 == 0.0 || (opt->ionoopt == IONOOPT_IFLC && P2 == 0.0))
|
|||
|
|
return 0.0;
|
|||
|
|
|
|||
|
|
/* P1-C1,P2-C2 DCB correction */
|
|||
|
|
//if (sys == SYS_GPS || sys == SYS_GLO)
|
|||
|
|
//{
|
|||
|
|
// if (obs->code[0] == CODE_L1C)
|
|||
|
|
// P1 += nav->cbias[sat - 1][1]; /* C1->P1 */
|
|||
|
|
// if (obs->code[1] == CODE_L2C)
|
|||
|
|
// P2 += nav->cbias[sat - 1][2]; /* C2->P2 */
|
|||
|
|
//}
|
|||
|
|
|
|||
|
|
if (opt->ionoopt == IONOOPT_IFLC)
|
|||
|
|
{ /* dual-frequency */
|
|||
|
|
|
|||
|
|
if (sys == SYS_GPS || sys == SYS_QZS)
|
|||
|
|
{ /* L1-L2,G1-G2 */
|
|||
|
|
gamma = SQR(FREQL1 / FREQL2);
|
|||
|
|
return (P2 - gamma * P1) / (1.0 - gamma);
|
|||
|
|
}
|
|||
|
|
else if (sys == SYS_GLO)
|
|||
|
|
{ /* G1-G2 */
|
|||
|
|
gamma = SQR(FREQ1_GLO / FREQ2_GLO);
|
|||
|
|
return (P2 - gamma * P1) / (1.0 - gamma);
|
|||
|
|
}
|
|||
|
|
else if (sys == SYS_GAL)
|
|||
|
|
{ /* E1-E5b */
|
|||
|
|
gamma = SQR(FREQL1 / FREQE5b);
|
|||
|
|
if (getseleph(SYS_GAL))
|
|||
|
|
{ /* F/NAV */
|
|||
|
|
P2 -= gettgd(sat, nav, 0) - gettgd(sat, nav, 1); /* BGD_E5aE5b */
|
|||
|
|
}
|
|||
|
|
return (P2 - gamma * P1) / (1.0 - gamma);
|
|||
|
|
}
|
|||
|
|
else if (sys == SYS_CMP)
|
|||
|
|
{ /* B1-B2 */
|
|||
|
|
gamma = SQR(((obs->code[0] == CODE_L2I) ? FREQ1_CMP : FREQL1) / FREQ2_CMP);
|
|||
|
|
if (obs->code[0] == CODE_L2I)
|
|||
|
|
b1 = gettgd(sat, nav, 0); /* TGD_B1I */
|
|||
|
|
else if (obs->code[0] == CODE_L1P)
|
|||
|
|
b1 = gettgd(sat, nav, 2); /* TGD_B1Cp */
|
|||
|
|
else
|
|||
|
|
b1 = gettgd(sat, nav, 2) + gettgd(sat, nav, 4); /* TGD_B1Cp+ISC_B1Cd */
|
|||
|
|
b2 = gettgd(sat, nav, 1); /* TGD_B2I/B2bI (m) */
|
|||
|
|
return ((P2 - gamma * P1) - (b2 - gamma * b1)) / (1.0 - gamma);
|
|||
|
|
}
|
|||
|
|
else if (sys == SYS_IRN)
|
|||
|
|
{ /* L5-S */
|
|||
|
|
gamma = SQR(FREQL5 / FREQs);
|
|||
|
|
return (P2 - gamma * P1) / (1.0 - gamma);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{ /* single-freq (L1/E1/B1) */
|
|||
|
|
*var = SQR(ERR_CBIAS);
|
|||
|
|
|
|||
|
|
if (sys == SYS_GPS || sys == SYS_QZS)
|
|||
|
|
{ /* L1 */
|
|||
|
|
b1 = gettgd(sat, nav, 0); /* TGD (m) */
|
|||
|
|
return P1 - b1;
|
|||
|
|
}
|
|||
|
|
else if (sys == SYS_GLO)
|
|||
|
|
{ /* G1 */
|
|||
|
|
gamma = SQR(FREQ1_GLO / FREQ2_GLO);
|
|||
|
|
b1 = gettgd(sat, nav, 0); /* -dtaun (m) */
|
|||
|
|
return P1 - b1 / (gamma - 1.0);
|
|||
|
|
}
|
|||
|
|
else if (sys == SYS_GAL)
|
|||
|
|
{ /* E1 */
|
|||
|
|
if (getseleph(SYS_GAL))
|
|||
|
|
b1 = gettgd(sat, nav, 0); /* BGD_E1E5a */
|
|||
|
|
else
|
|||
|
|
b1 = gettgd(sat, nav, 1); /* BGD_E1E5b */
|
|||
|
|
return P1 - b1;
|
|||
|
|
}
|
|||
|
|
else if (sys == SYS_CMP)
|
|||
|
|
{ /* B1I/B1Cp/B1Cd */
|
|||
|
|
if (obs->code[0] == CODE_L2I)
|
|||
|
|
b1 = gettgd(sat, nav, 0); /* TGD_B1I */
|
|||
|
|
else if (obs->code[0] == CODE_L1P)
|
|||
|
|
b1 = gettgd(sat, nav, 2); /* TGD_B1Cp */
|
|||
|
|
else
|
|||
|
|
b1 = gettgd(sat, nav, 2) + gettgd(sat, nav, 4); /* TGD_B1Cp+ISC_B1Cd */
|
|||
|
|
return P1 - b1;
|
|||
|
|
}
|
|||
|
|
else if (sys == SYS_IRN)
|
|||
|
|
{ /* L5 */
|
|||
|
|
gamma = SQR(FREQs / FREQL5);
|
|||
|
|
b1 = gettgd(sat, nav, 0); /* TGD (m) */
|
|||
|
|
return P1 - gamma * b1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
return P1;
|
|||
|
|
}
|
|||
|
|
/* ionospheric correction ------------------------------------------------------
|
|||
|
|
* compute ionospheric correction
|
|||
|
|
* args : gtime_t time I time
|
|||
|
|
* nav_t *nav I navigation data
|
|||
|
|
* int sat I satellite number
|
|||
|
|
* double *pos I receiver position {lat,lon,h} (rad|m)
|
|||
|
|
* double *azel I azimuth/elevation angle {az,el} (rad)
|
|||
|
|
* int ionoopt I ionospheric correction option (IONOOPT_???)
|
|||
|
|
* double *ion O ionospheric delay (L1) (m)
|
|||
|
|
* double *var O ionospheric delay (L1) variance (m^2)
|
|||
|
|
* return : status(1:ok,0:error)
|
|||
|
|
*-----------------------------------------------------------------------------*/
|
|||
|
|
extern int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos,
|
|||
|
|
const double *azel, int ionoopt, double *ion, double *var)
|
|||
|
|
{
|
|||
|
|
int err = 0;
|
|||
|
|
|
|||
|
|
// trace(4, "ionocorr: time=%s opt=%d sat=%2d pos=%.3f %.3f azel=%.3f %.3f\n",
|
|||
|
|
// time_str(time, 3), ionoopt, sat, pos[0] * R2D, pos[1] * R2D, azel[0] * R2D,
|
|||
|
|
// azel[1] * R2D);
|
|||
|
|
|
|||
|
|
/* SBAS ionosphere model */
|
|||
|
|
// if (ionoopt == IONOOPT_SBAS)
|
|||
|
|
// {
|
|||
|
|
// if (sbsioncorr(time, nav, pos, azel, ion, var))
|
|||
|
|
// return 1;
|
|||
|
|
// err = 1;
|
|||
|
|
// }
|
|||
|
|
/* IONEX TEC model */
|
|||
|
|
// if (ionoopt == IONOOPT_TEC)
|
|||
|
|
// {
|
|||
|
|
// if (iontec(time, nav, pos, azel, 1, ion, var))
|
|||
|
|
// return 1;
|
|||
|
|
// err = 1;
|
|||
|
|
// }
|
|||
|
|
/* QZSS broadcast ionosphere model */
|
|||
|
|
// if (ionoopt == IONOOPT_QZS && norm(nav->ion_qzs, 8) > 0.0)
|
|||
|
|
// {
|
|||
|
|
// *ion = ionmodel(time, nav->ion_qzs, pos, azel);
|
|||
|
|
// *var = SQR(*ion * ERR_BRDCI);
|
|||
|
|
// return 1;
|
|||
|
|
// }
|
|||
|
|
/* GPS broadcast ionosphere model */
|
|||
|
|
if (ionoopt == IONOOPT_BRDC || err == 1)
|
|||
|
|
{
|
|||
|
|
*ion = ionmodel(time, nav->ion_gps, pos, azel);
|
|||
|
|
*var = SQR(*ion * ERR_BRDCI);
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
*ion = 0.0;
|
|||
|
|
*var = ionoopt == IONOOPT_OFF ? SQR(ERR_ION) : 0.0;
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
/* tropospheric correction -----------------------------------------------------
|
|||
|
|
* compute tropospheric correction
|
|||
|
|
* args : gtime_t time I time
|
|||
|
|
* nav_t *nav I navigation data
|
|||
|
|
* double *pos I receiver position {lat,lon,h} (rad|m)
|
|||
|
|
* double *azel I azimuth/elevation angle {az,el} (rad)
|
|||
|
|
* int tropopt I tropospheric correction option (TROPOPT_???)
|
|||
|
|
* double *trp O tropospheric delay (m)
|
|||
|
|
* double *var O tropospheric delay variance (m^2)
|
|||
|
|
* return : status(1:ok,0:error)
|
|||
|
|
*-----------------------------------------------------------------------------*/
|
|||
|
|
extern int tropcorr(gtime_t time, const nav_t *nav, const double *pos,
|
|||
|
|
const double *azel, int tropopt, double *trp, double *var)
|
|||
|
|
{
|
|||
|
|
trace(4, "tropcorr: time=%s opt=%d pos=%.3f %.3f azel=%.3f %.3f\n",
|
|||
|
|
time_str(time, 3), tropopt, pos[0] * R2D, pos[1] * R2D, azel[0] * R2D,
|
|||
|
|
azel[1] * R2D);
|
|||
|
|
|
|||
|
|
/* Saastamoinen model */
|
|||
|
|
if (tropopt == TROPOPT_SAAS || tropopt == TROPOPT_EST || tropopt == TROPOPT_ESTG)
|
|||
|
|
{
|
|||
|
|
*trp = tropmodel(time, pos, azel, REL_HUMI);
|
|||
|
|
*var = SQR(ERR_SAAS / (sin(azel[1]) + 0.1));
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
/* SBAS (MOPS) troposphere model */
|
|||
|
|
// if (tropopt == TROPOPT_SBAS)
|
|||
|
|
//{
|
|||
|
|
// *trp = sbstropcorr(time, pos, azel, var);
|
|||
|
|
// return 1;
|
|||
|
|
// }
|
|||
|
|
/* no correction */
|
|||
|
|
*trp = 0.0;
|
|||
|
|
*var = tropopt == TROPOPT_OFF ? SQR(ERR_TROP) : 0.0;
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
/* pseudorange residuals
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>㵱ǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>в<EFBFBD> v<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ξ<EFBFBD><EFBFBD><EFBFBD> H<EFBFBD><EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>в<EFBFBD><EFBFBD>ķ<EFBFBD><EFBFBD><EFBFBD> var<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵķ<EFBFBD>λ<EFBFBD>Ǻ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> azel<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD>λʱ<EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD> vsat<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>в<EFBFBD> resp<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>붨λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǹ<EFBFBD><EFBFBD><EFBFBD> ns <EFBFBD>ͷ<EFBFBD><EFBFBD≯<EFBFBD><EFBFBD><EFBFBD> nv
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>17<EFBFBD><EFBFBD>
|
|||
|
|
int iter I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
obsd_t *obs I <EFBFBD>۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
int n I <EFBFBD>۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
double *rs I <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ú<EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ6*n<EFBFBD><EFBFBD>{x,y,z,vx,vy,vz}(ecef)(m,m/s)
|
|||
|
|
double *dts I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ2*n<EFBFBD><EFBFBD> {bias,drift} (s|s/s)
|
|||
|
|
double *vare I <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ú<EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><EFBFBD><EFBFBD><EFBFBD> (m^2)
|
|||
|
|
int *svh I <EFBFBD><EFBFBD><EFBFBD>ǽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־ (-1:correction not available)
|
|||
|
|
nav_t *nav I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
double *x I <EFBFBD><EFBFBD><EFBFBD>ε<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ֮ǰ<EFBFBD>Ķ<EFBFBD>λֵ
|
|||
|
|
prcopt_t *opt I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><EFBFBD>
|
|||
|
|
double *v O <EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD>̵<EFBFBD><EFBFBD>Ҷ˲<EFBFBD><EFBFBD>֣<EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>в<EFBFBD>
|
|||
|
|
double *H O <EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>еļ<EFBFBD><EFBFBD>ξ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
double *var O <EFBFBD><EFBFBD><EFBFBD>붨λ<EFBFBD><EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>в<EFBFBD><EFBFBD>ķ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
double *azel O <EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD>ǰ<EFBFBD><EFBFBD>λֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD> {<EFBFBD><EFBFBD>λ<EFBFBD>ǡ<EFBFBD><EFBFBD>߶Ƚ<EFBFBD>} (2*n)
|
|||
|
|
int *vsat O <EFBFBD><EFBFBD><EFBFBD>й۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD>ǰ<EFBFBD><EFBFBD>λʱ<EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD>Ч (1*n)
|
|||
|
|
double *resp O <EFBFBD><EFBFBD><EFBFBD>й۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>в(P-(r+c*dtr-c*dts+I+T)) (1*n)
|
|||
|
|
int *ns O <EFBFBD><EFBFBD><EFBFBD>붨λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵĸ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD>
|
|||
|
|
int O <EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ķ<EFBFBD><EFBFBD≯<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
-----------------------------------------------------*/
|
|||
|
|
static int rescode(int iter, const obsd_t *obs, int n, const double *rs,
|
|||
|
|
const double *dts, const double *vare, const int *svh,
|
|||
|
|
const nav_t *nav, const double *x, const prcopt_t *opt,
|
|||
|
|
const ssat_t *ssat, double *v, double *H, double *var,
|
|||
|
|
double *azel, int *vsat, double *resp, int *ns)
|
|||
|
|
{
|
|||
|
|
gtime_t time;
|
|||
|
|
double r, freq, dion = 0.0, dtrp = 0.0, vmeas, vion = 0.0, vtrp = 0.0, rr[3], pos[3], dtr, e[3], P;
|
|||
|
|
int i, j, nv = 0, sat, sys;
|
|||
|
|
int mask[NX - 3] = {0}; /* <20><><EFBFBD><EFBFBD>ϵͳ<CFB5><CDB3><EFBFBD><EFBFBD>, 1:ʹ<><CAB9><EFBFBD>˸<EFBFBD>ϵͳ 0:û<><C3BB>ʹ<EFBFBD>ø<EFBFBD>ϵͳ */
|
|||
|
|
|
|||
|
|
trace(3, "resprng : n=%d\n", n);
|
|||
|
|
|
|||
|
|
// 1<><31><EFBFBD><EFBFBD>֮ǰ<D6AE>õ<EFBFBD><C3B5>Ķ<EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>Ϣ<EFBFBD><CFA2>ֵ<EFBFBD><D6B5> rr <20><> dtr <20><><EFBFBD>飬<EFBFBD>Խ<EFBFBD><D4BD>й<EFBFBD><D0B9>ڵ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>α<EFBFBD><CEB1><EFBFBD>в<EFBFBD><D0B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؼ<EFBFBD><D8BC><EFBFBD>
|
|||
|
|
// X = [x,y,z,dtr,GPS-GAL,GPS-GLO,GPS-BDS,GPS-IRN]
|
|||
|
|
for (i = 0; i < 3; i++)
|
|||
|
|
rr[i] = x[i];
|
|||
|
|
dtr = x[3];
|
|||
|
|
|
|||
|
|
/* 2<><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD> ecef2pos <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD>еõ<D0B5><C3B5><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>Ϣ<EFBFBD><CFA2> ECEF ת<><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ */
|
|||
|
|
ecef2pos(rr, pos);
|
|||
|
|
trace(3, "rescode: rr=%.3f %.3f %.3f\n", rr[0], rr[1], rr[2]);
|
|||
|
|
|
|||
|
|
for (i = *ns = 0; i < n && i < MAXOBS; i++) /* <20><>ÿһ<C3BF><D2BB><EFBFBD>۲<EFBFBD><DBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѭ<EFBFBD><D1AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
|||
|
|
{
|
|||
|
|
// 3<><33><EFBFBD><EFBFBD> vsat<61><74>azel <20><> resp <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 0<><30><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>ζ<EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD><C7B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD><CFA2><EFBFBD>ᷢ<EFBFBD><E1B7A2><EFBFBD>仯
|
|||
|
|
vsat[i] = 0;
|
|||
|
|
azel[i * 2] = azel[1 + i * 2] = resp[i] = 0.0;
|
|||
|
|
time = obs[i].time;
|
|||
|
|
sat = obs[i].sat;
|
|||
|
|
// 4<><34><EFBFBD><EFBFBD><EFBFBD><EFBFBD> satsys <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֤<EFBFBD><D6A4><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD>ϵͳ
|
|||
|
|
if (!(sys = satsys(sat, NULL)))
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
/* reject duplicated observation data
|
|||
|
|
5<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ظ<EFBFBD><EFBFBD>Ĺ۲<EFBFBD><EFBFBD><EFBFBD> */
|
|||
|
|
if (i < n - 1 && i < MAXOBS - 1 && sat == obs[i + 1].sat)
|
|||
|
|
{
|
|||
|
|
trace(2, "duplicated obs data %s sat=%d\n", time_str(time, 3), sat);
|
|||
|
|
i++;
|
|||
|
|
continue;
|
|||
|
|
}
|
|||
|
|
/* 6<><36><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD>ѡ<EFBFBD><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD>λʱ<CEBB>ų<EFBFBD><C5B3><EFBFBD>Щ<EFBFBD><D0A9><EFBFBD><EFBFBD>ϵͳ<CFB5><CDB3><EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD> satexclude <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɵ<EFBFBD>*/
|
|||
|
|
if (satexclude(sat, vare[i], svh[i], opt))
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
/* geometric distance and elevation mask*/
|
|||
|
|
/*7<><37><EFBFBD><EFBFBD><EFBFBD><EFBFBD> geodist <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǻ͵<C7BA>ǰ<EFBFBD><C7B0><EFBFBD>ջ<EFBFBD>λ<EFBFBD><CEBB>֮<EFBFBD><D6AE><EFBFBD>ļ<EFBFBD><C4BC>ξ<EFBFBD><CEBE><EFBFBD> r <20>ͽ<EFBFBD><CDBD>ջ<EFBFBD><D5BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD>Ĺ۲<C4B9>ʸ<EFBFBD><CAB8><EFBFBD><EFBFBD>
|
|||
|
|
Ȼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>鼸<EFBFBD>ξ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD> >0<EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>תӰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD>SagnacЧӦ<EFBFBD><EFBFBD>*/
|
|||
|
|
if ((r = geodist(rs + i * 6, rr, e)) <= 0.0)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
// 8<><38><EFBFBD><EFBFBD><EFBFBD><EFBFBD> satazel <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڽ<EFBFBD><DABD>ջ<EFBFBD>λ<EFBFBD>ô<EFBFBD><C3B4><EFBFBD>վ<EFBFBD><D5BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD>ǵķ<C7B5>λ<EFBFBD>Ǻ<EFBFBD><C7BA><EFBFBD><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD><C7B5>ڽض<DABD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݡ<EFBFBD>
|
|||
|
|
if (satazel(pos, e, azel + i * 2) < opt->elmin)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
if (iter > 0)
|
|||
|
|
{
|
|||
|
|
/* test SNR mask TODO: ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>RTK<54>Ĺ<EFBFBD><C4B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD>»<EFBFBD>վ<EFBFBD><D5BE><EFBFBD>㶨λ<E3B6A8><CEBB>ʱ<EFBFBD><CAB1>ҲҪ<D2B2><D2AA><EFBFBD>˼<EFBFBD><CBBC>ߵ<EFBFBD><DFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><CBB5>㶨λ<E3B6A8><EFBFBD> */
|
|||
|
|
if (!snrmask(obs + i, azel + i * 2, opt))
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
/* ionospheric correction
|
|||
|
|
9<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> ionocorr <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ I (m)<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õĵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>ǽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> L1 <EFBFBD>ź<EFBFBD><EFBFBD>ϵģ<EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>ʱ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>Ƶ<EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>һ<EFBFBD><EFBFBD>Ƶ<EFBFBD>ʵIJ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> L1 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĺ<EFBFBD>ϵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><EFBFBD>ĵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
|
|||
|
|
if (!ionocorr(time, nav, sat, pos, azel + i * 2, opt->ionoopt, &dion, &vion))
|
|||
|
|
{
|
|||
|
|
continue;
|
|||
|
|
}
|
|||
|
|
if ((freq = sat2freq(sat, obs[i].code[0], nav)) == 0.0)
|
|||
|
|
continue;
|
|||
|
|
dion *= SQR(FREQL1 / freq);
|
|||
|
|
vion *= SQR(FREQL1 / freq);
|
|||
|
|
|
|||
|
|
/* tropospheric correction
|
|||
|
|
10<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> tropcorr <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ T (m<EFBFBD><EFBFBD>*/
|
|||
|
|
if (!tropcorr(time, nav, pos, azel + i * 2, opt->tropopt, &dtrp, &vtrp))
|
|||
|
|
{
|
|||
|
|
continue;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
/* psendorange with code bias correction
|
|||
|
|
11<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> prange <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>DCBУ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>α<EFBFBD><EFBFBD>ֵ <EFBFBD><EFBFBD>*/
|
|||
|
|
if ((P = prange(obs + i, nav, opt, &vmeas)) == 0.0)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
/* pseudorange residual
|
|||
|
|
12<EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD> ? ( r + d t r ? c ? d t s + I + T ) <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>в<EFBFBD>*/
|
|||
|
|
v[nv] = P - (r + dtr - CLIGHT * dts[i * 2] + dion + dtrp);
|
|||
|
|
|
|||
|
|
trace(3, "sat=%2d v=%.3f P=%.3f r=%.3f dtr=%.6f dts=%.6f dion=%.3f dtrp=%.3f\n",
|
|||
|
|
obs[i].sat, v[nv], P, r, dtr, dts[i], dion, dtrp);
|
|||
|
|
|
|||
|
|
/* design matrix
|
|||
|
|
13<EFBFBD><EFBFBD><EFBFBD><EFBFBD>װ<EFBFBD><EFBFBD><EFBFBD>ξ<EFBFBD><EFBFBD><EFBFBD> H <EFBFBD><EFBFBD>ǰ 3 <EFBFBD><EFBFBD>Ϊ 7<EFBFBD>м<EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ķ<EFBFBD><EFBFBD><EFBFBD> 4 <EFBFBD><EFBFBD>Ϊ 1<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ 0*/
|
|||
|
|
for (j = 0; j < NX; j++)
|
|||
|
|
{
|
|||
|
|
H[j + nv * NX] = j < 3 ? -e[j] : (j == 3 ? 1.0 : 0.0);
|
|||
|
|
}
|
|||
|
|
/* time system offset and receiver bias correction
|
|||
|
|
14<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬϵͳ<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD>GLO<EFBFBD><EFBFBD>GAL<EFBFBD><EFBFBD>CMP<EFBFBD><EFBFBD>֮<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>ƫ<EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD> H */
|
|||
|
|
if (sys == SYS_GLO)
|
|||
|
|
{
|
|||
|
|
v[nv] -= x[4];
|
|||
|
|
H[4 + nv * NX] = 1.0;
|
|||
|
|
mask[1] = 1;
|
|||
|
|
}
|
|||
|
|
else if (sys == SYS_GAL)
|
|||
|
|
{
|
|||
|
|
v[nv] -= x[5];
|
|||
|
|
H[5 + nv * NX] = 1.0;
|
|||
|
|
mask[2] = 1;
|
|||
|
|
}
|
|||
|
|
else if (sys == SYS_CMP)
|
|||
|
|
{
|
|||
|
|
v[nv] -= x[6];
|
|||
|
|
H[6 + nv * NX] = 1.0;
|
|||
|
|
mask[3] = 1;
|
|||
|
|
}
|
|||
|
|
// else if (sys == SYS_IRN)
|
|||
|
|
//{
|
|||
|
|
// v[nv] -= x[7];
|
|||
|
|
// H[7 + nv * NX] = 1.0;
|
|||
|
|
// mask[4] = 1;
|
|||
|
|
// }
|
|||
|
|
else
|
|||
|
|
mask[0] = 1;
|
|||
|
|
// 15<31><35><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>붨λ<EBB6A8><CEBB><EFBFBD><EFBFBD><EFBFBD>ǵĶ<C7B5>λ<EFBFBD><CEBB>Ч<EFBFBD>Ա<EFBFBD>־<EFBFBD><D6BE>Ϊ 1<><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD>ǵ<EFBFBD>α<EFBFBD><CEB1><EFBFBD>вֵ<EEB8B3><D6B5><EFBFBD><EFBFBD><EFBFBD>붨λ<EBB6A8><CEBB><EFBFBD><EFBFBD><EFBFBD>Ǹ<EFBFBD><C7B8><EFBFBD> ns <20><> 1
|
|||
|
|
vsat[i] = 1;
|
|||
|
|
resp[i] = v[nv];
|
|||
|
|
(*ns)++;
|
|||
|
|
|
|||
|
|
/* variance of pseudorange error
|
|||
|
|
16<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> varerr <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>ĵ<EFBFBD><EFBFBD><EFBFBD>ϵͳ<EFBFBD><EFBFBD><EFBFBD>Ȼ<EFBFBD><EFBFBD><EFBFBD>ۼӼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(URE<EFBFBD><EFBFBD>*/
|
|||
|
|
var[nv++] = varerr(opt, &ssat[i], &obs[i], azel[1 + i * 2], sys) + vare[i] + vmeas + vion + vtrp;
|
|||
|
|
trace(4, "sat=%2d azel=%5.1f %4.1f res=%7.3f sig=%5.3f\n", obs[i].sat,
|
|||
|
|
azel[i * 2] * R2D, azel[1 + i * 2] * R2D, resp[i], sqrt(var[nv - 1]));
|
|||
|
|
}
|
|||
|
|
/* constraint to avoid rank-deficient
|
|||
|
|
17<EFBFBD><EFBFBD>Ϊ<EFBFBD>˷<EFBFBD>ֹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><EFBFBD><EFBFBD> H <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
|
|||
|
|
for (i = 0; i < NX - 3; i++)
|
|||
|
|
{
|
|||
|
|
if (mask[i])
|
|||
|
|
continue;
|
|||
|
|
v[nv] = 0.0;
|
|||
|
|
for (j = 0; j < NX; j++)
|
|||
|
|
H[j + nv * NX] = j == i + 3 ? 1.0 : 0.0;
|
|||
|
|
var[nv++] = 0.01;
|
|||
|
|
}
|
|||
|
|
return nv;
|
|||
|
|
}
|
|||
|
|
static int rescode_weak(int iter, const obsd_t *obs, int n, const double *rs,
|
|||
|
|
const double *dts, const double *vare, const int *svh,
|
|||
|
|
const nav_t *nav, const double *x, const prcopt_t *opt,
|
|||
|
|
const ssat_t *ssat, double *v, double *H, double *var,
|
|||
|
|
double *azel, int *vsat, double *resp, int *ns)
|
|||
|
|
{
|
|||
|
|
gtime_t time;
|
|||
|
|
double r, freq, dion = 0.0, dtrp = 0.0, vmeas, vion = 0.0, vtrp = 0.0, rr[3], pos[3], dtr, e[3], P;
|
|||
|
|
int i, j, nv = 0, sat, sys;
|
|||
|
|
int mask[NX - 3] = {0}; /* <20><><EFBFBD><EFBFBD>ϵͳ<CFB5><CDB3><EFBFBD><EFBFBD>, 1:ʹ<><CAB9><EFBFBD>˸<EFBFBD>ϵͳ 0:û<><C3BB>ʹ<EFBFBD>ø<EFBFBD>ϵͳ */
|
|||
|
|
|
|||
|
|
trace(3, "resprng : n=%d\n", n);
|
|||
|
|
|
|||
|
|
// 1<><31><EFBFBD><EFBFBD>֮ǰ<D6AE>õ<EFBFBD><C3B5>Ķ<EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>Ϣ<EFBFBD><CFA2>ֵ<EFBFBD><D6B5> rr <20><> dtr <20><><EFBFBD>飬<EFBFBD>Խ<EFBFBD><D4BD>й<EFBFBD><D0B9>ڵ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>α<EFBFBD><CEB1><EFBFBD>в<EFBFBD><D0B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؼ<EFBFBD><D8BC><EFBFBD>
|
|||
|
|
// X = [x,y,z,dtr,GPS-GAL,GPS-GLO,GPS-BDS,GPS-IRN]
|
|||
|
|
for (i = 0; i < 3; i++)
|
|||
|
|
rr[i] = x[i];
|
|||
|
|
dtr = x[3];
|
|||
|
|
|
|||
|
|
/* 2<><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD> ecef2pos <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD>еõ<D0B5><C3B5><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>Ϣ<EFBFBD><CFA2> ECEF ת<><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ */
|
|||
|
|
ecef2pos(rr, pos);
|
|||
|
|
trace(3, "rescode: rr=%.3f %.3f %.3f\n", rr[0], rr[1], rr[2]);
|
|||
|
|
|
|||
|
|
for (i = *ns = 0; i < n && i < MAXOBS; i++) /* <20><>ÿһ<C3BF><D2BB><EFBFBD>۲<EFBFBD><DBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѭ<EFBFBD><D1AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
|||
|
|
{
|
|||
|
|
// 3<><33><EFBFBD><EFBFBD> vsat<61><74>azel <20><> resp <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 0<><30><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>ζ<EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD><C7B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD><CFA2><EFBFBD>ᷢ<EFBFBD><E1B7A2><EFBFBD>仯
|
|||
|
|
vsat[i] = 0;
|
|||
|
|
azel[i * 2] = azel[1 + i * 2] = resp[i] = 0.0;
|
|||
|
|
time = obs[i].time;
|
|||
|
|
sat = obs[i].sat;
|
|||
|
|
// 4<><34><EFBFBD><EFBFBD><EFBFBD><EFBFBD> satsys <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֤<EFBFBD><D6A4><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD>ϵͳ
|
|||
|
|
if (!(sys = satsys(sat, NULL)))
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
/* reject duplicated observation data
|
|||
|
|
5<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ظ<EFBFBD><EFBFBD>Ĺ۲<EFBFBD><EFBFBD><EFBFBD> */
|
|||
|
|
if (i < n - 1 && i < MAXOBS - 1 && sat == obs[i + 1].sat)
|
|||
|
|
{
|
|||
|
|
trace(2, "duplicated obs data %s sat=%d\n", time_str(time, 3), sat);
|
|||
|
|
i++;
|
|||
|
|
continue;
|
|||
|
|
}
|
|||
|
|
/* 6<><36><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD>ѡ<EFBFBD><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD>λʱ<CEBB>ų<EFBFBD><C5B3><EFBFBD>Щ<EFBFBD><D0A9><EFBFBD><EFBFBD>ϵͳ<CFB5><CDB3><EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD> satexclude <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɵ<EFBFBD>*/
|
|||
|
|
// if (satexclude(sat, vare[i], svh[i], opt))
|
|||
|
|
// continue;
|
|||
|
|
|
|||
|
|
/* geometric distance and elevation mask*/
|
|||
|
|
/*7<><37><EFBFBD><EFBFBD><EFBFBD><EFBFBD> geodist <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǻ͵<C7BA>ǰ<EFBFBD><C7B0><EFBFBD>ջ<EFBFBD>λ<EFBFBD><CEBB>֮<EFBFBD><D6AE><EFBFBD>ļ<EFBFBD><C4BC>ξ<EFBFBD><CEBE><EFBFBD> r <20>ͽ<EFBFBD><CDBD>ջ<EFBFBD><D5BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD>Ĺ۲<C4B9>ʸ<EFBFBD><CAB8><EFBFBD><EFBFBD>
|
|||
|
|
Ȼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>鼸<EFBFBD>ξ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD> >0<EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>תӰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD>SagnacЧӦ<EFBFBD><EFBFBD>*/
|
|||
|
|
if ((r = geodist(rs + i * 6, rr, e)) <= 0.0)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
// 8<><38><EFBFBD><EFBFBD><EFBFBD><EFBFBD> satazel <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڽ<EFBFBD><DABD>ջ<EFBFBD>λ<EFBFBD>ô<EFBFBD><C3B4><EFBFBD>վ<EFBFBD><D5BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD>ǵķ<C7B5>λ<EFBFBD>Ǻ<EFBFBD><C7BA><EFBFBD><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD><C7B5>ڽض<DABD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݡ<EFBFBD>
|
|||
|
|
if (satazel(pos, e, azel + i * 2) < RESCODE_MIN_EL)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
if (iter > 0)
|
|||
|
|
{
|
|||
|
|
/* test SNR mask TODO: ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>RTK<54>Ĺ<EFBFBD><C4B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD>»<EFBFBD>վ<EFBFBD><D5BE><EFBFBD>㶨λ<E3B6A8><CEBB>ʱ<EFBFBD><CAB1>ҲҪ<D2B2><D2AA><EFBFBD>˼<EFBFBD><CBBC>ߵ<EFBFBD><DFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><CBB5>㶨λ<E3B6A8><EFBFBD> */
|
|||
|
|
|
|||
|
|
if (obs[i].SNR[0] < 30000)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
/* ionospheric correction
|
|||
|
|
9<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> ionocorr <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ I (m)<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õĵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>ǽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> L1 <EFBFBD>ź<EFBFBD><EFBFBD>ϵģ<EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>ʱ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>Ƶ<EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>һ<EFBFBD><EFBFBD>Ƶ<EFBFBD>ʵIJ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> L1 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĺ<EFBFBD>ϵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><EFBFBD>ĵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
|
|||
|
|
if (!ionocorr(time, nav, sat, pos, azel + i * 2, opt->ionoopt, &dion, &vion))
|
|||
|
|
{
|
|||
|
|
continue;
|
|||
|
|
}
|
|||
|
|
if ((freq = sat2freq(sat, obs[i].code[0], nav)) == 0.0)
|
|||
|
|
continue;
|
|||
|
|
dion *= SQR(FREQL1 / freq);
|
|||
|
|
vion *= SQR(FREQL1 / freq);
|
|||
|
|
|
|||
|
|
/* tropospheric correction
|
|||
|
|
10<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> tropcorr <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ T (m<EFBFBD><EFBFBD>*/
|
|||
|
|
if (!tropcorr(time, nav, pos, azel + i * 2, opt->tropopt, &dtrp, &vtrp))
|
|||
|
|
{
|
|||
|
|
continue;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
/* psendorange with code bias correction
|
|||
|
|
11<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> prange <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>DCBУ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>α<EFBFBD><EFBFBD>ֵ <EFBFBD><EFBFBD>*/
|
|||
|
|
if ((P = prange(obs + i, nav, opt, &vmeas)) == 0.0)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
/* pseudorange residual
|
|||
|
|
12<EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD> ? ( r + d t r ? c ? d t s + I + T ) <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>в<EFBFBD>*/
|
|||
|
|
v[nv] = P - (r + dtr - CLIGHT * dts[i * 2] + dion + dtrp);
|
|||
|
|
|
|||
|
|
trace(3, "sat=%2d v=%.3f P=%.3f r=%.3f dtr=%.6f dts=%.6f dion=%.3f dtrp=%.3f\n",
|
|||
|
|
obs[i].sat, v[nv], P, r, dtr, dts[i], dion, dtrp);
|
|||
|
|
|
|||
|
|
/* design matrix
|
|||
|
|
13<EFBFBD><EFBFBD><EFBFBD><EFBFBD>װ<EFBFBD><EFBFBD><EFBFBD>ξ<EFBFBD><EFBFBD><EFBFBD> H <EFBFBD><EFBFBD>ǰ 3 <EFBFBD><EFBFBD>Ϊ 7<EFBFBD>м<EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ķ<EFBFBD><EFBFBD><EFBFBD> 4 <EFBFBD><EFBFBD>Ϊ 1<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ 0*/
|
|||
|
|
for (j = 0; j < NX; j++)
|
|||
|
|
{
|
|||
|
|
H[j + nv * NX] = j < 3 ? -e[j] : (j == 3 ? 1.0 : 0.0);
|
|||
|
|
}
|
|||
|
|
/* time system offset and receiver bias correction
|
|||
|
|
14<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬϵͳ<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD>GLO<EFBFBD><EFBFBD>GAL<EFBFBD><EFBFBD>CMP<EFBFBD><EFBFBD>֮<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>ƫ<EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD> H */
|
|||
|
|
if (sys == SYS_GLO)
|
|||
|
|
{
|
|||
|
|
v[nv] -= x[4];
|
|||
|
|
H[4 + nv * NX] = 1.0;
|
|||
|
|
mask[1] = 1;
|
|||
|
|
}
|
|||
|
|
else if (sys == SYS_GAL)
|
|||
|
|
{
|
|||
|
|
v[nv] -= x[5];
|
|||
|
|
H[5 + nv * NX] = 1.0;
|
|||
|
|
mask[2] = 1;
|
|||
|
|
}
|
|||
|
|
else if (sys == SYS_CMP)
|
|||
|
|
{
|
|||
|
|
v[nv] -= x[6];
|
|||
|
|
H[6 + nv * NX] = 1.0;
|
|||
|
|
mask[3] = 1;
|
|||
|
|
}
|
|||
|
|
// else if (sys == SYS_IRN)
|
|||
|
|
//{
|
|||
|
|
// v[nv] -= x[7];
|
|||
|
|
// H[7 + nv * NX] = 1.0;
|
|||
|
|
// mask[4] = 1;
|
|||
|
|
// }
|
|||
|
|
else
|
|||
|
|
mask[0] = 1;
|
|||
|
|
// 15<31><35><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>붨λ<EBB6A8><CEBB><EFBFBD><EFBFBD><EFBFBD>ǵĶ<C7B5>λ<EFBFBD><CEBB>Ч<EFBFBD>Ա<EFBFBD>־<EFBFBD><D6BE>Ϊ 1<><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD>ǵ<EFBFBD>α<EFBFBD><CEB1><EFBFBD>вֵ<EEB8B3><D6B5><EFBFBD><EFBFBD><EFBFBD>붨λ<EBB6A8><CEBB><EFBFBD><EFBFBD><EFBFBD>Ǹ<EFBFBD><C7B8><EFBFBD> ns <20><> 1
|
|||
|
|
vsat[i] = 1;
|
|||
|
|
resp[i] = v[nv];
|
|||
|
|
(*ns)++;
|
|||
|
|
|
|||
|
|
/* variance of pseudorange error
|
|||
|
|
16<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> varerr <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>ĵ<EFBFBD><EFBFBD><EFBFBD>ϵͳ<EFBFBD><EFBFBD><EFBFBD>Ȼ<EFBFBD><EFBFBD><EFBFBD>ۼӼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(URE<EFBFBD><EFBFBD>*/
|
|||
|
|
var[nv++] = varerr(opt, &ssat[i], &obs[i], azel[1 + i * 2], sys) + vare[i] + vmeas + vion + vtrp;
|
|||
|
|
trace(4, "sat=%2d azel=%5.1f %4.1f res=%7.3f sig=%5.3f\n", obs[i].sat,
|
|||
|
|
azel[i * 2] * R2D, azel[1 + i * 2] * R2D, resp[i], sqrt(var[nv - 1]));
|
|||
|
|
}
|
|||
|
|
/* constraint to avoid rank-deficient
|
|||
|
|
17<EFBFBD><EFBFBD>Ϊ<EFBFBD>˷<EFBFBD>ֹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><EFBFBD><EFBFBD> H <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
|
|||
|
|
for (i = 0; i < NX - 3; i++)
|
|||
|
|
{
|
|||
|
|
if (mask[i])
|
|||
|
|
continue;
|
|||
|
|
v[nv] = 0.0;
|
|||
|
|
for (j = 0; j < NX; j++)
|
|||
|
|
H[j + nv * NX] = j == i + 3 ? 1.0 : 0.0;
|
|||
|
|
var[nv++] = 0.01;
|
|||
|
|
}
|
|||
|
|
return nv;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* validate solution ---------------------------------------------------------*/
|
|||
|
|
static int valsol(const double *azel, const int *vsat, int n,
|
|||
|
|
const prcopt_t *opt, const double *v, int nv, int nx,
|
|||
|
|
char *msg)
|
|||
|
|
{
|
|||
|
|
double azels[MAXOBS * 2], dop[4], vv;
|
|||
|
|
int i, ns;
|
|||
|
|
|
|||
|
|
trace(3, "valsol : n=%d nv=%d\n", n, nv);
|
|||
|
|
|
|||
|
|
/* α<><CEB1><EFBFBD>в<EFBFBD> <20><><EFBFBD><EFBFBD>У<EFBFBD><D0A3> */
|
|||
|
|
vv = dot(v, v, nv);
|
|||
|
|
if (nv > nx && vv > chisqr[nv - nx - 1])
|
|||
|
|
{
|
|||
|
|
sprintf(msg, "Warning: large chi-square error nv=%d vv=%.1f cs=%.1f", nv, vv, chisqr[nv - nx - 1]);
|
|||
|
|
/* return 0; */ /* <20><>ֵ<EFBFBD><D6B5><EFBFBD>õ<EFBFBD>̫<EFBFBD>ϸ<EFBFBD><CFB8><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
|||
|
|
}
|
|||
|
|
/* large GDOP check */
|
|||
|
|
for (i = ns = 0; i < n; i++)
|
|||
|
|
{
|
|||
|
|
if (!vsat[i])
|
|||
|
|
continue;
|
|||
|
|
azels[ns * 2] = azel[i * 2];
|
|||
|
|
azels[1 + ns * 2] = azel[1 + i * 2];
|
|||
|
|
ns++;
|
|||
|
|
}
|
|||
|
|
dops(ns, azels, opt->elmin, dop);
|
|||
|
|
if (dop[0] <= 0.0 || dop[0] > opt->maxgdop)
|
|||
|
|
{
|
|||
|
|
sprintf(msg, "gdop error nv=%d gdop=%.1f", nv, dop[0]);
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
/* estimate receiver position
|
|||
|
|
ͨ<EFBFBD><EFBFBD>α<EFBFBD><EFBFBD>ʵ<EFBFBD>־<EFBFBD><EFBFBD>Զ<EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ջ<EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ú<EFBFBD><EFBFBD>Ӳ˳<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD>ֶ<EFBFBD>λ<EFBFBD><EFBFBD>ÿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>{<EFBFBD><EFBFBD>λ<EFBFBD>ǡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>}<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λʱ<EFBFBD><EFBFBD>Ч<EFBFBD>ԡ<EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>в
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>13<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
obsd_t *obs I <EFBFBD>۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
int n I <EFBFBD>۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
double *rs I <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ú<EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ6*n<EFBFBD><EFBFBD>{x,y,z,vx,vy,vz}(ecef)(m,m/s)
|
|||
|
|
double *dts I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ2*n<EFBFBD><EFBFBD> {bias,drift} (s|s/s)
|
|||
|
|
double *vare I <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ú<EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><EFBFBD><EFBFBD><EFBFBD> (m^2)
|
|||
|
|
int *svh I <EFBFBD><EFBFBD><EFBFBD>ǽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־ (-1:correction not available)
|
|||
|
|
nav_t *nav I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
prcopt_t *opt I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><EFBFBD>
|
|||
|
|
sol_t *sol IO solution
|
|||
|
|
double *azel IO <EFBFBD><EFBFBD>λ<EFBFBD>Ǻ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (rad)
|
|||
|
|
int *vsat IO <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>λʱ<EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD>Ч
|
|||
|
|
double *resp IO <EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>в<EFBFBD> (P-(r+c*dtr-c*dts+I+T))
|
|||
|
|
char *msg O <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
|
|||
|
|
int O 1<EFBFBD><EFBFBD>ʾ<EFBFBD>ɹ<EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
------------------------------------------------*/
|
|||
|
|
static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
|
|||
|
|
const double *vare, const int *svh, const nav_t *nav,
|
|||
|
|
const prcopt_t *opt, const ssat_t *ssat, sol_t *sol, double *azel,
|
|||
|
|
int *vsat, double *resp, char *msg)
|
|||
|
|
{
|
|||
|
|
double x[NX] = {0}, dx[NX], Q[NX * NX], *v, *H, *var, sig;
|
|||
|
|
int i, j, k, info, stat, nv, ns;
|
|||
|
|
|
|||
|
|
trace(3, "----> estpos : n=%d\n", n);
|
|||
|
|
|
|||
|
|
v = mat(n + 4, 1);
|
|||
|
|
H = mat(NX, n + 4);
|
|||
|
|
var = mat(n + 4, 1);
|
|||
|
|
|
|||
|
|
// 1<><31><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD> sol->rr <20><>ǰ 3 <20><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>Ϣ<EFBFBD><CFA2>ECEF<45><46><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5> x <20><><EFBFBD>顣
|
|||
|
|
/*<2A><><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>һ<EFBFBD>ζ<EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> sol Ϊ<>գ<EFBFBD><D5A3><EFBFBD> x <20><>ֵΪ 0<><30>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮ǰ<EFBFBD>й<EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD> 1 <EFBFBD>в<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Խ<EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD>Ԫ<EFBFBD>Ķ<EFBFBD>λֵ<EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>λ<EFBFBD>ij<EFBFBD>ʼֵ*/
|
|||
|
|
for (i = 0; i < 3; i++)
|
|||
|
|
x[i] = sol->rr[i];
|
|||
|
|
|
|||
|
|
for (i = 0; i < MAXITR; i++)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
/* pseudorange residuals (m)
|
|||
|
|
2<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD><EFBFBD><EFBFBD> rescode <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㵱ǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>в<EFBFBD> v<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ξ<EFBFBD><EFBFBD><EFBFBD> H<EFBFBD><EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>в<EFBFBD><EFBFBD>ķ<EFBFBD><EFBFBD><EFBFBD> var<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵķ<EFBFBD>λ<EFBFBD>Ǻ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> azel<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD>λʱ<EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD> vsat<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>в<EFBFBD> resp<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>붨λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǹ<EFBFBD><EFBFBD><EFBFBD> ns <EFBFBD>ͷ<EFBFBD><EFBFBD≯<EFBFBD><EFBFBD><EFBFBD> nv<EFBFBD><EFBFBD>*/
|
|||
|
|
nv = rescode(i, obs, n, rs, dts, vare, svh, nav, x, opt, ssat, v, H, var, azel, vsat, resp, &ns);
|
|||
|
|
|
|||
|
|
// 3<><33>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>з<EFBFBD><D0B7>̵ĸ<CCB5><C4B8><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>δ֪<CEB4><D6AA><EFBFBD>ĸ<EFBFBD><C4B8><EFBFBD>
|
|||
|
|
if (nv < NX)
|
|||
|
|
{
|
|||
|
|
nv = rescode_weak(i, obs, n, rs, dts, vare, svh, nav, x, opt, ssat, v, H, var, azel, vsat, resp, &ns);
|
|||
|
|
if (nv < NX)
|
|||
|
|
{
|
|||
|
|
sprintf(msg, "lack of valid sats ns=%d", nv);
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* 4<><34><EFBFBD><EFBFBD>α<EFBFBD><CEB1><EFBFBD>в<EFBFBD><D0B2>ı<EFBFBD><EFBFBD><D7BC><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD><EFBFBD>ΪȨ<CEAA>أ<EFBFBD><D8A3><EFBFBD> H <20><> v<>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>Ȩ<EFBFBD>ضԽ<D8B6><D4BD>õ<F3A3ACB5><C3B5><EFBFBD>Ȩ֮<C8A8><D6AE><EFBFBD><EFBFBD> H<><48> v*/
|
|||
|
|
for (j = 0; j < nv; j++)
|
|||
|
|
{
|
|||
|
|
sig = sqrt(var[j]);
|
|||
|
|
v[j] /= sig;
|
|||
|
|
for (k = 0; k < NX; k++)
|
|||
|
|
H[k + j * NX] /= sig;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* least square estimation
|
|||
|
|
5<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> lsq <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> dx=(HH^T)^{-1}Hv
|
|||
|
|
<EFBFBD><EFBFBD> Q=(HH^T)^{-1}<EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><EFBFBD><EFBFBD>ǰ x <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> dx <EFBFBD>Ͷ<EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>Ȩϵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD> Q
|
|||
|
|
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ڼ<EFBFBD>Ȩ<EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD>ˣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ȩ<EFBFBD><EFBFBD>ֵ<EFBFBD>ǶԽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڼ<EFBFBD><EFBFBD>費ͬ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><EFBFBD><EFBFBD>DZ˴˶<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵġ<EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ﶼ<EFBFBD>ǰ<EFBFBD>Ȩ<EFBFBD>ؾ<EFBFBD><EFBFBD><EFBFBD> W <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̵Ľ<EFBFBD><EFBFBD>ı<EFBFBD><EFBFBD><EFBFBD>ʽ<EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>Ӷ<EFBFBD> H <EFBFBD><EFBFBD> v <EFBFBD>ֱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ȩ<EFBFBD>ضԽ<EFBFBD><EFBFBD>õ<EFBFBD><EFBFBD><EFBFBD>Ȩ֮<EFBFBD><EFBFBD><EFBFBD><EFBFBD> H <EFBFBD><EFBFBD> v<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD>ʽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>м<EFBFBD>Ȩһ<EFBFBD><EFBFBD>*/
|
|||
|
|
if ((info = lsq(H, v, NX, nv, dx, Q)))
|
|||
|
|
{
|
|||
|
|
sprintf(msg, "lsq error info=%d", info);
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 6<><36><EFBFBD><EFBFBD> 5 <20><><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD> dx <20><><EFBFBD>뵽<EFBFBD><EBB5BD>ǰ x ֵ<>У<EFBFBD><D0A3>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD><EFBFBD> x ֵ
|
|||
|
|
for (j = 0; j < NX; j++)
|
|||
|
|
{
|
|||
|
|
x[j] += dx[j];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*7<><37><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 5 <20><><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><DEB8><EFBFBD> dx С<>ڽض<DABD><D8B6><EFBFBD><EFBFBD><EFBFBD>(Ŀǰ<C4BF><C7B0>10^{-4} )<29><>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> 6 <EFBFBD>еõ<EFBFBD><EFBFBD><EFBFBD> x ֵ<EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD>յĶ<EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> sol <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD>֮<EFBFBD><EFBFBD><EFBFBD>ٵ<EFBFBD><EFBFBD><EFBFBD> valsol <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ<EFBFBD>ϵ<EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD>
|
|||
|
|
(α<EFBFBD><EFBFBD><EFBFBD>в<EFBFBD>С<EFBFBD><EFBFBD>ij<EFBFBD><EFBFBD> <EFBFBD><EFBFBD>^2ֵ<EFBFBD><EFBFBD> GDOP С<EFBFBD><EFBFBD>ij<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD>ο<EFBFBD> RTKLIB Manual P162, E.6.33, E.6.34)<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD>ѭ<EFBFBD><EFBFBD>*/
|
|||
|
|
if (norm(dx, NX) < 1E-4)
|
|||
|
|
{
|
|||
|
|
sol->type = 0; /* type (0:xyz-ecef,1:enu-baseline) */
|
|||
|
|
sol->time = timeadd(obs[0].time, -x[3] / CLIGHT); /* sol->time <20>д洢<D0B4><E6B4A2><EFBFBD>Ǽ<EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD>ջ<EFBFBD><D5BB>Ӳ<EFBFBD><D3B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>źŹ۲<C5B9>ʱ<EFBFBD><CAB1> */
|
|||
|
|
//<2F>ⷽ<EFBFBD><E2B7BD>ʱ<EFBFBD><CAB1> dtr <20><>λ<EFBFBD><CEBB> m<><6D><EFBFBD>dz<EFBFBD><C7B3><EFBFBD><EFBFBD>˹<EFBFBD><CBB9><EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> sol->dtr ʱ<>ٳ<EFBFBD><D9B3>Թ<EFBFBD><D4B9><EFBFBD>
|
|||
|
|
sol->dtr[0] = x[3] / CLIGHT; /* receiver clock bias (s) */
|
|||
|
|
sol->dtr[1] = x[4] / CLIGHT; /* GLO-GPS time offset (s) */
|
|||
|
|
sol->dtr[2] = x[5] / CLIGHT; /* GAL-GPS time offset (s) */
|
|||
|
|
sol->dtr[3] = x[6] / CLIGHT; /* BDS-GPS time offset (s) */
|
|||
|
|
// sol->dtr[4] = x[7] / CLIGHT; /* IRN-GPS time offset (s) */
|
|||
|
|
for (j = 0; j < 6; j++)
|
|||
|
|
sol->rr[j] = j < 3 ? x[j] : 0.0;
|
|||
|
|
for (j = 0; j < 3; j++)
|
|||
|
|
sol->qr[j] = (float)Q[j + j * NX];
|
|||
|
|
sol->qr[3] = (float)Q[1]; /* cov xy */
|
|||
|
|
sol->qr[4] = (float)Q[2 + NX]; /* cov yz */
|
|||
|
|
sol->qr[5] = (float)Q[2]; /* cov zx */
|
|||
|
|
sol->ns = (uint8_t)ns;
|
|||
|
|
sol->age = sol->ratio = 0.0;
|
|||
|
|
|
|||
|
|
/* validate solution */
|
|||
|
|
if ((stat = valsol(azel, vsat, n, opt, v, nv, NX, msg)))
|
|||
|
|
{
|
|||
|
|
sol->stat = opt->sateph == EPHOPT_SBAS ? SOLQ_SBAS : SOLQ_SINGLE;
|
|||
|
|
}
|
|||
|
|
free(v);
|
|||
|
|
free(H);
|
|||
|
|
free(var);
|
|||
|
|
return stat;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 8<><38><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˹涨<CBB9><E6B6A8>ѭ<EFBFBD><D1AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɢ<EFBFBD><C9A2>Ϣ<EFBFBD><EFBFBD><F3A3ACB7><EFBFBD> 0
|
|||
|
|
if (i >= MAXITR)
|
|||
|
|
sprintf(msg, "iteration divergent i=%d", i);
|
|||
|
|
|
|||
|
|
free(v);
|
|||
|
|
free(H);
|
|||
|
|
free(var);
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
/* RAIM FDE (failure detection and exclution)
|
|||
|
|
ʹ<EFBFBD><EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>в<EFBFBD><EFBFBD>о<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><EFBFBD>Ķ<EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>н<EFBFBD><EFBFBD>ջ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>Լ<EFBFBD><EFBFBD>⣨RAIM<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Dz<EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD>ʣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD>㣬ѡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>в<EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD>ս<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֻ<EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD>۲<EFBFBD>ֵ<EFBFBD>Ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD><EFBFBD>ų<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ų<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ˡ<EFBFBD>
|
|||
|
|
ע<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֻ<EFBFBD><EFBFBD><EFBFBD>ڶԶ<EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽ<EFBFBD><EFBFBD>м<EFBFBD><EFBFBD>⡣
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>13<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
obsd_t *obs I <EFBFBD>۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
int n I <EFBFBD>۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
double *rs I <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ú<EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ6*n<EFBFBD><EFBFBD>{x,y,z,vx,vy,vz}(ecef)(m,m/s)
|
|||
|
|
double *dts I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ2*n<EFBFBD><EFBFBD> {bias,drift} (s|s/s)
|
|||
|
|
double *vare I <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ú<EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><EFBFBD><EFBFBD><EFBFBD> (m^2)
|
|||
|
|
int *svh I <EFBFBD><EFBFBD><EFBFBD>ǽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־ (-1:correction not available)
|
|||
|
|
nav_t *nav I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
prcopt_t *opt I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><EFBFBD>
|
|||
|
|
sol_t *sol IO solution
|
|||
|
|
double *azel IO <EFBFBD><EFBFBD>λ<EFBFBD>Ǻ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (rad)
|
|||
|
|
int *vsat IO <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>λʱ<EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD>Ч
|
|||
|
|
double *resp IO <EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>в<EFBFBD> (P-(r+c*dtr-c*dts+I+T))
|
|||
|
|
char *msg O <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
|
|||
|
|
int O (1:ok,0:error)
|
|||
|
|
-------------------------------*/
|
|||
|
|
static int raim_fde(const obsd_t *obs, int n, const double *rs,
|
|||
|
|
const double *dts, const double *vare, const int *svh,
|
|||
|
|
const nav_t *nav, const prcopt_t *opt, const ssat_t *ssat,
|
|||
|
|
sol_t *sol, double *azel, int *vsat, double *resp, char *msg)
|
|||
|
|
{
|
|||
|
|
obsd_t *obs_e;
|
|||
|
|
sol_t sol_e = {{0}};
|
|||
|
|
char tstr[32], name[16], msg_e[128];
|
|||
|
|
double *rs_e, *dts_e, *vare_e, *azel_e, *resp_e, rms_e, rms = 100.0;
|
|||
|
|
int i, j, k, nvsat, stat = 0, *svh_e, *vsat_e, sat = 0;
|
|||
|
|
|
|||
|
|
trace(3, "----> raim_fde: %s n=%2d\n", time_str(obs[0].time, 0), n);
|
|||
|
|
|
|||
|
|
if (!(obs_e = (obsd_t *)malloc(sizeof(obsd_t) * n)))
|
|||
|
|
return 0;
|
|||
|
|
rs_e = mat(6, n);
|
|||
|
|
dts_e = mat(2, n);
|
|||
|
|
vare_e = mat(1, n);
|
|||
|
|
azel_e = zeros(2, n);
|
|||
|
|
svh_e = imat(1, n);
|
|||
|
|
vsat_e = imat(1, n);
|
|||
|
|
resp_e = mat(1, n);
|
|||
|
|
|
|||
|
|
/*Դ<><D4B4><EFBFBD><EFBFBD><EFBFBD>кܶ<D0BA><DCB6><EFBFBD><EFBFBD><EFBFBD> i<><69>j<EFBFBD><6A>k<EFBFBD><6B>ѭ<EFBFBD><D1AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD>i<EFBFBD><69>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĵ<EFBFBD>ѭ<EFBFBD><D1AD><EFBFBD><EFBFBD>ÿ<EFBFBD>ν<EFBFBD><CEBD><EFBFBD><EFBFBD><EFBFBD> i<><69><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8> if (j==i) continueʵ<65>ֵģ<D6B5>
|
|||
|
|
j<EFBFBD><EFBFBD>ʾʣ<EFBFBD><EFBFBD>ʹ<EFBFBD>õ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>ѭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD>ν<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD><EFBFBD><EFBFBD>ݵĸ<EFBFBD>ֵ<EFBFBD><EFBFBD>k<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD>붨λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>ѭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> jһ<EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD>*/
|
|||
|
|
|
|||
|
|
// 1<><31><EFBFBD><EFBFBD>ѭ<EFBFBD><D1AD><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> i <20><><EFBFBD><EFBFBD><EFBFBD>ǡ<EFBFBD>
|
|||
|
|
for (i = 0; i < n; i++)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
/* satellite exclution */
|
|||
|
|
for (j = k = 0; j < n; j++)
|
|||
|
|
{
|
|||
|
|
if (j == i)
|
|||
|
|
continue;
|
|||
|
|
obs_e[k] = obs[j];
|
|||
|
|
matcpy(rs_e + 6 * k, rs + 6 * j, 6, 1);
|
|||
|
|
matcpy(dts_e + 2 * k, dts + 2 * j, 2, 1);
|
|||
|
|
vare_e[k] = vare[j];
|
|||
|
|
svh_e[k++] = svh[j];
|
|||
|
|
}
|
|||
|
|
/* estimate receiver position without a satellite
|
|||
|
|
2<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> i <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǻ<EFBFBD>ʣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݸ<EFBFBD><EFBFBD>Ƶ<EFBFBD>һ<EFBFBD>𣬵<EFBFBD><EFBFBD><EFBFBD> estpos <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD>ʣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǽ<EFBFBD><EFBFBD>ж<EFBFBD>λ<EFBFBD>Ķ<EFBFBD>λֵ<EFBFBD><EFBFBD>*/
|
|||
|
|
if (!estpos(obs_e, n - 1, rs_e, dts_e, vare_e, svh_e, nav, opt, ssat, &sol_e, azel_e,
|
|||
|
|
vsat_e, resp_e, msg_e))
|
|||
|
|
{
|
|||
|
|
trace(3, "raim_fde: exsat=%2d (%s)\n", obs[i].sat, msg);
|
|||
|
|
continue;
|
|||
|
|
}
|
|||
|
|
/*3<><33><EFBFBD>ۼ<EFBFBD>ʹ<EFBFBD>õ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ʵ<EFBFBD>ֶ<EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>α<EFBFBD><CEB1><EFBFBD>в<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> nvsat<5<EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><EFBFBD><EFBFBD>٣<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> RAIM_FDE <EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
|
|||
|
|
for (j = nvsat = 0, rms_e = 0.0; j < n - 1; j++)
|
|||
|
|
{
|
|||
|
|
if (!vsat_e[j])
|
|||
|
|
continue;
|
|||
|
|
rms_e += SQR(resp_e[j]);
|
|||
|
|
nvsat++;
|
|||
|
|
}
|
|||
|
|
if (nvsat < 5)
|
|||
|
|
{
|
|||
|
|
trace(3, "raim_fde: exsat=%2d lack of satellites nvsat=%2d\n",
|
|||
|
|
obs[i].sat, nvsat);
|
|||
|
|
continue;
|
|||
|
|
}
|
|||
|
|
/*4<><34><EFBFBD><EFBFBD><EFBFBD><EFBFBD>α<EFBFBD><CEB1><EFBFBD>в<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>͵ı<CDB5><EFBFBD><EFBFBD><EEA3AC><EFBFBD><EFBFBD>С<EFBFBD><D0A1> rms<6D><73><EFBFBD><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> stat <20><>Ϊ 1<><31>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>¸<EFBFBD><EFBFBD><EFBFBD> sol<EFBFBD><EFBFBD>azel<EFBFBD><EFBFBD>vsat(<EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD>Ϊ0)<EFBFBD><EFBFBD>resp<EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD> rms_e<EFBFBD><EFBFBD><EFBFBD>µ<EFBFBD> rms <EFBFBD>С<EFBFBD>*/
|
|||
|
|
rms_e = sqrt(rms_e / nvsat);
|
|||
|
|
|
|||
|
|
trace(3, "raim_fde: exsat=%2d rms=%8.3f\n", obs[i].sat, rms_e);
|
|||
|
|
|
|||
|
|
if (rms_e > rms)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
/* save result */
|
|||
|
|
for (j = k = 0; j < n; j++)
|
|||
|
|
{
|
|||
|
|
if (j == i)
|
|||
|
|
continue;
|
|||
|
|
matcpy(azel + 2 * j, azel_e + 2 * k, 2, 1);
|
|||
|
|
vsat[j] = vsat_e[k];
|
|||
|
|
resp[j] = resp_e[k++];
|
|||
|
|
}
|
|||
|
|
stat = 1;
|
|||
|
|
sol_e.eventime = sol->eventime;
|
|||
|
|
*sol = sol_e;
|
|||
|
|
sat = obs[i].sat;
|
|||
|
|
rms = rms_e;
|
|||
|
|
vsat[i] = 0;
|
|||
|
|
strcpy(msg, msg_e);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*5<><35><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD><C7A3>ظ<EFBFBD> 2-4<><34><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD>ܶ<EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>£<EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>в<EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õĽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD>յĽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
|
|||
|
|
|
|||
|
|
// 6<><36><EFBFBD><EFBFBD><EFBFBD><EFBFBD> stat<61><74>Ϊ 0<><30><EFBFBD><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>и<EFBFBD><D0B8>õĽ<C3B5><C4BD><EFBFBD><EFBFBD>֣<EFBFBD><D6A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD><CFA2>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if (stat)
|
|||
|
|
{
|
|||
|
|
time2str(obs[0].time, tstr, 2);
|
|||
|
|
satno2id(sat, name);
|
|||
|
|
trace(2, "%s: %s excluded by raim\n", tstr + 11, name);
|
|||
|
|
}
|
|||
|
|
free(obs_e);
|
|||
|
|
free(rs_e);
|
|||
|
|
free(dts_e);
|
|||
|
|
free(vare_e);
|
|||
|
|
free(azel_e);
|
|||
|
|
free(svh_e);
|
|||
|
|
free(vsat_e);
|
|||
|
|
free(resp_e);
|
|||
|
|
return stat;
|
|||
|
|
}
|
|||
|
|
/* range rate residuals
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>㶨<EFBFBD>ٷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ߵļ<EFBFBD><EFBFBD>ξ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҷ˵<EFBFBD><EFBFBD>ٶȲ<EFBFBD><EFBFBD>࣬<EFBFBD><EFBFBD><EFBFBD>ض<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>ʹ<EFBFBD>õ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>11<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
obsd_t *obs I <EFBFBD>۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
int n I <EFBFBD>۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
double *rs I <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ú<EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ6*n<EFBFBD><EFBFBD>{x,y,z,vx,vy,vz}(ecef)(m,m/s)
|
|||
|
|
double *dts I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ2*n<EFBFBD><EFBFBD> {bias,drift} (s|s/s)
|
|||
|
|
nav_t *nav I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
double *rr I <EFBFBD><EFBFBD><EFBFBD>ջ<EFBFBD>λ<EFBFBD>ú<EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ6<EFBFBD><EFBFBD>{x,y,z,vx,vy,vz}(ecef)(m,m/s)
|
|||
|
|
double *x I <EFBFBD><EFBFBD><EFBFBD>ε<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ֮ǰ<EFBFBD>Ķ<EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ4<EFBFBD><EFBFBD>{vx,vy,vz,drift}
|
|||
|
|
double *azel IO <EFBFBD><EFBFBD>λ<EFBFBD>Ǻ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (rad)
|
|||
|
|
int *vsat I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD>Ч
|
|||
|
|
double *v O <EFBFBD><EFBFBD><EFBFBD>ٷ<EFBFBD><EFBFBD>̵<EFBFBD><EFBFBD>Ҷ˲<EFBFBD><EFBFBD>֣<EFBFBD><EFBFBD>ٶȲв<EFBFBD>
|
|||
|
|
double *H O <EFBFBD><EFBFBD><EFBFBD>ٷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>еļ<EFBFBD><EFBFBD>ξ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
|
|||
|
|
int O <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>ʹ<EFBFBD>õ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ
|
|||
|
|
------------------------------------------------------*/
|
|||
|
|
static int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
|
|||
|
|
const nav_t *nav, const double *rr, const double *x,
|
|||
|
|
const double *azel, const int *vsat, double err, double *v,
|
|||
|
|
double *H)
|
|||
|
|
{
|
|||
|
|
double freq, rate, pos[3], E[9], a[3], e[3], vs[3], cosel, sig;
|
|||
|
|
int i, j, nv = 0;
|
|||
|
|
|
|||
|
|
trace(3, "resdop : n=%d\n", n);
|
|||
|
|
|
|||
|
|
// 1<><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD> ecef2pos <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ջ<EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD> ECEF ת<><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>
|
|||
|
|
ecef2pos(rr, pos);
|
|||
|
|
// 2<><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD> xyz2enu <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
xyz2enu(pos, E);
|
|||
|
|
|
|||
|
|
for (i = 0; i < n && i < MAXOBS; i++)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
freq = sat2freq(obs[i].sat, obs[i].code[0], nav);
|
|||
|
|
// 3<><33>ȥ<EFBFBD><C8A5><EFBFBD>ڶ<EFBFBD><DAB6><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if (obs[i].D[0] == 0.0 || freq == 0.0 || !vsat[i] || norm(rs + 3 + i * 6, 3) <= 0.0)
|
|||
|
|
{
|
|||
|
|
continue;
|
|||
|
|
}
|
|||
|
|
/* LOS (line-of-sight) vector in ECEF
|
|||
|
|
4<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㵱ǰ<EFBFBD><EFBFBD><EFBFBD>ջ<EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ENU<EFBFBD>е<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ȼ<EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD> ECEF <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ*/
|
|||
|
|
cosel = cos(azel[1 + i * 2]);
|
|||
|
|
a[0] = sin(azel[i * 2]) * cosel;
|
|||
|
|
a[1] = cos(azel[i * 2]) * cosel;
|
|||
|
|
a[2] = sin(azel[1 + i * 2]);
|
|||
|
|
matmul("TN", 3, 1, 3, 1.0, E, a, 0.0, e);
|
|||
|
|
|
|||
|
|
/* satellite velocity relative to receiver in ECEF
|
|||
|
|
5<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> ECEF <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڽ<EFBFBD><EFBFBD>ջ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>*/
|
|||
|
|
for (j = 0; j < 3; j++)
|
|||
|
|
{
|
|||
|
|
vs[j] = rs[j + 3 + i * 6] - x[j];
|
|||
|
|
}
|
|||
|
|
/* range rate with earth rotation correction
|
|||
|
|
6<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㿼<EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><EFBFBD>ξ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>仯<EFBFBD>ʣ<EFBFBD>
|
|||
|
|
У<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ<EFBFBD><EFBFBD> RTKLIB manual P159 (F.6.29)<EFBFBD><EFBFBD><EFBFBD>˹<EFBFBD>ʽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD> P140 (E.3.8b) <EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD>*/
|
|||
|
|
rate = dot(vs, e, 3) + OMGE / CLIGHT * (rs[4 + i * 6] * rr[0] + rs[1 + i * 6] * x[0] - rs[3 + i * 6] * rr[1] - rs[i * 6] * x[1]);
|
|||
|
|
|
|||
|
|
/* Std of range rate error (m/s) */
|
|||
|
|
sig = (err <= 0.0) ? 1.0 : err * CLIGHT / freq;
|
|||
|
|
|
|||
|
|
/* range rate residual (m/s)
|
|||
|
|
7<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݹ<EFBFBD>ʽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ķ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ղв<EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD>7<EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ղв<EFBFBD> b <EFBFBD><EFBFBD><EFBFBD>ܶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϲ<EFBFBD>ͬ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ estvel <EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><EFBFBD><EFBFBD>ţ<EFBFBD>ٵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD>˷<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>x<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>dx<EFBFBD><EFBFBD><EFBFBD>ټӵ<EFBFBD>x<EFBFBD>ϡ<EFBFBD>*/
|
|||
|
|
v[nv] = (-obs[i].D[0] * CLIGHT / freq - (rate + x[3] - CLIGHT * dts[1 + i * 2])) / sig;
|
|||
|
|
|
|||
|
|
/* design matrix
|
|||
|
|
8<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><EFBFBD>ξ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>붨λ<EFBFBD><EFBFBD>ͬ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ξ<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֻ<EFBFBD><EFBFBD> 4<EFBFBD><EFBFBD>δ֪<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD> NX<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ն<EFBFBD><EFBFBD>ٷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><EFBFBD>ξ<EFBFBD><EFBFBD><EFBFBD> G <EFBFBD>붨λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>һ<EFBFBD><EFBFBD>*/
|
|||
|
|
for (j = 0; j < 4; j++)
|
|||
|
|
{
|
|||
|
|
H[j + nv * 4] = ((j < 3) ? -e[j] : 1.0) / sig;
|
|||
|
|
}
|
|||
|
|
// 9<><39><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٽ<EFBFBD><D9BD>۲ⷽ<DBB2><E2B7BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 1
|
|||
|
|
nv++;
|
|||
|
|
}
|
|||
|
|
return nv;
|
|||
|
|
}
|
|||
|
|
/* estimate receiver velocity
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD>Ʋ<EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ջ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD>ţ<EFBFBD>ٵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>9<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
obsd_t *obs I <EFBFBD>۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
int n I <EFBFBD>۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
double *rs I <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ú<EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ6*n<EFBFBD><EFBFBD>{x,y,z,vx,vy,vz}(ecef)(m,m/s)
|
|||
|
|
double *dts I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ2*n<EFBFBD><EFBFBD> {bias,drift} (s|s/s)
|
|||
|
|
nav_t *nav I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
prcopt_t *opt I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><EFBFBD>
|
|||
|
|
sol_t *sol IO solution
|
|||
|
|
double *azel IO <EFBFBD><EFBFBD>λ<EFBFBD>Ǻ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (rad)
|
|||
|
|
int *vsat IO <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>λʱ<EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD>Ч
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
|
|||
|
|
int O (1:ok,0:error)
|
|||
|
|
------------------------------------------------*/
|
|||
|
|
static void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
|
|||
|
|
const nav_t *nav, const prcopt_t *opt, sol_t *sol,
|
|||
|
|
const double *azel, const int *vsat)
|
|||
|
|
{
|
|||
|
|
double x[4] = {0}, dx[4], Q[16], *v, *H;
|
|||
|
|
double err = opt->err[4]; /* Doppler error (Hz) */
|
|||
|
|
int i, j, nv;
|
|||
|
|
|
|||
|
|
trace(3, "----> estvel : n=%d\n", n);
|
|||
|
|
|
|||
|
|
v = mat(n, 1);
|
|||
|
|
H = mat(4, n);
|
|||
|
|
|
|||
|
|
/*1<><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڣ<EFBFBD><DAA3><EFBFBD><EFBFBD><EFBFBD> resdop<6F><70><EFBFBD><EFBFBD><EFBFBD>㶨<EFBFBD>ٷ<EFBFBD><D9B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ߵļ<DFB5><C4BC>ξ<EFBFBD><CEBE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҷ˵<D2B6><CBB5>ٶȲ<D9B6><C8B2>࣬<EFBFBD><E0A3AC><EFBFBD>ض<EFBFBD><D8B6><EFBFBD>ʱ<EFBFBD><CAB1>ʹ<EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ*/
|
|||
|
|
for (i = 0; i < MAXITR; i++)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
/* range rate residuals (m/s) */
|
|||
|
|
if ((nv = resdop(obs, n, rs, dts, nav, sol->rr, x, azel, vsat, err, v, H)) < 4)
|
|||
|
|
{
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
/* least square estimation
|
|||
|
|
2<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD>˷<EFBFBD> lsq <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>{<EFBFBD>ٶȡ<EFBFBD>ƵƯ}<EFBFBD>IJ<EFBFBD><EFBFBD><EFBFBD> dx<EFBFBD><EFBFBD><EFBFBD>ۼӵ<EFBFBD> x <EFBFBD><EFBFBD>*/
|
|||
|
|
//<2F><><EFBFBD>ﲻ<EFBFBD><EFB2BB><EFBFBD><EFBFBD>λʱ<CEBB><CAB1><EFBFBD><EFBFBD>ʼֵ<CABC><D6B5><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>һ<EFBFBD><D2BB>Ԫ<EFBFBD><D4AA>λ<EFBFBD><CEBB>(<28><> sol <20>ж<EFBFBD>ȡ<EFBFBD><C8A1>ʼֵ)<29><><EFBFBD><EFBFBD><EFBFBD>ﶨ<EFBFBD>ٵij<D9B5>ʼֱֵ<D6B5>Ӹ<EFBFBD><D3B8><EFBFBD>Ϊ 0.
|
|||
|
|
if (lsq(H, v, 4, nv, dx, Q))
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
for (j = 0; j < 4; j++)
|
|||
|
|
x[j] += dx[j];
|
|||
|
|
|
|||
|
|
/*3<><33><EFBFBD><EFBFBD><EFBFBD>鵱ǰ<E9B5B1><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD>ֵ<EFBFBD>Ƿ<EFBFBD>С<EFBFBD><D0A1> 1E-6<><36>
|
|||
|
|
<EFBFBD>ǣ<EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><EFBFBD>ܽӽ<EFBFBD><EFBFBD><EFBFBD>ʵֵ<EFBFBD>ˣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ջ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD>ٶȴ<EFBFBD><EFBFBD>뵽 sol->rr <EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD>ѭ<EFBFBD><EFBFBD>*/
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> sol_t <20><><EFBFBD>ʹ洢<CDB4><E6B4A2><EFBFBD>ٽ<EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>û<EFBFBD>д洢<D0B4><E6B4A2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ľ<EFBFBD><C4BD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>ƵƯ<C6B5><C6AF>
|
|||
|
|
if (norm(dx, 4) < 1E-6)
|
|||
|
|
{
|
|||
|
|
matcpy(sol->rr + 3, x, 3, 1);
|
|||
|
|
sol->qv[0] = (float)Q[0]; /* xx */
|
|||
|
|
sol->qv[1] = (float)Q[5]; /* yy */
|
|||
|
|
sol->qv[2] = (float)Q[10]; /* zz */
|
|||
|
|
sol->qv[3] = (float)Q[1]; /* xy */
|
|||
|
|
sol->qv[4] = (float)Q[6]; /* yz */
|
|||
|
|
sol->qv[5] = (float)Q[2]; /* zx */
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
free(v);
|
|||
|
|
free(H);
|
|||
|
|
}
|
|||
|
|
/* single-point positioning ----------------------------------------------------
|
|||
|
|
* compute receiver position, velocity, clock bias by single-point positioning
|
|||
|
|
* with pseudorange and doppler observables
|
|||
|
|
* args : obsd_t *obs I observation data
|
|||
|
|
* int n I number of observation data
|
|||
|
|
* nav_t *nav I navigation data
|
|||
|
|
* prcopt_t *opt I processing options
|
|||
|
|
* sol_t *sol IO solution
|
|||
|
|
* double *azel IO azimuth/elevation angle (rad) (NULL: no output)
|
|||
|
|
* ssat_t *ssat IO satellite status (NULL: no output)
|
|||
|
|
* char *msg O error message for error exit
|
|||
|
|
* return : status(1:ok,0:error)
|
|||
|
|
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>α<EFBFBD><EFBFBD><EFBFBD>Ͷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD>Ʋ<EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><EFBFBD>㶨λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ջ<EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>á<EFBFBD><EFBFBD>ٶȺ<EFBFBD><EFBFBD>Ӳ<EFBFBD>
|
|||
|
|
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>8<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
obsd_t *obs I <EFBFBD>۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
int n I <EFBFBD>۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
nav_t *nav I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
prcopt_t *opt I <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><EFBFBD>
|
|||
|
|
sol_t *sol IO solution
|
|||
|
|
double *azel IO <EFBFBD><EFBFBD>λ<EFBFBD>Ǻ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (rad) (NULL: no output)
|
|||
|
|
ssat_t *ssat IO <EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬ (NULL: no output)
|
|||
|
|
char *msg O <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
|
|||
|
|
int O (1:ok,0:error)
|
|||
|
|
*-----------------------------------------------------------------------------*/
|
|||
|
|
extern int pntpos(const obsd_t *obs, int n, const nav_t *nav,
|
|||
|
|
const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
|
|||
|
|
char *msg)
|
|||
|
|
{
|
|||
|
|
prcopt_t opt_ = *opt;
|
|||
|
|
double *rs, *dts, *var, *azel_, *resp;
|
|||
|
|
int i, stat, vsat[MAXOBS] = {0}, svh[MAXOBS];
|
|||
|
|
|
|||
|
|
trace(3, "--> pntpos : tobs=%s n=%d\n", time_str(obs[0].time, 3), n);
|
|||
|
|
|
|||
|
|
sol->stat = SOLQ_NONE;
|
|||
|
|
|
|||
|
|
if (n <= 0)
|
|||
|
|
{
|
|||
|
|
strcpy(msg, "no observation data");
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
sol->time = obs[0].time;
|
|||
|
|
msg[0] = '\0';
|
|||
|
|
sol->eventime = obs[0].eventime;
|
|||
|
|
|
|||
|
|
/* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD>ռ<EFBFBD> 96*n */
|
|||
|
|
rs = mat(6, n);
|
|||
|
|
dts = mat(2, n);
|
|||
|
|
var = mat(1, n);
|
|||
|
|
azel_ = zeros(2, n);
|
|||
|
|
resp = mat(1, n);
|
|||
|
|
|
|||
|
|
if (ssat) /* <20><>ssat<61><74><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ */
|
|||
|
|
{
|
|||
|
|
for (i = 0; i < MAXSAT; i++)
|
|||
|
|
{
|
|||
|
|
ssat[i].snr_rover[0] = 0;
|
|||
|
|
ssat[i].snr_base[0] = 0;
|
|||
|
|
}
|
|||
|
|
for (i = 0; i < n; i++)
|
|||
|
|
ssat[obs[i].sat - 1].snr_rover[0] = obs[i].SNR[0];
|
|||
|
|
}
|
|||
|
|
/*1<><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><D1A1> opt <20>е<EFBFBD>ģʽ<C4A3><CABD><EFBFBD>ǵ<EFBFBD><C7B5><EFBFBD>ģʽʱ<CABD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD> broadcast ģ<>ͣ<EFBFBD><CDA3><EFBFBD>Klobucharģ<72>ͣ<EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> Saastamoinen ģ<EFBFBD>ͣ<EFBFBD><EFBFBD>෴<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> opt <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
|
|||
|
|
if (opt_.mode != PMODE_SINGLE)
|
|||
|
|
{ /* for precise positioning */
|
|||
|
|
// opt_.ionoopt = IONOOPT_BRDC; /* <20><>ΪRTCM32<33><32><EFBFBD>ṩ<EFBFBD><E1B9A9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD><EFBFBD> */
|
|||
|
|
opt_.ionoopt = IONOOPT_OFF;
|
|||
|
|
opt_.tropopt = TROPOPT_SAAS;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* satellite positons, velocities and clocks
|
|||
|
|
2<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> satposs <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>á<EFBFBD><EFBFBD>ٶȡ<EFBFBD>ʱ<EFBFBD><EFBFBD>*/
|
|||
|
|
satposs(sol->time, obs, n, nav, opt_.sateph, rs, dts, var, svh);
|
|||
|
|
|
|||
|
|
/* estimate receiver position with pseudorange
|
|||
|
|
3<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> estpos <EFBFBD><EFBFBD><EFBFBD><EFBFBD>α<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><EFBFBD>ջ<EFBFBD>λ<EFBFBD>ã<EFBFBD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> valsol <EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> GDOP <EFBFBD><EFBFBD><EFBFBD>顣*/
|
|||
|
|
stat = estpos(obs, n, rs, dts, var, svh, nav, &opt_, ssat, sol, azel_, vsat, resp, msg);
|
|||
|
|
|
|||
|
|
/* 4<><34><EFBFBD><EFBFBD>3<EFBFBD>еļ<D0B5><C4BC><EFBFBD>ûͨ<C3BB><CDA8>RAIM FDE <20><><EFBFBD>ջ<EFBFBD><D5BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><D4BC>⣬<EFBFBD>о<EFBFBD><D0BE><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD>ԣ<EFBFBD><D4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD>ų<EFBFBD><C5B3><EFBFBD>*/
|
|||
|
|
if (!stat && n >= 6 && opt->posopt[4])
|
|||
|
|
{
|
|||
|
|
stat = raim_fde(obs, n, rs, dts, var, svh, nav, &opt_, ssat, sol, azel_, vsat, resp, msg);
|
|||
|
|
}
|
|||
|
|
/* estimate receiver velocity with Doppler
|
|||
|
|
5<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> estvel <EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD>Ʋ<EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ջ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȡ<EFBFBD>*/
|
|||
|
|
if (stat)
|
|||
|
|
{
|
|||
|
|
estvel(obs, n, rs, dts, nav, &opt_, sol, azel_, vsat);
|
|||
|
|
}
|
|||
|
|
if (azel)
|
|||
|
|
{
|
|||
|
|
for (i = 0; i < n * 2; i++)
|
|||
|
|
azel[i] = azel_[i];
|
|||
|
|
}
|
|||
|
|
if (ssat)
|
|||
|
|
{
|
|||
|
|
for (i = 0; i < MAXSAT; i++) /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
|||
|
|
{
|
|||
|
|
ssat[i].vs = 0;
|
|||
|
|
ssat[i].azel[0] = ssat[i].azel[1] = 0.0;
|
|||
|
|
ssat[i].resp[0] = ssat[i].resc[0] = 0.0;
|
|||
|
|
}
|
|||
|
|
for (i = 0; i < n; i++)
|
|||
|
|
{
|
|||
|
|
ssat[obs[i].sat - 1].azel[0] = azel_[i * 2];
|
|||
|
|
ssat[obs[i].sat - 1].azel[1] = azel_[1 + i * 2];
|
|||
|
|
if (!vsat[i])
|
|||
|
|
continue;
|
|||
|
|
ssat[obs[i].sat - 1].vs = 1;
|
|||
|
|
ssat[obs[i].sat - 1].resp[0] = resp[i]; /* residuals of pseudorange (m) */
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
free(rs);
|
|||
|
|
free(dts);
|
|||
|
|
free(var);
|
|||
|
|
free(azel_);
|
|||
|
|
free(resp);
|
|||
|
|
return stat;
|
|||
|
|
}
|