资讯

RTKLIB源码解析(一)——单点定位(pntpos.c)

更新时间:2018-11-22 03:39:04 来源:国产片零入围的“冷启示”

[已有97428人评论] [有偿投稿]

此时的小舞,看上去已经不是个美女,而是随时有可能爆发的火山,唐三的精神力何等强大,他能清晰地感觉到,小舞已经处于爆发的边缘,心中一动,学着先前魔魂大白鲨之王小白的语气道:“是我错了,我向你道歉。对不起,小舞,你知道的,我……”

五大行日赚25亿

因为她的真气虽强,但毕竟不够雄浑,功力还是不够深厚,所以无法支持她战斗太长时间,火拼到现在她的一身真气已经消耗了八成了。
雪飞鸿把伸臂半抱着施珍娜,示意她再坐一会儿,继续看下去,好戏才刚刚开始上演呢!如果不想替施珍娜做点事情,那么他怎么可能会忍受这些人的嘲讽?现在就让他们笑,直最后,他要让这些人都笑不出来,甚至,连哭都哭不出来……

裴柔迟疑了一下,走上前问李庆安道:“他所说的是真话吗?这两个小娘是你的女人?”

RTKLIB源码解析(一)——单点定位(pntpos.c)

标签: GNSS RTKLIB 单点定位

前段时间一直忙着写毕业论文,所以也没有太多时间来阅读 RTKLIB源码,最近好歹把 pntpos中的相关代码看了一遍,知道了 RTKLIB是如何实现单点伪距定位的。这里把每一个函数都做成了小卡片的形式,每个函数大都包含函数签名、所在文件、功能说明、参数说明、处理过程、注意事项和我的疑惑这几个部分,介绍了阅读代码时我自己的看法和疑惑。所以希望诸位看官能帮忙解答我的疑惑,与我交流,也希望能帮助后来也有需要阅读 RTKLIB源码的人,给他们多提供一份思路。总而言之,既为人,也为己。
这份文档是使用 Cmd Markdown完成的,在作业部落上其格式显式的非常完整,但是在博客园中目录代码块流程图似乎都没有显示出来,所以这里也贴上本文在作业部落上的链接RTKLIB源码解析(一)——单点定位(pntpos.c),对格式“零容忍”的同学请移步那里。

[TOC]


pntpos

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)
  • 所在文件:pntpos.c
  • 功能说明:依靠多普勒频移测量值和伪距来进行单点定位,给出接收机的位置、速度和钟差
  • 参数说明

    函数参数,8个:
    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
    返回类型:
    int                  O     (1:ok,0:error)
  • 调用关系
    如无特别说明,本文所出现的流程图中,纵向代表时间上的先后调用顺序,横向代表层次上的逐级调用顺序。
    ```flow
    st=>start: pntpos
    satposs_=>operation: satposs
    estpos_=>operation: estpos
    raim_fde_=>operation: raim_fde
    estvel_=>operation: estvel
    e=>end: end

st->satposs_->estpos_->raim_fde_->estvel_->e
```

pntpos satposs estpos raim_fde estvel

  • 处理过程

    1. 检查卫星个数是否大于 0
    2. 当处理选项 opt中的模式不是单点模式时,电离层校正采用 Klobuchar模型,对流层校正则采用 Saastamoinen模型;相反,当其为单点模式时,对输入参数 opt不做修改。
    3. 调用 satposs函数,按照所观测到的卫星顺序计算出每颗卫星的位置、速度、{钟差、频漂}。
    4. 调用 estpos函数,通过伪距实现绝对定位,计算出接收机的位置和钟差,顺带返回实现定位后每颗卫星的{方位角、仰角}、定位时有效性、定位后伪距残差。
    5. 调用 raim_fde函数,对上一步得到的定位结果进行接收机自主正直性检测(RAIM)。通过再次使用 vsat数组,这里只会在对定位结果有贡献的卫星数据进行检测。
    6. 调用 estvel函数,依靠多普勒频移测量值计算接收机的速度。这里只使用通过了上一步 RAIM_FDE操作的卫星数据,所以对于计算出的速度就没有再次进行 RAIM了。
    7. 首先将 ssat_t结构体数组的 vs(定位时有效性)、azel(方位角、仰角)、resp(伪距残余)、resc(载波相位残余)和 snr(信号强度)都置为 0,然后再将实现定位后的 azel、snr赋予 ssat_t结构体数组,而 vs、resp则只赋值给那些对定位有贡献的卫星,没有参与定位的卫星,这两个属性值为 0。
  • 注意事项

    1. 这里只计算了接收机的钟差,而没有计算接收机的频漂,原因在于 estvel函数中虽然计算得到了接收机频漂,但并没有将其输出到 sol_t:dtr中。
    2. C语言中用 malloc申请的内存需要自己调用 free来予以回收,源码中的 matimatzeros等函数都只是申请了内存,并没有进行内存的回收,在使用这些函数时,用户必须自己调用 free来回收内存!源码中将使用这些函数的代码放置在同一行,在调用函数结尾处也统一进行内存回收,位置较为明显,不致于轻易忘记。
  • 我的疑惑

    1. 源码中将 obs[0].time作为星历选择时间传递给 satposs函数,这样对于每一颗观测卫星,都要使用第一颗观测卫星的数据接收时间作为选择星历的时间标准。是否应该每颗卫星都使用自己的观测时间?或者应该使用每颗卫星自己的信号发射时间?,还是说这点差别对选择合适的星历其实没有关系?
    2. 这里规定能够执行 raim_fde函数的前提是数目大于等于 6,感觉不是只要大于等于 5就可以了吗?

satposs

