adding troposphere correction

src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc

@@ -107,7 +107,7 @@ arma::vec gps_l1_ca_ls_pvt::rotateSatellite(double traveltime, arma::vec X_sat)
     R3(1, 2) = 0.0;
     R3(2, 0) = 0.0;
     R3(2, 1) = 0.0;
-    R3(2, 2) = 1;
+    R3(2, 2) = 1.0;
 
     //--- Do the rotation ------------------------------------------------------
     arma::vec X_sat_rot;
@@ -147,6 +147,9 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
     double rho2;
     double traveltime;
     double trop;
+    double dlambda;
+    double dphi;
+    double h;
     arma::mat mat_tmp;
     arma::vec x;
 
@@ -164,21 +167,34 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
                     else
                         {
                             //--- Update equations -----------------------------------------
-                            rho2 = (X(0, i) - pos(0)) *
+                            rho2 = (X(0, i) - pos(0)) * (X(0, i) - pos(0))
-                                    (X(0, i) - pos(0))  + (X(1, i) - pos(1)) *
+                                                 + (X(1, i) - pos(1)) * (X(1, i) - pos(1))
-                                    (X(1, i) - pos(1)) + (X(2,i) - pos(2)) *
+                                                 + (X(2, i) - pos(2)) * (X(2, i) - pos(2));
-                                    (X(2,i) - pos(2));
                             traveltime = sqrt(rho2) / GPS_C_m_s;
 
                             //--- Correct satellite position (do to earth rotation) --------
                             Rot_X = rotateSatellite(traveltime, X.col(i)); //armadillo
 
-                            //--- Find DOA and range of satellites
+                            //--- Find satellites' DOA
                             topocent(&d_visible_satellites_Az[i], &d_visible_satellites_El[i],
                                     &d_visible_satellites_Distance[i], pos.subvec(0,2), Rot_X - pos.subvec(0,2));
-                            //[az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :));
 
+                            //--- Find receiver's height
+                            togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
+
+                            //--- Find delay due to troposphere (in meters)
+                            if(rho2 < 1.0e+17 && nmbOfSatellites > 3)
+                                {
+                                    std::cout << h << " h " << iter << "  iter" <<std::endl;
+                                    //std::cout << d_visible_satellites_El[i] << " d_visible_satellites_El[i]" << std::endl;
+                                    tropo(&trop, sin(d_visible_satellites_El[i] * GPS_PI/180.0), h/1000, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
+                                    if(trop > 100.0 ) trop = 0.0;
+                                    //std::cout << rho2 << " rho2" << std::endl;
+                                    std::cout << trop << " trop " <<  i << "i" << std::endl;
                                 }
+                        }
+
+
                     //--- Apply the corrections ----------------------------------------
                     omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0,2),2) - pos(3) - trop); // Armadillo
 
@@ -700,3 +716,104 @@ void gps_l1_ca_ls_pvt::topocent(double *Az, double *El, double *D, arma::vec x,
 
     *D = sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2));
 }
+
+
+void gps_l1_ca_ls_pvt::tropo(double *ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km)
+{
+    /*   Inputs:
+           sinel     - sin of elevation angle of satellite
+           hsta_km   - height of station in km
+           p_mb      - atmospheric pressure in mb at height hp_km
+           t_kel     - surface temperature in degrees Kelvin at height htkel_km
+           hum       - humidity in % at height hhum_km
+           hp_km     - height of pressure measurement in km
+           htkel_km  - height of temperature measurement in km
+           hhum_km   - height of humidity measurement in km
+
+       Outputs:
+           ddr_m     - range correction (meters)
+
+     Reference
+     Goad, C.C. & Goodman, L. (1974) A Modified Tropospheric
+     Refraction Correction Model. Paper presented at the
+     American Geophysical Union Annual Fall Meeting, San
+     Francisco, December 12-17
+
+     Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre
+     */
+
+    const double a_e    = 6378.137;    // semi-major axis of earth ellipsoid
+    const double b0     = 7.839257e-5;
+    const double tlapse = -6.5;
+    const double em     = -978.77 / (2.8704e6 * tlapse * 1.0e-5);
+
+    double tkhum  = t_kel + tlapse * (hhum_km - htkel_km);
+    double atkel  = 7.5*(tkhum - 273.15) / (237.3 + tkhum - 273.15);
+    double e0     = 0.0611 * hum * pow(10, atkel);
+    double tksea  = t_kel - tlapse * htkel_km;
+    double tkelh  = tksea + tlapse * hhum_km;
+    double e0sea  = e0 * pow((tksea / tkelh), (4 * em));
+    double tkelp  = tksea + tlapse * hp_km;
+    double psea   = p_mb * pow((tksea / tkelp), em);
+
+    if(sinel < 0) { sinel = 0.0; }
+
+    double tropo_delay   = 0.0;
+    bool done      = false;
+    double refsea  = 77.624e-6 / tksea;
+    double htop    = 1.1385e-5 / refsea;
+    refsea         = refsea * psea;
+    double ref     = refsea * pow(((htop - hsta_km) / htop), 4);
+
+    double a;
+    double b;
+    double rtop;
+
+    while(1)
+        {
+            rtop = pow((a_e + htop), 2) - pow((a_e + hsta_km), 2) * (1 - pow(sinel, 2));
+
+            // check to see if geometry is crazy
+            if(rtop < 0) { rtop = 0; }
+
+            rtop = sqrt(rtop) - (a_e + hsta_km) * sinel;
+
+            a    = -sinel / (htop - hsta_km);
+            b    = -b0 * (1 - pow(sinel,2)) / (htop - hsta_km);
+
+            arma::vec rn = arma::vec(8);
+            rn.zeros();
+
+            for(int i = 0; i<8; i++)
+                {
+                    rn(i) = pow(rtop, (i+1+1));
+
+                }
+
+            arma::rowvec alpha = {2 * a, pow(2*a, 2) + 4 * b /3, a * (pow(a, 2) + 3 * b),
+                    pow(a, 4)/5 + 2.4 * pow(a, 2) * b + 1.2 * pow(b, 2), 2 * a * b * (pow(a, 2) + 3 * b)/3,
+                    pow(b, 2) * (6 * pow(a, 2) + 4 * b) * 1.428571e-1, 0, 0};
+
+            if(pow(b, 2) > 1.0e-35)
+                {
+                    alpha(6) = a * pow(b, 3) /2;
+                    alpha(7) = pow(b, 4) / 9;
+                }
+
+            double dr = rtop;
+            arma::mat aux_ = alpha * rn;
+            dr = dr + aux_(0, 0);
+            tropo_delay = tropo_delay + dr * ref * 1000;
+
+            if(done == true)
+                {
+                    *ddr_m = tropo_delay;
+                    break;
+                }
+
+            done    = true;
+            refsea  = (371900.0e-6 / tksea - 12.92e-6) / tksea;
+            htop    = 1.1385e-5 * (1255 / tksea + 0.05) / refsea;
+            ref     = refsea * e0sea * pow(((htop - hsta_km) / htop), 4);
+        }
+}

src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h

@@ -65,6 +65,7 @@ private:
     arma::vec rotateSatellite(double traveltime, arma::vec X_sat);
     void topocent(double *Az, double *El, double *D, arma::vec x, arma::vec dx);
     void togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
+    void tropo(double *ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km);
 public:
     int d_nchannels;                                        //!< Number of available channels for positioning
     int d_valid_observations;                               //!< Number of valid pseudorange observations (valid satellites)