working on tdcp

include/gnss/processor/processor_gnss_tdcp.h

@@ -41,7 +41,7 @@ struct ParamsProcessorGnssTdcp : public ParamsProcessorGnssFix
         tdcp.tdcp.corr_iono = false;
         tdcp.relinearize_jacobian = false;
         tdcp.max_iterations = 1;
-        tdcp.old_nav = false;
+        tdcp.tdcp.use_old_nav = false;
         tdcp.tdcp.multi_freq = false;
     }
     std::string print() const override

src/processor/processor_gnss_tdcp.cpp

@@ -165,24 +165,19 @@ void ProcessorGnssTdcp::processKeyFrame(FrameBasePtr _keyframe, const double& _t
         WOLF_INFO("Past KF with feature fix found. Performing TDCP!");
 
         // TDCP
-        Eigen::Vector4d d;
-        Eigen::Matrix4d cov_d;
-        double residual;
-        std::set<int> discarded_sats, raim_discarded_sats;
-        if (GnssUtils::Tdcp(std::static_pointer_cast<CaptureGnss>(cap_gnss_ref)->getSnapshot(),
-                            std::static_pointer_cast<CaptureGnss>(cap_gnss)->getSnapshot(),
-                            feat_fix->getMeasurement(),
-                            d,
-                            cov_d,
-                            residual,
-                            discarded_sats,
-                            raim_discarded_sats,
-                            params_tdcp_->tdcp,
-                            params_tdcp_->compute_pos_opt))
+        std::set<int> discarded_sats;
+        auto tdcp_output = GnssUtils::Tdcp(std::static_pointer_cast<CaptureGnss>(cap_gnss_ref)->getSnapshot(),
+                                           std::static_pointer_cast<CaptureGnss>(cap_gnss)->getSnapshot(),
+                                           feat_fix->getMeasurement(),
+                                           Eigen::Vector4d::Zero(),
+                                           discarded_sats,
+                                           params_tdcp_->tdcp,
+                                           params_tdcp_->compute_pos_opt);
+        if (tdcp_output.success)
         {
             WOLF_INFO("TDCP successfully performed!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
             // EMPLACE FEATURE
-            auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(cap_gnss, d, cov_d);
+            auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(cap_gnss, tdcp_output.d, tdcp_output.cov_d);
 
             // ADD FACTOR
             emplaceFactor(ftr, KF_ref);

src/processor/processor_tracker_gnss.cpp

@@ -423,33 +423,31 @@ void ProcessorTrackerGnss::establishFactors()
                     std::cout << sat << " ";
                 std::cout << std::endl;
 
+                // reference ECEF position
+                Eigen::Vector3d x_r = sensor_gnss_->computeFrameAntennaPosEcef(KF);
+
                 // compute TDCP batch
-                Eigen::Vector4d d = Eigen::Vector4d::Zero();
-                Eigen::Matrix4d cov_d = Eigen::Matrix4d::Identity();
-                double residual;
-
-                if (GnssUtils::Tdcp(ref_cap_gnss->getSnapshot(),
-                                    last_cap_gnss->getSnapshot(),
-                                    (fix_last_.success ? fix_last_.pos : Eigen::Vector3d::Zero()),
-                                    common_sats,
-                                    d,
-                                    cov_d,
-                                    residual,
-                                    std::set<int>(),
-                                    params_tracker_gnss_->tdcp_batch_params))
+                auto tdcp_output = GnssUtils::Tdcp(ref_cap_gnss->getSnapshot(),
+                                                   last_cap_gnss->getSnapshot(),
+                                                   x_r,
+                                                   common_sats,
+                                                   Eigen::Vector4d::Zero(),
+                                                   params_tracker_gnss_->tdcp_batch_params);
+                if (tdcp_output.success)
                 {
-                    WOLF_DEBUG("TDCP BATCH d = ", d.transpose());
-                    WOLF_DEBUG("TDCP BATCH cov =\n", cov_d);
+                    WOLF_DEBUG("TDCP BATCH d = ", tdcp_output.d.transpose());
+                    WOLF_DEBUG("TDCP BATCH cov =\n", tdcp_output.cov_d);
 
                     // EMPLACE FEATURE
-                    auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(last_cap_gnss, Eigen::Vector3d(d.head<3>()), Eigen::Matrix3d(cov_d.topLeftCorner<3,3>()));
+                    auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(last_cap_gnss, Eigen::Vector3d(tdcp_output.d.head<3>()), Eigen::Matrix3d(tdcp_output.cov_d.topLeftCorner<3,3>()));
+                    //auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(cap_gnss, tdcp_output.d, tdcp_output.cov_d);
 
                     // EMPLACE FACTOR
                     FactorBase::emplace<FactorGnssTdcp3d>(ftr, ftr, KF, sensor_gnss_, shared_from_this());
                 }
                 else
                 {
-                    WOLF_DEBUG("TDCP BATCH failed");
+                    WOLF_WARN("TDCP BATCH failed with msg: ", tdcp_output.msg);
                 }
             }
         }