void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
             int ephopt, double *rs, double *dts, double *var, int *svh)
  • 所在文件:ephemeris.c
  • 功能说明:按照所观测到的卫星顺序计算出每颗卫星的位置、速度、{钟差、频漂}
  • 参数说明

    函数参数,9个:
    gtime_t  teph      I   time to select ephemeris (gpst)
    obsd_t   *obs      I   observation data
    int      n         I   number of observation data
    nav_t    *nav      I   navigation data
    int      ephopt    I   ephemeris option (EPHOPT_???)
    double   *rs       O   satellite positions and velocities,长度为6*n,{x,y,z,vx,vy,vz}(ecef)(m,m/s)
    double   *dts      O   satellite clocks,长度为2*n, {bias,drift} (s|s/s)
    double   *var      O   sat position and clock error variances (m^2)
    int      *svh      O   sat health flag (-1:correction not available)
    返回类型:
    none
  • 调用关系
    ```flow
    st_=>start: satposs
    ephclk1=>operation: ephclk
    satpos_=>operation: satpos
    cond=>condition: 钟差为0?
    ephclk2=>operation: ephclk
    e=>end: end

st_->ephclk1->satpos_->cond
cond(no)->e
cond(yes,rigth)->ephclk2->e
```
satposs ephclk satpos

  • 处理过程

    1. 按照观测数据的顺序,首先将将当前观测卫星的 rs、dts、var和svh数组的元素置 0。
    2. 通过判断某一频率下信号的伪距是否为 0,来得到此时所用的频率个数。注意,频率个数不能大于 NFREQ(默认为 3)。
    3. 用数据接收时间减去伪距信号传播时间,得到卫星信号的发射时间。
    4. 调用 ephclk函数,由广播星历计算出当前观测卫星的钟差。注意,此时的钟差是没有考虑相对论效应和 TGD的。
    5. 用 3中的信号发射时间减去 4中的钟偏,得到 GPS时间下的卫星信号发射时间。
    6. 调用 satpos函数,计算信号发射时刻卫星的 P(ecef,m)、V(ecef,m/s)、C((s|s/s))。注意,这里计算出的钟差是考虑了相对论效应的了,只是还没有考虑 TGD。
    7. 如果由 6中计算出的钟偏为 0,就再次调用 ephclk函数,将其计算出的卫星钟偏作为最终的结果。
  • 注意事项

    1. ephclk函数计算的钟偏是没有考虑相对论和 TGD的,而 satpos函数考虑了相对论,没有考虑 TGD。
  • 我的疑惑

    1. 对于处理过程中的第3步,数据接收时间减去伪距信号传播时间,这里的数据接收时间应该是有接收机得到的,本身应该是包含接收机钟差的,所以最终得到的信号发射时间应该也是不准确的。难道说接收机钟差较小,在此时可以忽略不计?
    2. ephclk函数最终是通过调用 eph2clk来计算卫星钟偏的,但对于后者,我有问题。见 eph2clk处我的疑惑
    3. 为什么要进行 7中操作?

estpos

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, sol_t *sol, double *azel, int *vsat,
           double *resp, char *msg)
  • 所在文件:pntpos.c
  • 功能说明:通过伪距实现绝对定位,计算出接收机的位置和钟差,顺带返回实现定位后每颗卫星的{方位角、仰角}、定位时有效性、定位后伪距残差。
  • 参数说明

    函数参数,13个:
    obsd_t   *obs      I   observation data
    int      n         I   number of observation data
    double   *rs       I   satellite positions and velocities,长度为6*n,{x,y,z,vx,vy,vz}(ecef)(m,m/s)
    double   *dts      I   satellite clocks,长度为2*n, {bias,drift} (s|s/s)
    double   *vare     I   sat position and clock error variances (m^2)
    int      *svh      I   sat health flag (-1:correction not available)
    nav_t    *nav      I   navigation data
    prcopt_t *opt      I   processing options
    sol_t    *sol      IO  solution
    double   *azel     IO  azimuth/elevation angle (rad)
    int      *vsat     IO  表征卫星在定位时是否有效
    double   *resp     IO  定位后伪距残差 (P-(r+c*dtr-c*dts+I+T))
    char     *msg      O   error message for error exit
    返回类型:
    int                O     (1:ok,0:error)
  • 调用关系
    ```flow
    st=>start: estpos
    rescode_=>operation: rescode
    lsq_=>operation: lsq
    valsol_=>operation: valsol
    e=>end: end

st->rescode_->lsq_->valsol_->e
```
estpos rescode lsq valsol

  • 处理过程

    1. sol->rr的前 3项赋值给 x数组。
    2. 开始迭代定位计算,首先调用 rescode函数,计算在当前接收机位置和钟差值的情况下,定位方程的右端部分 v(nv*1)、几何矩阵 H(NX*nv)、此时所得的伪距残余的方差 var、所有观测卫星的 azel{方位角、仰角}、定位时有效性 vsat、定位后伪距残差 resp、参与定位的卫星个数 ns和方程个数 nv
    3. 确定方程组中方程的个数要大于未知数的个数。
    4. 以伪距残余的标准差的倒数作为权重,对 H和 v分别左乘权重对角阵,得到加权之后的 H和 v。
    5. 调用 lsq函数,根据 $Delta x=(HH^T)^{-1}Hv$和 $Q=(HH^T)^{-1}$,得到当前 x的修改量和定位误差协方差矩阵中的权系数阵。
    6. 将 5中求得的 x加入到当前 x值中,得到更新之后的 x值。
    7. 如果 5中求得的修改量小于截断因子(目前是1e-4),则将 6中得到的 x值作为最终的定位结果,对 sol的相应参数赋值,之后再调用 valsol函数确认当前解是否符合要求(伪距残余小于某个 $chi^2$值和 GDOP小于某个门限值)。否则,进行下一次循环。
    8. 如果超过了规定的循环次数,则输出发散信息后,返回 0。
  • 注意事项

    1. 关于第 1步,如果是第一次定位,即输入的 sol为空,则 x初值为 0;如果之前有过定位,则通过 1中操作可以将上一历元的定位值作为该历元定位的初始值。
    2. 关于定位方程,书中的写法如下:
      $$
      G egin{bmatrix} Delta x Delta y Delta z delta t_u end{bmatrix} = b
      $$
      其中,
      $$
      G = egin{bmatrix}
      -e^1_x & -e^1_y & -e^1_z & 1
      -e^2_x & -e^2_y & -e^2_z & 1
      cdots & cdots & cdots & cdots
      -e^N_x & -e^N_y & -e^N_z & 1
      end{bmatrix}
      qquad
      b = egin{bmatrix}
      ho_r^1-(r_r^1+c·dt_r-c·dt_s^1+I^1+T^1)
      ho_r^2-(r_r^2+c·dt_r-c·dt_s^2+I^2+T^2)
      cdots
      ho_r^N-(r_r^N+c·dt_r-c·dt_s^N+I^N+T^N)
      end{bmatrix}
      $$
      而 RTKLIB中采用的方程是下面这样的(这里先假设未知数的个数为 4):
      $$
      H^T egin{bmatrix} Delta x Delta y Delta z delta t_u end{bmatrix} = b
      $$
      其中,$H=G^T$。
    3. 关于加权最小二乘,这里的权重值是对角阵,这是建立在假设不同测量值的误差之间是彼此独立的;另外,这个权重值并不单是距测量误差的,而是方程右端 b整体的测量误差。最后,大部分资料上这里都是把权重矩阵 W保留到方程的解的表达式当中,而这里是直接对 H和 v分别左乘权重对角阵,得到加权之后的 H和 v,其表示形式像是没有加权一样
    4. 如果某次迭代过程中步长小于门限值(1e-4),但经 valsol函数检验后该解无效,则会直接返回 0,并不会再进行下一次迭代计算。
    5. 因为该函数有两个返回路径,而且又调用了 mat函数来构建矩阵,所以在两个返回路径处都需要调用 free函数来回收内存。
    6. 源码中的 dtr实际上单位是 m,是乘以了光速之后的。
    7. 在对 sol结构体赋值时,并没有直接将接收机钟差 dtr赋值给 sol_dtr,而是通过在 sol中存储的是减去接收机钟差后的信号观测时间,来讲该信息包括到 sol中了。
    8. 源码中定位方程的个数 nv要大于有效观测卫星的个数 ns,这里为了防止亏秩,似乎又加了 3个未知数和观测方程。
    9. 在每一次重新调用 rescode函数时,其内部并没有对 v、H和 var清零处理,所以当方程数变少时,可能会存在尾部仍保留上一次数据的情况,但是因为数组相乘时都包含所需计算的长度,所以这种情况并不会对计算结果造成影响。
  • 我的疑惑

    $color{red}{1. 这里的 NX=7不明白,应该只有 4个未知数的啊!}$


