diff --git a/include/core/factor/factor_velocity_local_direction_3d.h b/include/core/factor/factor_velocity_local_direction_3d.h
new file mode 100644
index 0000000000000000000000000000000000000000..01e635e660a3cbd40d221a495d244241e2db2461
--- /dev/null
+++ b/include/core/factor/factor_velocity_local_direction_3d.h
@@ -0,0 +1,148 @@
+
+#ifndef FACTOR_VELOCITY_LOCAL_DIRECTION_3D_H_
+#define FACTOR_VELOCITY_LOCAL_DIRECTION_3D_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+namespace wolf {
+    
+WOLF_PTR_TYPEDEFS(FactorVelocityLocalDirection3d);
+
+//class
+class FactorVelocityLocalDirection3d: public FactorAutodiff<FactorVelocityLocalDirection3d,2,3,4>
+{
+    protected:
+        double min_vel_sq_norm_; // stored in squared norm for efficiency
+        int orthogonal_axis_; // 0: X, 1: Y, 2: Z
+
+    public:
+        FactorVelocityLocalDirection3d(FeatureBasePtr _ftr_ptr,
+                                       const double& _min_vel_norm,
+                                       const ProcessorBasePtr& _processor_ptr,
+                                       bool _apply_loss_function,
+                                       FactorStatus _status = FAC_ACTIVE);
+
+        ~FactorVelocityLocalDirection3d() override = default;
+
+        template<typename T>
+        bool operator ()(const T* const _v, const T* const _o, T* _residuals) const;
+
+        template<typename T>
+        Eigen::Matrix<T,2,1> computeElevationAzimuth(const Eigen::Matrix<T,3,1> vector) const
+        {
+            Eigen::Matrix<T,2,1> elev_azim;
+
+            // plane YZ
+            if (orthogonal_axis_ == 0)
+            {
+                elev_azim(0) = asin(vector(0) / vector.norm());
+                elev_azim(1) = atan2(vector(2), vector(1));
+            }
+            // plane XZ
+            if (orthogonal_axis_ == 1)
+            {
+                elev_azim(0) = asin(vector(1) / vector.norm());
+                elev_azim(1) = atan2(vector(0), vector(2));
+            }
+            // plane XY
+            if (orthogonal_axis_ == 2)
+            {
+                elev_azim(0) = asin(vector(2) / vector.norm());
+                elev_azim(1) = atan2(vector(1), vector(0));
+            }
+
+            return elev_azim;
+        }
+};
+
+FactorVelocityLocalDirection3d::FactorVelocityLocalDirection3d(FeatureBasePtr _ftr_ptr,
+                                                               const double& _min_vel_norm,
+                                                               const ProcessorBasePtr& _processor_ptr,
+                                                               bool _apply_loss_function,
+                                                               FactorStatus _status) :
+        FactorAutodiff<FactorVelocityLocalDirection3d,2,3,4>("FactorVelocityLocalDirection3d",
+                                                             TOP_ABS,
+                                                             _ftr_ptr,
+                                                             nullptr, nullptr, nullptr, nullptr,
+                                                             _processor_ptr,
+                                                             _apply_loss_function,
+                                                             _status,
+                                                             _ftr_ptr->getFrame()->getV(),
+                                                             _ftr_ptr->getFrame()->getO()),
+        min_vel_sq_norm_(_min_vel_norm*_min_vel_norm)
+{
+    MatrixSizeCheck<3,1>::check(_ftr_ptr->getMeasurement());
+    MatrixSizeCheck<1,1>::check(_ftr_ptr->getMeasurementSquareRootInformationUpper());
+
+    /* Decide the plane (two axis A1, A2 from X, Y, Z in local coordinates) in which compute elevation and azimuth
+     *    - elevation w.r.t. the plane
+     *    - azimuth computed with atan2(A2, A1)
+     *
+     * This is done to avoid the degenerated case: elevation = +/-PI/2
+     * We select the orthogonal axis as the farthest to the desired velocity in local coordinates,
+     * so the component one with lower value.
+     */
+
+    measurement_.minCoeff(&orthogonal_axis_);
+
+    // measurement: elevation & azimuth (w.r.t. selected plane)
+    measurement_ = computeElevationAzimuth(Eigen::Vector3d(measurement_));
+    measurement_sqrt_information_upper_ = Matrix2d::Identity() * measurement_sqrt_information_upper_(0,0);
+}
+
+template<typename T>
+inline bool FactorVelocityLocalDirection3d::operator ()(const T* const _v, const T* const _q, T* _residuals) const
+{
+    Eigen::Map<const Eigen::Matrix<T,3,1> > v(_v);
+    Eigen::Map<const Eigen::Quaternion<T> > q(_q);
+    Eigen::Map<Eigen::Matrix<T,2,1> > residuals(_residuals);
+
+    if (v.squaredNorm() < min_vel_sq_norm_)
+    {
+        _residuals[0] = T(0);
+        return true;
+    }
+
+//    std::cout << "----------\n";
+//    std::cout << "desired elevation: " << getMeasurement()(0) << "\n";
+//    std::cout << "desired azimuth:   " << getMeasurement()(1) << "\n";
+
+//    std::cout << "v: \n\t" << v(0) << "\n\t"
+//                           << v(1) << "\n\t"
+//                           << v(2) << "\n";
+//
+//    std::cout << "q: \n\t" << q.coeffs()(0) << "\n\t"
+//                           << q.coeffs()(1) << "\n\t"
+//                           << q.coeffs()(2) << "\n\t"
+//                           << q.coeffs()(3) << "\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";
+
+    // expectation
+    Eigen::Matrix<T,2,1> exp_elev_azim = computeElevationAzimuth(v_local);
+//    std::cout << "expected elevation: " << exp_elev_azim(0) << "\n";
+//    std::cout << "expected azimuth:   " << exp_elev_azim(1) << "\n";
+
+    // error
+    Eigen::Matrix<T,2,1> error = getMeasurement() - exp_elev_azim;
+    pi2pi(error(1));
+//    std::cout << "error (corrected): \n\t" << error(0) << "\n\t"
+//                                           << error(1) << "\n;
+
+    // residuals
+    residuals = getMeasurementSquareRootInformationUpper() * error;
+//    std::cout << "residuals: \n\t" << residuals(0) << "\n\t"
+//                                   << residuals(1) << "\n;
+
+    return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 8dbbcdb5734782c3853f90158265dfafe07d5bea..a6305564d0a80a4de10f4eecb74c0ad29ff6249f 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -220,8 +220,6 @@ target_link_libraries(gtest_factor_relative_pose_3d ${PLUGIN_NAME})
 # FactorVelocityLocalDirection3d class test
 wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp)
 target_link_libraries(gtest_factor_velocity_local_direction_3d ${PLUGIN_NAME})
