/*------------------------------------------------------------------------------
 * 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
计算当前迭代的伪距残差 v、几何矩阵 H、伪距残差的方差 var、所有观测卫星的方位角和仰角 azel、
定位时有效性 vsat、定位后伪距残差 resp、参与定位的卫星个数 ns 和方程个数 nv
函数参数，17个
int      iter      I   迭代次数
obsd_t   *obs      I   观测量数据
int      n         I   观测量数据的数量
double   *rs       I   卫星位置和速度，长度为6*n，{x,y,z,vx,vy,vz}(ecef)(m,m/s)
double   *dts      I   卫星钟差，长度为2*n， {bias,drift} (s|s/s)
double   *vare     I   卫星位置和钟差的协方差 (m^2)
int      *svh      I   卫星健康标志 (-1:correction not available)
nav_t    *nav      I   导航数据
double   *x        I   本次迭代开始之前的定位值
prcopt_t *opt      I   处理过程选项
double   *v        O   定位方程的右端部分，伪距残差
double   *H        O   定位方程中的几何矩阵
double   *var      O   参与定位的伪距残差的方差
double   *azel     O   对于当前定位值，所有观测卫星的 {方位角、高度角} (2*n)
int      *vsat     O   所有观测卫星在当前定位时是否有效 (1*n)
double   *resp     O   所有观测卫星的伪距残差，(P-(r+c*dtr-c*dts+I+T)) (1*n)
int      *ns       O   参与定位的卫星的个数
返回类型：
int                O   定位方程组的方程个数
-----------------------------------------------------*/
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}; /* 卫星系统掩码, 1:使用了该系统 0:没有使用该系统 */

    trace(3, "resprng : n=%d\n", n);

    // 1、将之前得到的定位解信息赋值给 rr 和 dtr 数组，以进行关于当前解的伪距残差的相关计算
    // 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、调用 ecef2pos 函数，将上一步中得到的位置信息由 ECEF 转化为大地坐标系 */
    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++) /* 对每一个观测量进行循环求解 */
    {
        // 3、将 vsat、azel 和 resp 数组置 0，因为在前后两次定位结果中，每颗卫星的上述信息都会发生变化
        vsat[i] = 0;
        azel[i * 2] = azel[1 + i * 2] = resp[i] = 0.0;
        time = obs[i].time;
        sat = obs[i].sat;
        // 4、调用 satsys 函数，验证卫星编号是否合理及其所属的导航系统
        if (!(sys = satsys(sat, NULL)))
            continue;

        /* reject duplicated observation data
        5、检测当前卫星是否存在重复的观测量 */
        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、可以在处理选项中事先指定定位时排除哪些导航系统或卫星，这是通过调用 satexclude 函数完成的*/
        if (satexclude(sat, vare[i], svh[i], opt))
            continue;

        /* geometric distance and elevation mask*/
        /*7、调用 geodist 函数，计算卫星和当前接收机位置之间的几何距离 r 和接收机到卫星方向的观测矢量。
        然后检验几何距离是否 >0。此函数中会进行地球自转影响的校正（Sagnac效应）*/
        if ((r = geodist(rs + i * 6, rr, e)) <= 0.0)
            continue;

        // 8、调用 satazel 函数，计算在接收机位置处的站心坐标系中卫星的方位角和仰角；若仰角低于截断值，不处理此数据。
        if (satazel(pos, e, azel + i * 2) < opt->elmin)
            continue;

        if (iter > 0)
        {
            /* test SNR mask TODO: 使用了与RTK的共有数组, 导致基站单点定位的时候也要求了极高的信噪比,造成了单点定位无解 */
            if (!snrmask(obs + i, azel + i * 2, opt))
                continue;

            /* ionospheric correction
            9、调用 ionocorr 函数，计算电离层延时 I (m)。所得的电离层延时是建立在 L1 信号上的，当使用其它频率信号时，
            依据所用信号频组中第一个频率的波长与 L1 波长的关系，对上一步得到的电离层延时进行修正*/
            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、调用 tropcorr 函数，计算对流层延时 T (m）*/
            if (!tropcorr(time, nav, pos, azel + i * 2, opt->tropopt, &dtrp, &vtrp))
            {
                continue;
            }
        }
        /* psendorange with code bias correction
        11、调用 prange 函数，得到经过DCB校正后的伪距值 ρ*/
        if ((P = prange(obs + i, nav, opt, &vmeas)) == 0.0)
            continue;

        /* pseudorange residual
        12、由 ρ ? ( r + d t r ? c ? d t s + I + T ) ，计算出此时的伪距残差*/
        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、组装几何矩阵 H ，前 3 行为 7中计算得到的视线单位向量的反向，第 4 行为 1，其它行为 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、处理不同系统（GPS、GLO、GAL、CMP）之间的时间偏差，修改矩阵 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、将参与定位的卫星的定位有效性标志设为 1，给当前卫星的伪距残差赋值，参与定位的卫星个数 ns 加 1
        vsat[i] = 1;
        resp[i] = v[nv];
        (*ns)++;

        /* variance of pseudorange error
        16、调用 varerr 函数，计算此时的导航系统误差，然后累加计算用户测距误差(URE）*/
        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、为了防止不满秩的情况，把矩阵 H 补满秩了*/
    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}; /* 卫星系统掩码, 1:使用了该系统 0:没有使用该系统 */

    trace(3, "resprng : n=%d\n", n);

    // 1、将之前得到的定位解信息赋值给 rr 和 dtr 数组，以进行关于当前解的伪距残差的相关计算
    // 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、调用 ecef2pos 函数，将上一步中得到的位置信息由 ECEF 转化为大地坐标系 */
    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++) /* 对每一个观测量进行循环求解 */
    {
        // 3、将 vsat、azel 和 resp 数组置 0，因为在前后两次定位结果中，每颗卫星的上述信息都会发生变化
        vsat[i] = 0;
        azel[i * 2] = azel[1 + i * 2] = resp[i] = 0.0;
        time = obs[i].time;
        sat = obs[i].sat;
        // 4、调用 satsys 函数，验证卫星编号是否合理及其所属的导航系统
        if (!(sys = satsys(sat, NULL)))
            continue;

        /* reject duplicated observation data
        5、检测当前卫星是否存在重复的观测量 */
        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、可以在处理选项中事先指定定位时排除哪些导航系统或卫星，这是通过调用 satexclude 函数完成的*/
        // if (satexclude(sat, vare[i], svh[i], opt))
        //     continue;

        /* geometric distance and elevation mask*/
        /*7、调用 geodist 函数，计算卫星和当前接收机位置之间的几何距离 r 和接收机到卫星方向的观测矢量。
        然后检验几何距离是否 >0。此函数中会进行地球自转影响的校正（Sagnac效应）*/
        if ((r = geodist(rs + i * 6, rr, e)) <= 0.0)
            continue;

        // 8、调用 satazel 函数，计算在接收机位置处的站心坐标系中卫星的方位角和仰角；若仰角低于截断值，不处理此数据。
        if (satazel(pos, e, azel + i * 2) < RESCODE_MIN_EL)
            continue;

        if (iter > 0)
        {
            /* test SNR mask TODO: 使用了与RTK的共有数组, 导致基站单点定位的时候也要求了极高的信噪比,造成了单点定位无解 */

            if (obs[i].SNR[0] < 30000)
                continue;

            /* ionospheric correction
            9、调用 ionocorr 函数，计算电离层延时 I (m)。所得的电离层延时是建立在 L1 信号上的，当使用其它频率信号时，
            依据所用信号频组中第一个频率的波长与 L1 波长的关系，对上一步得到的电离层延时进行修正*/
            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、调用 tropcorr 函数，计算对流层延时 T (m）*/
            if (!tropcorr(time, nav, pos, azel + i * 2, opt->tropopt, &dtrp, &vtrp))
            {
                continue;
            }
        }
        /* psendorange with code bias correction
        11、调用 prange 函数，得到经过DCB校正后的伪距值 ρ*/
        if ((P = prange(obs + i, nav, opt, &vmeas)) == 0.0)
            continue;

        /* pseudorange residual
        12、由 ρ ? ( r + d t r ? c ? d t s + I + T ) ，计算出此时的伪距残差*/
        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、组装几何矩阵 H ，前 3 行为 7中计算得到的视线单位向量的反向，第 4 行为 1，其它行为 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、处理不同系统（GPS、GLO、GAL、CMP）之间的时间偏差，修改矩阵 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、将参与定位的卫星的定位有效性标志设为 1，给当前卫星的伪距残差赋值，参与定位的卫星个数 ns 加 1
        vsat[i] = 1;
        resp[i] = v[nv];
        (*ns)++;

        /* variance of pseudorange error
        16、调用 varerr 函数，计算此时的导航系统误差，然后累加计算用户测距误差(URE）*/
        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、为了防止不满秩的情况，把矩阵 H 补满秩了*/
    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);

    /* 伪距残差 卡方校验 */
    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; */ /* 阈值设置的太严格了 仅报错 */
    }
    /* 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
通过伪距实现绝对定位，计算出接收机的位置和钟差，顺带返回实现定位后每颗卫星的{方位角、仰角}、定位时有效性、定位后伪距残差。
函数参数，13个：
obsd_t   *obs      I   观测量数据
int      n         I   观测量数据的数量
double   *rs       I   卫星位置和速度，长度为6*n，{x,y,z,vx,vy,vz}(ecef)(m,m/s)
double   *dts      I   卫星钟差，长度为2*n， {bias,drift} (s|s/s)
double   *vare     I   卫星位置和钟差的协方差 (m^2)
int      *svh      I   卫星健康标志 (-1:correction not available)
nav_t    *nav      I   导航数据
prcopt_t *opt      I   处理过程选项
sol_t    *sol      IO  solution
double   *azel     IO  方位角和俯仰角 (rad)
int      *vsat     IO  卫星在定位时是否有效
double   *resp     IO  定位后伪距残差 (P-(r+c*dtr-c*dts+I+T))
char     *msg      O   错误消息
返回类型:
int                O   1表示成功，0表示出错
------------------------------------------------*/
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、初始化：将 sol->rr 的前 3 项位置信息（ECEF）赋值给 x 数组。
    /*如果是第一次定位，即输入的 sol 为空，则 x 初值为 0；
    如果之前有过定位，则通过 1 中操作可以将上一历元的定位值作为该历元定位的初始值*/
    for (i = 0; i < 3; i++)
        x[i] = sol->rr[i];

    for (i = 0; i < MAXITR; i++)
    {

        /* pseudorange residuals (m)
        2、开始迭代定位计算，首先调用 rescode 函数，计算当前迭代的伪距残差 v、
        几何矩阵 H、伪距残差的方差 var、所有观测卫星的方位角和仰角 azel、
        定位时有效性 vsat、定位后伪距残差 resp、参与定位的卫星个数 ns 和方程个数 nv。*/
        nv = rescode(i, obs, n, rs, dts, vare, svh, nav, x, opt, ssat, v, H, var, azel, vsat, resp, &ns);

        // 3、确定方程组中方程的个数要大于未知数的个数
        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、以伪距残差的标准差的倒数作为权重，对 H 和 v分别左乘权重对角阵，得到加权之后的 H和 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、调用 lsq 函数，根据 dx=(HH^T)^{-1}Hv
        和  Q=(HH^T)^{-1}，得到当前 x 的修改量 dx 和定位误差协方差矩阵中的权系数阵 Q

        关于加权最小二乘，这里的权重值是对角阵，这是建立在假设不同测量值的误差之间是彼此独立的基础上的。
        大部分资料上这里都是把权重矩阵 W 保留到方程的解的表达式当中，而这里是直接对 H 和 v 分别左乘权重对角阵，得到加权之后的 H 和 v，其表示形式像是没有加权一样*/
        if ((info = lsq(H, v, NX, nv, dx, Q)))
        {
            sprintf(msg, "lsq error info=%d", info);
            break;
        }

        // 6、将 5 中求得的 dx 加入到当前 x 值中，得到更新之后的 x 值
        for (j = 0; j < NX; j++)
        {
            x[j] += dx[j];
        }

        /*7、如果 5 中求得的修改量 dx 小于截断因子(目前是10^{-4} )，
        则将 6 中得到的 x 值作为最终的定位结果，对 sol 的相应参数赋值，之后再调用 valsol 函数确认当前解是否符合要求
        (伪距残差小于某个 χ^2值和 GDOP 小于某个门限值，参考 RTKLIB Manual P162, E.6.33, E.6.34)。
        否则，进行下一次循环*/
        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 中存储的是减去接收机钟差后的信号观测时间 */
            //解方程时的 dtr 单位是 m，是乘以了光速之后的，解出结果后赋给 sol->dtr 时再除以光速
            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、如果超过了规定的循环次数，则输出发散信息后，返回 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)