raim_fde

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, sol_t *sol,
             double *azel, int *vsat, double *resp, char *msg)
  • 所在文件:pntpos.c
  • 功能说明:对计算得到的定位结果进行接收机自主正直性检测(RAIM)。通过再次使用 vsat数组,这里只会在对定位结果有贡献的卫星数据进行检测。
  • 参数说明

    函数参数,13个:
    obsd_t   *obs      I   observation data
    int      n         I   number of observation data
    double   *rs       I   satellite positions and velocities,长度为6*n,{x,y,z,vx,vy,vz}(ecef)(m,m/s)
    double   *dts      I   satellite clocks,长度为2*n, {bias,drift} (s|s/s)
    double   *vare     I   sat position and clock error variances (m^2)
    int      *svh      I   sat health flag (-1:correction not available)
    nav_t    *nav      I   navigation data
    prcopt_t *opt      I   processing options
    sol_t    *sol      IO  solution
    double   *azel     IO  azimuth/elevation angle (rad)
    int      *vsat     IO  表征卫星在定位时是否有效
    double   *resp     IO  定位后伪距残差 (P-(r+c*dtr-c*dts+I+T))
    char     *msg      O   error message for error exit
    返回类型:
    int                O     (1:ok,0:error)
  • 调用关系
    ```flow
    st=>start: raim_fde
    estpos_=>operation: estpos
    e=>end: end

st->estpos_->e
```
raim_fde estpos

  • 处理过程

    1. 关于观测卫星数目的循环,每次舍弃一颗卫星,计算使用余下卫星进行定位的定位值。
    2. 舍弃一颗卫星后,将剩下卫星的数据复制到一起,调用 estpos函数,计算使用余下卫星进行定位的定位值。
    3. 累加使用当前卫星实现定位后的伪距残差平方和与可用微信数目,如果 nvsat<5,则说明当前卫星数目过少,无法进行 RAIM_FDE操作。
    4. 计算伪距残差平方和的标准平均值,如果大于 rms,则说明当前定位结果更合理,将 stat置为 1,重新更新 solazelvsat(当前被舍弃的卫星,此值置为0)、resp等值,并将当前的 rms_e更新到 `rms"中。
    5. 继续弃用下一颗卫星,重复 2-4操作。总而言之,将同样是弃用一颗卫星条件下,伪距残差标准平均值最小的组合所得的结果作为最终的结果输出。
    6. 如果 stat不为 0,则说明在弃用卫星的前提下有更好的解出现,输出信息,指出弃用了哪科卫星。
    7. 使用 free函数回收相应内存。
  • 注意事项

    1. 使用了 matmalloc函数后要使用 free函数来回收内存。
    2. 源码中有很多关于 i、j、k的循环。其中,i表示最外面的大循环,每次将将第 i颗卫星舍弃不用,这是通过 if (j==i) continue实现的;j表示剩余使用的卫星的循环,每次进行相应数据的赋值;k表示参与定位的卫星的循环,与 j一起使用。
  • 我的疑惑

    1. 这里执行 RAIM至少要有 6颗可用卫星,可是我感觉 5颗就够了呀!

estvel

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)
  • 所在文件:pntpos.c
  • 功能说明:依靠多普勒频移测量值计算接收机的速度。
  • 参数说明

    函数参数,9个:
    obsd_t   *obs      I   observation data
    int      n         I   number of observation data
    double   *rs       I   satellite positions and velocities,长度为6*n,{x,y,z,vx,vy,vz}(ecef)(m,m/s)
    double   *dts      I   satellite clocks,长度为2*n, {bias,drift} (s|s/s)
    nav_t    *nav      I   navigation data
    prcopt_t *opt      I   processing options
    sol_t    *sol      IO  solution
    double   *azel     IO  azimuth/elevation angle (rad)
    int      *vsat     IO  表征卫星在定位时是否有效
    返回类型:
    int                O     (1:ok,0:error)
  • 调用关系
    ```flow
    st=>start: estvel
    resdop_=>operation: resdop
    lsq_=>operation: lsq
    e=>end: end