-wolf_add_gtest(gtest_factor_velocity_local_direction_3d_b gtest_factor_velocity_local_direction_3d_b.cpp)
-target_link_libraries(gtest_factor_velocity_local_direction_3d_b ${PLUGIN_NAME})
 
 # MakePosDef function test
 wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
diff --git a/test/gtest_factor_velocity_local_direction_3d.cpp b/test/gtest_factor_velocity_local_direction_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0834d95071973788df1aa053baba80adf3e95bf4
--- /dev/null
+++ b/test/gtest_factor_velocity_local_direction_3d.cpp
@@ -0,0 +1,287 @@
+#include "core/utils/utils_gtest.h"
+
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_velocity_local_direction_3d.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/math/rotations.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+
+int N_TESTS = 100;
+
+class FactorVelocityLocalDirection3dTest : public testing::Test
+{
+    protected:
+        ProblemPtr problem;
+        SolverManagerPtr solver;
+        FrameBasePtr frm;
+        StateBlockPtr sb_p;
+        StateBlockPtr sb_o;
+        StateBlockPtr sb_v;
+        CaptureBasePtr cap;
+
+        int n_solved;
+        std::vector<double> convergence, iterations, times, error;
+
+        virtual void SetUp()
+        {
+            // create problem
+            problem = Problem::create("POV", 3);
+
+            // create solver
+            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());
+            sb_p = frm->getP();
+            sb_p->fix();
+            sb_o = frm->getO();
+            sb_v = frm->getV();
+
+            // Capture
+            cap = CaptureBase::emplace<CaptureBase>(frm, "CaptureBase",
+                                                    frm->getTimeStamp(), nullptr);
+        }
+
+        FactorBasePtr emplaceFeatureAndFactor(const Vector3d& v_local,
+                                              const double& angle_stdev)
+        {
+            // emplace feature
+            FeatureBasePtr fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureBase",
+                                                                   v_local,
+                                                                   Matrix1d(angle_stdev * angle_stdev));
+
+            // emplace factor
+            return FactorBase::emplace<FactorVelocityLocalDirection3d>(fea,
+                                                                       fea,
+                                                                       0,
+                                                                       nullptr,
+                                                                       false);
+        }
+
+        bool testLocalVelocity(const Quaterniond& o,
+                               const Vector3d& v_local,
+                               bool estimate_o,
+                               bool estimate_v)
+        {
+            assert(cap->getFeatureList().empty());
+
+            // set state
+            sb_o->setState(o.coeffs());
+            sb_v->setState(o * v_local);
+
+            // fix or unfix & perturb
+            if (not estimate_o)
+                sb_o->fix();
+            else
+            {
+                sb_o->unfix();
+                sb_o->perturb();
+            }
+            if (not estimate_v)
+                sb_v->fix();
+            else
+            {
+                sb_v->unfix();
+                sb_v->setState(Vector3d::Random());
+            }
+
+            // emplace feature & factor
+            auto fac = emplaceFeatureAndFactor(v_local, 0.01);
+
+            // solve
+            std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+
+            // 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);
+            if (cos_angle_local > 1)
+                cos_angle_local = 1;
+            if (cos_angle_local < -1)
+                cos_angle_local = -1;
+
+            // 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);
+            if (cos_angle_global > 1)
+                cos_angle_global = 1;
+            if (cos_angle_global < -1)
+                cos_angle_global = -1;
+
+            // Check angle error
+            if (std::abs(acos(cos_angle_local)) > M_PI/180 or
+                std::abs(acos(cos_angle_global)) > M_PI/180)
+            {
+                WOLF_ERROR("\n\n!!!!!!!!!!!! ERROR TOO BIG  iteration: ", iterations.size());
+                WOLF_INFO("cos(angle local) = ", cos_angle_local);
+                WOLF_INFO("angle local = ", acos(cos_angle_local));
+                WOLF_INFO("cos(angle global) = ", cos_angle_global);
+                WOLF_INFO("angle global = ", acos(cos_angle_global));
+
+                problem->print(4,1,1,1);
+                WOLF_INFO(report);
+
+                // Reset
+                fac->getFeature()->remove();
+
+                return false;
+            }
+
+            // Reset
+            fac->getFeature()->remove();
+
+            // Update performaces
+            convergence.push_back(solver->hasConverged() ? 1 : 0);
+            iterations.push_back(solver->iterations());
+            times.push_back(solver->totalTime());
+            error.push_back(acos(cos_angle_local));
+
+            return true;
+        }
+
+        void printAverageResults()
+        {
+            WOLF_INFO("/////////////////// Average results: n_solved = ", iterations.size());
+            double conv_avg = Eigen::Map<Eigen::VectorXd>(convergence.data(), convergence.size()).mean();
+            double iter_avg = Eigen::Map<Eigen::VectorXd>(iterations.data(), iterations.size()).mean();
+            double time_avg = Eigen::Map<Eigen::VectorXd>(times.data(), times.size()).mean();
+            double err_avg  = Eigen::Map<Eigen::VectorXd>(error.data(), error.size()).mean();
+
+            double iter_stdev = (Eigen::Map<Eigen::ArrayXd>(iterations.data(), iterations.size()) - iter_avg).matrix().norm();
+            double time_stdev = (Eigen::Map<Eigen::ArrayXd>(times.data(), times.size()) - time_avg).matrix().norm();
+            double err_stdev  = (Eigen::Map<Eigen::ArrayXd>(error.data(), error.size()) - err_avg).matrix().norm();
+
+            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);
+        }
+};
+
+// Input odometry data and covariance
+Vector3d v_local(1.0, 0.0, 0.0);
+double angle_stdev = 0.1;
+
+TEST_F(FactorVelocityLocalDirection3dTest, check_tree)
+{
+    ASSERT_TRUE(problem->check(0));
+
+    emplaceFeatureAndFactor(Vector3d(1.0, 0.0, 0.0), 0.1);
+
+    ASSERT_TRUE(problem->check(0));
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV)
+{
+    for (auto i = 0; i < N_TESTS; i++)
+        ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
+                                      Vector3d::Random().normalized(),
+                                      false, true));
+    printAverageResults();
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, random_solveV)
+{
+    for (auto i = 0; i < N_TESTS; i++)
+        ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
+                                      Vector3d::Random().normalized(),
+                                      false, true));
+    printAverageResults();
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_degenerated)
+{
+    for (auto i = 0; i < N_TESTS; i++)
+            ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
+                                          (Vector3d() << 1, 0, 0).finished() + Vector3d::Random() * 1e-4,
+                                          false, true));
+    for (auto i = 0; i < N_TESTS; i++)
+            ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
+                                          (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4,
+                                          false, true));
+    for (auto i = 0; i < N_TESTS; i++)
+            ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
+                                          (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4,
+                                          false, true));
+    printAverageResults();
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, random_solveV_degenerated)
+{
+    for (auto i = 0; i < N_TESTS; i++)
+        ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
+                                      (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4,
+                                      false, true));
+    for (auto i = 0; i < N_TESTS; i++)
+        ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
+                                      (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4,
+                                      false, true));
+    for (auto i = 0; i < N_TESTS; i++)
+        ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
+                                      (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4,
+                                      false, true));
+    printAverageResults();
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveO)
+{
+    for (auto i = 0; i < N_TESTS; i++)
+        ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
+                                      Vector3d::Random().normalized(),
+                                      true, false));
+    printAverageResults();
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, random_solveO)
+{
+    for (auto i = 0; i < N_TESTS; i++)
+        ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
+                                      Vector3d::Random().normalized(),
+                                      true, false));
+    printAverageResults();
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveO_degenerated)
+{
+    for (auto i = 0; i < N_TESTS; i++)
+            ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
+                                          (Vector3d() << 1, 0, 0).finished() + Vector3d::Random() * 1e-4,
+                                          true, false));
+    for (auto i = 0; i < N_TESTS; i++)
+            ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
+                                          (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4,
+                                          true, false));
+    for (auto i = 0; i < N_TESTS; i++)
+            ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(),
+                                          (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4,
+                                          true, false));
+    printAverageResults();
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, random_solveO_degenerated)
+{
+    for (auto i = 0; i < N_TESTS; i++)
+        ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
+                                      (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4,
+                                      true, false));
+    for (auto i = 0; i < N_TESTS; i++)
+        ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
+                                      (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4,
+                                      true, false));
+    for (auto i = 0; i < N_TESTS; i++)
+        ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
+                                      (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4,
+                                      true, false));
+    printAverageResults();
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}