WIP

RTKLIB @ 4ab9a199

-Subproject commit 0260d91932df0ca0691842aa99b39acde5d49c45
+Subproject commit 4ab9a199ff46b1220fb4fe99b019c8df526e53e9

include/gnss_utils/tdcp.h

@@ -48,6 +48,19 @@ bool Tdcp(SnapshotPtr               snapshot_r,
           double&                   residual,
           std::set<int>             raim_discarded_sats,
           const TdcpBatchParams&    tdcp_params);
+
+bool Tdcp(const Eigen::Vector3d&    x_r,
+          Eigen::Matrix<double, Eigen::Dynamic, 4>& A,
+          Eigen::VectorXd& r,
+          Eigen::VectorXd& drho_m,
+          Eigen::Matrix<double, 3, Eigen::Dynamic>& s_k,
+          Eigen::Matrix<double, 3, Eigen::Dynamic>& s_r,
+          Eigen::Vector4d&          d,
+          Eigen::Matrix4d&          cov_d,
+          double&                   residual,
+          std::set<int>             raim_discarded_sats,
+          const TdcpBatchParams&    tdcp_params);
+
 }  // namespace GnssUtils
 
 #endif /* INCLUDE_GNSS_UTILS_Tdcp_H_ */

include/gnss_utils/utils/satellite.h

@@ -25,6 +25,24 @@ namespace GnssUtils
                                  const Navigation&   nav,
                                  const int&          eph_opt); // see rtklib.h L396
 
