Errors fixed and passing z axis rotation gtest

test/gtest_solve_problem_force_torque_inertial_dynamics.cpp

@@ -35,6 +35,7 @@
 #include <core/state_block/state_block_derived.h>
 #include <core/state_block/factory_state_block.h>
 #include <core/ceres_wrapper/solver_ceres.h>
+#include <core/math/rotations.h>
 
 #include <Eigen/Dense>
 #include <Eigen/Geometry>
@@ -586,31 +587,43 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
     C_KF0->getFrame()->unfix();
     C_KF0->getFrame()->getStateBlock('P')->fix();
     C_KF0->getFrame()->getStateBlock('P')->setState(Vector3d(0, 0, 0));
+    C_KF0->getFrame()->getStateBlock('O')->fix();
+    C_KF0->getFrame()->getStateBlock('O')->setState((Quaterniond(0,0,0,1)).coeffs());
 
     // initialiation of all the variables needed in the following iteration process
-    Vector3d         dL;
-    Vector3d         L_time_actual;
-    Vector3d         ang_vel_actual;
-    Vector3d         inertia_actual;
-    double           dt            = 1;  // time increment
-    CaptureMotionPtr C_time_before = C_KF0;
-    std::string      report;
-
-    for (TimeStamp t = 1.0; t <= 50.0; t += 1.0)
+    Vector3d    torque_true(0, 0, 1);
+    Vector3d    dL;
+    Vector3d    L_true(0, 0, 0);
+    Vector3d    ang_vel_true(0, 0, 0);
+    Vector3d    ang_acc_true;
+    Vector3d angle_true;
+    Quaterniond q_true(0,0,0,1);
+    double      dt = 1;  // time increment
+    Vector3d position_data (0,0,0);
+    Quaterniond q_data;
+    std::string report;
+
+    for (TimeStamp t = 1.0; t <= 18.0; t += 1.0)
     {
         // SIMULATOR
 
         // calculate dL = torque*dt
-        dL = data.segment<3>(9) * dt;
+        dL = torque_true * dt;
         // actualize L = L + dt
-        L_time_actual = C_time_before->getFrame()->getStateBlock('L')->getState();
-        L_time_actual = L_time_actual + dL;  // L = L + dt
+        L_true = L_true + dL;  // L = L + dt
         // w = L*i_inv  we actualize the data
-        inertia_actual     = p->getSensor()->getStateBlock('i')->getState();
-        ang_vel_actual(0)  = L_time_actual(0) / inertia_actual(0);
-        ang_vel_actual(1)  = L_time_actual(1) / inertia_actual(1);
-        ang_vel_actual(2)  = L_time_actual(2) / inertia_actual(2);
-        data.segment<3>(3) = ang_vel_actual;
+        ang_vel_true(0)  = L_true(0) / inertia_true(0);
+        ang_vel_true(1)  = L_true(1) / inertia_true(1);
+        ang_vel_true(2)  = L_true(2) / inertia_true(2);
+        data.segment<3>(3) = ang_vel_true;
+
+        ang_acc_true(0) = torque_true(0) / inertia_true(0);
+        ang_acc_true(1) = torque_true(1) / inertia_true(1);
+        ang_acc_true(2) = torque_true(2) / inertia_true(2);
+
+        angle_true = ang_vel_true*dt + 0.5*ang_acc_true*dt*dt;
+        q_true = q_true * exp_q(angle_true);
+
 
         // ESTIMATOR
 
@@ -619,22 +632,21 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
         C->process();
 
         p->getOrigin()->getFrame()->unfix();
-        p->getOrigin()->getStateBlock('P')->fix();
-        p->getOrigin()->getStateBlock('P')->setState(Vector3d(0, 0, 0));
+        p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+        p->getOrigin()->getFrame()->getStateBlock('P')->setState(position_data);
+        q_data = q_true;
+        p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+        p->getOrigin()->getFrame()->getStateBlock('O')->setState(q_data.coeffs());
 
         report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
         WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
         WOLF_INFO("-----------------------------");
-
-        WOLF_INFO("True inertia     : ", inertia_true, " m^2 Kg.");
-        WOLF_INFO("Guess inertia    : ", inertia_guess, " m^2 Kg.");
-        WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
-        WOLF_INFO("-----------------------------");
-
-        // saving of the actual capture to use it in the next iteration
-        C_time_before = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
     }
 
+    WOLF_INFO("True inertia     : ", inertia_true, " m^2 Kg.");
+    WOLF_INFO("Guess inertia    : ", inertia_guess, " m^2 Kg.");
+    WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+    WOLF_INFO("-----------------------------");
     auto inertia_estimated = S->getStateBlock('i')->getState();
 
     ASSERT_NEAR(inertia_estimated(2), inertia_true(2), 1e-6);