st->resdop_->lsq_->e
```
estvel resdop lsq

  • 处理过程

    1. 在最大迭代次数限制内,调用 resdop,计算定速方程组左边的几何矩阵和右端的速度残余,返回定速时所使用的卫星数目。
    2. 调用 lsq函数,解出 {速度、频漂}的步长,累加到 x中。
    3. 检查当前计算出的步长的绝对值是否小于 1E-6。是,则说明当前解已经很接近真实值了,将接收机三个方向上的速度存入到 sol_t.rr中;否,则进行下一次循环。
    4. 执行完所有迭代次数,使用 free回收内存。
  • 注意事项

    1. 最终向 sol_t类型存储定速解时,并没有存储所计算出的接收器时钟频漂。
    2. 这里不像定位时,初始值可能为上一历元的位置(从 sol中读取初始值),定速的初始值直接给定为 0.

ephclk

int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav, double *dts)
  • 所在文件:ephemeris.c
  • 功能说明:通过广播星历来确定卫星钟偏
  • 参数说明

    函数参数,5个:
    gtime_t  time      I   transmission time by satellite clock
    gtime_t  teph      I   time to select ephemeris (gpst)
    int      sat       I   satellite number (1-MAXSAT)
    nav_t    *nav      I   navigation data
    double   *dts      O   satellite clocks,长度为2*n, {bias,drift} (s|s/s)
    返回类型:
    int                O     (1:ok,0:error)
  • 调用关系
    ```flow
    st=>start: ephclk
    satsys_=>operation: satsys
    seleph_=>operation: seleph
    eph2clk_=>operation: eph2clk
    e=>end: end

st->satsys_->seleph_->eph2clk_->e
```
ephclk satsys seleph eph2clk

  • 处理过程

    1. 首先调用 satsys函数,根据卫星编号确定该卫星所属的导航系统和该卫星在该系统中的 PRN编号。
    2. 对于 GPS导航系统,调用 seleph函数来选择 toe值与星历选择时间标准 teph最近的那个星历。
    3. 调用 eph2clk函数,通过广播星历和信号发射时间计算出卫星钟差。
  • 注意事项

    1. 此时计算出的卫星钟偏是没有考虑相对论效应和 TGD的。
    2. 目前我还只关心 RTKLIB对于 GPS导航所进行的数据处理,所以这里在选择星历和计算钟差时都只介绍与 GPS系统有关的函数
  • 我的疑惑

    1. 见 eph2clk处

satpos

int satpos(gtime_t time, gtime_t teph, int sat, int ephopt, const nav_t *nav, 
           double *rs, double *dts, double *var, int *svh)
  • 所在文件:ephemeris.c
  • 功能说明:计算信号发射时刻卫星的 P(ecef,m)、V(ecef,m/s)、C((s|s/s))
  • 参数说明

    函数参数,9个:
    gtime_t time      I   time (gpst)
    gtime_t teph      I   time to select ephemeris (gpst)
    int     sat       I   satellite number
    nav_t   *nav      I   navigation data
    int     ephopt    I   ephemeris option (EPHOPT_???)
    double  *rs       O   sat position and velocity {x,y,z,vx,vy,vz} (ecef)(m|m/s)
    double  *dts      O   sat clock {bias,drift} (s|s/s)
    double  *var      O   sat position and clock error variance (m^2)
    int     *svh      O   sat health flag (-1:correction not available)
    返回类型:
    int               O     (1:ok,0:error)
  • 调用关系
    ```flow
    st=>start: satpos
    ephpos_=>operation: ephpos
    e=>end: end