使用伪距残差判决法对计算得到的定位结果进行接收机自主正直性检测（RAIM），每次舍弃一颗卫星测量值，
用剩余的值组成一组进行定位运算，选择定位后伪距残差最小的一组作为最终结果。
这样如果只有一个异常观测值的话，这个错误可以被排除掉；有两个或以上错误则排除不了。
注意这里只会在对定位结果有贡献的卫星数据进行检测。
函数参数，13个：
obsd_t   *obs      I   观测数据
int      n         I   观测数据的数量
double   *rs       I   卫星位置和速度，长度为6*n，{x,y,z,vx,vy,vz}(ecef)(m,m/s)
double   *dts      I   卫星钟差，长度为2*n， {bias,drift} (s|s/s)
double   *vare     I   卫星位置和钟差的协方差 (m^2)
int      *svh      I   卫星健康标志 (-1:correction not available)
nav_t    *nav      I   导航数据
prcopt_t *opt      I   处理过程选项
sol_t    *sol      IO  solution
double   *azel     IO  方位角和俯仰角 (rad)
int      *vsat     IO  卫星在定位时是否有效
double   *resp     IO  定位后伪距残差 (P-(r+c*dtr-c*dts+I+T))
char     *msg      O   错误消息
返回类型:
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);

    /*源码中有很多关于 i、j、k的循环。其中，i表示最外面的大循环，每次将将第 i颗卫星舍弃不用，这是通过 if (j==i) continue实现的；
    j表示剩余使用的卫星的循环，每次进行相应数据的赋值；k表示参与定位的卫星的循环，与 j一起使用*/

    // 1、大循环是每次舍弃第 i 颗卫星。
    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、舍弃第 i 颗卫星后，将剩下卫星的数据复制到一起，调用 estpos 函数计算使用剩下卫星进行定位的定位值。*/
        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、累加使用当前卫星实现定位后的伪距残差平方和与可用卫星数目，
        如果 nvsat<5，则说明当前卫星数目过少，无法进行 RAIM_FDE 操作*/
        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、计算伪距残差平方和的标准差，如果小于 rms，则说明当前定位结果更合理，将 stat 置为 1，
        重新更新 sol、azel、vsat(当前被舍弃的卫星，此值置为0)、resp等值，并将当前的 rms_e更新到 rms 中。*/
        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、继续弃用下一颗卫星，重复 2-4操作。
        总而言之，将同样是弃用一颗卫星条件下，伪距残差标准平均值最小的组合所得的结果作为最终的结果输出。*/

    // 6、如果 stat不为 0，则说明在弃用卫星的前提下有更好的解出现，输出信息，指出弃用了哪颗卫星
    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
