Resolve "TDCP batch implementation"

Closes #15 (closed)

include/gnss_utils/tdcp.h

@@ -14,40 +14,53 @@ struct TdcpBatchParams
   int    raim_n;
   double raim_min_residual;
   bool   relinearize_jacobian;
-  bool   old_nav;
   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);
-
-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);
-
-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);
+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);
+
+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);
+
+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);
+
+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
 
 #endif /* INCLUDE_GNSS_UTILS_Tdcp_H_ */