st->ephpos_->e
```
satpos ephpos

  • 处理过程

    1. 判断星历选项的值,如果是 EPHOPT_BRDC,调用 ephpos函数,根据广播星历计算出算信号发射时刻卫星的 P、V、C
  • 注意事项

    1. 此时计算出的卫星钟差考虑了相对论,还没有考虑 TGD。
    2. 目前还只阅读了如何从广播星历中计算卫星 P、V、C的代码,关于如何从精密星历中计算,等对精密星历的理论背景有了更多了解之后再予以添加。

satsys

int satsys(int sat, int *prn)
  • 所在文件:rtkcmn.c
  • 功能说明:根据卫星编号确定该卫星所属的导航系统和该卫星在该系统中的 PRN编号
  • 参数说明

    函数参数,2个:
    int    sat       I   satellite number (1-MAXSAT)
    int    *prn      IO  satellite prn/slot number (NULL: no output)
    返回类型:
    int         satellite system (SYS_GPS,SYS_GLO,...)
  • 处理过程

    1. 为处理意外情况(卫星号不在 1-MAXSAT之内),先令卫星系统为 SYS_NONE
    2. 按照 TRKLIB中定义相应宏的顺序来判断是否是 GPS、GLO、GAL系统等,判断标准是将卫星号减去前面的导航系统所拥有的卫星个数,来判断剩余卫星个数是否小于等于本系统的卫星个数。
    3. 确定所属的系统后,通过加上最小卫星编号的 PRN再减去 1,得到该卫星在该系统中的 PRN编号。
  • 注意事项

    1. 这里的卫星号是从 1开始排序的,这也是很多函数中与之有关的数组在调用时形式写为 A[B.sat-1]

seleph

eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav)
  • 所在文件:ephemeris.c
  • 功能说明:从导航数据中选择星历。当 iode>=0时,选择与输入期号相同的星历;否则,选择 toe值与星历选择时间标准 time最近的那个星历。
  • 参数说明

    函数参数,4个:
    gtime_t  time      I   time to select ephemeris (gpst)
    int      sat       I   satellite number (1-MAXSAT)
    int      iode      I   星历数据期号
    nav_t    *nav      I   navigation data
    返回类型:
    eph_t *         星历数据
  • 处理过程

    1. 根据该卫星所属的导航系统给与星历参考时间的最大时间间隔 tmax赋予相应的值。
    2. 遍历导航数据,首先确保所查找星历的卫星号是否相同,接着确保星历期号是否相同。
    3. 接着确保星历选择时间与代查找星历的参考时间的间隔是否小于 tmax
    4. 对于通过了 2-3验证的星历,如果此时对于输入的期号,有 iode>=0,则该星历就是所要寻找的星历;否则,验证 3中的 t是否满足 t<=tmin。满足的话,就令 tmin=t,该星历目前是距离参考时间最近的。
    5. 循环 2-4步操作,遍历完导航数据中的所有星历。如果都没有符合条件的,就输出信息并返回 NULL;否则,返回所查找到的星历。
  • 注意事项

    1. 对于 GPS系统,星历数据期号每 2h更新一次,所以 RTKLIB中对 MAXDTOE的定义为 7200。
    2. 如果存在两个相邻时间段的星历,通过 4中令 tmin=t的操作可以最终查找出参考时间距 time最近的那个星历。
  • 我的疑惑

    1. 为什么 tmaxtmin都要加 1?
    2. IODE正常情况下应该都是 >=0的,为什么还要考虑 <0的情况?
    3. 考虑到星历中卫星的健康状况,这里在选择星历时是否也应该验证 eph.svh==0呢?

eph2clk

int eph2clk (gtime_t time, const eph_t *eph)
  • 所在文件:ephemeris.c
  • 功能说明:根据信号发射时间和广播星历,计算卫星钟差
  • 参数说明

    函数参数,2个
    gtime_t time     I   time by satellite clock (gpst)
    eph_t    *eph    I   broadcast ephemeris
    返回类型:
    double    satellite clock bias (s) without relativeity correction
  • 处理过程

    1. 计算与星历参考时间的偏差 dt = t-toc。
    2. 利用二项式校正计算出卫星钟差,从 dt中减去这部分,然后再进行一次上述操作,得到最终的 dt。
    3. 使用二项式校正得到最终的钟差。
  • 我的疑惑

    1. 看不懂上述处理过程,跟以往资料上都不一样。咋回事?

ephpos

int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
           int iode, double *rs, double *dts, double *var, int *svh)
  • 所在文件:ephemeris.c
  • 功能说明:根据广播星历计算出算信号发射时刻卫星的 P、V、C
  • 参数说明

    函数参数,9个
    gtime_t  time      I   transmission time by satellite clock
    gtime_t  teph      I   time to select ephemeris (gpst)
    int      sat       I   satellite number (1-MAXSAT)
    nav_t    *nav      I   navigation data
    int      iode      I   星历数据期号
    double   *rs       O   satellite positions and velocities,长度为6*n,{x,y,z,vx,vy,vz}(ecef)(m,m/s)
    double   *dts      O   satellite clocks,长度为2*n, {bias,drift} (s|s/s)
    double   *var      O   sat position and clock error variances (m^2)
    int      *svh      O   sat health flag (-1:correction not available)
    返回类型:
    int                O    (1:ok,0:error)
  • 调用关系
    ```flow
    st=>start: ephpos
    satsys_=>operation: satsys
    seleph_=>operation: seleph
    eph2pos_=>operation: eph2pos
    e=>end: end

st->satsys_->seleph_->eph2pos_->e
```
ephpos satsys seleph eph2pos

  • 处理过程

    1. 调用 satsys函数,确定该卫星所属的导航系统。
    2. 如果该卫星属于 GPS系统,则调用 seleph函数来选择广播星历。
    3. 根据选中的广播星历,调用 eph2pos函数来计算信号发射时刻卫星的 位置、钟差和相应结果的误差。
    4. 在信号发射时刻的基础上给定一个微小的时间间隔,再次计算新时刻的 P、V、C。与 3结合,通过扰动法计算出卫星的速度和频漂。
  • 注意事项

    1. 这里是使用扰动法计算卫星的速度和频漂,并没有使用那些位置和钟差公式对时间求导的结果。
    2. 由于是调用的 eph2pos函数,计算得到的钟差考虑了相对论效应,还没有考虑 TGD

eph2pos

void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, double *var)
  • 所在文件:ephemeris.c
  • 功能说明:根据广播星历计算出算信号发射时刻卫星的位置和钟差
  • 参数说明

    函数参数,5个
    gtime_t  time      I   transmission time by satellite clock
    eph_t    *eph      I   broadcast ephemeris
    double   *rs       O   satellite positions and velocities,长度为6*n,{x,y,z,vx,vy,vz}(ecef)(m,m/s)
    double   *dts      O   satellite clocks,长度为2*n, {bias,drift} (s|s/s)
    double   *var      O   sat position and clock error variances (m^2)
    返回类型:
    none
  • 处理过程

    1. 与大部分资料上计算卫星位置和钟差的过程是一样的,只是这里在计算偏近点角 E时采用的是牛顿法来进行迭代求解。
    2. 计算误差直接采用 URA值来标定,具体对应关系可在 ICD-GPS-200C P83中找到。
  • 注意事项

    1. 这里在计算钟差时,就与大部分资料上介绍的一样了,只进行了一次二项式校正。另外,这里的钟差考虑了相对论效应,并没有考虑 TGD。
    2. 在计算偏近点角 E时,这里采用的是牛顿法来进行迭代求解,这里与很多资料上可能不一样,具体内容可在 RTKLIB manual P143中找到。
    3. 补充几个程序中的参数说明:
      C mu, 地球引力常数 eph->A, 轨道长半径 omgea, 地球自转角速度

rescode

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,
                   double *v, double *H, double *var, double *azel, int *vsat,
                   double *resp, int *ns)
  • 所在文件:pntpos.c
  • 功能说明:计算在当前接收机位置和钟差值的情况下,定位方程的右端部分 v(nv*1)、几何矩阵 H(NX*nv)、此时所得的伪距残余的方差 var、所有观测卫星的 azel{方位角、仰角}、定位时有效性 vsat、定位后伪距残差 resp、参与定位的卫星个数 ns和方程个数 nv
  • 参数说明

    函数参数,17int      iter      I    迭代次数
    obsd_t   *obs      I    observation data
    int      n         I    number of observation data
    double   *rs       I   satellite positions and velocities,长度为6*n,{x,y,z,vx,vy,vz}(ecef)(m,m/s)
    double   *dts      I   satellite clocks,长度为2*n, {bias,drift} (s|s/s)
    double   *vare     I   sat position and clock error variances (m^2)
    int      *svh      I   sat health flag (-1:correction not available)
    nav_t    *nav      I   navigation data
    double   *x        I   本次迭代开始之前的定位值
    prcopt_t *opt      I   processing options
    double   *v        O   定位方程的右端部分,伪距残余
    double   *H        O   定位方程中的几何矩阵
    double   *var      O   参与定位的伪距残余方差
    double   *azel     O   对于当前定位值,每一颗观测卫星的 {方位角、高度角}
    int      *vsat     O   每一颗观测卫星在当前定位时是否有效
    double   *resp     O   每一颗观测卫星的伪距残余, (P-(r+c*dtr-c*dts+I+T))
    int      *ns       O   参与定位的卫星的个数
    返回类型:
    int                O   定位方程组的方程个数
  • 调用关系
    ```flow
    st=>start: rescode
    ecef2pos_=>operation: ecef2pos
    satsys_=>operation: satsys
    geodist_=>operation: geodist
    satazel_=>operation: satazel
    prange_=>operation: prange
    satexclude_=>operation: satexclude
    ionocorr_=>operation: ionocorr
    tropcorr_=>operation: tropcorr
    varerr_=>operation: varerr
    e=>end: end

