fuxiaoer/RTK_base
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
RTK_base/RTK/pntpos.c

1221 lines
45 KiB
C
Raw Permalink Normal View History

同步测试代码
2022-06-22 09:23:36 +08:00
/*------------------------------------------------------------------------------
* 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) */
}
}
no message
2022-06-27 15:36:14 +08:00
rt_free(rs);
rt_free(dts);
rt_free(var);
rt_free(azel_);
rt_free(resp);
同步测试代码
2022-06-22 09:23:36 +08:00
return stat;
}
Reference in New Issue Copy Permalink