1
0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-06-15 09:39:57 +00:00
gnss-sdr/src/algorithms/libs/rtklib/rtklib_rtkpos.cc

2198 lines
81 KiB
C++
Raw Normal View History

2017-04-24 15:01:35 +00:00
/*!
* \file rtklib_rtkpos.cc
* \brief rtklib ppp-related functions
* \authors <ul>
* <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* </ul>
*
* This is a derived work from RTKLIB http://www.rtklib.com/
* The original source code at https://github.com/tomojitakasu/RTKLIB is
* released under the BSD 2-clause license with an additional exclusive clause
* that does not apply here. This additional clause is reproduced below:
*
* " The software package includes some companion executive binaries or shared
* libraries necessary to execute APs on Windows. These licenses succeed to the
* original ones of these software. "
*
* Neither the executive binaries nor the shared libraries are required by, used
* or included in GNSS-SDR.
*
* -------------------------------------------------------------------------
* Copyright (C) 2007-2013, T. Takasu
* Copyright (C) 2017, Javier Arribas
* Copyright (C) 2017, Carles Fernandez
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are
* met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
* HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*
*----------------------------------------------------------------------------*/
2017-05-06 08:01:54 +00:00
#include <iostream>
2017-04-24 15:01:35 +00:00
#include "rtklib_rtkpos.h"
#include "rtklib_pntpos.h"
2017-04-24 22:48:13 +00:00
#include "rtklib_ephemeris.h"
#include "rtklib_ppp.h"
#include "rtklib_tides.h"
#include "rtklib_lambda.h"
2017-05-02 18:01:21 +00:00
static int resamb_WLNL(rtk_t *rtk __attribute((unused)), const obsd_t *obs __attribute((unused)), const int *sat __attribute((unused)),
const int *iu __attribute((unused)), const int *ir __attribute((unused)), int ns __attribute__((unused)), const nav_t *nav __attribute((unused)),
const double *azel __attribute((unused))) {return 0;}
static int resamb_TCAR(rtk_t *rtk __attribute((unused)), const obsd_t *obs __attribute((unused)), const int *sat __attribute((unused)),
const int *iu __attribute((unused)), const int *ir __attribute((unused)), int ns __attribute((unused)), const nav_t *nav __attribute((unused)),
const double *azel __attribute((unused))) {return 0;}
/* global variables ----------------------------------------------------------*/
static int statlevel = 0; /* rtk status output level (0:off) */
static FILE *fp_stat = NULL; /* rtk status file pointer */
static char file_stat[1024] = ""; /* rtk status file original path */
static gtime_t time_stat = {0, 0}; /* rtk status file time */
2017-04-24 22:48:13 +00:00
/* open solution status file ---------------------------------------------------
* open solution status file and set output level
* args : char *file I rtk status file
* int level I rtk status level (0: off)
* return : status (1:ok,0:error)
* notes : file can constain time keywords (%Y,%y,%m...) defined in reppath().
* The time to replace keywords is based on UTC of CPU time.
* output : solution status file record format
*
* $POS,week,tow,stat,posx,posy,posz,posxf,posyf,poszf
* week/tow : gps week no/time of week (s)
* stat : solution status
* posx/posy/posz : position x/y/z ecef (m) float
* posxf/posyf/poszf : position x/y/z ecef (m) fixed
*
* $VELACC,week,tow,stat,vele,veln,velu,acce,accn,accu,velef,velnf,veluf,accef,accnf,accuf
* week/tow : gps week no/time of week (s)
* stat : solution status
* vele/veln/velu : velocity e/n/u (m/s) float
* acce/accn/accu : acceleration e/n/u (m/s^2) float
* velef/velnf/veluf : velocity e/n/u (m/s) fixed
* accef/accnf/accuf : acceleration e/n/u (m/s^2) fixed
*
* $CLK,week,tow,stat,clk1,clk2,clk3,clk4
* week/tow : gps week no/time of week (s)
* stat : solution status
* clk1 : receiver clock bias GPS (ns)
* clk2 : receiver clock bias GLO-GPS (ns)
* clk3 : receiver clock bias GAL-GPS (ns)
* clk4 : receiver clock bias BDS-GPS (ns)
*
* $ION,week,tow,stat,sat,az,el,ion,ion-fixed
* week/tow : gps week no/time of week (s)
* stat : solution status
* sat : satellite id
* az/el : azimuth/elevation angle(deg)
* ion : vertical ionospheric delay L1 (m) float
* ion-fixed: vertical ionospheric delay L1 (m) fixed
*
* $TROP,week,tow,stat,rcv,ztd,ztdf
* week/tow : gps week no/time of week (s)
* stat : solution status
* rcv : receiver (1:rover,2:base station)
* ztd : zenith total delay (m) float
* ztdf : zenith total delay (m) fixed
*
* $HWBIAS,week,tow,stat,frq,bias,biasf
* week/tow : gps week no/time of week (s)
* stat : solution status
* frq : frequency (1:L1,2:L2,...)
* bias : h/w bias coefficient (m/MHz) float
* biasf : h/w bias coefficient (m/MHz) fixed
*
* $SAT,week,tow,sat,frq,az,el,resp,resc,vsat,snr,fix,slip,lock,outc,slipc,rejc
* week/tow : gps week no/time of week (s)
* sat/frq : satellite id/frequency (1:L1,2:L2,...)
* az/el : azimuth/elevation angle (deg)
* resp : pseudorange residual (m)
* resc : carrier-phase residual (m)
* vsat : valid data flag (0:invalid,1:valid)
* snr : signal strength (dbHz)
* fix : ambiguity flag (0:no data,1:float,2:fixed,3:hold,4:ppp)
* slip : cycle-slip flag (bit1:slip,bit2:parity unknown)
* lock : carrier-lock count
* outc : data outage count
* slipc : cycle-slip count
* rejc : data reject (outlier) count
*
*-----------------------------------------------------------------------------*/
int rtkopenstat(const char *file, int level)
2017-04-24 15:01:35 +00:00
{
gtime_t time = utc2gpst(timeget());
2017-04-24 22:48:13 +00:00
char path[1024];
trace(3, "rtkopenstat: file=%s level=%d\n", file, level);
2017-04-24 22:48:13 +00:00
if (level <= 0) return 0;
2017-04-24 22:48:13 +00:00
reppath(file, path, time, "","");
2017-04-24 22:48:13 +00:00
if (!(fp_stat = fopen(path, "w")))
2017-04-24 22:48:13 +00:00
{
trace(1, "rtkopenstat: file open error path=%s\n", path);
2017-04-24 22:48:13 +00:00
return 0;
}
strcpy(file_stat, file);
time_stat = time;
statlevel = level;
2017-04-24 22:48:13 +00:00
return 1;
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
/* close solution status file --------------------------------------------------
* close solution status file
* args : none
* return : none
*-----------------------------------------------------------------------------*/
void rtkclosestat(void)
2017-04-24 15:01:35 +00:00
{
trace(3, "rtkclosestat:\n");
2017-04-24 22:48:13 +00:00
if (fp_stat) fclose(fp_stat);
fp_stat = NULL;
file_stat[0] = '\0';
statlevel = 0;
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
/* write solution status to buffer -------------------------------------------*/
2017-05-06 08:01:54 +00:00
void rtkoutstat(rtk_t *rtk, char *buff)
2017-04-24 15:01:35 +00:00
{
2017-05-06 08:01:54 +00:00
ssat_t *ssat;
double tow,pos[3],vel[3],acc[3],vela[3]={0},acca[3]={0},xa[3];
int i,j,week,est,nfreq,nf=NF_RTK(&rtk->opt);
char id[32];
if (statlevel<=0||!fp_stat) return;
trace(3,"outsolstat:\n");
/* swap solution status file */
swapsolstat();
est=rtk->opt.mode>=PMODE_DGPS;
nfreq=est?nf:1;
tow=time2gpst(rtk->sol.time,&week);
/* receiver position */
if (est) {
for (i=0;i<3;i++) xa[i]=i<rtk->na?rtk->xa[i]:0.0;
fprintf(fp_stat,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow,
rtk->sol.stat,rtk->x[0],rtk->x[1],rtk->x[2],xa[0],xa[1],xa[2]);
}
else {
fprintf(fp_stat,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow,
rtk->sol.stat,rtk->sol.rr[0],rtk->sol.rr[1],rtk->sol.rr[2],
0.0,0.0,0.0);
}
/* receiver velocity and acceleration */
if (est&&rtk->opt.dynamics) {
ecef2pos(rtk->sol.rr,pos);
ecef2enu(pos,rtk->x+3,vel);
ecef2enu(pos,rtk->x+6,acc);
if (rtk->na>=6) ecef2enu(pos,rtk->xa+3,vela);
if (rtk->na>=9) ecef2enu(pos,rtk->xa+6,acca);
fprintf(fp_stat,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
week,tow,rtk->sol.stat,vel[0],vel[1],vel[2],acc[0],acc[1],acc[2],
vela[0],vela[1],vela[2],acca[0],acca[1],acca[2]);
}
else {
ecef2pos(rtk->sol.rr,pos);
ecef2enu(pos,rtk->sol.rr+3,vel);
fprintf(fp_stat,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
week,tow,rtk->sol.stat,vel[0],vel[1],vel[2],
0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0);
}
/* receiver clocks */
fprintf(fp_stat,"$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n",
week,tow,rtk->sol.stat,1,rtk->sol.dtr[0]*1E9,rtk->sol.dtr[1]*1E9,
rtk->sol.dtr[2]*1E9,rtk->sol.dtr[3]*1E9);
/* ionospheric parameters */
if (est&&rtk->opt.ionoopt==IONOOPT_EST) {
for (i=0;i<MAXSAT;i++) {
ssat=rtk->ssat+i;
if (!ssat->vs) continue;
satno2id(i+1,id);
j=II_RTK(i+1,&rtk->opt);
xa[0]=j<rtk->na?rtk->xa[j]:0.0;
fprintf(fp_stat,"$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n",week,tow,rtk->sol.stat,
id,ssat->azel[0]*R2D,ssat->azel[1]*R2D,rtk->x[j],xa[0]);
}
}
/* tropospheric parameters */
if (est&&(rtk->opt.tropopt==TROPOPT_EST||rtk->opt.tropopt==TROPOPT_ESTG)) {
for (i=0;i<2;i++) {
j=IT_RTK(i,&rtk->opt);
xa[0]=j<rtk->na?rtk->xa[j]:0.0;
fprintf(fp_stat,"$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat,
i+1,rtk->x[j],xa[0]);
}
}
/* receiver h/w bias */
if (est&&rtk->opt.glomodear==2) {
for (i=0;i<nfreq;i++) {
j=IL_RTK(i,&rtk->opt);
xa[0]=j<rtk->na?rtk->xa[j]:0.0;
fprintf(fp_stat,"$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat,
i+1,rtk->x[j],xa[0]);
}
}
if (rtk->sol.stat==SOLQ_NONE||statlevel<=1) return;
/* residuals and status */
for (i=0;i<MAXSAT;i++) {
ssat=rtk->ssat+i;
if (!ssat->vs) continue;
satno2id(i+1,id);
for (j=0;j<nfreq;j++) {
fprintf(fp_stat,"$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\n",
week,tow,id,j+1,ssat->azel[0]*R2D,ssat->azel[1]*R2D,
ssat->resp [j],ssat->resc[j], ssat->vsat[j],ssat->snr[j]*0.25,
ssat->fix [j],ssat->slip[j]&3,ssat->lock[j],ssat->outc[j],
ssat->slipc[j],ssat->rejc[j]);
}
}
2017-04-24 22:48:13 +00:00
}
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* swap solution status file -------------------------------------------------*/
void swapsolstat(void)
{
gtime_t time = utc2gpst(timeget());
2017-04-24 22:48:13 +00:00
char path[1024];
if ((int)(time2gpst(time , NULL)/INT_SWAP_STAT) ==
(int)(time2gpst(time_stat, NULL)/INT_SWAP_STAT))
2017-04-24 15:01:35 +00:00
{
2017-04-24 22:48:13 +00:00
return;
2017-04-24 15:01:35 +00:00
}
time_stat = time;
2017-04-24 22:48:13 +00:00
if (!reppath(file_stat, path, time, "",""))
2017-04-24 15:01:35 +00:00
{
2017-04-24 22:48:13 +00:00
return;
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
if (fp_stat) fclose(fp_stat);
if (!(fp_stat = fopen(path, "w")))
2017-04-24 22:48:13 +00:00
{
trace(2, "swapsolstat: file open error path=%s\n", path);
2017-04-24 22:48:13 +00:00
return;
}
trace(3, "swapsolstat: path=%s\n", path);
2017-04-24 22:48:13 +00:00
}
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* output solution status ----------------------------------------------------*/
void outsolstat(rtk_t *rtk)
{
2017-05-06 08:01:54 +00:00
ssat_t *ssat;
double tow,pos[3],vel[3],acc[3],vela[3]={0},acca[3]={0},xa[3];
int i,j,week,est,nfreq,nf=NF_RTK(&rtk->opt);
char id[32];
2017-04-24 15:01:35 +00:00
2017-05-06 08:01:54 +00:00
if (statlevel<=0||!fp_stat) return;
2017-04-24 15:01:35 +00:00
2017-05-06 08:01:54 +00:00
trace(3,"outsolstat:\n");
2017-04-24 22:48:13 +00:00
2017-05-06 08:01:54 +00:00
/* swap solution status file */
swapsolstat();
2017-04-24 22:48:13 +00:00
2017-05-06 08:01:54 +00:00
est=rtk->opt.mode >= PMODE_DGPS;
nfreq=est?nf:1;
tow=time2gpst(rtk->sol.time,&week);
2017-05-06 08:01:54 +00:00
/* receiver position */
if (est) {
for (i=0;i<3;i++) xa[i]=i<rtk->na?rtk->xa[i]:0.0;
fprintf(fp_stat,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow,
rtk->sol.stat,rtk->x[0],rtk->x[1],rtk->x[2],xa[0],xa[1],xa[2]);
}
else {
fprintf(fp_stat,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow,
rtk->sol.stat,rtk->sol.rr[0],rtk->sol.rr[1],rtk->sol.rr[2],
0.0,0.0,0.0);
}
/* receiver velocity and acceleration */
if (est&&rtk->opt.dynamics) {
ecef2pos(rtk->sol.rr,pos);
ecef2enu(pos,rtk->x+3,vel);
ecef2enu(pos,rtk->x+6,acc);
if (rtk->na>=6) ecef2enu(pos,rtk->xa+3,vela);
if (rtk->na>=9) ecef2enu(pos,rtk->xa+6,acca);
fprintf(fp_stat,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
week,tow,rtk->sol.stat,vel[0],vel[1],vel[2],acc[0],acc[1],acc[2],
vela[0],vela[1],vela[2],acca[0],acca[1],acca[2]);
}
else {
ecef2pos(rtk->sol.rr,pos);
ecef2enu(pos,rtk->sol.rr+3,vel);
fprintf(fp_stat,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
week,tow,rtk->sol.stat,vel[0],vel[1],vel[2],
0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0);
}
/* receiver clocks */
fprintf(fp_stat,"$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n",
week,tow,rtk->sol.stat,1,rtk->sol.dtr[0]*1E9,rtk->sol.dtr[1]*1E9,
rtk->sol.dtr[2]*1E9,rtk->sol.dtr[3]*1E9);
/* ionospheric parameters */
if (est&&rtk->opt.ionoopt==IONOOPT_EST) {
for (i=0;i<MAXSAT;i++) {
ssat=rtk->ssat+i;
if (!ssat->vs) continue;
satno2id(i+1,id);
j=II_RTK(i+1,&rtk->opt);
xa[0]=j<rtk->na?rtk->xa[j]:0.0;
fprintf(fp_stat,"$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n",week,tow,rtk->sol.stat,
id,ssat->azel[0]*R2D,ssat->azel[1]*R2D,rtk->x[j],xa[0]);
}
}
/* tropospheric parameters */
if (est&&(rtk->opt.tropopt==TROPOPT_EST||rtk->opt.tropopt==TROPOPT_ESTG)) {
for (i=0;i<2;i++) {
j=IT_RTK(i,&rtk->opt);
xa[0]=j<rtk->na?rtk->xa[j]:0.0;
fprintf(fp_stat,"$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat,
i+1,rtk->x[j],xa[0]);
}
}
/* receiver h/w bias */
if (est&&rtk->opt.glomodear==2) {
for (i=0;i<nfreq;i++) {
j=IL_RTK(i,&rtk->opt);
xa[0]=j<rtk->na?rtk->xa[j]:0.0;
fprintf(fp_stat,"$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat,
i+1,rtk->x[j],xa[0]);
}
}
if (rtk->sol.stat==SOLQ_NONE||statlevel<=1) return;
/* residuals and status */
for (i=0;i<MAXSAT;i++) {
ssat=rtk->ssat+i;
if (!ssat->vs) continue;
satno2id(i+1,id);
for (j=0;j<nfreq;j++) {
fprintf(fp_stat,"$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\n",
week,tow,id,j+1,ssat->azel[0]*R2D,ssat->azel[1]*R2D,
ssat->resp [j],ssat->resc[j], ssat->vsat[j],ssat->snr[j]*0.25,
ssat->fix [j],ssat->slip[j]&3,ssat->lock[j],ssat->outc[j],
ssat->slipc[j],ssat->rejc[j]);
}
}
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
/* save error message --------------------------------------------------------*/
void errmsg(rtk_t *rtk, const char *format, ...)
2017-04-24 15:01:35 +00:00
{
char buff[256], tstr[32];
2017-04-24 22:48:13 +00:00
int n;
va_list ap;
time2str(rtk->sol.time, tstr, 2);
n = sprintf(buff, "%s: ", tstr+11);
va_start(ap, format);
n += vsprintf(buff+n, format, ap);
2017-04-24 22:48:13 +00:00
va_end(ap);
n = n<MAXERRMSG-rtk->neb ? n : MAXERRMSG-rtk->neb;
memcpy(rtk->errbuf+rtk->neb, buff, n);
rtk->neb += n;
trace(2, "%s", buff);
2017-04-24 22:48:13 +00:00
}
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* single-differenced observable ---------------------------------------------*/
double sdobs(const obsd_t *obs, int i, int j, int f)
{
double pi = f<NFREQ ? obs[i].L[f] : obs[i].P[f-NFREQ];
double pj = f<NFREQ ? obs[j].L[f] : obs[j].P[f-NFREQ];
return pi == 0.0 || pj == 0.0 ? 0.0 : pi-pj;
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
/* single-differenced geometry-free linear combination of phase --------------*/
double gfobs_L1L2(const obsd_t *obs, int i, int j, const double *lam)
{
double pi = sdobs(obs, i, j, 0)*lam[0], pj = sdobs(obs, i, j, 1)*lam[1];
return pi == 0.0 || pj == 0.0 ? 0.0 : pi-pj;
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
double gfobs_L1L5(const obsd_t *obs, int i, int j, const double *lam)
2017-04-24 15:01:35 +00:00
{
double pi = sdobs(obs, i, j, 0)*lam[0], pj = sdobs(obs, i, j, 2)*lam[2];
return pi == 0.0 || pj == 0.0 ? 0.0 : pi-pj;
2017-04-24 22:48:13 +00:00
}
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* single-differenced measurement error variance -----------------------------*/
double varerr(int sat __attribute((unused)), int sys, double el, double bl, double dt, int f,
2017-04-24 22:48:13 +00:00
const prcopt_t *opt)
{
double a, b, c = opt->err[3]*bl/1e4, d = SPEED_OF_LIGHT * opt->sclkstab*dt, fact = 1.0;
double sinel = sin(el);
int i = sys == SYS_GLO ? 1 : (sys == SYS_GAL ? 2 : 0), nf = NF_RTK(opt);
2017-04-24 22:48:13 +00:00
/* extended error model */
if (f >= nf && opt->exterr.ena[0])
2017-04-24 22:48:13 +00:00
{ /* code */
a = opt->exterr.cerr[i][ (f-nf)*2];
b = opt->exterr.cerr[i][1+(f-nf)*2];
if (sys == SYS_SBS) {a *= EFACT_SBS; b *= EFACT_SBS;}
2017-04-24 22:48:13 +00:00
}
else if (f<nf && opt->exterr.ena[1])
2017-04-24 22:48:13 +00:00
{ /* phase */
a = opt->exterr.perr[i][ f*2];
b = opt->exterr.perr[i][1+f*2];
if (sys == SYS_SBS) {a *= EFACT_SBS; b *= EFACT_SBS;}
2017-04-24 22:48:13 +00:00
}
else
{ /* normal error model */
if (f >= nf) fact=opt->eratio[f-nf];
if (fact <= 0.0) fact=opt->eratio[0];
fact*=sys == SYS_GLO ? EFACT_GLO : (sys == SYS_SBS ? EFACT_SBS : EFACT_GPS);
a = fact*opt->err[1];
b = fact*opt->err[2];
2017-04-24 22:48:13 +00:00
}
return 2.0*(opt->ionoopt == IONOOPT_IFLC ? 3.0 : 1.0)*(a*a+b*b/sinel/sinel+c*c)+d*d;
2017-04-24 22:48:13 +00:00
}
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* baseline length -----------------------------------------------------------*/
double baseline(const double *ru, const double *rb, double *dr)
{
int i;
for (i = 0;i<3;i++) dr[i] = ru[i]-rb[i];
2017-05-04 07:19:57 +00:00
return norm_rtk(dr, 3);
2017-04-24 22:48:13 +00:00
}
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* initialize state and covariance -------------------------------------------*/
void initx_rtk(rtk_t *rtk, double xi, double var, int i)
2017-04-24 22:48:13 +00:00
{
int j;
rtk->x[i] = xi;
for (j = 0;j<rtk->nx;j++)
2017-04-24 15:01:35 +00:00
{
rtk->P[i+j*rtk->nx] = rtk->P[j+i*rtk->nx] = i == j ? var : 0.0;
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
}
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* select common satellites between rover and reference station --------------*/
int selsat(const obsd_t *obs, double *azel, int nu, int nr,
const prcopt_t *opt, int *sat, int *iu, int *ir)
{
int i, j, k = 0;
2017-04-24 15:01:35 +00:00
trace(3, "selsat : nu=%d nr=%d\n", nu, nr);
2017-04-24 15:01:35 +00:00
for (i = 0, j = nu;i<nu && j<nu+nr;i++, j++)
2017-04-24 15:01:35 +00:00
{
2017-04-24 22:48:13 +00:00
if (obs[i].sat<obs[j].sat) j--;
else if (obs[i].sat>obs[j].sat) i--;
else if (azel[1+j*2] >= opt->elmin) { /* elevation at base station */
sat[k] = obs[i].sat; iu[k] = i; ir[k++] = j;
trace(4, "(%2d) sat=%3d iu=%2d ir=%2d\n", k-1, obs[i].sat, i, j);
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
}
return k;
}
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* temporal update of position/velocity/acceleration -------------------------*/
void udpos(rtk_t *rtk, double tt)
{
double *F, *FP, *xp, pos[3], Q[9] = {0}, Qv[9], var = 0.0;
int i, j;
2017-04-24 15:01:35 +00:00
trace(3, "udpos : tt=%.3f\n", tt);
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* fixed mode */
if (rtk->opt.mode == PMODE_FIXED)
2017-04-24 22:48:13 +00:00
{
for (i = 0;i<3;i++) initx_rtk(rtk, rtk->opt.ru[i], 1E-8, i);
2017-04-24 22:48:13 +00:00
return;
}
/* initialize position for first epoch */
2017-05-04 07:19:57 +00:00
if (norm_rtk(rtk->x, 3) <= 0.0)
2017-04-24 22:48:13 +00:00
{
for (i = 0;i<3;i++) initx_rtk(rtk, rtk->sol.rr[i], VAR_POS, i);
2017-04-24 22:48:13 +00:00
if (rtk->opt.dynamics)
{
for (i = 3;i<6;i++) initx_rtk(rtk, rtk->sol.rr[i], VAR_VEL, i);
for (i = 6;i<9;i++) initx_rtk(rtk, 1E-6, VAR_ACC, i);
2017-04-24 22:48:13 +00:00
}
}
/* static mode */
if (rtk->opt.mode == PMODE_STATIC) return;
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* kinmatic mode without dynamics */
if (!rtk->opt.dynamics)
{
for (i = 0;i<3;i++) initx_rtk(rtk, rtk->sol.rr[i], VAR_POS, i);
2017-04-24 22:48:13 +00:00
return;
}
/* check variance of estimated postion */
for (i = 0;i<3;i++) var += rtk->P[i+i*rtk->nx]; var/=3.0;
2017-04-24 22:48:13 +00:00
if (var>VAR_POS)
{
/* reset position with large variance */
for (i = 0;i<3;i++) initx_rtk(rtk, rtk->sol.rr[i], VAR_POS, i);
for (i = 3;i<6;i++) initx_rtk(rtk, rtk->sol.rr[i], VAR_VEL, i);
for (i = 6;i<9;i++) initx_rtk(rtk, 1E-6, VAR_ACC, i);
trace(2, "reset rtk position due to large variance: var=%.3f\n", var);
2017-04-24 22:48:13 +00:00
return;
}
/* state transition of position/velocity/acceleration */
F = eye(rtk->nx); FP = mat(rtk->nx, rtk->nx); xp = mat(rtk->nx, 1);
2017-04-24 22:48:13 +00:00
for (i = 0;i<6;i++)
2017-04-24 22:48:13 +00:00
{
F[i+(i+3)*rtk->nx] = tt;
2017-04-24 22:48:13 +00:00
}
/* x=F*x, P=F*P*F+Q */
matmul("NN", rtk->nx, 1, rtk->nx, 1.0, F, rtk->x, 0.0, xp);
matcpy(rtk->x, xp, rtk->nx, 1);
matmul("NN", rtk->nx, rtk->nx, rtk->nx, 1.0, F, rtk->P, 0.0, FP);
matmul("NT", rtk->nx, rtk->nx, rtk->nx, 1.0, FP, F, 0.0, rtk->P);
2017-04-24 22:48:13 +00:00
/* process noise added to only acceleration */
Q[0] = Q[4] = std::pow(rtk->opt.prn[3], 2.0); Q[8] = std::pow(rtk->opt.prn[4], 2.0);
ecef2pos(rtk->x, pos);
covecef(pos, Q, Qv);
for (i = 0;i<3;i++) for (j = 0;j<3;j++)
2017-04-24 22:48:13 +00:00
{
rtk->P[i+6+(j+6)*rtk->nx] += Qv[i+j*3];
2017-04-24 22:48:13 +00:00
}
free(F); free(FP); free(xp);
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
/* temporal update of ionospheric parameters ---------------------------------*/
void udion(rtk_t *rtk, double tt, double bl, const int *sat, int ns)
2017-04-24 15:01:35 +00:00
{
double el, fact;
int i, j;
2017-04-24 15:01:35 +00:00
trace(3, "udion : tt=%.1f bl=%.0f ns=%d\n", tt, bl, ns);
2017-04-24 15:01:35 +00:00
for (i = 1;i <= MAXSAT;i++)
2017-04-24 22:48:13 +00:00
{
j = II_RTK(i, &rtk->opt);
if (rtk->x[j] != 0.0 &&
rtk->ssat[i-1].outc[0]>GAP_RESION && rtk->ssat[i-1].outc[1]>GAP_RESION)
rtk->x[j] = 0.0;
2017-04-24 22:48:13 +00:00
}
for (i = 0;i<ns;i++)
2017-04-24 22:48:13 +00:00
{
j = II_RTK(sat[i], &rtk->opt);
2017-04-24 22:48:13 +00:00
if (rtk->x[j] == 0.0)
2017-04-24 22:48:13 +00:00
{
initx_rtk(rtk, 1E-6, std::pow(rtk->opt.std[1]*bl/1e4, 2.0), j);
2017-04-24 22:48:13 +00:00
}
else
{
/* elevation dependent factor of process noise */
el = rtk->ssat[sat[i]-1].azel[1];
fact = cos(el);
rtk->P[j+j*rtk->nx] += std::pow(rtk->opt.prn[1]*bl/1e4*fact, 2.0)*tt;
2017-04-24 22:48:13 +00:00
}
}
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
/* temporal update of tropospheric parameters --------------------------------*/
void udtrop(rtk_t *rtk, double tt, double bl __attribute((unused)))
2017-04-24 15:01:35 +00:00
{
int i, j, k;
2017-04-24 15:01:35 +00:00
trace(3, "udtrop : tt=%.1f\n", tt);
2017-04-24 15:01:35 +00:00
for (i = 0;i<2;i++)
2017-04-24 22:48:13 +00:00
{
j = IT_RTK(i, &rtk->opt);
2017-04-24 22:48:13 +00:00
if (rtk->x[j] == 0.0)
2017-04-24 22:48:13 +00:00
{
initx_rtk(rtk, INIT_ZWD, std::pow(rtk->opt.std[2], 2.0), j); /* initial zwd */
2017-04-24 22:48:13 +00:00
if (rtk->opt.tropopt >= TROPOPT_ESTG)
2017-04-24 22:48:13 +00:00
{
for (k = 0;k<2;k++) initx_rtk(rtk, 1e-6, VAR_GRA, ++j);
2017-04-24 22:48:13 +00:00
}
}
else
{
rtk->P[j+j*rtk->nx] += std::pow(rtk->opt.prn[2], 2.0)*tt;
2017-04-24 22:48:13 +00:00
if (rtk->opt.tropopt >= TROPOPT_ESTG)
2017-04-24 22:48:13 +00:00
{
for (k = 0;k<2;k++)
2017-04-24 22:48:13 +00:00
{
rtk->P[++j*(1+rtk->nx)]+=std::pow(rtk->opt.prn[2]*0.3, 2.0)*fabs(rtk->tt);
2017-04-24 22:48:13 +00:00
}
}
}
}
}
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* temporal update of receiver h/w biases ------------------------------------*/
void udrcvbias(rtk_t *rtk, double tt)
{
int i, j;
2017-04-24 22:48:13 +00:00
trace(3, "udrcvbias: tt=%.1f\n", tt);
2017-04-24 15:01:35 +00:00
for (i = 0;i<NFREQGLO;i++)
2017-04-24 15:01:35 +00:00
{
j = IL_RTK(i, &rtk->opt);
2017-04-24 22:48:13 +00:00
if (rtk->x[j] == 0.0)
2017-04-24 22:48:13 +00:00
{
initx_rtk(rtk, 1E-6, VAR_HWBIAS, j);
2017-04-24 22:48:13 +00:00
}
/* hold to fixed solution */
else if (rtk->nfix >= rtk->opt.minfix && rtk->sol.ratio>rtk->opt.thresar[0])
2017-04-24 22:48:13 +00:00
{
initx_rtk(rtk, rtk->xa[j], rtk->Pa[j+j*rtk->na], j);
2017-04-24 22:48:13 +00:00
}
else
{
rtk->P[j+j*rtk->nx]+=std::pow(PRN_HWBIAS, 2.0)*tt;
2017-04-24 22:48:13 +00:00
}
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
}
/* detect cycle slip by LLI --------------------------------------------------*/
void detslp_ll(rtk_t *rtk, const obsd_t *obs, int i, int rcv)
{
unsigned int slip, LLI;
int f, sat = obs[i].sat;
2017-04-24 22:48:13 +00:00
trace(3, "detslp_ll: i=%d rcv=%d\n", i, rcv);
2017-04-24 22:48:13 +00:00
for (f = 0;f<rtk->opt.nf;f++)
2017-04-24 15:01:35 +00:00
{
2017-04-24 22:48:13 +00:00
if (obs[i].L[f] == 0.0) continue;
2017-04-24 22:48:13 +00:00
/* restore previous LLI */
if (rcv == 1) LLI = getbitu(&rtk->ssat[sat-1].slip[f], 0, 2); /* rover */
else LLI = getbitu(&rtk->ssat[sat-1].slip[f], 2, 2); /* base */
2017-04-24 22:48:13 +00:00
/* detect slip by cycle slip flag in LLI */
if (rtk->tt >= 0.0) { /* forward */
2017-04-24 22:48:13 +00:00
if (obs[i].LLI[f]&1)
{
errmsg(rtk, "slip detected forward (sat=%2d rcv=%d F=%d LLI=%x)\n",
sat, rcv, f+1, obs[i].LLI[f]);
2017-04-24 22:48:13 +00:00
}
slip = obs[i].LLI[f];
2017-04-24 22:48:13 +00:00
}
else
{ /* backward */
if (LLI&1)
{
errmsg(rtk, "slip detected backward (sat=%2d rcv=%d F=%d LLI=%x)\n",
sat, rcv, f+1, LLI);
2017-04-24 22:48:13 +00:00
}
slip = LLI;
2017-04-24 22:48:13 +00:00
}
/* detect slip by parity unknown flag transition in LLI */
if (((LLI&2) && !(obs[i].LLI[f]&2)) || (!(LLI&2) && (obs[i].LLI[f]&2)))
2017-04-24 22:48:13 +00:00
{
errmsg(rtk, "slip detected half-cyc (sat=%2d rcv=%d F=%d LLI=%x->%x)\n",
sat, rcv, f+1, LLI, obs[i].LLI[f]);
2017-04-24 22:48:13 +00:00
slip|=1;
}
/* save current LLI */
if (rcv == 1) setbitu(&rtk->ssat[sat-1].slip[f], 0, 2, obs[i].LLI[f]);
else setbitu(&rtk->ssat[sat-1].slip[f], 2, 2, obs[i].LLI[f]);
2017-04-24 22:48:13 +00:00
/* save slip and half-cycle valid flag */
rtk->ssat[sat-1].slip[f]|=(unsigned char)slip;
rtk->ssat[sat-1].half[f] = (obs[i].LLI[f]&2) ? 0 : 1;
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
}
/* detect cycle slip by L1-L2 geometry free phase jump -----------------------*/
void detslp_gf_L1L2(rtk_t *rtk, const obsd_t *obs, int i, int j,
const nav_t *nav)
{
int sat = obs[i].sat;
double g0, g1;
2017-04-24 22:48:13 +00:00
trace(3, "detslp_gf_L1L2: i=%d j=%d\n", i, j);
2017-04-24 22:48:13 +00:00
if (rtk->opt.nf <= 1 || (g1=gfobs_L1L2(obs, i, j, nav->lam[sat-1])) == 0.0) return;
2017-04-24 22:48:13 +00:00
g0 = rtk->ssat[sat-1].gf; rtk->ssat[sat-1].gf = g1;
2017-04-24 22:48:13 +00:00
if (g0 != 0.0 && fabs(g1-g0)>rtk->opt.thresslip)
2017-04-24 15:01:35 +00:00
{
2017-04-24 22:48:13 +00:00
rtk->ssat[sat-1].slip[0]|=1;
rtk->ssat[sat-1].slip[1]|=1;
errmsg(rtk, "slip detected (sat=%2d GF_L1_L2=%.3f %.3f)\n", sat, g0, g1);
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
}
/* detect cycle slip by L1-L5 geometry free phase jump -----------------------*/
void detslp_gf_L1L5(rtk_t *rtk, const obsd_t *obs, int i, int j,
const nav_t *nav)
{
int sat = obs[i].sat;
double g0, g1;
2017-04-24 22:48:13 +00:00
trace(3, "detslp_gf_L1L5: i=%d j=%d\n", i, j);
2017-04-24 22:48:13 +00:00
if (rtk->opt.nf <= 2 || (g1=gfobs_L1L5(obs, i, j, nav->lam[sat-1])) == 0.0) return;
2017-04-24 22:48:13 +00:00
g0 = rtk->ssat[sat-1].gf2; rtk->ssat[sat-1].gf2 = g1;
2017-04-24 15:01:35 +00:00
if (g0 != 0.0 && fabs(g1-g0)>rtk->opt.thresslip)
2017-04-24 15:01:35 +00:00
{
2017-04-24 22:48:13 +00:00
rtk->ssat[sat-1].slip[0]|=1;
rtk->ssat[sat-1].slip[2]|=1;
2017-04-24 15:01:35 +00:00
errmsg(rtk, "slip detected (sat=%2d GF_L1_L5=%.3f %.3f)\n", sat, g0, g1);
2017-04-24 22:48:13 +00:00
}
}
/* detect cycle slip by doppler and phase difference -------------------------*/
2017-04-30 08:02:06 +00:00
void detslp_dop(rtk_t *rtk __attribute__((unused)), const obsd_t *obs __attribute__((unused)), int i __attribute__((unused)), int rcv __attribute__((unused)),
const nav_t *nav __attribute__((unused)))
2017-04-24 22:48:13 +00:00
{
/* detection with doppler disabled because of clock-jump issue (v.2.3.0) */
#if 0
int f,sat = obs[i].sat;
2017-04-24 22:48:13 +00:00
double tt,dph,dpt,lam,thres;
trace(3,"detslp_dop: i=%d rcv=%d\n",i,rcv);
for (f = 0;f<rtk->opt.nf;f++)
2017-04-24 22:48:13 +00:00
{
if (obs[i].L[f] == 0.0 || obs[i].D[f] == 0.0 || rtk->ph[rcv-1][sat-1][f] == 0.0)
2017-04-24 22:48:13 +00:00
{
continue;
}
if (fabs(tt = timediff(obs[i].time,rtk->pt[rcv-1][sat-1][f]))<DTTOL) continue;
if ((lam = nav->lam[sat-1][f])<=0.0) continue;
2017-04-24 22:48:13 +00:00
/* cycle slip threshold (cycle) */
thres = MAXACC*tt*tt/2.0/lam+rtk->opt.err[4]*fabs(tt)*4.0;
2017-04-24 22:48:13 +00:00
/* phase difference and doppler x time (cycle) */
dph = obs[i].L[f]-rtk->ph[rcv-1][sat-1][f];
dpt = -obs[i].D[f]*tt;
2017-04-24 22:48:13 +00:00
if (fabs(dph-dpt)<=thres) continue;
rtk->slip[sat-1][f]| = 1;
2017-04-24 22:48:13 +00:00
errmsg(rtk,"slip detected (sat=%2d rcv=%d L%d=%.3f %.3f thres=%.3f)\n",
sat,rcv,f+1,dph,dpt,thres);
}
#endif
}
/* temporal update of phase biases -------------------------------------------*/
void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat,
const int *iu, const int *ir, int ns, const nav_t *nav)
{
double cp, pr, cp1, cp2, pr1, pr2, *bias, offset, lami, lam1, lam2, C1, C2;
int i, j, f, slip, reset, nf = NF_RTK(&rtk->opt);
2017-04-24 22:48:13 +00:00
trace(3, "udbias : tt=%.1f ns=%d\n", tt, ns);
2017-04-24 22:48:13 +00:00
for (i = 0;i<ns;i++)
2017-04-24 22:48:13 +00:00
{
/* detect cycle slip by LLI */
for (f = 0;f<rtk->opt.nf;f++) rtk->ssat[sat[i]-1].slip[f] &= 0xFC;
detslp_ll(rtk, obs, iu[i], 1);
detslp_ll(rtk, obs, ir[i], 2);
2017-04-24 22:48:13 +00:00
/* detect cycle slip by geometry-free phase jump */
detslp_gf_L1L2(rtk, obs, iu[i], ir[i], nav);
detslp_gf_L1L5(rtk, obs, iu[i], ir[i], nav);
2017-04-24 22:48:13 +00:00
/* detect cycle slip by doppler and phase difference */
detslp_dop(rtk, obs, iu[i], 1, nav);
detslp_dop(rtk, obs, ir[i], 2, nav);
2017-04-24 22:48:13 +00:00
/* update half-cycle valid flag */
for (f = 0;f<nf;f++)
2017-04-24 22:48:13 +00:00
{
rtk->ssat[sat[i]-1].half[f] =
!((obs[iu[i]].LLI[f]&2) || (obs[ir[i]].LLI[f]&2));
2017-04-24 22:48:13 +00:00
}
}
for (f = 0;f<nf;f++)
2017-04-24 22:48:13 +00:00
{
/* reset phase-bias if instantaneous AR or expire obs outage counter */
for (i = 1;i <= MAXSAT;i++)
2017-04-24 22:48:13 +00:00
{
reset = ++rtk->ssat[i-1].outc[f]>(unsigned int)rtk->opt.maxout;
2017-04-24 22:48:13 +00:00
if (rtk->opt.modear == ARMODE_INST && rtk->x[IB_RTK(i, f, &rtk->opt)] != 0.0)
2017-04-24 22:48:13 +00:00
{
initx_rtk(rtk, 0.0, 0.0, IB_RTK(i, f, &rtk->opt));
2017-04-24 22:48:13 +00:00
}
else if (reset && rtk->x[IB_RTK(i, f, &rtk->opt)] != 0.0)
2017-04-24 22:48:13 +00:00
{
initx_rtk(rtk, 0.0, 0.0, IB_RTK(i, f, &rtk->opt));
trace(3, "udbias : obs outage counter overflow (sat=%3d L%d n=%d)\n",
i, f+1, rtk->ssat[i-1].outc[f]);
2017-04-24 22:48:13 +00:00
}
if (rtk->opt.modear != ARMODE_INST && reset)
2017-04-24 22:48:13 +00:00
{
rtk->ssat[i-1].lock[f] = -rtk->opt.minlock;
2017-04-24 22:48:13 +00:00
}
}
/* reset phase-bias if detecting cycle slip */
for (i = 0;i<ns;i++)
2017-04-24 22:48:13 +00:00
{
j = IB_RTK(sat[i], f, &rtk->opt);
2017-04-24 22:48:13 +00:00
rtk->P[j+j*rtk->nx]+=rtk->opt.prn[0]*rtk->opt.prn[0]*tt;
slip = rtk->ssat[sat[i]-1].slip[f];
if (rtk->opt.ionoopt == IONOOPT_IFLC) slip|=rtk->ssat[sat[i]-1].slip[1];
if (rtk->opt.modear == ARMODE_INST || !(slip&1)) continue;
rtk->x[j] = 0.0;
rtk->ssat[sat[i]-1].lock[f] = -rtk->opt.minlock;
2017-04-24 22:48:13 +00:00
}
bias = zeros(ns, 1);
2017-04-24 22:48:13 +00:00
/* estimate approximate phase-bias by phase - code */
for (i = j = 0, offset = 0.0;i<ns;i++)
2017-04-24 22:48:13 +00:00
{
if (rtk->opt.ionoopt != IONOOPT_IFLC)
2017-04-24 22:48:13 +00:00
{
cp = sdobs(obs, iu[i], ir[i], f); /* cycle */
pr = sdobs(obs, iu[i], ir[i], f+NFREQ);
lami = nav->lam[sat[i]-1][f];
if (cp == 0.0 || pr == 0.0 || lami <= 0.0) continue;
2017-04-24 22:48:13 +00:00
bias[i] = cp-pr/lami;
2017-04-24 22:48:13 +00:00
}
else
{
cp1 = sdobs(obs, iu[i], ir[i], 0);
cp2 = sdobs(obs, iu[i], ir[i], 1);
pr1 = sdobs(obs, iu[i], ir[i], NFREQ);
pr2 = sdobs(obs, iu[i], ir[i], NFREQ+1);
lam1 = nav->lam[sat[i]-1][0];
lam2 = nav->lam[sat[i]-1][1];
if (cp1 == 0.0 || cp2 == 0.0 || pr1 == 0.0 || pr2 == 0.0 || lam1 <= 0.0 || lam2 <= 0.0) continue;
C1 = std::pow(lam2, 2.0)/(std::pow(lam2, 2.0)-std::pow(lam1, 2.0));
C2 = -std::pow(lam1, 2.0)/(std::pow(lam2, 2.0)-std::pow(lam1, 2.0));
bias[i] = (C1*lam1*cp1+C2*lam2*cp2)-(C1*pr1+C2*pr2);
2017-04-24 22:48:13 +00:00
}
if (rtk->x[IB_RTK(sat[i], f, &rtk->opt)] != 0.0)
2017-04-24 22:48:13 +00:00
{
offset+=bias[i]-rtk->x[IB_RTK(sat[i], f, &rtk->opt)];
2017-04-24 22:48:13 +00:00
j++;
}
}
/* correct phase-bias offset to enssure phase-code coherency */
if (j>0) {
for (i = 1;i <= MAXSAT;i++)
2017-04-24 22:48:13 +00:00
{
if (rtk->x[IB_RTK(i, f, &rtk->opt)] != 0.0) rtk->x[IB_RTK(i, f, &rtk->opt)]+=offset/j;
2017-04-24 22:48:13 +00:00
}
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
/* set initial states of phase-bias */
for (i = 0;i<ns;i++)
2017-04-24 22:48:13 +00:00
{
if (bias[i] == 0.0 || rtk->x[IB_RTK(sat[i], f, &rtk->opt)] != 0.0) continue;
initx_rtk(rtk, bias[i], std::pow(rtk->opt.std[0], 2.0), IB_RTK(sat[i], f, &rtk->opt));
2017-04-24 22:48:13 +00:00
}
free(bias);
}
}
/* temporal update of states --------------------------------------------------*/
void udstate(rtk_t *rtk, const obsd_t *obs, const int *sat,
const int *iu, const int *ir, int ns, const nav_t *nav)
{
double tt = fabs(rtk->tt), bl = 0.0, dr[3];
2017-04-24 15:01:35 +00:00
trace(3, "udstate : ns=%d\n", ns);
2017-04-24 22:48:13 +00:00
/* temporal update of position/velocity/acceleration */
udpos(rtk, tt);
2017-04-24 22:48:13 +00:00
/* temporal update of ionospheric parameters */
if (rtk->opt.ionoopt >= IONOOPT_EST)
2017-04-24 22:48:13 +00:00
{
bl = baseline(rtk->x, rtk->rb, dr);
udion(rtk, tt, bl, sat, ns);
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
/* temporal update of tropospheric parameters */
if (rtk->opt.tropopt >= TROPOPT_EST)
2017-04-24 22:48:13 +00:00
{
udtrop(rtk, tt, bl);
2017-04-24 22:48:13 +00:00
}
/* temporal update of eceiver h/w bias */
if (rtk->opt.glomodear == 2 && (rtk->opt.navsys&SYS_GLO))
2017-04-24 22:48:13 +00:00
{
udrcvbias(rtk, tt);
2017-04-24 22:48:13 +00:00
}
/* temporal update of phase-bias */
if (rtk->opt.mode>PMODE_DGPS)
{
udbias(rtk, tt, obs, sat, iu, ir, ns, nav);
2017-04-24 22:48:13 +00:00
}
}
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* undifferenced phase/code residual for satellite ---------------------------*/
void zdres_sat(int base, double r, const obsd_t *obs, const nav_t *nav,
const double *azel, const double *dant,
const prcopt_t *opt, double *y)
{
const double *lam = nav->lam[obs->sat-1];
double f1, f2, C1, C2, dant_if;
int i, nf = NF_RTK(opt);
2017-04-24 22:48:13 +00:00
if (opt->ionoopt == IONOOPT_IFLC)
2017-04-24 22:48:13 +00:00
{ /* iono-free linear combination */
if (lam[0] == 0.0 || lam[1] == 0.0) return;
2017-04-24 22:48:13 +00:00
if (testsnr(base, 0, azel[1], obs->SNR[0]*0.25, &opt->snrmask) ||
testsnr(base, 1, azel[1], obs->SNR[1]*0.25, &opt->snrmask)) return;
2017-04-24 22:48:13 +00:00
f1 = SPEED_OF_LIGHT/lam[0];
f2 = SPEED_OF_LIGHT/lam[1];
C1 = std::pow(f1, 2.0)/(std::pow(f1, 2.0)-std::pow(f2, 2.0));
C2 = -std::pow(f2, 2.0)/(std::pow(f1, 2.0)-std::pow(f2, 2.0));
dant_if = C1*dant[0]+C2*dant[1];
2017-04-24 22:48:13 +00:00
if (obs->L[0] != 0.0 && obs->L[1] != 0.0)
2017-04-24 22:48:13 +00:00
{
y[0] = C1*obs->L[0]*lam[0]+C2*obs->L[1]*lam[1]-r-dant_if;
2017-04-24 22:48:13 +00:00
}
if (obs->P[0] != 0.0 && obs->P[1] != 0.0)
2017-04-24 22:48:13 +00:00
{
y[1] = C1*obs->P[0]+C2*obs->P[1]-r-dant_if;
2017-04-24 22:48:13 +00:00
}
}
else
{
for (i = 0;i<nf;i++)
2017-04-24 22:48:13 +00:00
{
if (lam[i] == 0.0) continue;
2017-04-24 22:48:13 +00:00
/* check snr mask */
if (testsnr(base, i, azel[1], obs->SNR[i]*0.25, &opt->snrmask))
2017-04-24 22:48:13 +00:00
{
continue;
}
/* residuals = observable - pseudorange */
if (obs->L[i] != 0.0) y[i ] = obs->L[i]*lam[i]-r-dant[i];
if (obs->P[i] != 0.0) y[i+nf] = obs->P[i] -r-dant[i];
2017-04-24 22:48:13 +00:00
}
}
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
/* undifferenced phase/code residuals ----------------------------------------*/
int zdres(int base, const obsd_t *obs, int n, const double *rs,
const double *dts, const int *svh, const nav_t *nav,
const double *rr, const prcopt_t *opt, int index, double *y,
double *e, double *azel)
2017-04-24 15:01:35 +00:00
{
double r, rr_[3], pos[3], dant[NFREQ] = {0}, disp[3];
double zhd, zazel[] = {0.0, 90.0*D2R};
int i, nf = NF_RTK(opt);
2017-04-24 15:01:35 +00:00
trace(3, "zdres : n=%d\n", n);
2017-04-24 15:01:35 +00:00
for (i = 0;i<n*nf*2;i++) y[i] = 0.0;
2017-04-24 15:01:35 +00:00
2017-05-04 07:19:57 +00:00
if (norm_rtk(rr, 3) <= 0.0) return 0; /* no receiver position */
2017-04-24 22:48:13 +00:00
for (i = 0;i<3;i++) rr_[i] = rr[i];
2017-04-24 22:48:13 +00:00
/* earth tide correction */
if (opt->tidecorr)
{
tidedisp(gpst2utc(obs[0].time), rr_, opt->tidecorr, &nav->erp,
opt->odisp[base], disp);
for (i = 0;i<3;i++) rr_[i]+=disp[i];
2017-04-24 15:01:35 +00:00
}
ecef2pos(rr_, pos);
2017-04-24 22:48:13 +00:00
for (i = 0;i<n;i++)
2017-04-24 15:01:35 +00:00
{
2017-04-24 22:48:13 +00:00
/* compute geometric-range and azimuth/elevation angle */
if ((r = geodist(rs+i*6, rr_, e+i*3)) <= 0.0) continue;
if (satazel(pos, e+i*3, azel+i*2)<opt->elmin) continue;
2017-04-24 22:48:13 +00:00
/* excluded satellite? */
if (satexclude(obs[i].sat, svh[i], opt)) continue;
2017-04-24 22:48:13 +00:00
/* satellite clock-bias */
r+=-SPEED_OF_LIGHT*dts[i*2];
/* troposphere delay model (hydrostatic) */
zhd = tropmodel(obs[0].time, pos, zazel, 0.0);
r+=tropmapf(obs[i].time, pos, azel+i*2, NULL)*zhd;
2017-04-24 22:48:13 +00:00
/* receiver antenna phase center correction */
antmodel(opt->pcvr+index, opt->antdel[index], azel+i*2, opt->posopt[1],
2017-04-24 22:48:13 +00:00
dant);
/* undifferenced phase/code residual for satellite */
zdres_sat(base, r, obs+i, nav, azel+i*2, dant, opt, y+i*nf*2);
2017-04-24 15:01:35 +00:00
}
trace(4, "rr_=%.3f %.3f %.3f\n", rr_[0], rr_[1], rr_[2]);
trace(4, "pos=%.9f %.9f %.3f\n", pos[0]*R2D, pos[1]*R2D, pos[2]);
for (i = 0;i<n;i++)
2017-04-24 15:01:35 +00:00
{
trace(4, "sat=%2d %13.3f %13.3f %13.3f %13.10f %6.1f %5.1f\n",
obs[i].sat, rs[i*6], rs[1+i*6], rs[2+i*6], dts[i*2], azel[i*2]*R2D,
2017-04-24 22:48:13 +00:00
azel[1+i*2]*R2D);
2017-04-24 15:01:35 +00:00
}
trace(4, "y=\n"); tracemat(4, y, nf*2, n, 13, 3);
2017-04-24 22:48:13 +00:00
return 1;
}
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* test valid observation data -----------------------------------------------*/
int validobs(int i, int j, int f, int nf, double *y)
{
/* if no phase observable, psudorange is also unusable */
return y[f+i*nf*2] != 0.0 && y[f+j*nf*2] != 0.0 &&
(f<nf || (y[f-nf+i*nf*2] != 0.0 && y[f-nf+j*nf*2] != 0.0));
2017-04-24 22:48:13 +00:00
}
/* double-differenced measurement error covariance ---------------------------*/
void ddcov(const int *nb, int n, const double *Ri, const double *Rj,
int nv, double *R)
{
int i, j, k = 0, b;
2017-04-24 15:01:35 +00:00
trace(3, "ddcov : n=%d\n", n);
2017-04-24 15:01:35 +00:00
for (i = 0;i<nv*nv;i++) R[i] = 0.0;
for (b = 0;b<n;k+=nb[b++])
2017-04-24 15:01:35 +00:00
{
2017-04-24 22:48:13 +00:00
for (i = 0;i<nb[b];i++) for (j = 0;j<nb[b];j++)
2017-04-24 22:48:13 +00:00
{
R[k+i+(k+j)*nv] = Ri[k+i]+(i == j ? Rj[k+i] : 0.0);
2017-04-24 22:48:13 +00:00
}
2017-04-24 15:01:35 +00:00
}
trace(5, "R=\n"); tracemat(5, R, nv, nv, 8, 6);
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
/* baseline length constraint ------------------------------------------------*/
int constbl(rtk_t *rtk, const double *x, const double *P, double *v,
double *H, double *Ri, double *Rj, int index)
2017-04-24 15:01:35 +00:00
{
const double thres = 0.1; /* threshold for nonliearity (v.2.3.0) */
double xb[3], b[3], bb, var = 0.0;
2017-04-24 22:48:13 +00:00
int i;
2017-04-24 15:01:35 +00:00
trace(3, "constbl : \n");
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* no constraint */
if (rtk->opt.baseline[0] <= 0.0) return 0;
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* time-adjusted baseline vector and length */
for (i = 0;i<3;i++)
2017-04-24 22:48:13 +00:00
{
xb[i] = rtk->rb[i]+rtk->rb[i+3]*rtk->sol.age;
b[i] = x[i]-xb[i];
2017-04-24 22:48:13 +00:00
}
2017-05-04 07:19:57 +00:00
bb = norm_rtk(b, 3);
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* approximate variance of solution */
if (P)
{
for (i = 0;i<3;i++) var+=P[i+i*rtk->nx];
2017-04-24 22:48:13 +00:00
var/=3.0;
}
/* check nonlinearity */
if (var>thres*thres*bb*bb)
2017-04-24 15:01:35 +00:00
{
trace(3, "constbl : equation nonlinear (bb=%.3f var=%.3f)\n", bb, var);
2017-04-24 22:48:13 +00:00
return 0;
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
/* constraint to baseline length */
v[index] = rtk->opt.baseline[0]-bb;
2017-04-24 22:48:13 +00:00
if (H)
{
for (i = 0;i<3;i++) H[i+index*rtk->nx] = b[i]/bb;
2017-04-24 22:48:13 +00:00
}
Ri[index] = 0.0;
Rj[index] = std::pow(rtk->opt.baseline[1], 2.0);
2017-04-24 15:01:35 +00:00
trace(4, "baseline len v=%13.3f R=%8.6f %8.6f\n", v[index], Ri[index], Rj[index]);
2017-04-24 22:48:13 +00:00
return 1;
}
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* precise tropspheric model -------------------------------------------------*/
double prectrop(gtime_t time, const double *pos, int r,
const double *azel, const prcopt_t *opt, const double *x,
double *dtdx)
{
double m_w = 0.0, cotz, grad_n, grad_e;
int i = IT_RTK(r, opt);
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* wet mapping function */
tropmapf(time, pos, azel, &m_w);
2017-04-24 15:01:35 +00:00
if (opt->tropopt >= TROPOPT_ESTG && azel[1]>0.0)
2017-04-24 22:48:13 +00:00
{
/* m_w=m_0+m_0*cot(el)*(Gn*cos(az)+Ge*sin(az)): ref [6] */
cotz = 1.0/tan(azel[1]);
grad_n = m_w*cotz*cos(azel[0]);
grad_e = m_w*cotz*sin(azel[0]);
2017-04-24 22:48:13 +00:00
m_w+=grad_n*x[i+1]+grad_e*x[i+2];
dtdx[1] = grad_n*x[i];
dtdx[2] = grad_e*x[i];
2017-04-24 15:01:35 +00:00
}
else dtdx[1] = dtdx[2] = 0.0;
dtdx[0] = m_w;
2017-04-24 22:48:13 +00:00
return m_w*x[i];
}
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* glonass inter-channel bias correction -------------------------------------*/
double gloicbcorr(int sat1 __attribute((unused)), int sat2 __attribute((unused)), const prcopt_t *opt, double lam1,
2017-04-24 22:48:13 +00:00
double lam2, int f)
2017-04-24 15:01:35 +00:00
{
2017-04-24 22:48:13 +00:00
double dfreq;
if (f>=NFREQGLO || f >= opt->nf || !opt->exterr.ena[2]) return 0.0;
2017-04-24 15:01:35 +00:00
dfreq = (SPEED_OF_LIGHT/lam1-SPEED_OF_LIGHT/lam2)/(f == 0 ? DFRQ1_GLO : DFRQ2_GLO);
2017-04-24 22:48:13 +00:00
return opt->exterr.gloicb[f]*0.01*dfreq; /* (m) */
}
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* test navi system (m=0:gps/qzs/sbs,1:glo,2:gal,3:bds) ----------------------*/
int test_sys(int sys, int m)
{
switch (sys)
2017-04-24 15:01:35 +00:00
{
case SYS_GPS: return m == 0;
case SYS_QZS: return m == 0;
case SYS_SBS: return m == 0;
case SYS_GLO: return m == 1;
case SYS_GAL: return m == 2;
case SYS_BDS: return m == 3;
2017-04-24 22:48:13 +00:00
}
return 0;
}
/* double-differenced phase/code residuals -----------------------------------*/
int ddres(rtk_t *rtk, const nav_t *nav, double dt, const double *x,
const double *P, const int *sat, double *y, double *e,
double *azel, const int *iu, const int *ir, int ns, double *v,
double *H, double *R, int *vflg)
{
prcopt_t *opt = &rtk->opt;
double bl, dr[3], posu[3], posr[3], didxi = 0.0, didxj = 0.0, *im;
double *tropr, *tropu, *dtdxr, *dtdxu, *Ri, *Rj, lami, lamj, fi, fj, df, *Hi = NULL;
int i, j, k, m, f, ff, nv = 0, nb[NFREQ*4*2+2] = {0}, b = 0, sysi, sysj, nf = NF_RTK(opt);
2017-04-24 22:48:13 +00:00
trace(3, "ddres : dt=%.1f nx=%d ns=%d\n", dt, rtk->nx, ns);
2017-04-24 22:48:13 +00:00
bl = baseline(x, rtk->rb, dr);
ecef2pos(x, posu); ecef2pos(rtk->rb, posr);
2017-04-24 22:48:13 +00:00
Ri = mat(ns*nf*2+2, 1); Rj = mat(ns*nf*2+2, 1); im = mat(ns, 1);
tropu = mat(ns, 1); tropr = mat(ns, 1); dtdxu = mat(ns, 3); dtdxr = mat(ns, 3);
2017-04-24 22:48:13 +00:00
for (i = 0;i<MAXSAT;i++) for (j = 0;j<NFREQ;j++)
2017-04-24 15:01:35 +00:00
{
rtk->ssat[i].resp[j] = rtk->ssat[i].resc[j] = 0.0;
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
/* compute factors of ionospheric and tropospheric delay */
for (i = 0;i<ns;i++)
2017-04-24 22:48:13 +00:00
{
if (opt->ionoopt >= IONOOPT_EST)
2017-04-24 22:48:13 +00:00
{
im[i] = (ionmapf(posu, azel+iu[i]*2)+ionmapf(posr, azel+ir[i]*2))/2.0;
2017-04-24 22:48:13 +00:00
}
if (opt->tropopt >= TROPOPT_EST)
2017-04-24 22:48:13 +00:00
{
tropu[i] = prectrop(rtk->sol.time, posu, 0, azel+iu[i]*2, opt, x, dtdxu+i*3);
tropr[i] = prectrop(rtk->sol.time, posr, 1, azel+ir[i]*2, opt, x, dtdxr+i*3);
2017-04-24 22:48:13 +00:00
}
}
for (m = 0;m<4;m++) /* m=0:gps/qzs/sbs, 1:glo, 2:gal, 3:bds */
2017-04-24 15:01:35 +00:00
for (f = opt->mode>PMODE_DGPS ? 0 : nf;f<nf*2;f++)
2017-04-24 22:48:13 +00:00
{
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* search reference satellite with highest elevation */
for (i = -1, j = 0;j<ns;j++)
2017-04-24 22:48:13 +00:00
{
sysi = rtk->ssat[sat[j]-1].sys;
if (!test_sys(sysi, m)) continue;
if (!validobs(iu[j], ir[j], f, nf, y)) continue;
if (i<0 || azel[1+iu[j]*2] >= azel[1+iu[i]*2]) i = j;
2017-04-24 22:48:13 +00:00
}
if (i<0) continue;
/* make double difference */
for (j = 0;j<ns;j++)
2017-04-24 22:48:13 +00:00
{
if (i == j) continue;
sysi = rtk->ssat[sat[i]-1].sys;
sysj = rtk->ssat[sat[j]-1].sys;
if (!test_sys(sysj, m)) continue;
if (!validobs(iu[j], ir[j], f, nf, y)) continue;
ff = f%nf;
lami = nav->lam[sat[i]-1][ff];
lamj = nav->lam[sat[j]-1][ff];
if (lami <= 0.0 || lamj <= 0.0) continue;
2017-04-24 22:48:13 +00:00
if (H)
{
Hi = H+nv*rtk->nx;
for (k = 0;k<rtk->nx;k++) Hi[k] = 0.0;
2017-04-24 22:48:13 +00:00
}
/* double-differenced residual */
v[nv] = (y[f+iu[i]*nf*2]-y[f+ir[i]*nf*2])-
2017-04-24 22:48:13 +00:00
(y[f+iu[j]*nf*2]-y[f+ir[j]*nf*2]);
/* partial derivatives by rover position */
if (H)
{
for (k = 0;k<3;k++)
2017-04-24 22:48:13 +00:00
{
Hi[k] = -e[k+iu[i]*3]+e[k+iu[j]*3];
2017-04-24 22:48:13 +00:00
}
}
/* double-differenced ionospheric delay term */
if (opt->ionoopt == IONOOPT_EST)
2017-04-24 22:48:13 +00:00
{
fi = lami/lam_carr[0]; fj = lamj/lam_carr[0];
didxi = (f<nf ? -1.0 : 1.0)*fi*fi*im[i];
didxj = (f<nf ? -1.0 : 1.0)*fj*fj*im[j];
v[nv] -= didxi*x[II_RTK(sat[i], opt)]-didxj*x[II_RTK(sat[j], opt)];
2017-04-24 22:48:13 +00:00
if (H)
{
Hi[II_RTK(sat[i], opt)] = didxi;
Hi[II_RTK(sat[j], opt)] = -didxj;
2017-04-24 22:48:13 +00:00
}
}
/* double-differenced tropospheric delay term */
if (opt->tropopt == TROPOPT_EST || opt->tropopt == TROPOPT_ESTG)
2017-04-24 22:48:13 +00:00
{
v[nv] -= (tropu[i]-tropu[j])-(tropr[i]-tropr[j]);
for (k = 0;k<(opt->tropopt<TROPOPT_ESTG ? 1 : 3);k++)
2017-04-24 22:48:13 +00:00
{
if (!H) continue;
Hi[IT_RTK(0, opt)+k] = (dtdxu[k+i*3]-dtdxu[k+j*3]);
Hi[IT_RTK(1, opt)+k] = -(dtdxr[k+i*3]-dtdxr[k+j*3]);
2017-04-24 22:48:13 +00:00
}
}
/* double-differenced phase-bias term */
if (f<nf)
{
if (opt->ionoopt != IONOOPT_IFLC)
2017-04-24 22:48:13 +00:00
{
v[nv] -= lami*x[IB_RTK(sat[i], f, opt)]-lamj*x[IB_RTK(sat[j], f, opt)];
2017-04-24 22:48:13 +00:00
if (H)
{
Hi[IB_RTK(sat[i], f, opt)] = lami;
Hi[IB_RTK(sat[j], f, opt)] = -lamj;
2017-04-24 22:48:13 +00:00
}
}
else
{
v[nv] -= x[IB_RTK(sat[i], f, opt)]-x[IB_RTK(sat[j], f, opt)];
2017-04-24 22:48:13 +00:00
if (H)
{
Hi[IB_RTK(sat[i], f, opt)] = 1.0;
Hi[IB_RTK(sat[j], f, opt)] = -1.0;
2017-04-24 22:48:13 +00:00
}
}
}
/* glonass receiver h/w bias term */
if (rtk->opt.glomodear == 2 && sysi == SYS_GLO && sysj == SYS_GLO && ff<NFREQGLO)
2017-04-24 22:48:13 +00:00
{
df = (SPEED_OF_LIGHT/lami-SPEED_OF_LIGHT/lamj)/1E6; /* freq-difference (MHz) */
v[nv] -= df*x[IL_RTK(ff, opt)];
if (H) Hi[IL_RTK(ff, opt)] = df;
2017-04-24 22:48:13 +00:00
}
/* glonass interchannel bias correction */
else if (sysi == SYS_GLO && sysj == SYS_GLO)
2017-04-24 22:48:13 +00:00
{
v[nv] -= gloicbcorr(sat[i], sat[j], &rtk->opt, lami, lamj, f);
2017-04-24 22:48:13 +00:00
}
if (f<nf) rtk->ssat[sat[j]-1].resc[f ] = v[nv];
else rtk->ssat[sat[j]-1].resp[f-nf] = v[nv];
2017-04-24 22:48:13 +00:00
/* test innovation */
if (opt->maxinno>0.0 && fabs(v[nv])>opt->maxinno)
2017-04-24 22:48:13 +00:00
{
if (f<nf)
{
rtk->ssat[sat[i]-1].rejc[f]++;
rtk->ssat[sat[j]-1].rejc[f]++;
}
errmsg(rtk, "outlier rejected (sat=%3d-%3d %s%d v=%.3f)\n",
sat[i], sat[j], f<nf ? "L" : "P", f%nf+1, v[nv]);
2017-04-24 22:48:13 +00:00
continue;
}
/* single-differenced measurement error variances */
Ri[nv] = varerr(sat[i], sysi, azel[1+iu[i]*2], bl, dt, f, opt);
Rj[nv] = varerr(sat[j], sysj, azel[1+iu[j]*2], bl, dt, f, opt);
2017-04-24 22:48:13 +00:00
/* set valid data flags */
if (opt->mode>PMODE_DGPS)
{
if (f<nf) rtk->ssat[sat[i]-1].vsat[f] = rtk->ssat[sat[j]-1].vsat[f] = 1;
2017-04-24 22:48:13 +00:00
}
else
{
rtk->ssat[sat[i]-1].vsat[f-nf] = rtk->ssat[sat[j]-1].vsat[f-nf] = 1;
2017-04-24 22:48:13 +00:00
}
trace(4, "sat=%3d-%3d %s%d v=%13.3f R=%8.6f %8.6f\n", sat[i],
sat[j], f<nf ? "L" : "P", f%nf+1, v[nv], Ri[nv], Rj[nv]);
2017-04-24 22:48:13 +00:00
vflg[nv++] = (sat[i]<<16)|(sat[j]<<8)|((f<nf ? 0 : 1)<<4)|(f%nf);
2017-04-24 22:48:13 +00:00
nb[b]++;
}
#if 0 /* residuals referenced to reference satellite (2.4.2 p11) */
/* restore single-differenced residuals assuming sum equal zero */
if (f<nf)
{
for (j=0,s=0.0;j<MAXSAT;j++) s+=rtk->ssat[j].resc[f];
s/=nb[b]+1;
for (j=0;j<MAXSAT;j++)
{
if (j == sat[i]-1||rtk->ssat[j].resc[f]!=0.0) rtk->ssat[j].resc[f]-=s;
2017-04-24 22:48:13 +00:00
}
}
else
{
for (j=0,s=0.0;j<MAXSAT;j++) s+=rtk->ssat[j].resp[f-nf];
s/=nb[b]+1;
for (j=0;j<MAXSAT;j++)
{
if (j == sat[i]-1||rtk->ssat[j].resp[f-nf]!=0.0)
2017-04-24 22:48:13 +00:00
rtk->ssat[j].resp[f-nf]-=s;
}
}
#endif
b++;
}
/* end of system loop */
/* baseline length constraint for moving baseline */
if (opt->mode == PMODE_MOVEB && constbl(rtk, x, P, v, H, Ri, Rj, nv))
2017-04-24 15:01:35 +00:00
{
vflg[nv++] = 3<<4;
2017-04-24 22:48:13 +00:00
nb[b++]++;
2017-04-24 15:01:35 +00:00
}
if (H) {trace(5, "H=\n"); tracemat(5, H, rtk->nx, nv, 7, 4);}
2017-04-24 22:48:13 +00:00
/* double-differenced measurement error covariance */
ddcov(nb, b, Ri, Rj, nv, R);
2017-04-24 22:48:13 +00:00
free(Ri); free(Rj); free(im);
free(tropu); free(tropr); free(dtdxu); free(dtdxr);
return nv;
}
/* time-interpolation of residuals (for post-mission) ------------------------*/
double intpres(gtime_t time, const obsd_t *obs, int n, const nav_t *nav,
rtk_t *rtk, double *y)
{
static obsd_t obsb[MAXOBS];
static double yb[MAXOBS*NFREQ*2], rs[MAXOBS*6], dts[MAXOBS*2], var[MAXOBS];
static double e[MAXOBS*3], azel[MAXOBS*2];
static int nb = 0, svh[MAXOBS*2];
prcopt_t *opt = &rtk->opt;
double tt = timediff(time, obs[0].time), ttb, *p, *q;
int i, j, k, nf = NF_RTK(opt);
2017-04-24 22:48:13 +00:00
trace(3, "intpres : n=%d tt=%.1f\n", n, tt);
2017-04-24 22:48:13 +00:00
if (nb == 0 || fabs(tt)<DTTOL)
2017-04-24 22:48:13 +00:00
{
nb = n; for (i = 0;i<n;i++) obsb[i] = obs[i];
2017-04-24 22:48:13 +00:00
return tt;
}
ttb = timediff(time, obsb[0].time);
if (fabs(ttb)>opt->maxtdiff*2.0 || ttb == tt) return tt;
2017-04-24 22:48:13 +00:00
satposs(time, obsb, nb, nav, opt->sateph, rs, dts, var, svh);
2017-04-24 22:48:13 +00:00
if (!zdres(1, obsb, nb, rs, dts, svh, nav, rtk->rb, opt, 1, yb, e, azel))
2017-04-24 22:48:13 +00:00
{
return tt;
}
for (i = 0;i<n;i++)
2017-04-24 22:48:13 +00:00
{
for (j = 0;j<nb;j++) if (obsb[j].sat == obs[i].sat) break;
if (j >= nb) continue;
for (k = 0, p = y+i*nf*2, q = yb+j*nf*2;k<nf*2;k++, p++, q++)
2017-04-24 22:48:13 +00:00
{
if (*p == 0.0 || *q == 0.0) *p = 0.0; else *p = (ttb*(*p)-tt*(*q))/(ttb-tt);
2017-04-24 22:48:13 +00:00
}
}
return fabs(ttb)>fabs(tt) ? ttb : tt;
2017-04-24 15:01:35 +00:00
}
2017-04-24 22:48:13 +00:00
/* single to double-difference transformation matrix (D') --------------------*/
int ddmat(rtk_t *rtk, double *D)
2017-04-24 15:01:35 +00:00
{
int i, j, k, m, f, nb = 0, nx = rtk->nx, na = rtk->na, nf = NF_RTK(&rtk->opt), nofix;
2017-04-24 22:48:13 +00:00
trace(3, "ddmat :\n");
2017-04-24 22:48:13 +00:00
for (i = 0;i<MAXSAT;i++) for (j = 0;j<NFREQ;j++)
2017-04-24 22:48:13 +00:00
{
rtk->ssat[i].fix[j] = 0;
2017-04-24 22:48:13 +00:00
}
for (i = 0;i<na;i++) D[i+i*nx] = 1.0;
2017-04-24 22:48:13 +00:00
for (m = 0;m<4;m++)
{ /* m=0:gps/qzs/sbs, 1:glo, 2:gal, 3:bds */
nofix = (m == 1 && rtk->opt.glomodear == 0) || (m == 3 && rtk->opt.bdsmodear == 0);
2017-04-24 22:48:13 +00:00
for (f = 0, k = na;f<nf;f++, k+=MAXSAT)
2017-04-24 22:48:13 +00:00
{
for (i = k;i<k+MAXSAT;i++)
2017-04-24 22:48:13 +00:00
{
#if 0
if (rtk->x[i] == 0.0||!test_sys(rtk->ssat[i-k].sys,m)||
2017-04-24 22:48:13 +00:00
!rtk->ssat[i-k].vsat[f])
{
#else
if (rtk->x[i] == 0.0 || !test_sys(rtk->ssat[i-k].sys, m) ||
!rtk->ssat[i-k].vsat[f] || !rtk->ssat[i-k].half[f])
{
2017-04-24 22:48:13 +00:00
#endif
continue;
2017-04-24 22:48:13 +00:00
}
if (rtk->ssat[i-k].lock[f]>0 && !(rtk->ssat[i-k].slip[f]&2) &&
rtk->ssat[i-k].azel[1] >= rtk->opt.elmaskar && !nofix)
2017-04-24 22:48:13 +00:00
{
rtk->ssat[i-k].fix[f] = 2; /* fix */
break;
2017-04-24 22:48:13 +00:00
}
else rtk->ssat[i-k].fix[f] = 1;
}
for (j = k;j<k+MAXSAT;j++)
{
if (i == j || rtk->x[j] == 0.0 || !test_sys(rtk->ssat[j-k].sys, m) ||
!rtk->ssat[j-k].vsat[f])
{
continue;
}
if (rtk->ssat[j-k].lock[f]>0 && !(rtk->ssat[j-k].slip[f]&2) &&
rtk->ssat[i-k].vsat[f] &&
rtk->ssat[j-k].azel[1] >= rtk->opt.elmaskar && !nofix)
{
D[i+(na+nb)*nx] = 1.0;
D[j+(na+nb)*nx] = -1.0;
nb++;
rtk->ssat[j-k].fix[f] = 2; /* fix */
}
else rtk->ssat[j-k].fix[f] = 1;
2017-04-24 22:48:13 +00:00
}
}
}
trace(5, "D=\n"); tracemat(5, D, nx, na+nb, 2, 0);
return nb;
}
2017-04-24 22:48:13 +00:00
2017-04-24 15:01:35 +00:00
/* restore single-differenced ambiguity --------------------------------------*/
void restamb(rtk_t *rtk, const double *bias, int nb __attribute((unused)), double *xa)
{
int i, n, m, f, index[MAXSAT], nv = 0, nf = NF_RTK(&rtk->opt);
2017-04-24 15:01:35 +00:00
trace(3, "restamb :\n");
2017-04-24 15:01:35 +00:00
for (i = 0;i<rtk->nx;i++) xa[i] = rtk->x [i];
for (i = 0;i<rtk->na;i++) xa[i] = rtk->xa[i];
2017-04-24 22:48:13 +00:00
for (m = 0;m<4;m++) for (f = 0;f<nf;f++)
{
for (n = i = 0;i<MAXSAT;i++)
{
if (!test_sys(rtk->ssat[i].sys, m) || rtk->ssat[i].fix[f] != 2)
{
continue;
}
index[n++] = IB_RTK(i+1, f, &rtk->opt);
}
if (n<2) continue;
2017-04-24 22:48:13 +00:00
xa[index[0]] = rtk->x[index[0]];
2017-04-24 22:48:13 +00:00
for (i = 1;i<n;i++)
{
xa[index[i]] = xa[index[0]]-bias[nv++];
}
}
}
2017-04-24 22:48:13 +00:00
2017-04-30 08:02:06 +00:00
/* hold integer ambiguity ----------------------------------------------------*/
void holdamb(rtk_t *rtk, const double *xa)
{
double *v, *H, *R;
int i, n, m, f, info, index[MAXSAT], nb = rtk->nx-rtk->na, nv = 0, nf = NF_RTK(&rtk->opt);
2017-04-24 22:48:13 +00:00
trace(3, "holdamb :\n");
2017-04-24 22:48:13 +00:00
v = mat(nb, 1); H = zeros(nb, rtk->nx);
2017-04-24 22:48:13 +00:00
for (m = 0;m<4;m++) for (f = 0;f<nf;f++)
{
for (n = i = 0;i<MAXSAT;i++)
{
if (!test_sys(rtk->ssat[i].sys, m) || rtk->ssat[i].fix[f] != 2 ||
rtk->ssat[i].azel[1]<rtk->opt.elmaskhold)
{
continue;
}
index[n++] = IB_RTK(i+1, f, &rtk->opt);
rtk->ssat[i].fix[f] = 3; /* hold */
2017-04-24 22:48:13 +00:00
}
/* constraint to fixed ambiguity */
for (i = 1;i<n;i++)
{
v[nv] = (xa[index[0]]-xa[index[i]])-(rtk->x[index[0]]-rtk->x[index[i]]);
2017-04-24 22:48:13 +00:00
H[index[0]+nv*rtk->nx] = 1.0;
H[index[i]+nv*rtk->nx] = -1.0;
nv++;
}
}
if (nv>0)
{
R = zeros(nv, nv);
for (i = 0;i<nv;i++) R[i+i*nv] = VAR_HOLDAMB;
2017-04-24 22:48:13 +00:00
/* update states with constraints */
if ((info = filter(rtk->x, rtk->P, H, v, R, rtk->nx, nv)))
{
errmsg(rtk, "filter error (info=%d)\n", info);
}
free(R);
}
free(v); free(H);
}
2017-04-24 22:48:13 +00:00
2017-04-30 08:02:06 +00:00
/* resolve integer ambiguity by LAMBDA ---------------------------------------*/
int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa)
{
prcopt_t *opt = &rtk->opt;
int i, j, ny, nb, info, nx = rtk->nx, na = rtk->na;
double *D, *DP, *y, *Qy, *b, *db, *Qb, *Qab, *QQ, s[2];
2017-04-24 22:48:13 +00:00
trace(3, "resamb_LAMBDA : nx=%d\n", nx);
2017-04-24 22:48:13 +00:00
rtk->sol.ratio = 0.0;
2017-04-24 22:48:13 +00:00
if (rtk->opt.mode <= PMODE_DGPS || rtk->opt.modear == ARMODE_OFF ||
rtk->opt.thresar[0]<1.0)
{
return 0;
}
/* single to double-difference transformation matrix (D') */
D = zeros(nx, nx);
if ((nb = ddmat(rtk, D)) <= 0)
{
errmsg(rtk, "no valid double-difference\n");
free(D);
return 0;
}
ny = na+nb; y = mat(ny, 1); Qy = mat(ny, ny); DP = mat(ny, nx);
b = mat(nb, 2); db = mat(nb, 1); Qb = mat(nb, nb); Qab = mat(na, nb); QQ = mat(na, nb);
2017-04-24 22:48:13 +00:00
/* transform single to double-differenced phase-bias (y=D'*x, Qy=D'*P*D) */
matmul("TN", ny, 1, nx, 1.0, D , rtk->x, 0.0, y );
matmul("TN", ny, nx, nx, 1.0, D , rtk->P, 0.0, DP);
matmul("NN", ny, ny, nx, 1.0, DP, D , 0.0, Qy);
2017-04-24 22:48:13 +00:00
/* phase-bias covariance (Qb) and real-parameters to bias covariance (Qab) */
for (i = 0;i<nb;i++) for (j = 0;j<nb;j++) Qb [i+j*nb] = Qy[na+i+(na+j)*ny];
for (i = 0;i<na;i++) for (j = 0;j<nb;j++) Qab[i+j*na] = Qy[ i+(na+j)*ny];
2017-04-24 22:48:13 +00:00
trace(4, "N(0)="); tracemat(4, y+na, 1, nb, 10, 3);
2017-04-24 22:48:13 +00:00
/* lambda/mlambda integer least-square estimation */
if (!(info = lambda(nb, 2, y+na, Qb, b, s)))
{
trace(4, "N(1)="); tracemat(4, b , 1, nb, 10, 3);
trace(4, "N(2)="); tracemat(4, b+nb, 1, nb, 10, 3);
2017-04-24 22:48:13 +00:00
rtk->sol.ratio = s[0]>0 ? (float)(s[1]/s[0]) : 0.0f;
if (rtk->sol.ratio>999.9) rtk->sol.ratio = 999.9f;
2017-04-24 22:48:13 +00:00
/* validation by popular ratio-test */
if (s[0] <= 0.0 || s[1]/s[0] >= opt->thresar[0])
{
/* transform float to fixed solution (xa=xa-Qab*Qb\(b0-b)) */
for (i = 0;i<na;i++)
{
rtk->xa[i] = rtk->x[i];
for (j = 0;j<na;j++) rtk->Pa[i+j*na] = rtk->P[i+j*nx];
}
for (i = 0;i<nb;i++)
{
bias[i] = b[i];
y[na+i] -= b[i];
}
if (!matinv(Qb, nb))
{
matmul("NN", nb, 1, nb, 1.0, Qb , y+na, 0.0, db);
matmul("NN", na, 1, nb, -1.0, Qab, db , 1.0, rtk->xa);
2017-04-24 22:48:13 +00:00
/* covariance of fixed solution (Qa=Qa-Qab*Qb^-1*Qab') */
matmul("NN", na, nb, nb, 1.0, Qab, Qb , 0.0, QQ);
matmul("NT", na, na, nb, -1.0, QQ , Qab, 1.0, rtk->Pa);
2017-04-24 22:48:13 +00:00
trace(3, "resamb : validation ok (nb=%d ratio=%.2f s=%.2f/%.2f)\n",
nb, s[0] == 0.0 ? 0.0 : s[1]/s[0], s[0], s[1]);
2017-04-24 22:48:13 +00:00
/* restore single-differenced ambiguity */
restamb(rtk, bias, nb, xa);
}
else nb = 0;
}
else
{ /* validation failed */
errmsg(rtk, "ambiguity validation failed (nb=%d ratio=%.2f s=%.2f/%.2f)\n",
nb, s[1]/s[0], s[0], s[1]);
nb = 0;
}
}
else
{
errmsg(rtk, "lambda error (info=%d)\n", info);
}
free(D); free(y); free(Qy); free(DP);
free(b); free(db); free(Qb); free(Qab); free(QQ);
2017-04-24 22:48:13 +00:00
return nb; /* number of ambiguities */
}
2017-04-24 22:48:13 +00:00
2017-04-30 08:02:06 +00:00
/* validation of solution ----------------------------------------------------*/
int valpos(rtk_t *rtk, const double *v, const double *R, const int *vflg,
int nv, double thres)
{
2017-04-24 22:48:13 +00:00
#if 0
prcopt_t *opt = &rtk->opt;
double vv = 0.0;
2017-04-24 22:48:13 +00:00
#endif
double fact = thres*thres;
int i, stat = 1, sat1, sat2, type, freq;
char stype;
2017-04-24 22:48:13 +00:00
trace(3, "valpos : nv=%d thres=%.1f\n", nv, thres);
2017-04-24 22:48:13 +00:00
/* post-fit residual test */
for (i = 0;i<nv;i++)
{
if (v[i]*v[i] <= fact*R[i+i*nv]) continue;
sat1 = (vflg[i]>>16)&0xFF;
sat2 = (vflg[i]>> 8)&0xFF;
type = (vflg[i]>> 4)&0xF;
freq = vflg[i]&0xF;
stype = type == 0 ? 'L' : (type == 1 ? 'L' : 'C');
errmsg(rtk, "large residual (sat=%2d-%2d %s%d v=%6.3f sig=%.3f)\n",
sat1, sat2, stype, freq+1, v[i], std::sqrt(R[i+i*nv]));
}
2017-04-24 22:48:13 +00:00
#if 0 /* omitted v.2.4.0 */
if (stat&&nv>NP(opt))
{
/* chi-square validation */
for (i = 0; i<nv; i++) vv += v[i]*v[i]/R[i+i*nv];
2017-04-24 22:48:13 +00:00
if (vv > chisqr[nv-NP(opt)-1])
{
errmsg(rtk,"residuals validation failed (nv=%d np=%d vv=%.2f cs=%.2f)\n",
nv, NP(opt), vv, chisqr[nv-NP(opt)-1]);
stat = 0;
}
else
{
trace(3,"valpos : validation ok (%s nv=%d np=%d vv=%.2f cs=%.2f)\n",
rtk->tstr, nv, NP(opt), vv, chisqr[nv-NP(opt)-1]);
}
}
2017-04-24 15:01:35 +00:00
#endif
return stat;
}
2017-04-24 22:48:13 +00:00
2017-04-30 08:02:06 +00:00
/* relative positioning ------------------------------------------------------*/
int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
const nav_t *nav)
{
prcopt_t *opt = &rtk->opt;
gtime_t time = obs[0].time;
double *rs, *dts, *var, *y, *e, *azel, *v, *H, *R, *xp, *Pp, *xa, *bias, dt;
int i, j, f, n = nu+nr, ns, ny, nv, sat[MAXSAT], iu[MAXSAT], ir[MAXSAT], niter;
int info, vflg[MAXOBS*NFREQ*2+1], svh[MAXOBS*2];
int stat = rtk->opt.mode <= PMODE_DGPS ? SOLQ_DGPS : SOLQ_FLOAT;
int nf = opt->ionoopt == IONOOPT_IFLC ? 1 : opt->nf;
2017-04-24 22:48:13 +00:00
trace(3, "relpos : nx=%d nu=%d nr=%d\n", rtk->nx, nu, nr);
2017-04-24 22:48:13 +00:00
dt = timediff(time, obs[nu].time);
2017-04-24 22:48:13 +00:00
rs = mat(6, n); dts = mat(2, n); var = mat(1, n); y = mat(nf*2, n); e = mat(3, n);
azel = zeros(2, n);
2017-04-24 22:48:13 +00:00
for (i = 0;i<MAXSAT;i++)
{
rtk->ssat[i].sys = satsys(i+1, NULL);
for (j = 0;j<NFREQ;j++) rtk->ssat[i].vsat[j] = rtk->ssat[i].snr[j] = 0;
}
/* satellite positions/clocks */
satposs(time, obs, n, nav, opt->sateph, rs, dts, var, svh);
2017-04-24 22:48:13 +00:00
/* undifferenced residuals for base station */
if (!zdres(1, obs+nu, nr, rs+nu*6, dts+nu*2, svh+nu, nav, rtk->rb, opt, 1,
y+nu*nf*2, e+nu*3, azel+nu*2))
{
errmsg(rtk, "initial base station position error\n");
2017-04-24 22:48:13 +00:00
free(rs); free(dts); free(var); free(y); free(e); free(azel);
return 0;
}
/* time-interpolation of residuals (for post-processing) */
if (opt->intpref)
{
dt = intpres(time, obs+nu, nr, nav, rtk, y+nu*nf*2);
}
/* select common satellites between rover and base-station */
if ((ns = selsat(obs, azel, nu, nr, opt, sat, iu, ir)) <= 0)
{
errmsg(rtk, "no common satellite\n");
2017-04-24 22:48:13 +00:00
free(rs); free(dts); free(var); free(y); free(e); free(azel);
return 0;
}
/* temporal update of states */
udstate(rtk, obs, sat, iu, ir, ns, nav);
2017-04-24 22:48:13 +00:00
trace(4, "x(0)="); tracemat(4, rtk->x, 1, NR_RTK(opt), 13, 4);
2017-04-24 22:48:13 +00:00
xp = mat(rtk->nx, 1); Pp = zeros(rtk->nx, rtk->nx); xa = mat(rtk->nx, 1);
matcpy(xp, rtk->x, rtk->nx, 1);
2017-04-24 22:48:13 +00:00
ny = ns*nf*2+2;
v = mat(ny, 1); H = zeros(rtk->nx, ny); R = mat(ny, ny); bias = mat(rtk->nx, 1);
2017-04-24 22:48:13 +00:00
/* add 2 iterations for baseline-constraint moving-base */
niter = opt->niter+(opt->mode == PMODE_MOVEB && opt->baseline[0]>0.0 ? 2 : 0);
2017-04-24 22:48:13 +00:00
for (i = 0;i<niter;i++)
{
/* undifferenced residuals for rover */
if (!zdres(0, obs, nu, rs, dts, svh, nav, xp, opt, 0, y, e, azel))
{
errmsg(rtk, "rover initial position error\n");
stat = SOLQ_NONE;
break;
}
/* double-differenced residuals and partial derivatives */
if ((nv = ddres(rtk, nav, dt, xp, Pp, sat, y, e, azel, iu, ir, ns, v, H, R, vflg))<1)
{
errmsg(rtk, "no double-differenced residual\n");
stat = SOLQ_NONE;
break;
}
/* kalman filter measurement update */
matcpy(Pp, rtk->P, rtk->nx, rtk->nx);
if ((info = filter(xp, Pp, H, v, R, rtk->nx, nv)))
{
errmsg(rtk, "filter error (info=%d)\n", info);
stat = SOLQ_NONE;
break;
}
trace(4, "x(%d)=", i+1); tracemat(4, xp, 1, NR_RTK(opt), 13, 4);
}
if (stat != SOLQ_NONE && zdres(0, obs, nu, rs, dts, svh, nav, xp, opt, 0, y, e, azel))
{
/* post-fit residuals for float solution */
nv = ddres(rtk, nav, dt, xp, Pp, sat, y, e, azel, iu, ir, ns, v, NULL, R, vflg);
2017-04-24 22:48:13 +00:00
/* validation of float solution */
if (valpos(rtk, v, R, vflg, nv, 4.0))
{
/* update state and covariance matrix */
matcpy(rtk->x, xp, rtk->nx, 1);
matcpy(rtk->P, Pp, rtk->nx, rtk->nx);
2017-04-24 22:48:13 +00:00
/* update ambiguity control struct */
rtk->sol.ns = 0;
for (i = 0;i<ns;i++) for (f = 0;f<nf;f++)
{
if (!rtk->ssat[sat[i]-1].vsat[f]) continue;
rtk->ssat[sat[i]-1].lock[f]++;
rtk->ssat[sat[i]-1].outc[f] = 0;
if (f == 0) rtk->sol.ns++; /* valid satellite count by L1 */
}
/* lack of valid satellites */
if (rtk->sol.ns<4) stat = SOLQ_NONE;
}
else stat = SOLQ_NONE;
}
/* resolve integer ambiguity by WL-NL */
if (stat != SOLQ_NONE && rtk->opt.modear == ARMODE_WLNL)
{
if (resamb_WLNL(rtk, obs, sat, iu, ir, ns, nav, azel))
{
stat = SOLQ_FIX;
}
}
/* resolve integer ambiguity by TCAR */
else if (stat != SOLQ_NONE && rtk->opt.modear == ARMODE_TCAR)
{
if (resamb_TCAR(rtk, obs, sat, iu, ir, ns, nav, azel))
{
stat = SOLQ_FIX;
}
}
/* resolve integer ambiguity by LAMBDA */
else if (stat != SOLQ_NONE && resamb_LAMBDA(rtk, bias, xa)>1)
{
if (zdres(0, obs, nu, rs, dts, svh, nav, xa, opt, 0, y, e, azel))
{
/* post-fit reisiduals for fixed solution */
nv = ddres(rtk, nav, dt, xa, NULL, sat, y, e, azel, iu, ir, ns, v, NULL, R, vflg);
2017-04-24 22:48:13 +00:00
/* validation of fixed solution */
if (valpos(rtk, v, R, vflg, nv, 4.0))
{
/* hold integer ambiguity */
if (++rtk->nfix >= rtk->opt.minfix &&
rtk->opt.modear == ARMODE_FIXHOLD)
{
holdamb(rtk, xa);
}
stat = SOLQ_FIX;
}
}
}
2017-04-24 22:48:13 +00:00
/* save solution status */
if (stat == SOLQ_FIX)
{
for (i = 0;i<3;i++)
{
rtk->sol.rr[i] = rtk->xa[i];
rtk->sol.qr[i] = (float)rtk->Pa[i+i*rtk->na];
}
rtk->sol.qr[3] = (float)rtk->Pa[1];
rtk->sol.qr[4] = (float)rtk->Pa[1+2*rtk->na];
rtk->sol.qr[5] = (float)rtk->Pa[2];
}
else
{
for (i = 0;i<3;i++)
{
rtk->sol.rr[i] = rtk->x[i];
rtk->sol.qr[i] = (float)rtk->P[i+i*rtk->nx];
}
rtk->sol.qr[3] = (float)rtk->P[1];
rtk->sol.qr[4] = (float)rtk->P[1+2*rtk->nx];
rtk->sol.qr[5] = (float)rtk->P[2];
rtk->nfix = 0;
}
for (i = 0;i<n;i++) for (j = 0;j<nf;j++)
{
if (obs[i].L[j] == 0.0) continue;
rtk->ssat[obs[i].sat-1].pt[obs[i].rcv-1][j] = obs[i].time;
rtk->ssat[obs[i].sat-1].ph[obs[i].rcv-1][j] = obs[i].L[j];
}
for (i = 0;i<ns;i++) for (j = 0;j<nf;j++)
{
/* output snr of rover receiver */
rtk->ssat[sat[i]-1].snr[j] = obs[iu[i]].SNR[j];
}
for (i = 0;i<MAXSAT;i++) for (j = 0;j<nf;j++)
{
if (rtk->ssat[i].fix[j] == 2 && stat != SOLQ_FIX) rtk->ssat[i].fix[j] = 1;
if (rtk->ssat[i].slip[j]&1) rtk->ssat[i].slipc[j]++;
}
free(rs); free(dts); free(var); free(y); free(e); free(azel);
free(xp); free(Pp); free(xa); free(v); free(H); free(R); free(bias);
2017-04-24 22:48:13 +00:00
if (stat != SOLQ_NONE) rtk->sol.stat = stat;
2017-04-24 22:48:13 +00:00
return stat != SOLQ_NONE;
}
2017-04-24 15:01:35 +00:00
2017-04-30 08:02:06 +00:00
/* initialize rtk control ------------------------------------------------------
* initialize rtk control struct
* args : rtk_t *rtk IO rtk control/result struct
* prcopt_t *opt I positioning options (see rtklib.h)
* return : none
*-----------------------------------------------------------------------------*/
void rtkinit(rtk_t *rtk, const prcopt_t *opt)
{
sol_t sol0 = { {0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0};
ambc_t ambc0 = {{ {0, 0}, {0, 0}, {0, 0}, {0, 0} }, {}, {}, {} , 0, {}};
ssat_t ssat0 = {0, 0, {0.0}, {0.0}, {0.0}, {'0'}, {'0'}, {'0'}, {'0'}, {'0'}, {}, {}, {}, {}, 0.0, 0.0, 0.0, 0.0, {{0,0},{0,0}}, {{},{}}};
int i;
2017-04-24 15:01:35 +00:00
trace(3, "rtkinit :\n");
rtk->sol = sol0;
for (i = 0;i<6;i++) rtk->rb[i] = 0.0;
rtk->nx = opt->mode <= PMODE_FIXED ? NX_RTK(opt) : pppnx(opt);
rtk->na = opt->mode <= PMODE_FIXED ? NR_RTK(opt) : pppnx(opt);
rtk->tt = 0.0;
rtk->x = zeros(rtk->nx, 1);
rtk->P = zeros(rtk->nx, rtk->nx);
rtk->xa = zeros(rtk->na, 1);
rtk->Pa = zeros(rtk->na, rtk->na);
rtk->nfix = rtk->neb = 0;
for (i = 0;i<MAXSAT;i++)
{
rtk->ambc[i] = ambc0;
rtk->ssat[i] = ssat0;
}
for (i = 0;i<MAXERRMSG;i++) rtk->errbuf[i] = 0;
rtk->opt = *opt;
}
2017-04-24 15:01:35 +00:00
2017-04-24 22:48:13 +00:00
/* free rtk control ------------------------------------------------------------
* free memory for rtk control struct
* args : rtk_t *rtk IO rtk control/result struct
* return : none
*-----------------------------------------------------------------------------*/
void rtkfree(rtk_t *rtk)
{
trace(3, "rtkfree :\n");
2017-04-24 22:48:13 +00:00
rtk->nx = rtk->na = 0;
free(rtk->x ); rtk->x = NULL;
free(rtk->P ); rtk->P = NULL;
free(rtk->xa); rtk->xa = NULL;
free(rtk->Pa); rtk->Pa = NULL;
}
2017-04-24 22:48:13 +00:00
/* precise positioning ---------------------------------------------------------
* input observation data and navigation message, compute rover position by
* precise positioning
* args : rtk_t *rtk IO rtk control/result struct
* rtk->sol IO solution
* .time O solution time
* .rr[] IO rover position/velocity
* (I:fixed mode,O:single mode)
* .dtr[0] O receiver clock bias (s)
* .dtr[1] O receiver glonass-gps time offset (s)
* .Qr[] O rover position covarinace
* .stat O solution status (SOLQ_???)
* .ns O number of valid satellites
* .age O age of differential (s)
* .ratio O ratio factor for ambiguity validation
* rtk->rb[] IO base station position/velocity
* (I:relative mode,O:moving-base mode)
* rtk->nx I number of all states
* rtk->na I number of integer states
* rtk->ns O number of valid satellite
* rtk->tt O time difference between current and previous (s)
* rtk->x[] IO float states pre-filter and post-filter
* rtk->P[] IO float covariance pre-filter and post-filter
* rtk->xa[] O fixed states after AR
* rtk->Pa[] O fixed covariance after AR
* rtk->ssat[s] IO sat(s+1) status
* .sys O system (SYS_???)
* .az [r] O azimuth angle (rad) (r=0:rover,1:base)
* .el [r] O elevation angle (rad) (r=0:rover,1:base)
* .vs [r] O data valid single (r=0:rover,1:base)
* .resp [f] O freq(f+1) pseudorange residual (m)
* .resc [f] O freq(f+1) carrier-phase residual (m)
* .vsat [f] O freq(f+1) data vaild (0:invalid,1:valid)
* .fix [f] O freq(f+1) ambiguity flag
* (0:nodata,1:float,2:fix,3:hold)
* .slip [f] O freq(f+1) slip flag
* (bit8-7:rcv1 LLI, bit6-5:rcv2 LLI,
* bit2:parity unknown, bit1:slip)
* .lock [f] IO freq(f+1) carrier lock count
* .outc [f] IO freq(f+1) carrier outage count
* .slipc[f] IO freq(f+1) cycle slip count
* .rejc [f] IO freq(f+1) data reject count
* .gf IO geometry-free phase (L1-L2) (m)
* .gf2 IO geometry-free phase (L1-L5) (m)
* rtk->nfix IO number of continuous fixes of ambiguity
* rtk->neb IO bytes of error message buffer
* rtk->errbuf IO error message buffer
* rtk->tstr O time string for debug
* rtk->opt I processing options
* obsd_t *obs I observation data for an epoch
* obs[i].rcv=1:rover,2:reference
* sorted by receiver and satellte
* int n I number of observation data
* nav_t *nav I navigation messages
* return : status (0:no solution,1:valid solution)
* notes : before calling function, base station position rtk->sol.rb[] should
* be properly set for relative mode except for moving-baseline
*-----------------------------------------------------------------------------*/
int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
{
prcopt_t *opt = &rtk->opt;
sol_t solb = { {0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0};
gtime_t time;
int i, nu, nr;
char msg[128] = "";
trace(3, "rtkpos : time=%s n=%d\n", time_str(obs[0].time, 3), n);
trace(4, "obs=\n"); traceobs(4, obs, n);
/*trace(5,"nav=\n"); tracenav(5,nav);*/
/* set base staion position */
if (opt->refpos <= POSOPT_RINEX && opt->mode != PMODE_SINGLE &&
opt->mode != PMODE_MOVEB)
{
for (i = 0;i<6;i++) rtk->rb[i] = i<3 ? opt->rb[i] : 0.0;
}
/* count rover/base station observations */
for (nu = 0;nu <n && obs[nu ].rcv == 1;nu++) ;
for (nr = 0;nu+nr<n && obs[nu+nr].rcv == 2;nr++) ;
2017-04-24 22:48:13 +00:00
time = rtk->sol.time; /* previous epoch */
2017-04-24 22:48:13 +00:00
/* rover position by single point positioning */
if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, NULL, rtk->ssat, msg))
{
errmsg(rtk, "point pos error (%s)\n", msg);
if (!rtk->opt.dynamics)
{
outsolstat(rtk);
return 0;
}
}
2017-04-30 08:02:06 +00:00
if (time.time != 0) rtk->tt = timediff(rtk->sol.time, time);
2017-04-24 22:48:13 +00:00
/* single point positioning */
if (opt->mode == PMODE_SINGLE)
{
outsolstat(rtk);
return 1;
}
/* suppress output of single solution */
if (!opt->outsingle)
{
rtk->sol.stat = SOLQ_NONE;
}
/* precise point positioning */
if (opt->mode >= PMODE_PPP_KINEMA)
{
pppos(rtk, obs, nu, nav);
outsolstat(rtk);
return 1;
}
/* check number of data of base station and age of differential */
if (nr == 0)
{
errmsg(rtk, "no base station observation data for rtk\n");
outsolstat(rtk);
return 1;
}
if (opt->mode == PMODE_MOVEB)
{ /* moving baseline */
2017-04-24 22:48:13 +00:00
/* estimate position/velocity of base station */
if (!pntpos(obs+nu, nr, nav, &rtk->opt, &solb, NULL, NULL, msg))
{
errmsg(rtk, "base station position error (%s)\n", msg);
return 0;
}
rtk->sol.age = (float)timediff(rtk->sol.time, solb.time);
2017-04-24 22:48:13 +00:00
if (fabs(rtk->sol.age)>TTOL_MOVEB)
{
errmsg(rtk, "time sync error for moving-base (age=%.1f)\n", rtk->sol.age);
return 0;
}
for (i = 0;i<6;i++) rtk->rb[i] = solb.rr[i];
2017-04-24 22:48:13 +00:00
/* time-synchronized position of base station */
2017-04-30 08:02:06 +00:00
for (i = 0;i<3;i++) rtk->rb[i] += rtk->rb[i+3]*rtk->sol.age;
}
else
{
rtk->sol.age = (float)timediff(obs[0].time, obs[nu].time);
2017-04-24 22:48:13 +00:00
if (fabs(rtk->sol.age)>opt->maxtdiff)
{
errmsg(rtk, "age of differential error (age=%.1f)\n", rtk->sol.age);
outsolstat(rtk);
return 1;
}
}
/* relative potitioning */
relpos(rtk, obs, nu, nr, nav);
outsolstat(rtk);
2017-04-24 22:48:13 +00:00
return 1;
}