st->ecef2pos_->satsys_->geodist_->satazel_->prange_->satexclude_->ionocorr_->tropcorr_->varerr_->e
```
rescode ecef2pos satsys geodist satazel prange satexclude ionocorr tropcorr varerr

  • 处理过程

    1. 将之前得到的定位解信息赋值给 rrdtr数组,以进行关于当前解的伪距残余的相关计算。
    2. 调用 ecef2pos函数,将上一步中得到的位置信息由 ECEF转化为大地坐标系。
    3. vsatazelresp数组置 0,因为在前后两次定位结果中,每颗卫星的上述信息都会发生变化。
    4. 调用 satsys函数,验证卫星编号是否合理及其所属的导航系统。
    5. 检测当前观测卫星是否和下一个相邻数据重复。是,则 i++后继续下一次循环;否,则进入下一步。
    6. 调用 geodist函数,计算卫星和当前接收机位置之间的几何距离和 receiver-to-satellite方向的单位向量。然后检验几何距离是否 >0。
    7. 调用 satazel函数,计算在接收机位置处的站心坐标系中卫星的方位角和仰角,检验仰角是否 $geq$截断值。
    8. 调用 prange函数,得到伪距值。
    9. 可以在处理选项中事先指定只选用哪些导航系统或卫星来进行定位,这是通过调用 satexclude函数完成的。
    10. 调用 ionocorr函数,计算电离层延时(m)。
    11. 上一步中所得的电离层延时是建立在 L1信号上的,当使用其它频率信号时,依据所用信号频组中第一个频率的波长与 L1波长的关系,对上一步得到的电离层延时进行修正。
    12. 调用 tropcorr函数,计算对流层延时(m)。
    13. 由 $ ho_r^i-(r_r^i+dt_r-c·dt_s^i+I^i+T^i)$,计算出此时的伪距残余。
    14. 组装几何矩阵,前 3行为 6中计算得到的视线单位向量的反向,第 4行为 1,其它行为 0。
    15. 将参与定位的卫星的定位有效性标志设为 1,给当前卫星的伪距残余赋值,参与定位的卫星个数 ns加 1.
    16. 调用 varerr函数,计算此时的导航系统误差(可能会包括 IFLC选项时的电离层延时),然后累加计算用户测距误差(URE)。
    17. 为了防止亏秩,人为的添加了几组观测方程。
  • 注意事项

    1. 返回值 vresp的主要区别在于长度不一致, v是需要参与定位方程组的解算的,维度为 nv1;而 resp仅表示所有观测卫星的伪距残余,维度为 n1,对于没有参与定位的卫星,该值为 0。
    2. 源码中 dtr的单位是 m。
    3. 16中的 URE值包括 ①卫星星历和钟差的误差 ②大气延时误差 ③伪距测量的码偏移误差 ④导航系统的误差
  • 我的疑惑

    1. 关于 5中的去除重复数据的过程,有以下几个看法:
      ① 这样做的前提是相同卫星的重复数据必须相邻出现!
      ② 为什么在这里要进行重复数据检测,在构建 obsd_t结构体时就可以进行这项工作呀?
      ③ 5中当数据重复时,i++后继续下一次循环,这样的话会直接略去后面所重复的数据,这样做就会将两个相邻重复数据都忽略掉,就相当于重复数据都不使用。感觉可以用其中一个的啊!
    2. 11步中,为什么要选择所用信号频组中第一个频率的波长来进行电离层延时修正呢?还有,电离层延时的值发生了改变,那这里的方差是否也需要跟着一起改变呢?
    3. 在计算电离/对流层延时时,均传入了 iter>0?opt->ionoopt:IONOOPT_BRDCiter>0?opt->tropopt:TROPOPT_SAAS参数,都强调了当 iter==0时,会强制使用 KlobucharSaastamoinen模型。这会不会是因为这两种模型都是属于对接收机位置不是非常敏感的类型?
    4. 这里亏秩应该就是欠定方程的意思吧?这里 17中的操作没有看懂,也没能找到相关理论依据

lsq

int lsq(const double *A, const double *y, int n, int m, double *x, double *Q)
  • 所在文件:rtkcmn.c
  • 功能说明:计算得到方程 $x=(AA^T)^{-1}Ay$ 左边的值和该值的协方差矩阵 $Q=(AA^T)^{-1}$。
  • 参数说明

    函数参数,6double *A        I   transpose of (weighted) design matrix (n x m)
    double *y        I   (weighted) measurements (m x 1)
    int    n,m       I   number of parameters and measurements (n<=m)
    double *x        O   estmated parameters (n x 1)
    double *Q        O   esimated parameters covariance matrix (n x n)
    返回类型:
    int              O   (0:ok,0>:error)
  • 调用关系
    ```flow
    st=>start: lsq
    matmul_=>operation: matmul
    matinv_=>operation: matinv
    e=>end: end