+    Eigen::Vector2d computeAzel(const Satellite& sat, const Eigen::Vector3d& receiver_ecef)
+    {
+        Eigen::Vector3d unit_vector;
+        geodist(sat.pose.data(), receiver_ecef.data(), unit_vector.data());
+
+        Eigen::Vector2d azel;
+        Eigen::Vector3d pos_enu; // TODO
+        satazel(pos_enu.data(), unit_vector.data(), azel.data());
+    }
+
+    Azels computeAzels(const Satellites& sats, const Eigen::Vector3d& receiver_ecef)
+    {
+        Azels azels;
+        for (auto sat : sats)
+            azels.insert(sat.first, computeAzel(sat, receiver_ecef));
+        return azels;
+    }
+
     struct Satellite
     {
         int sys;

src/tdcp.cpp

@@ -149,6 +149,8 @@ bool Tdcp(SnapshotPtr               snapshot_r,
         {
             tr = obs_r.time.time + obs_r.time.sec;
             tk = obs_k.time.time + obs_k.time.sec;
+            std::cout << "tr: " << tr << std::endl;
+            std::cout << "tk: " << tk << std::endl;
         }
 
         // Carrier phase
@@ -171,12 +173,9 @@ bool Tdcp(SnapshotPtr               snapshot_r,
     }
     int n_differences = row_2_sat_freq.size();
 
-    // Initial guess
-    Eigen::Vector4d d_0(d), delta_d(Eigen::Vector4d::Zero());
-
     #if GNSS_UTILS_TDCP_DEBUG == 1
         std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl;
-        std::cout << "d initial guess: " << d_0.transpose() << std::endl;
+        std::cout << "d initial guess: " << d.transpose() << std::endl;
         std::cout << "common sats: ";
         for (auto row_sat_freq_it : row_2_sat_freq)
             std::cout << row_sat_freq_it.second.first << " ";
@@ -189,7 +188,6 @@ bool Tdcp(SnapshotPtr               snapshot_r,
     Eigen::VectorXd                          drho_m(Eigen::VectorXd::Zero(n_differences));
     Eigen::Matrix<double, 3, Eigen::Dynamic> s_k(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3, n_differences));
     Eigen::Matrix<double, 3, Eigen::Dynamic> s_r(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3, n_differences));
-    int                                      sat_i = 0;
 
     for (auto row_sat_freq_it : row_2_sat_freq)
     {
@@ -240,6 +238,36 @@ bool Tdcp(SnapshotPtr               snapshot_r,
     }
 
     // LEAST SQUARES SOLUTION =======================================================================
+    std::set<int> raim_discarded_rows;
+    bool result = Tdcp(x_r, A, r, drho_m, s_k, s_r, d, cov_d, residual, raim_discarded_rows, tdcp_params);
+
+    for (auto disc_row : raim_discarded_rows)
+        raim_discarded_sats.insert(row_2_sat_freq[disc_row].first);
+
+    // weight covariance with the measurement noise (proportional to time)
+    double sq_sigma_Tdcp = (tk - tr) * tdcp_params.tdcp.sigma_atm * tdcp_params.tdcp.sigma_atm +
+                           2 * tdcp_params.tdcp.sigma_carrier * tdcp_params.tdcp.sigma_carrier;
+    cov_d *= sq_sigma_Tdcp;
+
+    return result;
+}
+
+bool Tdcp(const Eigen::Vector3d&    x_r,
+          Eigen::Matrix<double, Eigen::Dynamic, 4>& A,
+          Eigen::VectorXd& r,
+          Eigen::VectorXd& drho_m,
+          Eigen::Matrix<double, 3, Eigen::Dynamic>& s_k,
+          Eigen::Matrix<double, 3, Eigen::Dynamic>& s_r,
+          Eigen::Vector4d&          d,
+          Eigen::Matrix4d&          cov_d,
+          double&                   residual,
+          std::set<int>             raim_discarded_rows,
+          const TdcpBatchParams&    tdcp_params)
+{
+
+    // Initial guess
+    Eigen::Vector4d d_0(d), delta_d(Eigen::Vector4d::Zero());
+
     for (int j = 0; j < tdcp_params.max_iterations; j++)
     {
         // fill A and r
@@ -301,10 +329,9 @@ bool Tdcp(SnapshotPtr               snapshot_r,
             int             worst_sat_row = -1;
             double          best_residual = 1e12;
             Eigen::Vector4d best_d;
-            int             n_removed_rows = 1;
 
             // remove some satellites
-            while (raim_discarded_sats.size() < tdcp_params.raim_n)
+            while (raim_discarded_rows.size() < tdcp_params.raim_n)
             {
                 auto A_raim = A;
                 auto r_raim = r;
@@ -312,21 +339,13 @@ bool Tdcp(SnapshotPtr               snapshot_r,
                 // solve removing each satellite
                 for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++)
                 {
-                    int sat_removed = row_2_sat_freq.at(row_removed).first;
-
-                    // Multi-freq: some rows for the same satellite
-                    n_removed_rows = 1;
-                    if (tdcp_params.tdcp.multi_freq)
-                        while (row_removed + n_removed_rows < A_raim.rows() and
-                               row_2_sat_freq.at(row_removed + n_removed_rows).first == sat_removed)
-                            n_removed_rows++;
+                    // skip already discarded rows
+                    if (raim_discarded_rows.count(row_removed) != 0)
+                        continue;
 
-                    // remove satellite rows
-                    for (auto r = 0; r < n_removed_rows; r++)
-                    {
-                        A_raim.row(row_removed + r) = Eigen::Vector4d::Zero().transpose();
-                        r_raim(row_removed + r)     = 0;  // not necessary
-                    }
+                    // remove satellite row
+                    A_raim.row(row_removed) = Eigen::Vector4d::Zero().transpose();
+                    r_raim(row_removed)     = 0;  // not necessary
 
                     // solve
                     Eigen::Vector4d delta_d_raim = -(A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim;
@@ -340,8 +359,7 @@ bool Tdcp(SnapshotPtr               snapshot_r,
                         // residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / A_raim.rows());
 
                         #if GNSS_UTILS_TDCP_DEBUG == 1
-                            std::cout << "RAIM excluding sat " << sat_removed << " - row " << row_removed << "(n_rows "
-                            << n_removed_rows << ")" << std::endl;
+                            std::cout << "RAIM excluding row " << << row_removed << std::endl;
                             std::cout << "Displacement vector =" << d_raim.head<3>().transpose() << std::endl;
                             std::cout << "Displacement update =" << delta_d_raim.head<3>().transpose() << std::endl;
                             printf("Ref distance      = %7.3f m\n", d_0.norm());
@@ -363,12 +381,8 @@ bool Tdcp(SnapshotPtr               snapshot_r,
                         }
                     }
                     // restore initial A and r
-                    for (auto row = 0; row < n_removed_rows; row++)
-                    {
-                        A_raim.row(row_removed + row) = A.row(row_removed + row);
-                        r_raim(row_removed + row)     = r(row_removed + row);
-                    }
-                    row_removed += (n_removed_rows - 1);
+                    A_raim.row(row_removed) = A.row(row_removed);
+                    r_raim(row_removed)     = r(row_removed);
                 }
 
                 // No successful RAIM solution
@@ -379,13 +393,9 @@ bool Tdcp(SnapshotPtr               snapshot_r,
                 }
 
                 // store removed sat
-                raim_discarded_sats.insert(row_2_sat_freq[worst_sat_row].first);
-
-                // decrease n_common_sats
-                n_differences -= n_removed_rows;
-                n_common_sats--;
+                raim_discarded_rows.insert(worst_sat_row);
 
-                // Remove selected satellite from problem
+                /*// Remove selected satellite from problem
                 std::cout << "resizing problem..." << std::endl;
                 auto A_aux      = A;
                 auto r_aux      = r;
@@ -406,7 +416,7 @@ bool Tdcp(SnapshotPtr               snapshot_r,
                     s_k.rightCols(back_elements_changing) = s_k_aux.rightCols(back_elements_changing);
                     r.tail(back_elements_changing)        = r_aux.tail(back_elements_changing);
                     drho_m.tail(back_elements_changing)   = drho_m_aux.tail(back_elements_changing);
-                }
+                }*/
 
                 // apply the RAIM solution
                 d     = best_d;
@@ -415,9 +425,10 @@ bool Tdcp(SnapshotPtr               snapshot_r,
 
             #if GNSS_UTILS_TDCP_DEBUG == 1
                 std::cout << "Tdcp After RAIM " << std::endl;
-                std::cout << "\tExcluded sats : ";
-                std::cout << "\tCommon sats   : " << n_common_sats << std::endl;
-                std::cout << "\tRows          : " << n_differences << std::endl;
+                std::cout << "\tExcluded rows : ";
+                for (auto excl_row : raim_discarded_rows)
+                    std::cout << excl_row << " ";
+                std::cout << std::endl;
                 std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
                 std::cout << "\tSolution      = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
                 std::cout << "\tClock offset  = " << d(3) << std::endl;
@@ -430,9 +441,10 @@ bool Tdcp(SnapshotPtr               snapshot_r,
 
         #if GNSS_UTILS_TDCP_DEBUG == 1
         std::cout << "Tdcp iteration " << j << std::endl;
-        std::cout << "\tExcluded sats : ";
-        std::cout << "\tCommon sats   : " << common_sats.size() << std::endl;
-        std::cout << "\tRows          : " << n_differences << std::endl;
+        std::cout << "\tExcluded rows : ";
+        for (auto excl_row : raim_discarded_rows)
+            std::cout << excl_row << " ";
+        std::cout << std::endl;
         std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
         std::cout << "\tSolution      = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
         std::cout << "\tClock offset  = " << d(3) << std::endl;
@@ -443,19 +455,17 @@ bool Tdcp(SnapshotPtr               snapshot_r,
         #endif
     }
 
-    // weight covariance with the measurement noise (proportional to time)
-    double sq_sigma_Tdcp = (tk - tr) * tdcp_params.tdcp.sigma_atm * tdcp_params.tdcp.sigma_atm +
-                           2 * tdcp_params.tdcp.sigma_carrier * tdcp_params.tdcp.sigma_carrier;
-    cov_d *= sq_sigma_Tdcp;
-    // residual = (r - A * d).norm() / A.rows();
-
     // Computing error on measurement space
-    for (int row = 0; row < n_differences; row++)
-        r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
+    r.setZero();
+    for (int row = 0; row < r.size(); row++)
+        if (raim_discarded_rows.count(row) == 0)
+            r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
 
-    residual = sqrt(r.squaredNorm() / r.size());
+    // residual = (r - A * d).norm() / A.rows();
+    residual = sqrt(r.squaredNorm() / (r.size() - raim_discarded_rows.size()));
 
     return true;
 }
 
+
 }  // namespace GnssUtils