diff --git a/src/processor/processor_force_torque_inertial_dynamics.cpp b/src/processor/processor_force_torque_inertial_dynamics.cpp
index fa5078ebcd386463c0a459d36e37087e9aafa37a..395b253961f01ed16d87eb42abded8692424aa7a 100644
--- a/src/processor/processor_force_torque_inertial_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_dynamics.cpp
@@ -177,12 +177,9 @@ void ProcessorForceTorqueInertialDynamics::data2tangent(const Eigen::VectorXd& _
const Matrix3d& fd_cross = skew(fd);
const Matrix3d& c_cross = skew(c);
- // tangent(a,w) = data(a,w) - bias(a,w)
- // tangent(f) = data(f)
- // tangent(t) = data(t) - model(c) x data(f)
- _tangent.segment<6>(0) = awd - _bias;
- _tangent.segment<3>(6) = fd;
- _tangent.segment<3>(9) = td - c.cross(fd); // c x fd
+ _tangent.segment<6>(0) = awd - _bias; // awt = awd - awb
+ _tangent.segment<3>(6) = fd; // ft = fd
+ _tangent.segment<3>(9) = td - c.cross(fd); // tt = td - c x fd
// J_tangent_data
_J_tangent_data.setIdentity(12, 12);
@@ -295,15 +292,52 @@ void ProcessorForceTorqueInertialDynamics::computeCurrentDelta(const Eigen::Vect
* J_delta_tangent
* J_delta_model
*
- * these are obtained directly by the function g() = tangent2delta().
- *
- * Note 1: It is unclear to me (JS, 4-aug-2022) if this function tangent2delta() is completely accurate
- * with regard to the two different reference frames: BASE and COM. It is possible that
- * we have to revise the math.
- *
- * Note 2: It is even possible that the function tangent2delta() is OK, but then that the function
- * betweenStates() does not account for the difference in reference frames. So we also need to revise
- * the math inside betweenStates() with regard to the two reference frames BASE and COM.
+ * these are obtained directly by the function g() = tangent2delta(). This function does:
+ *
+ * angacc = I.inv * ( tt - wt x ( I * wt ) )
+ * --> J_aa_i, J_aa_tt, J_aa_wt
+ * acc_dyn = ft / _mass - angacc x com - wt x ( wt x com )
+ * --> J_ad_ft, J_ad_m, J_ad_aa, J_ad_c, J_ad_wt
+ * ==> J_ad_wt, J_ad_ft, J_ad_tt, J_ad_c, J_ad_i, J_ad_m
+ *
+ * delta_p_imu = 0.5 * at * dt^2 --> J_dpi_at
+ * delta_v_imu = at * _dt --> J_dvi_at
+ * delta_p_dyn = 0.5 * acc_dyn * dt^2 --> J_dpd_ad
+ * delta_v_dyn = acc_dyn * _dt --> J_dvd_ad
+ * delta_L = tt * _dt --> J_dL_tt
+ * delta_q = exp_q(wt * _dt) --> J_dq_wt
+ *
+ * Assembling Jacobians gives:
+ *
+ * at wt ft tt
+ * J_delta_tangent = [ J_dpi_at 0 0 0
+ * J_dvi_at 0 0 0
+ * 0 J_dpd_wt J_dpd_ft J_dpd_tt
+ * 0 J_dvd_wt J_dvd_ft J_dvd_tt
+ * 0 0 0 J_dL_tt
+ * 0 J_dq_wt 0 0 ]
+ * with
+ * J_dpd_wt = J_dpd_ad * ( J_ad_wt + J_ad_aa * J_aa_wt )
+ * J_dpd_ft = J_dpd_ad * J_ad_ft
+ * J_dpd_tt = J_dpd_ad * J_ad_aa * J_aa_tt
+ * J_dvd_wt = J_dvd_ad * ( J_ad*wt + J_ad_aa * J_aa_wt )
+ * J_dvd_ft = J_dvd_ad * J_ad_ft
+ * J_dvd_tt = J_dvd_ad * J_ad_aa * J_aa_tt
+ *
+ * c i m
+ * J_delta_model = [ 0 0 0
+ * 0 0 0
+ * J_dpd_c J_dpd_i J_dpd_m
+ * J_dvd_c J_dvd_i J_dvd_m
+ * 0 0 0
+ * 0 0 0 ]
+ * with
+ * J_dpd_c = J_dpd_ad * J_ad_c
+ * J_dpd_i = J_dpd_ad * J_ad_aa * J_aa_i
+ * J_dpd_m = J_dpd_ad * J_ad_m
+ * J_dvd_c = J_dvd_ad * J_ad_c
+ * J_dvd_i = J_dvd_ad * J_ad_aa * J_aa_i
+ * J_dvd_m = J_dvd_ad * J_ad_m
*
* 3) On a third stage, we need to apply the chain rule for the functions f() and g(),
* that is delta = g( f( data, bias, model), model) -- yes, it's a mess!!
@@ -317,10 +351,12 @@ void ProcessorForceTorqueInertialDynamics::computeCurrentDelta(const Eigen::Vect
*
* J_delta_calib = [ J_delta_bias | J_delta_model ]
*
+ *
* 4) On a fourth stage, we compute the covariance of the delta:
*
* Q_delta = J_delta_data * Q_data * J_delta_data.transpose()
*
+ *
* 5) The function returns the following quantities, which relate to the variables used above:
*
* - _delta = delta
@@ -346,26 +382,6 @@ void ProcessorForceTorqueInertialDynamics::computeCurrentDelta(const Eigen::Vect
* J_(axb)_b = a_x
*/
- // Matrix<double, 12, 1> tangent = _data;
- // tangent.head<6>() -= _calib.head<6>(); // J_tangent_data = Identity(12,12)
- // Matrix<double, 12, 6> J_tangent_I; // J_tangent_I = [-Identity(6,6) ; Zero(6,6)]
- // J_tangent_I.topRows<6>() = -Matrix6d::Identity();
- // J_tangent_I.bottomRows<6>() = Matrix6d::Zero();
-
- // // go from tangent to delta. We need to dynamic model for this
- // const Matrix<double, 7, 1>& model = sensor_fti_->getModel();
- // Matrix<double, 18, 12> J_delta_tangent;
- // Matrix<double, 18, 7> J_delta_model;
- // fti::tangent2delta(tangent, model, _dt, _delta, J_delta_tangent, J_delta_model);
-
- // // Compute cov and compose jacobians
- // Matrix<double, 18, 6> J_delta_I = J_delta_tangent * J_tangent_I;
- // _jacobian_calib << J_delta_I, J_delta_model; // J_delta_calib = _jacobian_calib = [J_delta_I ; J_delta_model]
- // const auto& J_delta_data = J_delta_tangent; // * J_tangent_data, which is the Identity;
- // _delta_cov = J_delta_data * _data_cov * J_delta_data.transpose();
-
- ////////////////////// NOU CODI QUE HAURE DE REVISAR I CANVIAR PEL QUE HI HA ADALT //////////////////////////
-
/*
* 1. tangent = f(data, bias, model) --> J_tangent_data, J_tangent_bias, J_tangent_model
*
@@ -465,13 +481,13 @@ bool ProcessorForceTorqueInertialDynamics::voteForKeyFrame() const
// time span
if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_force_torque_inertial_dynamics_->max_time_span)
{
- WOLF_DEBUG("PM: vote: time span");
+ WOLF_DEBUG("PM ", getName(), " vote: time span");
return true;
}
// buffer length
if (getBuffer().size() > params_force_torque_inertial_dynamics_->max_buff_length)
{
- WOLF_DEBUG("PM: vote: buffer length");
+ WOLF_DEBUG("PM ", getName(), " vote: buffer length");
return true;
}
// dist_traveled
@@ -482,14 +498,14 @@ bool ProcessorForceTorqueInertialDynamics::voteForKeyFrame() const
double dist = (X1.at('P') - X0.at('P')).norm();
if (dist > params_force_torque_inertial_dynamics_->dist_traveled)
{
- WOLF_DEBUG("PM: vote: distance traveled");
+ WOLF_DEBUG("PM ", getName(), " vote: distance traveled");
return true;
}
// angle turned
double angle = 2.0 * acos(getMotion().delta_integr_(15));
if (angle > params_force_torque_inertial_dynamics_->angle_turned)
{
- WOLF_DEBUG("PM: vote: angle turned");
+ WOLF_DEBUG("PM ", getName(), " vote: angle turned");
return true;
}