From 7c124e051dbd77858b724accc3b6c7642aef7cc2 Mon Sep 17 00:00:00 2001
From: oaq <info@ourairquality.org>
Date: Wed, 13 Nov 2024 00:13:07 +1100
Subject: [PATCH] tropmodel, rtkpos: use the precise mapping function

Have tropmodel use the precise mapping function, rather than 1/cos(z).

Update the exponent in the pressure vs temperature function, from
5.2568 to 5.255877 which is found in references on the matter.

Update the rtkpos RTK code to also account for the wet component when
not estimating it, and to also allow use of the SBAS model when that
is selected.
---
 src/rtkcmn.c | 47 ++++++++++++++++++++++++++---------------------
 src/rtkpos.c | 21 +++++++++++++++------
 2 files changed, 41 insertions(+), 27 deletions(-)

diff --git a/src/rtkcmn.c b/src/rtkcmn.c
index 5ff96f154..556377fd6 100644
--- a/src/rtkcmn.c
+++ b/src/rtkcmn.c
@@ -3677,35 +3677,40 @@ extern int seliflc(int optnf,int sys)
     /* use L1/L5 for Galileo if L5 is enabled */
     return((optnf==2||sys!=SYS_GAL)?1:2);
 }
-/* troposphere model -----------------------------------------------------------
-* compute tropospheric delay by standard atmosphere and saastamoinen model
-* args   : gtime_t time     I   time
+
+/* Troposphere model -----------------------------------------------------------
+* Compute tropospheric delay by standard atmosphere and saastamoinen model
+* Args   : gtime_t time     I   time
 *          double *pos      I   receiver position {lat,lon,h} (rad,m)
 *          double *azel     I   azimuth/elevation angle {az,el} (rad)
 *          double humi      I   relative humidity
-* return : tropospheric delay (m)
-*-----------------------------------------------------------------------------*/
-extern double tropmodel(gtime_t time, const double *pos, const double *azel,
-                        double humi)
-{
-    const double temp0=15.0; /* temperature at sea level */
-    double hgt,pres,temp,e,z,trph,trpw;
+* Return : tropospheric delay (m)
+*----------------------------------------------------------------------------*/
+extern double tropmodel(gtime_t time, const double *pos, const double *azel, double humi) {
+  const double temp0 = 15.0; // Temperature at sea level
 
-    if (pos[2]<-100.0||1E4<pos[2]||azel[1]<=0) return 0.0;
+  if (pos[2] < -100.0 || 1E4 < pos[2] || azel[1] <= 0) return 0.0;
 
-    /* standard atmosphere */
-    hgt=pos[2]<0.0?0.0:pos[2];
+  // Standard atmosphere
+  double hgt = pos[2] < 0.0 ? 0.0 : pos[2];
 
-    pres=1013.25*pow(1.0-2.2557E-5*hgt,5.2568);
-    temp=temp0-6.5E-3*hgt+273.16;
-    e=6.108*humi*exp((17.15*temp-4684.0)/(temp-38.45));
+  double temp = temp0 - 6.5E-3 * hgt + 273.16;
+  double pres = 1013.25 * pow(288.15 / temp, -5.255877);
+  double e = 6.108 * humi * exp((17.15 * temp - 4684.0) / (temp - 38.45));
 
-    /* saastamoinen model */
-    z=PI/2.0-azel[1];
-    trph=0.0022768*pres/(1.0-0.00266*cos(2.0*pos[0])-0.00028*hgt/1E3)/cos(z);
-    trpw=0.002277*(1255.0/temp+0.05)*e/cos(z);
-    return trph+trpw;
+  // Saastamoinen model
+  double z = PI / 2.0 - azel[1];
+  double trpzh = 0.0022768 * pres / (1.0 - 0.00266 * cos(2.0 * pos[0]) - 0.00028 * hgt / 1E3);
+  double trpzw = 0.002277 * (1255.0 / temp + 0.05) * e;
+
+  // Fast exit path.
+  if (fabs(z) < 1e-10) return trpzh + trpzw;
+
+  // Precise mapping function
+  double mapfw, mapfh = tropmapf(time, pos, azel, &mapfw);
+  return trpzh * mapfh + trpzw * mapfw;
 }
+
 #ifndef IERS_MODEL
 
 static double interpc(const double coef[], double lat)
diff --git a/src/rtkpos.c b/src/rtkpos.c
index 6558c8071..66fce6607 100644
--- a/src/rtkpos.c
+++ b/src/rtkpos.c
@@ -54,6 +54,7 @@
 #define STD_PREC_VAR_THRESH 0  /* pos variance threshold to skip standard precision */
                               /* solution: 0   = run every epoch, */
                               /*           0.5 = skip except for first*/
+#define REL_HUMI 0.7           /* Relative humidity for Saastamoinen model */
 
 /* constants/macros ----------------------------------------------------------*/
 
@@ -1020,7 +1021,7 @@ static int zdres(int base, const obsd_t *obs, int n, const double *rs,
                  double *y, double *e, double *azel, double *freq)
 {
     double r,rr_[3],pos[3],dant[NFREQ]={0},disp[3];
-    double mapfh,zhd,zazel[]={0.0,90.0*D2R};
+    double mapfh,zazel[]={0.0,90.0*D2R};
     int i,nf=NF(opt);
 
     trace(3,"zdres   : n=%d rr=%.2f %.2f %.2f\n",n,rr[0], rr[1], rr[2]);
@@ -1054,17 +1055,25 @@ static int zdres(int base, const obsd_t *obs, int n, const double *rs,
         /* adjust range for satellite clock-bias */
         r+=-CLIGHT*dts[i*2];
 
-        /* adjust range for troposphere delay model (hydrostatic) */
-        zhd=tropmodel(obs[0].time,pos,zazel,0.0);
-        mapfh=tropmapf(obs[i].time,pos,azel+i*2,NULL);
-        r+=mapfh*zhd;
+        /* Adjust range for troposphere delay model */
+        double dtrp = 0.0;
+        if (opt->tropopt <= TROPOPT_SAAS) {
+          dtrp = tropmodel(obs[0].time, pos, azel + i * 2, REL_HUMI);
+        } else if (opt->tropopt == TROPOPT_SBAS) {
+          double vart;
+          dtrp = sbstropcorr(obs[0].time, pos, azel + i * 2, &vart);
+        } else if (opt->tropopt >= TROPOPT_EST) {
+          // Hydrostatic only
+          dtrp = tropmodel(obs[0].time, pos, azel + i * 2, 0.0);
+        }
+        r += dtrp;
+        trace(4, "sat=%d r=%.6f c*dts=%.6f dtrp=%.6f\n", obs[i].sat, r, CLIGHT * dts[i * 2], dtrp);
 
         /* calc receiver antenna phase center correction */
         antmodel(opt->pcvr+base,opt->antdel[base],azel+i*2,opt->posopt[1],
                  dant);
 
         /* calc undifferenced phase/code residual for satellite */
-        trace(4,"sat=%d r=%.6f c*dts=%.6f zhd=%.6f map=%.6f\n",obs[i].sat,r,CLIGHT*dts[i*2],zhd,mapfh);
         zdres_sat(base,r,obs+i,nav,azel+i*2,dant,opt,y+i*nf*2,freq+i*nf);
     }
     trace(4,"rr_=%.3f %.3f %.3f\n",rr_[0],rr_[1],rr_[2]);