Resolve "TDCP batch implementation"

Closes #15 (closed)

include/gnss_utils/tdcp.h

@@ -17,48 +17,49 @@ struct TdcpBatchParams
   int    max_iterations;
 };
 
-bool Tdcp(SnapshotPtr               snapshot_r,
-          SnapshotPtr               snapshot_k,
-          Eigen::Vector4d&          d,
-          Eigen::Matrix4d&          cov_d,
-          double&                   residual,
-          const std::set<int>&      discarded_sats,
-          std::set<int>&            raim_discarded_sats,
-          const TdcpBatchParams&    tdcp_params,
-          const Options&            opt);
+struct TdcpOutput
+{
+    bool success = false;
+    std::string msg = "";
+    std::set<int> raim_discarded_sats;
+    std::set<int> used_sats;
+    Eigen::Vector4d d = Eigen::Vector4d::Zero();
+    Eigen::Matrix4d cov_d = Eigen::Matrix4d::Zero();
+    double dt = 0;
+    double residual = 0;
+};
+
+TdcpOutput Tdcp(SnapshotPtr               snapshot_r,
+                SnapshotPtr               snapshot_k,
+                const Eigen::Vector4d&    d_0,
+                const std::set<int>&      discarded_sats,
+                const TdcpBatchParams&    tdcp_params,
+                const Options&            opt);
 
-bool Tdcp(SnapshotPtr               snapshot_r,
-          SnapshotPtr               snapshot_k,
-          const Eigen::Vector3d&    x_r,
-          Eigen::Vector4d&          d,
-          Eigen::Matrix4d&          cov_d,
-          double&                   residual,
-          const std::set<int>&      discarded_sats,
-          std::set<int>&            raim_discarded_sats,
-          const TdcpBatchParams&    tdcp_params,
-          const Options&            opt);
+TdcpOutput Tdcp(SnapshotPtr               snapshot_r,
+                SnapshotPtr               snapshot_k,
+                const Eigen::Vector3d&    x_r,
+                const Eigen::Vector4d&    d_0,
+                const std::set<int>&      discarded_sats,
+                const TdcpBatchParams&    tdcp_params,
+                const Options&            opt);
 
-bool Tdcp(SnapshotPtr               snapshot_r,
-          SnapshotPtr               snapshot_k,
-          const Eigen::Vector3d&    x_r,
-          const std::set<int>       common_sats,
-          Eigen::Vector4d&          d,
-          Eigen::Matrix4d&          cov_d,
-          double&                   residual,
-          std::set<int>&            raim_discarded_sats,
-          const TdcpBatchParams&    tdcp_params);
+TdcpOutput Tdcp(SnapshotPtr               snapshot_r,
+                SnapshotPtr               snapshot_k,
+                const Eigen::Vector3d&    x_r,
+                const std::set<int>&      common_sats,
+                const Eigen::Vector4d&    d_0,
+                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);
+TdcpOutput Tdcp(const Eigen::Vector3d&  x_r,
+                Eigen::MatrixXd&        A,
+                Eigen::VectorXd&        r,
+                Eigen::VectorXd&        drho_m,
+                Eigen::MatrixXd&        s_k,
+                Eigen::MatrixXd&        s_r,
+                const Eigen::Vector4d&        d_0,
+                std::set<int>&          raim_discarded_rows,
+                const TdcpBatchParams&  tdcp_params);
 
 }  // namespace GnssUtils