计算定速方程组左边的几何矩阵和右端的速度残余，返回定速时所使用的卫星数目
函数参数，11个：
obsd_t   *obs      I   观测数据
int      n         I   观测数据的数量
double   *rs       I   卫星位置和速度，长度为6*n，{x,y,z,vx,vy,vz}(ecef)(m,m/s)
double   *dts      I   卫星钟差，长度为2*n， {bias,drift} (s|s/s)
nav_t    *nav      I   导航数据
double   *rr       I   接收机位置和速度，长度为6，{x,y,z,vx,vy,vz}(ecef)(m,m/s)
double   *x        I   本次迭代开始之前的定速值，长度为4，{vx,vy,vz,drift}
double   *azel     IO  方位角和俯仰角 (rad)
int      *vsat     I   卫星在定速时是否有效
double   *v        O   定速方程的右端部分，速度残差
double   *H        O   定速方程中的几何矩阵
返回类型:
int                O   定速时所使用的卫星数目
------------------------------------------------------*/
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、调用 ecef2pos 函数，将接收机位置由 ECEF 转换为大地坐标系。
    ecef2pos(rr, pos);
    // 2、调用 xyz2enu 函数，计算此时的坐标转换矩阵
    xyz2enu(pos, E);

    for (i = 0; i < n && i < MAXOBS; i++)
    {

        freq = sat2freq(obs[i].sat, obs[i].code[0], nav);
        // 3、去除在定速时不可用的卫星
        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、计算当前接收机位置下 ENU中的视向量，然后转换得到 ECEF 中视向量的值*/
        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、计算 ECEF 中卫星相对于接收机的速度*/
        for (j = 0; j < 3; j++)
        {
            vs[j] = rs[j + 3 + i * 6] - x[j];
        }
        /* range rate with earth rotation correction
        6、计算考虑了地球自转的用户和卫星之间的几何距离变化率，
        校正公式见 RTKLIB manual P159 (F.6.29)，此公式可由 P140 (E.3.8b) 对时间求导得到*/
        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、根据公式计算出定速方程组右端项的多普勒残差
        第7步中计算多普勒残差 b 与很多资料不同，因为 estvel 中用的是牛顿迭代法，
        其最小二乘法并不是直接求解x，而是求解dx，再加到x上。*/
        v[nv] = (-obs[i].D[0] * CLIGHT / freq - (rate + x[3] - CLIGHT * dts[1 + i * 2])) / sig;

        /* design matrix
        8、构建左端项的几何矩阵，这里与定位不同，构建几何矩阵时，就只有 4个未知数，而定位时是有 NX个
        多普勒定速方程中几何矩阵 G 与定位方程中的一样*/
        for (j = 0; j < 4; j++)
        {
            H[j + nv * 4] = ((j < 3) ? -e[j] : 1.0) / sig;
        }
        // 9、最后再将观测方程数增 1
        nv++;
    }
    return nv;
}
/* estimate receiver velocity
依靠多普勒频移测量值计算接收机的速度，用牛顿迭代法。
函数参数，9个：
obsd_t   *obs      I   观测数据
int      n         I   观测数据的数量
double   *rs       I   卫星位置和速度，长度为6*n，{x,y,z,vx,vy,vz}(ecef)(m,m/s)
double   *dts      I   卫星钟差，长度为2*n， {bias,drift} (s|s/s)
nav_t    *nav      I   导航数据
prcopt_t *opt      I   处理过程选项
sol_t    *sol      IO  solution
double   *azel     IO  方位角和俯仰角 (rad)
int      *vsat     IO  卫星在定位时是否有效
返回类型:
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、在最大迭代次数限制内，调用 resdop，计算定速方程组左边的几何矩阵和右端的速度残余，返回定速时所使用的卫星数目*/
    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、调用最小二乘法 lsq 函数，解出{速度、频漂}的步长 dx，累加到 x 中*/
        //这里不像定位时，初始值可能为上一历元的位置(从 sol 中读取初始值)，这里定速的初始值直接给定为 0.
        if (lsq(H, v, 4, nv, dx, Q))
            break;

        for (j = 0; j < 4; j++)
            x[j] += dx[j];

        /*3、检查当前计算出的步长的绝对值是否小于 1E-6。
        是，则说明当前解已经很接近真实值了，将接收机三个方向上的速度存入到 sol->rr 中；否，则进行下一次循环*/
        //最终向 sol_t 类型存储定速解时，并没有存储所计算出的接收器时钟频漂。
        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)
