diff --git a/src/problem.cpp b/src/problem.cpp
index d7bdec581973a55a58b7fe0658934096fe8374e5..2b44e3181124d1910bdc81f1cdb548d700a2602d 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -12,6 +12,7 @@
 #include "processor_motion.h"
 
 #include "processor_tracker.h"
+//#include "processors/processor_tracker_feature_trifocal.h"
 #include "capture_pose.h"
 
 // IRI libs includes
@@ -769,6 +770,13 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
                         ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p);
                         if (pt)
                         {
+//                            ProcessorTrackerFeatureTrifocalPtr ptt = std::dynamic_pointer_cast<ProcessorTrackerFeatureTrifocal>(pt);
+//                            if (ptt)
+//                            {
+//                                if (ptt->getPrevOriginPtr())
+//                                    cout << "      p: C" << ptt->getPrevOriginPtr()->id() << " - " << (ptt->getPrevOriginPtr()->getFramePtr()->isKey() ? "  KF" : "  F")
+//                                    << ptt->getPrevOriginPtr()->getFramePtr()->id() << endl;
+//                            }
                             if (pt->getOriginPtr())
                                 cout << "      o: C" << pt->getOriginPtr()->id() << " - " << (pt->getOriginPtr()->getFramePtr()->isKey() ? "  KF" : "  F")
                                 << pt->getOriginPtr()->getFramePtr()->id() << endl;
diff --git a/src/processor_IMU.cpp b/src/processor_IMU.cpp
index 4c944933433a540fc9d4d873d63fc97bce812351..a3c926185c078a7775173cbe1f42d458d0bf08bb 100644
--- a/src/processor_IMU.cpp
+++ b/src/processor_IMU.cpp
@@ -299,7 +299,7 @@ void ProcessorIMU::statePlusDelta(const Eigen::VectorXs& _x,
     assert(_x.size() == 10 && "Wrong _x vector size");
     assert(_delta.size() == 10 && "Wrong _delta vector size");
     assert(_x_plus_delta.size() == 10 && "Wrong _x_plus_delta vector size");
-    assert(_dt >= 0 && "Time interval _Dt is negative!");
+    assert(_dt >= 0 && "Time interval _dt is negative!");
 
     _x_plus_delta = imu::composeOverState(_x, _delta, _dt);
 }
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index 3fa2234ad2722e14db221471cb6dd9a2565ab3c2..de044f52334c94b49688df4f2aa3d2c6150f0b3a 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -382,6 +382,7 @@ void ProcessorMotion::integrateOneStep()
 {
     // Set dt
     dt_ = updateDt();
+    assert(dt_ >= 0 && "Time interval _dt is negative!");
 
     // get vector of parameters to calibrate
     calib_ = getBuffer().getCalibrationPreint();
diff --git a/src/test/gtest_constraint_autodiff_distance_3D.cpp b/src/test/gtest_constraint_autodiff_distance_3D.cpp
index 07954416a9a8e291983703ff6e75aa34eda4a1ce..768be80e93b39146ebc42a91b63f7b3011e42066 100644
--- a/src/test/gtest_constraint_autodiff_distance_3D.cpp
+++ b/src/test/gtest_constraint_autodiff_distance_3D.cpp
@@ -50,7 +50,7 @@ class ConstraintAutodiffDistance3D_Test : public testing::Test
             pose2  << pos2, vquat2;
 
             dist = Vector1s(sqrt(2.0));
-            dist_cov(0.01);
+            dist_cov = Matrix1s(0.01);
 
             problem = Problem::create("PO 3D");
             ceres_manager = std::make_shared<CeresManager>(problem);
@@ -79,7 +79,7 @@ TEST_F(ConstraintAutodiffDistance3D_Test, ground_truth)
     ASSERT_NEAR(res, 0.0, 1e-8);
 }
 
-TEST_F(ConstraintAutodiffDistance3D_Test, inexact)
+TEST_F(ConstraintAutodiffDistance3D_Test, expected_residual)
 {
     Scalar measurement = 1.400;
 
@@ -93,6 +93,17 @@ TEST_F(ConstraintAutodiffDistance3D_Test, inexact)
     ASSERT_NEAR(res, res_expected, 1e-8);
 }
 
+TEST_F(ConstraintAutodiffDistance3D_Test, solve)
+{
+    Scalar measurement = 1.400;
+    f2->setMeasurement(Vector1s(measurement));
+
+    std::string report = ceres_manager->solve(2);
+
+    // Check distance between F1 and F2 positions -- must match the measurement
+    ASSERT_NEAR( (F1->getPPtr()->getState() - F2->getPPtr()->getState()).norm(), measurement, 1e-10);
+}
+
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);
diff --git a/src/test/gtest_processor_tracker_feature_trifocal.cpp b/src/test/gtest_processor_tracker_feature_trifocal.cpp
index 6c6ad24cc87921c2475fd3e4383a2bf5c12738e6..30e27e56ded25f77bb4eab24ef2e1fc387c96066 100644
--- a/src/test/gtest_processor_tracker_feature_trifocal.cpp
+++ b/src/test/gtest_processor_tracker_feature_trifocal.cpp
@@ -133,7 +133,6 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
         proc_trk->process(capt_trk);
 
         CaptureBasePtr prev = proc_trk->getPrevOriginPtr();
-        WOLF_INFO("PTrifocal prev: C", (prev ? prev->id() : 0), " KF", (prev ? prev->getFramePtr()->id(): 0));
         problem->print(2,0,0,0);
 
         // Only odom creating KFs