diff --git a/include/core/factor/factor_velocity_local_direction_3d.h b/include/core/factor/factor_velocity_local_direction_3d.h
index 13ff8451ccb352b905fb7427e001a40eccc4b79a..31bf83599189cda1033ac38ea43dd36a9cb99500 100644
--- a/include/core/factor/factor_velocity_local_direction_3d.h
+++ b/include/core/factor/factor_velocity_local_direction_3d.h
@@ -18,12 +18,15 @@ class FactorVelocityLocalDirection3d: public FactorAutodiff<FactorVelocityLocalD
 {
     protected:
         double min_vel_sq_norm_; // stored in squared norm for efficiency
+        bool residual_local_, residual_angle_;
 
     public:
         FactorVelocityLocalDirection3d(FeatureBasePtr _ftr_ptr,
                                        const double& _min_vel_norm,
                                        const ProcessorBasePtr& _processor_ptr,
                                        bool _apply_loss_function,
+                                       bool _residual_local = true,
+                                       bool _residual_angle = true,
                                        FactorStatus _status = FAC_ACTIVE) :
                 FactorAutodiff<FactorVelocityLocalDirection3d,1,3,4>("FactorVelocityDirection3d",
                                                                      TOP_ABS,
@@ -34,11 +37,13 @@ class FactorVelocityLocalDirection3d: public FactorAutodiff<FactorVelocityLocalD
                                                                      _status,
                                                                      _ftr_ptr->getFrame()->getV(),
                                                                      _ftr_ptr->getFrame()->getO()),
-                min_vel_sq_norm_(_min_vel_norm*_min_vel_norm)
+                min_vel_sq_norm_(_min_vel_norm*_min_vel_norm),
+                residual_local_(_residual_local),
+                residual_angle_(_residual_angle)
         {
             MatrixSizeCheck<3,1>::check(_ftr_ptr->getMeasurement());
             MatrixSizeCheck<1,1>::check(_ftr_ptr->getMeasurementSquareRootInformationUpper());
-            assert(abs(_ftr_ptr->getMeasurement().norm() - 1.0) < wolf::Constants::EPS && "velocity direction measurement must be normalized");
+            assert(std::abs(_ftr_ptr->getMeasurement().norm() - 1.0) < wolf::Constants::EPS && "velocity direction measurement must be normalized");
         }
 
         ~FactorVelocityLocalDirection3d() override = default;
@@ -58,68 +63,74 @@ inline bool FactorVelocityLocalDirection3d::operator ()(const T* const _v, const
         _residuals[0] = T(0);
         return true;
     }
-    std::cout << "----------\ndesired v_local: " << getMeasurement()(0) << " "
-                                                 << getMeasurement()(1) << " "
-                                                 << getMeasurement()(2) << "\n";
-    //std::cout << "v: " << v(0) << " "
-    //                   << v(1) << " "
-    //                   << v(2) << "\n";
-    //std::cout << "q: " << q.coeffs()(0) << " "
-    //                   << q.coeffs()(1) << " "
-    //                   << q.coeffs()(2) << " "
-    //                   << q.coeffs()(3) << "\n";
-
-//    // desired direction in global coordinates
-//    Eigen::Matrix<T,3,1> v_desired_global = q * getMeasurement().cast<T>();
-//
-//    //std::cout << "v_desired_global: " << v_desired_global(0) << " "
-//    //                                  << v_desired_global(1) << " "
-//    //                                  << v_desired_global(2) << "\n";
-//
-//    std::cout << "v.dot(v_desired_global) - v.norm() = " << v.dot(v_desired_global) - v.norm() << "\n";
-//    std::cout << "angle = " << acos(v.dot(v_desired_global) / v.norm()) << std::endl;
-//
-//    _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * (v.dot(v_desired_global) - v.norm());
+
+//    std::cout << "----------\n";
+//    std::cout << "v desired (local): \n\t" << getMeasurement()(0) << "\n\t"
+//                                           << getMeasurement()(1) << "\n\t"
+//                                           << getMeasurement()(2) << "\n";
 //
-//    return true;
+//    Eigen::Matrix<T,3,1> v_desired_global = q * getMeasurement().cast<T>();
+//    std::cout << "v desired (global): \n\t" << v_desired_global(0) << "\n\t"
+//                                            << v_desired_global(1) << "\n\t"
+//                                            << v_desired_global(2) << "\n";
 //
-    // velocity in local coordinates
-    Eigen::Matrix<T,3,1> v_local = q.conjugate() * v;
-
-    std::cout << "v_local: \n\t" << v_local(0) << "\n\t"
-                                 << v_local(1) << "\n\t"
-                                 << v_local(2) << "\n";
-    std::cout << "v_desired: \n\t" << getMeasurement()(0) << "\n\t"
-                                   << getMeasurement()(1) << "\n\t"
-                                   << getMeasurement()(2) << "\n";
+//    Eigen::Matrix<T,3,1> v_local = q.conjugate() * v;
+//    std::cout << "v (local): \n\t" << v_local(0) << "\n\t"
+//                                   << v_local(1) << "\n\t"
+//                                   << v_local(2) << "\n";
 //
-//    std::cout << "v_local.dot(getMeasurement()) - v.norm() = " << v_local.dot(getMeasurement()) - v.norm() << "\n";
+//    std::cout << "v: \n\t" << v(0) << "\n\t"
+//                           << v(1) << "\n\t"
+//                           << v(2) << "\n";
 //
-//    _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * (v_local.dot(getMeasurement()) - v.norm());
+//    std::cout << "q: \n\t" << q.coeffs()(0) << "\n\t"
+//                           << q.coeffs()(1) << "\n\t"
+//                           << q.coeffs()(2) << "\n\t"
+//                           << q.coeffs()(3) << "\n";
 //
-//    return true;
+//    std::cout << "v.dot(v_desired_global) - v.norm() = " << v.dot(v_desired_global) - v.norm() << "\n";
+//    std::cout << "angle = " << acos(v.dot(v_desired_global) / v.norm()) << std::endl;
 
+    // DOT PRODUCT NORM RESIDUAL -------------------------------------------------
+    // global coordinates
+    if (not residual_angle_ and not residual_local_)
+    {
+        _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * (v.dot(q * getMeasurement().cast<T>()) - v.norm());
+        return true;
+    }
+    // local coordinates
+    if (not residual_angle_ and residual_local_)
+    {
+        _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * ((q.conjugate() * v).dot(getMeasurement().cast<T>()) / v.norm() - T(1.0));
+        return true;
+    }
+
+    // ANGLE RESIDUAL ------------------------------------------------------------
+    T cos_angle;
+    // global coordinates
+    if (not residual_local_)
+        cos_angle = v.dot(q * getMeasurement().cast<T>()) / v.norm(); // direction already normalized
+
+    // local coordinates
+    else
+        cos_angle = (q.conjugate() * v).dot(getMeasurement().cast<T>()) / v.norm(); // direction already normalized
 
-    // error: angle between specified local velocity direction (measurement) and velocity in local coordinates
-    T cos_angle =  v_local.dot(getMeasurement()) / v.norm(); // direction already normalized
+    // numeric issues
     while (cos_angle > T(1) or cos_angle < T(-1))
     {
-        WOLF_WARN("cos_angle > 1 or < -1!! ", cos_angle);
-        WOLF_WARN("v.norm() = ", v.norm());
-        cos_angle *= T(0.999);
+        //WOLF_WARN("cos_angle > 1 or < -1!! ", cos_angle);
+        //WOLF_WARN("v.norm() = ", v.norm());
+        cos_angle *= T(0.9);
     }
 
-    std::cout << "FactorVelocityDirection3d cos_angle = " << cos_angle << std::endl;
-    std::cout << "FactorVelocityDirection3d angle = " << acos(cos_angle) << std::endl;
-
+    // in perfect fitting Jacobians are ill defined
     if (cos_angle == T(1))
     {
-        std::cout << "FactorVelocityDirection3d detected cos_error == T(1). Returning residual T(0) to avoid NaN jacobian\n";
+        //std::cout << "FactorVelocityDirection3d detected cos_error == T(1). Returning residual T(0) to avoid NaN jacobian\n";
         _residuals[0] = T(0);
         return true;
     }
 
-
     // residual
     _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * acos(cos_angle);
 
diff --git a/include/core/processor/processor_fix_wing_model.h b/include/core/processor/processor_fix_wing_model.h
index 81adaf0344883e4fd472597b9d5ea28a3ea78710..8c23917354ca1d0e0c3723045ded8d118e3c3abc 100644
--- a/include/core/processor/processor_fix_wing_model.h
+++ b/include/core/processor/processor_fix_wing_model.h
@@ -29,7 +29,7 @@ struct ParamsProcessorFixWingModel : public ParamsProcessorBase
             angle_stdev      = _server.getParam<double>          (prefix + _unique_name + "/angle_stdev");
             min_vel_norm     = _server.getParam<double>          (prefix + _unique_name + "/min_vel_norm");
 
-            assert(abs(velocity_local.norm() - 1.0) < wolf::Constants::EPS && "ParamsProcessorFixWingModel: 'velocity_local' must be normalized");
+            assert(std::abs(velocity_local.norm() - 1.0) < wolf::Constants::EPS && "ParamsProcessorFixWingModel: 'velocity_local' must be normalized");
         }
         std::string print() const override
         {
diff --git a/test/gtest_factor_velocity_local_direction_3d.cpp b/test/gtest_factor_velocity_local_direction_3d.cpp
index d0eeb3b850559e502b2c3a76ebe8953ae6799bc5..37fa70557ea733d149f0f356fa145699460fdba4 100644
--- a/test/gtest_factor_velocity_local_direction_3d.cpp
+++ b/test/gtest_factor_velocity_local_direction_3d.cpp
@@ -20,13 +20,21 @@ class FactorVelocityLocalDirection3dTest : public testing::Test
         StateBlockPtr sb_v;
         CaptureBasePtr cap;
 
+        int n_tests, n_solved;
+        std::vector<VectorXd> convergence, iterations, time, error;
+
         virtual void SetUp()
         {
+            n_tests = 1e2;
+            n_solved = 0;
+
             // create problem
             problem = Problem::create("POV", 3);
 
             // create solver
-            solver = std::make_shared<SolverCeres>(problem);
+            auto params_solver = std::make_shared<ParamsCeres>();
+            params_solver->solver_options.max_num_iterations = 1e3;
+            solver = std::make_shared<SolverCeres>(problem, params_solver);
 
             // Frame
             frm = problem->emplaceFrame(0.0, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
@@ -38,9 +46,30 @@ class FactorVelocityLocalDirection3dTest : public testing::Test
             // Capture
             cap = CaptureBase::emplace<CaptureBase>(frm, "CaptureBase",
                                                     frm->getTimeStamp(), nullptr);
+
+            // init
+            convergence = std::vector<VectorXd>({VectorXd(n_tests),
+                                                 VectorXd(n_tests),
+                                                 VectorXd(n_tests),
+                                                 VectorXd(n_tests)});
+            iterations = std::vector<VectorXd>({VectorXd(n_tests),
+                                                VectorXd(n_tests),
+                                                VectorXd(n_tests),
+                                                VectorXd(n_tests)});
+            time = std::vector<VectorXd>({VectorXd(n_tests),
+                                          VectorXd(n_tests),
+                                          VectorXd(n_tests),
+                                          VectorXd(n_tests)});
+            error = std::vector<VectorXd>({VectorXd(n_tests),
+                                           VectorXd(n_tests),
+                                           VectorXd(n_tests),
+                                           VectorXd(n_tests)});
         }
 
-        FactorBasePtr emplaceFeatureAndFactor(const Vector3d& v_local, const double& angle_stdev )
+        FactorBasePtr emplaceFeatureAndFactor(const Vector3d& v_local,
+                                              const double& angle_stdev,
+                                              bool residual_local,
+                                              bool residual_angle)
         {
             // emplace feature
             FeatureBasePtr fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureBase",
@@ -52,7 +81,9 @@ class FactorVelocityLocalDirection3dTest : public testing::Test
                                                                        fea,
                                                                        0,
                                                                        nullptr,
-                                                                       false);
+                                                                       false,
+                                                                       residual_local,
+                                                                       residual_angle);
         }
 
         void testLocalVelocity(const Quaterniond& o,
@@ -60,6 +91,8 @@ class FactorVelocityLocalDirection3dTest : public testing::Test
                                bool estimate_o,
                                bool estimate_v)
         {
+            assert(n_solved <= n_tests);
+
             // set state
             sb_o->setState(o.coeffs());
             sb_v->setState(o * v_local);
@@ -77,36 +110,95 @@ class FactorVelocityLocalDirection3dTest : public testing::Test
             else
             {
                 sb_v->unfix();
+                //sb_v->setState(Vector3d::Random());
                 sb_v->perturb();
             }
+            auto init_o = sb_o->getState();
+            auto init_v = sb_v->getState();
+
+            // COMPARE residuals
+            for (auto res = 0; res < 4; res++)
+            {
+                bool residual_local = (res == 0 or res == 1);
+                bool residual_angle = (res == 0 or res == 2);
+                //WOLF_INFO("============= Residual ",
+                //          (residual_local ? "local " : "global "),
+                //          (residual_angle ? "angle" : "dot product norm error"));
+
+                // emplace feature & factor
+                auto fac = emplaceFeatureAndFactor(v_local, 0.001, residual_local, residual_angle);
+
+                std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+                //WOLF_INFO(report);
+
+                // Check in local coordinates
+                auto v_est_local_normalized = (Quaterniond(Vector4d(sb_o->getState())).conjugate() * sb_v->getState()).normalized();
+                auto cos_angle_local = v_est_local_normalized.dot(v_local);
+                //WOLF_INFO("cos(angle local) = ", cos_angle_local);
+                //WOLF_INFO_COND(cos_angle_local > 1, "cos(angle local) > 1 in ", cos_angle_local -1.0);
+                //WOLF_INFO("angle local = ", acos(cos_angle_local));
+                //EXPECT_NEAR(v_est_local_normalized.dot(v_local), 1, 1e-2);
+                if (cos_angle_local > 1)
+                    cos_angle_local = 1;
+                //EXPECT_NEAR(acos(cos_angle_local), 0, 5*M_PI/180);
+
+                // Check in global coordinates
+                auto v_global = Quaterniond(Vector4d(sb_o->getState())) * v_local;
+                auto v_est_normalized = sb_v->getState().normalized();
+                auto cos_angle_global = v_est_normalized.dot(v_global);
+                //WOLF_INFO("cos(angle global) = ", cos_angle_global);
+                //WOLF_INFO("angle global = ", acos(cos_angle_global));
+                //EXPECT_NEAR(cos_angle_global, 1, 1e-2);
+                if (cos_angle_global > 1)
+                    cos_angle_global = 1;
+                //EXPECT_NEAR(acos(cos_angle_global), 0, 5*M_PI/180);
+
+                // PRINT
+                /*WOLF_INFO_COND(solver->hasConverged(), "\tSolver CONVERGED");
+                WOLF_INFO_COND(not solver->hasConverged(), "\tSolver NOT CONVERGED");
+                WOLF_INFO("\tSolver iterations: ", solver->iterations());
+                WOLF_INFO("\tSolver total time: ", solver->totalTime());
+                WOLF_INFO("\tangle error:       ", acos(cos_angle_local));*/
+
+                // Update performaces
+                convergence.at(res)(n_solved) = (solver->hasConverged() ? 1 : 0);
+                iterations.at(res)(n_solved) = solver->iterations();
+                time.at(res)(n_solved) = solver->totalTime();
+                error.at(res)(n_solved) = acos(cos_angle_local);
+
+                // Reset
+                fac->getFeature()->remove();
+                sb_o->setState(init_o);
+                sb_v->setState(init_v);
+                ASSERT_EQ(cap->getFeatureList().size(),0);
+            }
+            n_solved++;
+        }
 
