diff --git a/include/bodydynamics/factor/factor_angular_momentum.h b/include/bodydynamics/factor/factor_angular_momentum.h
index b6678df82a08933c8c3e9abe627045d44f2ddb3b..bdec1d9bb1fa78d0d15d705335b7c223854b30d4 100644
--- a/include/bodydynamics/factor/factor_angular_momentum.h
+++ b/include/bodydynamics/factor/factor_angular_momentum.h
@@ -50,7 +50,7 @@ WOLF_PTR_TYPEDEFS(FactorAngularMomentum);
* The residual computed has the following components, in this order
* - angular velocity error
*/
-class FactorAngularMomentum : public FactorAutodiff<FactorAngularMomentum, 3, 3, 6, 3> // State Blocks: L, I, i
+class FactorAngularMomentum : public FactorAutodiff<FactorAngularMomentum, 3, 3, 6, 3, 4> // State Blocks: L, I, i, q
{
public:
FactorAngularMomentum(const FeatureBasePtr& _ftr, // gets measurement and access to parents
@@ -64,13 +64,15 @@ class FactorAngularMomentum : public FactorAutodiff<FactorAngularMomentum, 3, 3,
bool operator()(const T* const _L, // ang momentum
const T* const _I, // IMU bias (acc and gyro)
const T* const _i, // inertia matrix
+ const T* const _q, // quaternion
T* _res) const; // residual
- template <typename D1, typename D2, typename D3, typename D4>
+ template <typename D1, typename D2, typename D3, typename D4, typename D5>
bool residual(const MatrixBase<D1>& _L,
const MatrixBase<D2>& _I,
const MatrixBase<D3>& _i,
- MatrixBase<D4>& _res) const;
+ const QuaternionBase<D4>& _q,
+ MatrixBase<D5>& _res) const;
Vector3d residual() const;
@@ -83,7 +85,7 @@ inline FactorAngularMomentum::FactorAngularMomentum(const FeatureBasePtr& _ftr
const ProcessorBasePtr& _processor,
bool _apply_loss_function,
FactorStatus _status)
- : FactorAutodiff<FactorAngularMomentum, 3, 3, 6, 3>(
+ : FactorAutodiff<FactorAngularMomentum, 3, 3, 6, 3, 4>(
"FactorAngularMomentum",
TOP_MOTION,
_ftr, // parent feature
@@ -94,20 +96,22 @@ inline FactorAngularMomentum::FactorAngularMomentum(const FeatureBasePtr& _ftr
_processor, // processor
_apply_loss_function,
_status,
- _ftr->getFrame()->getStateBlock('L'), // current linear momentum
- _ftr->getCapture()->getStateBlock('I'), // IMU bias
- _processor->getSensor()->getStateBlock('i')), // moment of inertia
+ _ftr->getFrame()->getStateBlock('L'), // current linear momentum
+ _ftr->getCapture()->getStateBlock('I'), // IMU bias
+ _processor->getSensor()->getStateBlock('i'), // moment of inertia
+ _ftr->getFrame()->getStateBlock('O')), // quaternion
measurement_ang_vel_(_ftr->getMeasurement()),
sqrt_info_upper_(_ftr->getMeasurementSquareRootInformationUpper()) // Buscar funcio correcte
{
//
}
-template <typename D1, typename D2, typename D3, typename D4>
+template <typename D1, typename D2, typename D3, typename D4, typename D5>
inline bool FactorAngularMomentum::residual(const MatrixBase<D1>& _L,
const MatrixBase<D2>& _I,
const MatrixBase<D3>& _i,
- MatrixBase<D4>& _res) const
+ const QuaternionBase<D4>& _q,
+ MatrixBase<D5>& _res) const
{
MatrixSizeCheck<3, 1>::check(_res);
@@ -117,21 +121,23 @@ inline bool FactorAngularMomentum::residual(const MatrixBase<D1>& _L,
* w_real = w_m - w_b
*
* Compute the angular velocity obtained by the relation between L pre-integrated and the i
- * w_est = i^(-1)*L
+ * w_est = i^(-1)*q.conjugate()*L
*
* Compute error between them
- * err = w_m - w_b - i^(-1)*L
+ * err = w_m - w_b - i^(-1)*q.conjugate()*L
*
* Compute residual
* res = W * err , with W the sqrt of the info matrix.
*/
- typedef typename D4::Scalar T;
+ typedef typename D5::Scalar T;
Matrix<T, 3, 1> w_real = measurement_ang_vel_ - _I.template segment<3>(3);
- const auto& Lx = _L(0);
- const auto& Ly = _L(1);
- const auto& Lz = _L(2);
+ Matrix<T, 3, 1> L_local;
+ L_local = _q.conjugate()*_L; //we transform que _L from the global frame to the local frame
+ const auto& Lx = L_local(0);
+ const auto& Ly = L_local(1);
+ const auto& Lz = L_local(2);
const auto& ixx = _i(0);
const auto& iyy = _i(1);
const auto& izz = _i(2);
@@ -148,20 +154,23 @@ inline Vector3d FactorAngularMomentum::residual() const
Vector3d L = getFrame()->getStateBlock('L')->getState();
Vector6d I = getCapture()->getStateBlock('I')->getState();
Vector3d i = getSensor()->getStateBlock('i')->getState();
+ Quaterniond q;
+ q.coeffs() = getFrame()->getStateBlock('O')->getState();
- residual(L, I, i, res);
+ residual(L, I, i, q, res);
return res;
}
template <typename T>
-inline bool FactorAngularMomentum::operator()(const T* const _L, const T* const _I, const T* const _i, T* _res) const
+inline bool FactorAngularMomentum::operator()(const T* const _L, const T* const _I, const T* const _i, const T* const _q, T* _res) const
{
Map<const Matrix<T, 3, 1>> L(_L);
Map<const Matrix<T, 6, 1>> I(_I);
Map<const Matrix<T, 3, 1>> i(_i);
+ Map<const Quaternion<T>> q(_q);
Map<Matrix<T, 3, 1>> res(_res);
- residual(L, I, i, res);
+ residual(L, I, i, q, res);
return true;
}
diff --git a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
index 546a46752444ad089586a9b2eab34062babf93eb..7fe58bdb468ead465fdafd96c641a4a37e53bee1 100644
--- a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
@@ -219,9 +219,9 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
//need to change with the real values, waiting for Pep's answer
bias_true = Vector6d::Zero();
- cdm_true = {0, 0, 0.0341};
- inertia_true = {0.0176, 0.0180, 0.0296}; // rounded {0.017598, 0.017957, 0.029599}
- mass_true = 1.952;
+ cdm_true = {0, 0, 0};
+ inertia_true = {0.0134943, 0.0141622, 0.0237319}; // rounded {0.017598, 0.017957, 0.029599}
+ mass_true = 1.9828;
}
};
@@ -251,24 +251,24 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
S->getStateBlock('m')->unfix();
S->setStateBlockDynamic('I', false);
- // add regularization
- S->addPriorParameter('C', // cdm
- cdm_guess, // cdm
- Matrix3d::Identity(), // cov
- 0, // start: X coordinate
- 3); // size
-
- S->addPriorParameter('i', // inertia
- inertia_guess, // inertia
- .00001*Matrix3d::Identity(), // cov
- 0, // start: X coordinate
- 3);
-
- S->addPriorParameter('m', // mass
- Vector1d(mass_guess), // mass
- Matrix1d::Identity(), // cov
- 0, // start
- 1);
+ // // add regularization
+ // S->addPriorParameter('C', // cdm
+ // cdm_guess, // cdm
+ // Matrix3d::Identity(), // cov
+ // 0, // start: X coordinate
+ // 3); // size
+
+ // S->addPriorParameter('i', // inertia
+ // inertia_guess, // inertia
+ // Matrix3d::Identity(), // cov
+ // 0, // start: X coordinate
+ // 3);
+
+ // S->addPriorParameter('m', // mass
+ // Vector1d(mass_guess), // mass
+ // Matrix1d::Identity(), // cov
+ // 0, // start
+ // 1);
std::string report;
@@ -322,8 +322,8 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
// solve!
report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
- WOLF_INFO(report);
- P->print(1, 0, 1, 1);
+ // WOLF_INFO(report);
+ // P->print(1, 0, 1, 1);
// results of this iteration
WOLF_INFO("Torque : ", torque[i].transpose()," N m .");
@@ -333,6 +333,23 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
WOLF_INFO("-----------------------------");
+
+ for (auto ftr : p->getOrigin()->getFeatureList())
+ {
+ if (std::dynamic_pointer_cast<FeatureMotion>(ftr) != nullptr)
+ {
+ auto fac = std::static_pointer_cast<FactorForceTorqueInertialDynamics>(ftr->getFactorList().front());
+ WOLF_INFO("Residual FTI: ", fac->residual().transpose());
+ }
+ else
+ {
+ auto fac = std::static_pointer_cast<FactorAngularMomentum>(ftr->getFactorList().front());
+ WOLF_INFO("Residual L: ", fac->residual().transpose());
+ }
+ }
+
+
+ if (i>100)break;
}
}
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index 9508e6cee38df8090ac3748e98a58d70a1ae1197..43629f400a4f763490ea6bd6210deaae1104ae80 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -709,7 +709,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__u
data.segment<3>(3) = ang_vel_true;
data.segment<3>(6) = -mass_true * gravity();
data.segment<3>(9) = torque_true;
- MatrixXd data_cov = 1 * MatrixXd::Identity(12, 12);
+ MatrixXd data_cov = S->getNoiseCov();
Vector3d position_data = position_true;
Vector4d orient_data = quat_true.coeffs();
@@ -856,7 +856,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__u
data.segment<3>(3) = ang_vel_true + gyro_bias_true;
data.segment<3>(6) = -mass_true * gravity();
data.segment<3>(9) = torque_true;
- MatrixXd data_cov = MatrixXd::Identity(12, 12);
+ MatrixXd data_cov = S->getNoiseCov();
Vector3d position_data = position_true;
Vector4d orient_data = quat_true.coeffs();