Fixing errors from the z rotation test

test/gtest_solve_problem_force_torque_inertial_dynamics.cpp

@@ -558,7 +558,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_
 // Here we only fix P and O from each key frame
 TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis_test)
 {
-    S->getStateBlock('i')->setState(Vector3d(0.01, 0.01, 0.01));
+    S->getStateBlock('i')->setState(Vector3d(0.01, 0.01, 0.02));
     Vector3d inertia_guess = S->getStateBlock('i')->getState();
 
     // Data
@@ -583,13 +583,10 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
     C0_0->process();
     CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
 
+    C_KF0->getFrame()->unfix();
     C_KF0->getFrame()->getStateBlock('P')->fix();
     C_KF0->getFrame()->getStateBlock('O')->fix();
 
-    Vector3d dL = data.segment<3>(9);
-    Vector3d ang_mom = C_KF0->getFrame()->getStateBlock('L')->getState();
-    ang_mom = ang_mom + dL;
-
     WOLF_INFO("======== SOLVE PROBLEM =======")
     std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
     WOLF_INFO(report);  // should show a very low iteration number (possibly 1)
@@ -599,29 +596,40 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
     WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 kg.");
     WOLF_INFO("-----------------------------");
 
+    Vector3d dL = data.segment<3>(9); //dL = torque*dt
+    Vector3d ang_mom = C_KF0->getFrame()->getStateBlock('L')->getState();
+    ang_mom = ang_mom + dL; //L = L + dt
+
     Vector3d ang_vel;
+    Vector3d actual_inertia;
 
-    for (TimeStamp t = 0.0; t <= 50.0; t += 1.0)
+    for (TimeStamp t = 1.0; t <= 50.0; t += 1.0)
     {
-        // w = i_inv*L we actualize the data //Això segur que es pot millorar
-        ang_vel(0) = ang_mom(0) /
-                          (p->getSensor()->getStateBlock('i')->getState()(0));
-        ang_vel(1) = ang_mom(1) /
-                          (p->getSensor()->getStateBlock('i')->getState()(1));
-        ang_vel(2) = ang_mom(2) /
-                          (p->getSensor()->getStateBlock('i')->getState()(2));
+        // w = L*i_inv  we actualize the data 
+        actual_inertia = p->getSensor()->getStateBlock('i')->getState();
+        ang_vel(0) = ang_mom(0) / actual_inertia(0);
+        ang_vel(1) = ang_mom(1) / actual_inertia(1);
+        ang_vel(2) = ang_mom(2) / actual_inertia(2);
         data.segment<3>(3) = ang_vel;
+
         CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
         C->setTimeStamp(t);
         C->process();
-        // increment de L
-        //dL és tota l'estona la mateixa (?) diria que si dL = torque*dt
-        ang_mom = ang_mom + dL;
-        p->getOrigin()->getFrame()->getStateBlock('P')->fix();
-        p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+
+        p->getOrigin()->getFrame()->unfix();
+        p->getOrigin()->getStateBlock('P')->fix();
+        p->getOrigin()->getStateBlock('O')->fix();
+
         report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
-        WOLF_INFO("Estimated inertia: ", S->getStateBlock('m')->getState(), " m^2 Kg.");
+        WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
         WOLF_INFO("-----------------------------");
+
+        //L increment
+        //dL = torque*dt 
+        //dL in this specific problem is always the same because torque is not changing
+        ang_mom = p->getOrigin()->getFrame()->getStateBlock('L')->getState();
+        ang_mom = ang_mom + dL;
+
     }
 
     auto inertia_estimated = S->getStateBlock('i')->getState();