-            // emplace feature & factor
-            emplaceFeatureAndFactor(v_local, 0.01);
-
-            std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
-            WOLF_INFO(report);
-
-            // Check in local coordinates
-            auto v_est_local_normalized = (Quaterniond(Vector4d(sb_o->getState())).conjugate() * sb_v->getState()).normalized();
-            auto cos_angle_local = v_est_local_normalized.dot(v_local);
-            WOLF_INFO("cos(angle local) = ", cos_angle_local);
-            WOLF_INFO_COND(cos_angle_local > 1, "cos(angle local) > 1 in ", cos_angle_local -1.0);
-            WOLF_INFO("angle local = ", acos(cos_angle_local));
-            ASSERT_NEAR(v_est_local_normalized.dot(v_local), 1, 1e-3);
-            if (cos_angle_local > 1)
-                cos_angle_local = 1;
-            ASSERT_NEAR(acos(cos_angle_local), 0, 1e-3);
-
-            // Check in global coordinates
-            auto v_global = Quaterniond(Vector4d(sb_o->getState())) * v_local;
-            auto v_est_normalized = sb_v->getState().normalized();
-            auto cos_angle_global = v_est_normalized.dot(v_global);
-            WOLF_INFO("cos(angle global) = ", cos_angle_global);
-            WOLF_INFO("angle global = ", acos(cos_angle_global));
-            ASSERT_NEAR(cos_angle_global, 1, 1e-3);
-            if (cos_angle_global > 1)
-                cos_angle_global = 1;
-            ASSERT_NEAR(acos(cos_angle_global), 0, 1e-3);
+        void printAverageResults()
+        {
+            WOLF_INFO("/////////////////// Average results: ");
+            for (auto res = 0; res < 4; res++)
+            {
+                double conv_avg = convergence.at(res).head(n_solved).mean();
+                double iter_avg = iterations.at(res).head(n_solved).mean();
+                double time_avg = time.at(res).head(n_solved).mean();
+                double err_avg  = error.at(res).head(n_solved).mean();
+
+                double iter_stdev = (iterations.at(res).head(n_solved).array() - conv_avg).matrix().norm();
+                double time_stdev = (time.at(res).head(n_solved).array() - conv_avg).matrix().norm();
+                double err_stdev  = (error.at(res).head(n_solved).array() - conv_avg).matrix().norm();
+
+                bool residual_local = (res == 0 or res == 1);
+                bool residual_angle = (res == 0 or res == 2);
+                WOLF_INFO("======= Residual ",
+                          (residual_local ? "local " : "global "),
+                          (residual_angle ? "angle" : "dot product norm error"));
+                WOLF_INFO("\tconvergence: ", 100 * conv_avg, "%");
+                WOLF_INFO("\titerations:  ", iter_avg, " - stdev: ", iter_stdev);
+                WOLF_INFO("\ttime:        ", 1000 * time_avg, " ms", " - stdev: ", time_stdev);
+                WOLF_INFO("\tangle error: ", err_avg, " - stdev: ", err_stdev);
+            }
         }
 };
 