* 依靠伪距和多普勒频移测量值来进行单点定位，给出接收机的位置、速度和钟差
* 函数参数，8个：
obsd_t   *obs      I   观测数据
int      n         I   观测数据的数量
nav_t    *nav      I   导航数据
prcopt_t *opt      I   处理过程选项
sol_t    *sol      IO  solution
double   *azel     IO  方位角和俯仰角 (rad) (NULL: no output)
ssat_t   *ssat     IO  卫星状态            (NULL: no output)
char     *msg      O   错误消息
返回类型:
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;

    /* 申请地址空间 96*n */
    rs = mat(6, n);
    dts = mat(2, n);
    var = mat(1, n);
    azel_ = zeros(2, n);
    resp = mat(1, n);

    if (ssat) /* 若ssat存在 清空信噪比信息 */
    {
        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、当处理选项 opt 中的模式不是单点模式时，电离层校正采用 broadcast 模型，即Klobuchar模型，
    对流层校正则采用 Saastamoinen 模型；相反，当其为单点模式时，对输入参数 opt 不做修改*/
    if (opt_.mode != PMODE_SINGLE)
    { /* for precise positioning */
        // opt_.ionoopt = IONOOPT_BRDC; /* 因为RTCM32不提供电离层参数,所以这里的电离层校正反而会引入新的误差 */
        opt_.ionoopt = IONOOPT_OFF;
        opt_.tropopt = TROPOPT_SAAS;
    }

    /* satellite positons, velocities and clocks
    2、调用 satposs 计算卫星们位置、速度、时钟*/
    satposs(sol->time, obs, n, nav, opt_.sateph, rs, dts, var, svh);

    /* estimate receiver position with pseudorange
    3、调用 estpos 根据伪距估计接收机位置，其中会调用 valsol 进行 卡方检验和 GDOP 检验。*/
    stat = estpos(obs, n, rs, dts, var, svh, nav, &opt_, ssat, sol, azel_, vsat, resp, msg);

    /* 4、若3中的检验没通过RAIM FDE 接收机自主完好性监测，判决定位结果的有效性，并进行错误排除。*/
    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、调用 estvel 根据多普勒频移测量值计算接收机的速度。*/
    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++) /* 都先清零 */
        {
            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) */
        }
    }
    rt_free(rs);
    rt_free(dts);
    rt_free(var);
    rt_free(azel_);
    rt_free(resp);
    return stat;
}