st->matmul_->matinv_->e
```
lsq matmul

  • 处理过程

    1. 首先计算右半部分 $A_y=Ay$。
    2. 再计算左半部分括号里面的值 $Q=AA^T$。
    3. 计算 Q矩阵的逆 $Q^{-1}$,但仍存储在 Q中,最后再右乘 $A_y$,得到 x的值。
  • 注意事项

    1. 对于加权最小二乘,可以直接在调用该函数之前直接将 A、y进行加权处理,之后在调用该函数,这样得到的就是加权最小二乘的解。
    2. 所有的矩阵都是列优先存储的,对于整个源代码来说,矩阵都是这样存储的。所以对于代码中出现的一维矩阵,基本都应该是列向量。在阅读数组下标时,记住这一点是非常重要的。
    3. 矩阵求逆并不简单,尤其是对于接近奇异的矩阵。但是由于这是个基本功能,并不打算继续深入下去。

valsol

int valsol(const double *azel, const int *vsat, int n,
           const prcopt_t *opt, const double *v, int nv, int nx, char *msg)
  • 所在文件:pntpos.c
  • 功能说明:确认当前解是否符合要求,即伪距残余小于某个 $chi^2$值和 GDOP小于某个门限值)
  • 参数说明

    函数参数,8double   *azel     I   azimuth/elevation angle (rad)
    int      *vsat     I   表征卫星在定位时是否有效
    int      n         I   number of observation data
    prcopt_t *opt      I   processing options
    double   *v        I   定位后伪距残差 (P-(r+c*dtr-c*dts+I+T))
    int      nv        I   定位方程的方程个数
    int      nx        I   未知数的个数
    char     *msg      O   error message for error exit
    返回类型:
    int                O    (1:ok,0:error)
  • 调用关系
    ```flow
    st=>start: valsol
    dops_=>operation: dops
    e=>end: end

st->dops_->e
```
valsol dops

  • 处理过程

    1. 先计算定位后伪距残余平方加权和 vv
    2. 检查是否满足 $vv>chi^2(nv-nx-1)$。是,则说明此时的定位解误差过大,返回 0;否则,转到下一步。
    3. 复制 azel,这里只复制那些对于定位结果有贡献的卫星的 zael值,并且统计实现定位时所用卫星的数目。
    4. 调用 dops函数,计算各种精度因子(DOP),检验是否有 0<GDOP<max。否,则说明该定位解的精度不符合要求,返回 0;是,则返回 1。
  • 注意事项

    1. 关于 2中的 $chi^2$检验,这里这里与 RTKLIB manual P160中不一致(但与教材一致),文档中满足 $frac{v^Tv}{nv-nx-1}>chi^2(nv-nx-1)$时就会不合格。与文档中相比,这里的写法将会放宽对于位置解的检验。

matmul

源码中定义了两个 matmul函数,一个是在包含了 LAPACK/BLAS/MKL库使用,调用其中的 degmn函数来完成矩阵相乘操作。这里主要说明在没有包含上述库时自定义的矩阵相乘函数。

void matmul(const char *tr, int n, int k, int m, double alpha,
            const double *A, const double *B, double beta, double *C)
  • 所在文件:rtkcmn.c
  • 功能说明:可进行如下矩阵运算 C = alphaAB + beta*C,并且能通过 tr标志来选择是否对 A、B进行转置
  • 参数说明

    函数参数,6char     *tr       I   transpose flags ("N":normal,"T":transpose)
    int      n,k,m     I   size of (transposed) matrix A,B
    double   alpha     I   alpha
    double   *A,*B     I   (transposed) matrix A (n x m), B (m x k)
    double   beta      I   beta
    double   *C        IO  matrix C (n x k)
    返回类型:
    none
  • 处理过程

    1. 根据 tr的值确定矩阵相乘标志,即
      $$
      egin{matrix}
      AB ightarrow NN ightarrow 1
      A
      B^T ightarrow NT ightarrow 2
      A^TB ightarrow TN ightarrow 3
      A^T
      B^T ightarrow TT ightarrow 4
      end{matrix}
      $$
    2. 按照 f的值,分别执行相应的元素相乘并累加操作。
    3. 对于 2中得到的乘积 tmp,执行 C = alphatmp + betaC操作。
  • 注意事项

    1. 在调用该函数时,要确保矩阵的维度是否和上述参数说明一致
    2. 所有的矩阵都是列优先存储的,记住这一点,对于看明白 2中不同相乘方式时,元素如何相乘累加是至关重要的。
    3. 矩阵求逆并不简单,尤其是对于接近奇异的矩阵。但是由于这是个基本功能,并不打算继续深入下去。

dops

void dops(int ns, const double *azel, double elmin, double *dop)
  • 所在文件:rtkcmn.c
  • 功能说明:由站心坐标系时的权系数矩阵表达式$ ilde{H} = ( ilde{G} ilde{G}^T)^{-1}$,计算各种精度因子。
  • 参数说明

    函数参数,4int      ns        I   number of satellites
    double   *azel     I   satellite azimuth/elevation angle (rad)
    double   elmin     I   elevation cutoff angle (rad)
    double   *dop      O   DOPs {GDOP,PDOP,HDOP,VDOP}
    返回类型:
    none
  • 调用关系
    ```flow
    st=>start: dops
    matmul_=>operation: matmul
    matinv_=>operation: matinv
    e=>end: end