@@ -118,76 +210,31 @@ TEST_F(FactorVelocityLocalDirection3dTest, check_tree)
 {
     ASSERT_TRUE(problem->check(0));
 
-    emplaceFeatureAndFactor(Vector3d(1.0, 0.0, 0.0), 0.1);
+    emplaceFeatureAndFactor(Vector3d(1.0, 0.0, 0.0), 0.1,true,true);
 
     ASSERT_TRUE(problem->check(0));
-    ASSERT_DEATH({emplaceFeatureAndFactor(Vector3d(10.0, 0.0, 0.0), 0.1);},""); // Not normalized
-}
-
-TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_x)
-{
-    testLocalVelocity(Quaterniond::Identity(),
-                      (Vector3d() << 1, 0, 0).finished(),
-                      false, true);
-}
-
-TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_y)
-{
-    testLocalVelocity(Quaterniond::Identity(),
-                      (Vector3d() << 0, 1, 0).finished(),
-                      false, true);
-}
-
-TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_z)
-{
-    testLocalVelocity(Quaterniond::Identity(),
-                      (Vector3d() << 0, 0, 1).finished(),
-                      false, true);
-}
-
-TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_nx)
-{
-    testLocalVelocity(Quaterniond::Identity(),
-                      (Vector3d() << -1, 0, 0).finished(),
-                      false, true);
+    ASSERT_DEATH({emplaceFeatureAndFactor(Vector3d(10.0, 0.0, 0.0), 0.1,true,true);},""); // Not normalized
 }
 
-TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_ny)
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV)
 {
-    testLocalVelocity(Quaterniond::Identity(),
-                      (Vector3d() << 0, -1, 0).finished(),
-                      false, true);
+    for (auto i = 0; i < n_tests; i++)
+        testLocalVelocity(Quaterniond::Identity(),
+                          Vector3d::Random().normalized(),
+                          false, true);
+    printAverageResults();
 }
 
-TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_nz)
+TEST_F(FactorVelocityLocalDirection3dTest, random_solveV)
 {
-    testLocalVelocity(Quaterniond::Identity(),
-                      (Vector3d() << 0, 0, -1).finished(),
-                      false, true);
+    for (auto i = 0; i < n_tests; i++)
+        testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
+                          Vector3d::Random().normalized(),
+                          false, true);
+    printAverageResults();
 }
 
-TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_xy)
-{
-    testLocalVelocity(Quaterniond::Identity(),
-                      (Vector3d() << 0, 1, 1).finished().normalized(),
-                      false, true);
-}
-
-TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_nxny)
-{
-    testLocalVelocity(Quaterniond::Identity(),
-                      (Vector3d() << 0, -1, -1).finished().normalized(),
-                      false, true);
-}
-
-TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_random)
-{
-    testLocalVelocity(Quaterniond::Identity(),
-                      Vector3d::Random().normalized(),
-                      false, true);
-}
-
-TEST_F(FactorVelocityLocalDirection3dTest, xturned_solveV_x)
+/*TEST_F(FactorVelocityLocalDirection3dTest, xturned_solveV_x)
 {
     testLocalVelocity(Quaterniond((Vector4d() << 1, 1, 0, 0).finished().normalized()),
                       (Vector3d() << 1, 0, 0).finished(),
@@ -248,7 +295,7 @@ TEST_F(FactorVelocityLocalDirection3dTest, zturned_solveV_z)
     testLocalVelocity(Quaterniond((Vector4d() << 1, 0, 0, 1).finished().normalized()),
                       (Vector3d() << 0, 0, 1).finished(),
                       false, true);
-}
+}//*/
 
 // SOLVE O (not observable but error minimization ok)
 /*TEST_F(FactorVelocityLocalDirection3dTest, origin_solveO_x)