st->matmul_->matinv_->e
```
dops matmul matinv

  • 处理过程

    1. 先按照如下站心坐标系时的几何矩阵 $ ilde{G}$的表达式求出其值。
      $$
      ilde{G} = egin{bmatrix}
      cos heta^1sinalpha^1 & cos heta^2sinalpha^2 & vdots & cos heta^Nsinalpha^N
      cos heta^1cosalpha^1 & cos heta^2cosalpha^2 & vdots & cos heta^Ncosalpha^N
      sin heta^1 & sin heta^2 & vdots & sin heta^N
      1 & 1 & vdots & 1
      end{bmatrix}
      $$
    2. 检验上述矩阵的列数是否$geq$ 4。
    3. 调用 matmul和matinv函数,计算出 $ ilde{H} = ( ilde{G} ilde{G}^T)^{-1}$的值。
    4. 如果能正确计算出逆矩阵,就按照顺序计算出 GDOP、PDOP、HDOP和 VDOP的值。
  • 我的疑惑

    1. 1中几何矩阵 $ ilde{G}$与书中的不一致,前 3行均少了一个负号,我自己推导之后也觉得应该有负号,即为
      $$
      ilde{G} = egin{bmatrix}
      -cos heta^1sinalpha^1 & -cos heta^2sinalpha^2 & vdots & -cos heta^Nsinalpha^N
      -cos heta^1cosalpha^1 & -cos heta^2cosalpha^2 & vdots & -cos heta^Ncosalpha^N
      -sin heta^1 & -sin heta^2 & vdots & -sin heta^N
      1 & 1 & vdots & 1
      end{bmatrix}
      $$
      不过,由于 $ ilde{H} = ( ilde{G} ilde{G}^T)^{-1}$,在括号里面的矩阵相乘时,是否有负号只对底边靠左 3个元素和右边靠上 3个元素有影响(多了个负号),然后再进行求逆之后,前 3行是否有负号就对对角线上的元素似乎没有影响了。
    2. 感觉在 rescode函数中,在检验一个观测卫星的伪距信息是否可用时,已经进行过是否大于截断高度角的检测了。这里所用的卫星仰角又都是属于参与了定位的卫星,所以感觉这里应该不需要再进行一次高度角检测了吧?

ecef2enu

void ecef2enu(const double *pos, const double *r, double *e)
  • 所在文件:rtkcmn.c
  • 功能说明:将 ECEF坐标系(X、Y、Z)中的向量转换成站心坐标系。
  • 参数说明

    函数参数,3double *pos      I   geodetic position {lat,lon} (rad)
    double *r        I   vector in ecef coordinate {x,y,z}
    double *e        O   vector in local tangental coordinate {e,n,u}
    返回类型:
    none
  • 调用关系
    ```flow
    st=>start: ecef2enu
    xyz2enu_=>operation: xyz2enu
    matmul_=>operation: matmul
    e=>end: end

st->xyz2enu_->matmul_->e
```
ecef2enu xyz2enu matmul

  • 处理过程

    1. 先调用 xyz2enu函数计算此时的坐标变换矩阵 E。
    2. 调用 matmul计算 $E*r$的值,即为目标值。

xyz2enu

void xyz2enu(const double *pos, double *E)
  • 所在文件:rtkcmn.c
  • 功能说明:计算将ECEF中的向量转换到站心坐标系中的转换矩阵。
  • 参数说明

    函数参数,2double *pos      I   geodetic position {lat,lon} (rad)
    double *E        O   vector in local tangental coordinate {e,n,u}
    返回类型:
    none
  • 处理过程

    1. 按照大部分资料上的写法计算 3*3的矩阵,优先按列存储。

ecef2pos

void ecef2pos(const double *r, double *pos)
  • 所在文件:rtkcmn.c
  • 功能说明:将 ECEF坐标系(X、Y、Z)转换成大地坐标系($lambda、phi、h$)。
  • 参数说明

    函数参数,2double  *r        I   ecef position {x,y,z} (m)
    double  *pos      O   geodetic position {lat,lon,h} (rad,m)
    返回类型:
    none
  • 处理过程

    这里采用的方法与很多资料上的并不一致,而关于源码中方法的具体理论推导和计算过程,见 ECEF和大地坐标系的相互转化一文。


geodist

看完这篇文章有何感觉?

  • 0人
      雷囧
  • 0人
      恶心
  • 0人
      期待
  • 1人
      难过
  • 0人
      超赞
  • 0人
      路过
  •  
    0
 

一周热点资讯

市园林部门为街道树木“把脉看病”神东布尔台煤矿供热锅炉燃煤达标治理工程通过竣工预验收 “这家伙什么都好就是花心不够好。”黄药师看到刘皓和小龙女,李莫愁几人的时候心里想道,不过一想到自己女儿目前的状况,心里也是一阵郁闷,对郭靖更加不爽了,如果不是郭靖的话,自己的女儿嫁给这样的人多完美啊,哪里会像现在这样一个三十多岁的美妇嫁给刘皓,这样对追求完美的黄药师来说当然郁闷了。[详细]
最新福利内涵动态GIF图 在学校就差没干过老师胡志强:对照验收标准打响“创卫”决战 大师的出场人员安排看上去很随意,但却恰到好处的克制了对手。利用对手的特点占据了上风,如果不是史莱克学院的出场名单中并没有唐三的存在。恐怕此时单是士气,雷霆学院就已经要出现问题了。[详细]

八卦周边

《长安三怪探》热播 王茂蕾变身痴汉惹猜疑Ella晒与吴尊十年前旧照 网友:居然没怎么变 “那走吧。”唐欣的班主任露出了一丝笑容,说道。只是在唐欣的眼中那笑容却是有些肮脏。[详细]
周玲安美国街拍小秀性感 嘻哈装扮十分抢镜电影《谜域之噬魂岭》荣获美国国际恐怖电影节大奖 压力陡然一轻,宁荣荣的心却沉了下去,她当然知道施展武魂真身对于一名魂圣的魂力消耗有多大,更何况之前唐三还帮助其他人通关,魂力消耗也不小,绝非先前那一个时辰的休息就能为安全恢复的,为了自己,三哥承受了那么大,之后他还要带着小舞一起冲级三百三十三级台阶啊!一百多级的压力就如此恐怖了,那三百多级的压力可想而知,就算他的魂力已经高达七十六级那也是难以逾越的天堑。[详细]
游迅简介 | 商务合作 | 广告服务 | 人才招聘 | 法律声明 | 内容导航 | 合作伙伴 | 游戏帮助 | 问题反馈

CopyRight?2004年-20162016201620162016年 迅网 All Rights Reserved
备案编号:沪ICP备12047477号-6