diff --git a/include/bodydynamics/factor/factor_angular_momentum.h b/include/bodydynamics/factor/factor_angular_momentum.h
index 5c120ae2eb369c9c7e373b66f5d55dbd2996f0ba..bdec1d9bb1fa78d0d15d705335b7c223854b30d4 100644
--- a/include/bodydynamics/factor/factor_angular_momentum.h
+++ b/include/bodydynamics/factor/factor_angular_momentum.h
@@ -48,12 +48,12 @@ WOLF_PTR_TYPEDEFS(FactorAngularMomentum);
* 'i' inertia matrix components (we are assuming that it is a diagonal matrix)
*
* The residual computed has the following components, in this order
- * - angular velocity error according to FT preintegration
+ * - angular velocity error
*/
-class FactorAngularMomentum : public FactorAutodiff<FactorAngularMomentum, 3, 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 FeatureMotionPtr& _ftr, // gets measurement and access to parents
+ FactorAngularMomentum(const FeatureBasePtr& _ftr, // gets measurement and access to parents
const ProcessorBasePtr& _processor, // gets access to processor and sensor
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
@@ -64,50 +64,54 @@ 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;
private:
- Matrix<double, 12, 1> data;
- Matrix<double, 3, 3> sqrt_info_upper_;
+ Vector3d measurement_ang_vel_; // gyroscope measurement data
+ Matrix3d sqrt_info_upper_;
};
-inline FactorAngularMomentum::FactorAngularMomentum(const FeatureBasePtr& _ftr,
+inline FactorAngularMomentum::FactorAngularMomentum(const FeatureBasePtr& _ftr,
const ProcessorBasePtr& _processor,
- bool _apply_loss_function,
- FactorStatus _status)
- : FactorAutodiff<FactorAngularMomentum, 3, 3, 3, 6, 3>(
+ bool _apply_loss_function,
+ FactorStatus _status)
+ : FactorAutodiff<FactorAngularMomentum, 3, 3, 6, 3, 4>(
"FactorAngularMomentum",
TOP_MOTION,
- _ftr, // parent feature
- nullptr, // frame other
- nullptr, // capture other
- nullptr, // feature other
- nullptr, // landmark other
- _processor, // processor
+ _ftr, // parent feature
+ nullptr, // frame other
+ nullptr, // capture other
+ nullptr, // feature other
+ nullptr, // landmark other
+ _processor, // processor
_apply_loss_function,
_status,
- _ftr->getFrame()->getStateBlock('L'), // previous frame L
- _capture_origin->getStateBlock('I'), // previous frame IMU bias
- _processor->getSensor()->getStateBlock('i'), // sensor i
- data(ftr->getMeasurement()),
- sqrt_info_upper_(_processor->getSensor()->getNoiseCov().block<3,3>(3,3)))
+ _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>
-inline bool FactorAngularMomentum::residual(const MatrixBase<D1>& _L,
- const MatrixBase<D2>& _I,
- const MatrixBase<D3>& _i,
- MatrixBase<D4>& _res) const
+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,
+ const QuaternionBase<D4>& _q,
+ MatrixBase<D5>& _res) const
{
MatrixSizeCheck<3, 1>::check(_res);
@@ -117,25 +121,29 @@ 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.
*/
- Matrix<T, 3, 1> w_real = data.segment<3>(3) - _I.segment<3>(3);
- double Lx = _L(0);
- double Ly = _L(1);
- double Lz = _L(2);
- double ixx = _i(0);
- double iyy = _i(1);
- double izz = _i(2);
- Matrix<T, 3, 1> w_est = [Lx/ixx, Ly/iyy, Lz/izz];
- Matrix<T, 3, 1> err = w_real - w_est;
- _res = sqrt_info_upper_ * err;
+ typedef typename D5::Scalar T;
+
+ Matrix<T, 3, 1> w_real = measurement_ang_vel_ - _I.template segment<3>(3);
+ 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);
+ Matrix<T, 3, 1> w_est(Lx / ixx, Ly / iyy, Lz / izz);
+ Matrix<T, 3, 1> err = w_real - w_est;
+ _res = sqrt_info_upper_ * err;
return true;
}
@@ -144,29 +152,29 @@ inline Vector3d FactorAngularMomentum::residual() const
{
Vector3d res(3);
Vector3d L = getFrame()->getStateBlock('L')->getState();
- Vector6d I = getFeature()->getCapture()->getSensor()->getState();
- Vector3d i = getFeature()->getSensor()->getStateBlock('i')->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<Matrix<T, 3, 1>> res(_res);
+ 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;
}
} // namespace wolf
-#endif // FACTOR_ANGULAR_MOMENTUM_H
\ No newline at end of file
+#endif // FACTOR_ANGULAR_MOMENTUM_H
diff --git a/include/bodydynamics/math/force_torque_inertial_delta_tools.h b/include/bodydynamics/math/force_torque_inertial_delta_tools.h
index 42081eba2c3aa4e5e1bc68cf349fa6a57de6cc20..e5a87d1bf0c6898ecbb5b5a10b3a5a4b4ad709f1 100644
--- a/include/bodydynamics/math/force_torque_inertial_delta_tools.h
+++ b/include/bodydynamics/math/force_torque_inertial_delta_tools.h
@@ -1219,10 +1219,10 @@ void forces2acc(const MatrixBase<D1>& _force,
const Matrix3t& J_angacc_angvel = I_inv * (skew(I * _angvel) - angvel_x * I); // (38)
// Output Jacobians
- _J_acc_force = Matrix<S, 3, 3>::Identity() / _mass; // (59)
- _J_acc_torque = J_acc_angacc * J_angacc_torque; // (?)
_J_acc_angvel =
J_acc_angacc * J_angacc_angvel - angvel_x * J_angvelcom_angvel + skew(_angvel.cross(_com)); // (37)
+ _J_acc_force = Matrix<S, 3, 3>::Identity() / _mass; // (59)
+ _J_acc_torque = J_acc_angacc * J_angacc_torque; // (?)
_J_acc_com = -angacc_x - angvel_x * angvel_x; // (51)
_J_acc_inertia = J_acc_angacc * J_angacc_inertia; // (55)
_J_acc_mass = -_force / (_mass * _mass); // (50)
diff --git a/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h b/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
index dd9eef9ee86e6fd6daab00cdd142fc6713217d59..91a58b87620b66168be69be70ab84586917e605c 100644
--- a/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
+++ b/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
@@ -72,7 +72,6 @@ class ProcessorForceTorqueInertialDynamics : public ProcessorMotion
ParamsProcessorForceTorqueInertialDynamicsPtr params_force_torque_inertial_dynamics_;
SensorForceTorqueInertialPtr sensor_fti_;
Matrix6d imu_drift_cov_;
- Matrix3d gyro_noise_cov_;
public:
/** \brief convert raw CaptureMotion data to the delta-state format.
diff --git a/include/bodydynamics/sensor/sensor_force_torque_inertial.h b/include/bodydynamics/sensor/sensor_force_torque_inertial.h
index 2d509f80415ce98f27d84ee11440086f0fc83595..21f42957039fea53471e2cef9eb60ef67d96e7ad 100644
--- a/include/bodydynamics/sensor/sensor_force_torque_inertial.h
+++ b/include/bodydynamics/sensor/sensor_force_torque_inertial.h
@@ -37,6 +37,8 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
// noise std dev
double acc_noise_std;
double gyro_noise_std;
+ double accb_initial_std;
+ double gyrob_initial_std;
double acc_drift_std;
double gyro_drift_std;
double force_noise_std;
@@ -44,34 +46,43 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
Vector3d com;
Vector3d inertia;
double mass;
+ bool imu_bias_fix, com_fix, inertia_fix, mass_fix;
ParamsSensorForceTorqueInertial() = default;
ParamsSensorForceTorqueInertial(std::string _unique_name, const ParamsServer& _server)
: ParamsSensorBase(_unique_name, _server)
{
- acc_noise_std = _server.getParam<double>(prefix + _unique_name + "/acc_noise_std");
- gyro_noise_std = _server.getParam<double>(prefix + _unique_name + "/gyro_noise_std");
- acc_drift_std = _server.getParam<double>(prefix + _unique_name + "/acc_drift_std");
- gyro_drift_std = _server.getParam<double>(prefix + _unique_name + "/gyro_drift_std");
- force_noise_std = _server.getParam<double>(prefix + _unique_name + "/force_noise_std");
- torque_noise_std = _server.getParam<double>(prefix + _unique_name + "/torque_noise_std");
- com = _server.getParam<Vector3d>(prefix + _unique_name + "/com");
- inertia = _server.getParam<Vector3d>(prefix + _unique_name + "/inertia");
- mass = _server.getParam<double>(prefix + _unique_name + "/mass");
+ acc_noise_std = _server.getParam<double>(prefix + _unique_name + "/acc_noise_std");
+ gyro_noise_std = _server.getParam<double>(prefix + _unique_name + "/gyro_noise_std");
+ accb_initial_std = _server.getParam<double>(prefix + _unique_name + "/accb_initial_std");
+ gyrob_initial_std = _server.getParam<double>(prefix + _unique_name + "/gyrob_initial_std");
+ acc_drift_std = _server.getParam<double>(prefix + _unique_name + "/acc_drift_std");
+ gyro_drift_std = _server.getParam<double>(prefix + _unique_name + "/gyro_drift_std");
+ force_noise_std = _server.getParam<double>(prefix + _unique_name + "/force_noise_std");
+ torque_noise_std = _server.getParam<double>(prefix + _unique_name + "/torque_noise_std");
+ com = _server.getParam<Vector3d>(prefix + _unique_name + "/com");
+ inertia = _server.getParam<Vector3d>(prefix + _unique_name + "/inertia");
+ mass = _server.getParam<double>(prefix + _unique_name + "/mass");
+ imu_bias_fix = _server.getParam<bool> (prefix + _unique_name + "/imu_bias_fix");
+ com_fix = _server.getParam<bool> (prefix + _unique_name + "/com_fix");
+ inertia_fix = _server.getParam<bool> (prefix + _unique_name + "/inertia_fix");
+ mass_fix = _server.getParam<bool> (prefix + _unique_name + "/mass_fix");
}
~ParamsSensorForceTorqueInertial() override = default;
std::string print() const override
{
- return ParamsSensorBase::print() + "\n" //
- + "acc_noise_std: " + std::to_string(acc_noise_std) + "\n" //
- + "gyro_noise_std: " + std::to_string(gyro_noise_std) + "\n" //
- + "acc_drift_std: " + std::to_string(acc_drift_std) + "\n" //
- + "gyro_drift_std: " + std::to_string(gyro_drift_std) + "\n" //
- + "force_noise_std: " + std::to_string(force_noise_std) + "\n" //
- + "torque_noise_std: " + std::to_string(torque_noise_std) + "\n" //
- + "com: print not implemented." + "\n" //
- + "inertia: print not implemented. \n" //
- + "mass: " + std::to_string(mass) + "\n"; //
+ return ParamsSensorBase::print() + "\n" //
+ + "acc_noise_std: " + std::to_string(acc_noise_std) + "\n" //
+ + "gyro_noise_std: " + std::to_string(gyro_noise_std) + "\n" //
+ + "accb_initial_std: " + std::to_string(accb_initial_std) + "\n" //
+ + "gyrob_initial_std: " + std::to_string(gyrob_initial_std) + "\n" //
+ + "acc_drift_std: " + std::to_string(acc_drift_std) + "\n" //
+ + "gyro_drift_std: " + std::to_string(gyro_drift_std) + "\n" //
+ + "force_noise_std: " + std::to_string(force_noise_std) + "\n" //
+ + "torque_noise_std: " + std::to_string(torque_noise_std) + "\n" //
+ + "com: print not implemented." + "\n" //
+ + "inertia: print not implemented. \n" //
+ + "mass: " + std::to_string(mass) + "\n"; //
}
};
@@ -90,14 +101,17 @@ class SensorForceTorqueInertial : public SensorBase
WOLF_SENSOR_CREATE(SensorForceTorqueInertial, ParamsSensorForceTorqueInertial, 7);
// getters return by copy
- Vector6d getImuBias() const; // Imu bias [acc, gyro]
- Vector3d getAccBias() const; // Acc bias
- Vector3d getGyroBias() const; // Gyro bias
- double getMass() const; // Total mass
- Vector3d getInertia() const; // Inertia vector (diagonal of inertia matrix)
- Vector3d getCom() const; // centre of mass
- Vector7d getModel() const; // dynamic model [com, inertia, mass]
- ParamsSensorForceTorqueInertialPtr getParamsSensorForceTorqueInertial(){return params_fti_;}
+ Vector6d getImuBias() const; // Imu bias [acc, gyro]
+ Vector3d getAccBias() const; // Acc bias
+ Vector3d getGyroBias() const; // Gyro bias
+ double getMass() const; // Total mass
+ Vector3d getInertia() const; // Inertia vector (diagonal of inertia matrix)
+ Vector3d getCom() const; // centre of mass
+ Vector7d getModel() const; // dynamic model [com, inertia, mass]
+ ParamsSensorForceTorqueInertialPtr getParamsSensorForceTorqueInertial()
+ {
+ return params_fti_;
+ }
};
inline double SensorForceTorqueInertial::getMass() const
diff --git a/src/processor/processor_force_torque.cpp b/src/processor/processor_force_torque.cpp
index dd7f5806463b14bbe56ee458d542048675879b7a..7739f6d27f7447dc207f596b2ca81a9f46739e65 100644
--- a/src/processor/processor_force_torque.cpp
+++ b/src/processor/processor_force_torque.cpp
@@ -67,19 +67,19 @@ bool ProcessorForceTorque::voteForKeyFrame() const
// time span
if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_force_torque_->max_time_span)
{
- WOLF_DEBUG("PM: vote: time span");
+ WOLF_DEBUG("PM ", getName(), " vote: time span");
return true;
}
// buffer length
if (getBuffer().size() > params_motion_force_torque_->max_buff_length)
{
- WOLF_DEBUG("PM: vote: buffer length");
+ WOLF_DEBUG("PM ", getName(), " vote: buffer length");
return true;
}
// Some other measure of movement?
- // WOLF_DEBUG( "PM: do not vote" );
+ // WOLF_DEBUG( "PM ", getName(), " vote: do not vote" );
return false;
}
diff --git a/src/processor/processor_force_torque_inertial_dynamics.cpp b/src/processor/processor_force_torque_inertial_dynamics.cpp
index 678074d6f632f70b47712da7ffcf056fa8a64146..15c72e2f7a5dda65029222ad07e00b37c144dafe 100644
--- a/src/processor/processor_force_torque_inertial_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_dynamics.cpp
@@ -28,6 +28,7 @@
// bodydynamics
#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
+#include "bodydynamics/factor/factor_angular_momentum.h"
#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
#include "bodydynamics/capture/capture_force_torque_inertial.h"
@@ -98,8 +99,12 @@ void ProcessorForceTorqueInertialDynamics::emplaceFeaturesAndFactors(CaptureBase
// - feature has the current gyro measurement
// - factor relates w_measured, IMU_bias(last capture), L(last capture) and i(sensor)
- // auto measurement_gyro = motion.data_.segment<3>(3);
- // auto meas_cov = sensor_fti_->getNoiseCov().block<3,3>(3,3); // ??
+ auto measurement_gyro = motion.data_.segment<3>(3);
+ auto gyro_cov = motion.data_cov_.block<3, 3>(3, 3);
+
+ auto ftr_base = FeatureBase::emplace<FeatureBase>(_capture_own, "FeatureBase", measurement_gyro, gyro_cov);
+
+ FactorBase::emplace<FactorAngularMomentum>(ftr_base, ftr_base, shared_from_this(), params_->apply_loss_function);
// 3. Feature and factor bias -- IMU bias drift for acc and gyro
//---------------------------------------------------------------
@@ -131,7 +136,7 @@ void ProcessorForceTorqueInertialDynamics::emplaceFeaturesAndFactors(CaptureBase
0, // take all of first second block
-1, // take all of first second block
shared_from_this(), // this processor
- false); // loss function
+ params_->apply_loss_function); // loss function
}
}
}
@@ -148,14 +153,12 @@ void ProcessorForceTorqueInertialDynamics::configure(SensorBasePtr _sensor)
{
sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor);
- auto gyro_noise_std = sensor_fti_->getParamsSensorForceTorqueInertial()->gyro_noise_std;
auto acc_drift_std = sensor_fti_->getParamsSensorForceTorqueInertial()->acc_drift_std;
auto gyro_drift_std = sensor_fti_->getParamsSensorForceTorqueInertial()->gyro_drift_std;
ArrayXd imu_drift_sigmas(6);
imu_drift_sigmas << acc_drift_std, acc_drift_std, acc_drift_std, gyro_drift_std, gyro_drift_std, gyro_drift_std;
imu_drift_cov_ = imu_drift_sigmas.square().matrix().asDiagonal();
- gyro_noise_cov_ = Array3d(gyro_noise_std, gyro_noise_std, gyro_noise_std).square().matrix().asDiagonal();
}
void ProcessorForceTorqueInertialDynamics::data2tangent(const Eigen::VectorXd& _data,
@@ -174,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);
@@ -292,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!!
@@ -314,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
@@ -343,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
*
@@ -385,7 +404,7 @@ void ProcessorForceTorqueInertialDynamics::computeCurrentDelta(const Eigen::Vect
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); // Aquà ja farà bé ell sol el J_delta_model?
+ tangent, model, _dt, _delta, J_delta_tangent, J_delta_model);
// 3. Compose jacobians
Matrix<double, 18, 6> J_delta_bias = J_delta_tangent * J_tangent_bias;
@@ -462,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
@@ -479,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;
}
diff --git a/src/sensor/sensor_force_torque_inertial.cpp b/src/sensor/sensor_force_torque_inertial.cpp
index aa38e8ab62d4bd0bdc0fee48bd15c227ea52fc8d..0c3a35d31d95e34543339bfe1e706264df19e287 100644
--- a/src/sensor/sensor_force_torque_inertial.cpp
+++ b/src/sensor/sensor_force_torque_inertial.cpp
@@ -34,20 +34,28 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd&
: SensorBase("SensorForceTorqueInertial",
FactoryStateBlock::create("StatePoint3d", _extrinsics.head(3), true),
FactoryStateBlock::create("StateQuaternion", _extrinsics.tail(4), true),
- FactoryStateBlock::create("StateParams6", Vector6d::Zero(), false), // IMU bias
+ FactoryStateBlock::create("StateParams6", Vector6d::Zero(), _params->imu_bias_fix), // IMU bias
12,
false,
false,
false),
params_fti_(_params)
{
- addStateBlock('C', FactoryStateBlock::create("StateParams3", params_fti_->com, true)); // centre of mass
- addStateBlock('i', FactoryStateBlock::create("StateParams3", params_fti_->inertia, true)); // inertial vector
- addStateBlock('m', FactoryStateBlock::create("StateParams1", Vector1d(params_fti_->mass), true)); // total mass
+ addStateBlock('C', FactoryStateBlock::create("StateParams3", params_fti_->com, params_fti_->com_fix)); // centre of mass
+ addStateBlock('i', FactoryStateBlock::create("StateParams3", params_fti_->inertia, params_fti_->inertia_fix)); // inertial vector
+ addStateBlock('m', FactoryStateBlock::create("StateParams1", Vector1d(params_fti_->mass), params_fti_->mass_fix)); // total mass
setStateBlockDynamic('I', false);
setStateBlockDynamic('C', false);
setStateBlockDynamic('i', false);
setStateBlockDynamic('m', false);
+
+ VectorXd noise_std_vector(12);
+ Vector3d acc_noise_std_vector = Vector3d::Ones() * params_fti_->acc_noise_std;
+ Vector3d gyro_noise_std_vector = Vector3d::Ones() * params_fti_->gyro_noise_std;
+ Vector3d force_noise_std_vector = Vector3d::Ones() * params_fti_->force_noise_std;
+ Vector3d torque_noise_std_vector =Vector3d::Ones() * params_fti_->torque_noise_std;
+ noise_std_vector << acc_noise_std_vector, gyro_noise_std_vector, force_noise_std_vector, torque_noise_std_vector;
+ setNoiseStd(noise_std_vector);
}
// TODO: Adapt this API to that of MR !448
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 52e1fd976f3619fc9b303b7c9ef601c740d10677..c6d84c19a4e3c5bf6140d5ae057524ad32cffd3c 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -37,5 +37,7 @@ wolf_add_gtest(gtest_sensor_force_torque gtest_sensor_force_torque.cpp)
wolf_add_gtest(gtest_sensor_inertial_kinematics gtest_sensor_inertial_kinematics.cpp)
+wolf_add_gtest(gtest_simulation_problem_force_torque_inertial_dynamics gtest_simulation_problem_force_torque_inertial_dynamics.cpp)
+
wolf_add_gtest(gtest_solve_problem_force_torque_inertial_dynamics gtest_solve_problem_force_torque_inertial_dynamics.cpp)
diff --git a/test/dades_simulacio_pep.csv b/test/dades_simulacio_pep.csv
new file mode 100644
index 0000000000000000000000000000000000000000..e52426eecfc24f699e66baf17a7547171b0dcd50
--- /dev/null
+++ b/test/dades_simulacio_pep.csv
@@ -0,0 +1,381 @@
+timestamp,pos_x,pos_y,pos_z,quat_x,quat_y,quat_z,quat_w,vlin_x,vlin_y,vlin_z,vang_x,vang_y,vang_z,f_x,f_y,f_z,m_x,m_y,m_z
+0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,30.24140688670505,0.00013608714916246711,0.0014521640071535336,5.128857392352534e-05
+0.02,1.225134672257865e-08,-1.2259260745627694e-09,0.002128145411468811,1.9224644385208175e-06,1.9212242769870645e-05,4.1019743724623865e-07,0.9999999998135127,-1.4317549759835751e-06,1.4326791822140724e-07,0.10640727057210408,0.00019224644386403226,0.0019212242771064925,4.101974372717375e-05,0.0,0.0,29.887893140652935,5.355186669397938e-05,0.000632055519802166,4.980384573760899e-05
+0.04,3.164793119129389e-07,-3.145199751668115e-08,0.006314985419629783,4.601336319320625e-06,4.678476956232382e-05,1.2166216785179206e-06,0.9999999988942664,1.3954502268081405e-06,-1.4561235248549083e-07,0.20934200098822997,0.00026788676993719067,0.002757252725995719,8.064227015362827e-05,0.0,0.0,29.540464010158914,6.529613184591554e-06,0.0001467468847405895,4.8374456304805384e-05
+0.06,1.2380977715680128e-06,-1.2186983166693075e-07,0.012492280418762047,7.372358116038826e-06,7.629798429331784e-05,2.406581495967047e-06,0.9999999970592371,8.064988856122653e-06,-8.22739440841769e-07,0.30886475336605246,0.00027710020369252434,0.002951321689266104,0.00011899536599298057,0.0,0.0,29.198910712249972,-1.8547764069065664e-05,-0.00012464283122537445,4.6998259156440075e-05
+0.08,3.097877446883576e-06,-3.0210021946895046e-07,0.020592960737205568,9.881332287758513e-06,0.00010416347763031251,3.968358568423615e-06,0.9999999945182907,1.98959164523503e-05,-2.0235505135256084e-06,0.4050340262559032,0.00025089220835405955,0.0027865498928493805,0.00015617630694591205,0.0,0.0,28.863025564019104,-3.0351715194498752e-05,-0.00026189346931571045,4.5673342991900894e-05
+0.1,6.184536975284903e-06,-5.976110773142468e-07,0.0305510856069671,1.196158501223723e-05,0.00012856755356196802,5.890994766694367e-06,0.9999999916463004,3.845464710395817e-05,-3.900771944711037e-06,0.4979062661756617,0.00020801493173149216,0.0024404086842509344,0.00019226117408965902,0.0,0.0,28.53260186078871,-3.437789080273923e-05,-0.0003172687898949089,4.4397966278411e-05
+0.12,1.0741866558453952e-05,-1.0286174094126725e-06,0.042301802349252376,1.3556311555146249e-05,0.00014877933433270332,8.164145814907884e-06,0.9999999888071414,6.491503804013118e-05,-6.559699256681581e-06,0.5875358781141918,0.0001594553377684207,0.0020211798764281767,0.00022731143112637397,0.0,0.0,28.20743377978916,-3.4066289298806396e-05,-0.00032464551987276824,4.3170511002704366e-05
+0.14,1.6964758003386982e-05,-1.6098581052830123e-06,0.05578130575425297,1.4669972065107598e-05,0.00016470242579285052,1.07779808308616e-05,0.9999999862708688,9.986526609854465e-05,-1.004211670665144e-05,0.6739752352525765,0.00011134016457341262,0.0015923118001836536,0.0002613784838798486,0.0,0.0,27.887316305105017,-3.1531907447757135e-05,-0.00030565857607245395,4.198945051493422e-05
+0.16,2.500042579451631e-05,-2.350860516675195e-06,0.07092679764020307,1.533841142411551e-05,0.00017658869745700473,1.3723113150197237e-05,0.9999999841964204,0.00014333164313205587,-1.433221477027244e-05,0.757274688107806,6.680824198505375e-05,0.0011886307843512953,0.0002945068023644371,0.0,0.0,27.572045165552257,-2.804161484570855e-05,-0.0002738395995421916,4.085332740515585e-05
+0.18,3.495241247522585e-05,-3.2564484559282456e-06,0.0876764465807755,1.561096353220603e-05,0.000184859339958222,1.69905522905314e-05,0.9999999826473215,0.00019489192643186188,-1.937047455305273e-05,0.8374825731884221,2.7208863368858957e-05,0.0008270689021236481,0.0003267360381102831,0.0,0.0,27.261416778835994,-2.432504458349527e-05,-0.00023740142256944985,3.9760738377768134e-05
+0.2,4.688585843664052e-05,-4.327344872539917e-06,0.10596934779176233,1.554014637097892e-05,0.00018999631381550708,2.0571670093161243e-05,0.9999999816183553,0.00025381091328892526,-2.5068584840409175e-05,0.9146452211820351,-7.139186698592418e-06,0.0005137031066314508,0.0003581024473668731,0.0,0.0,26.955228197803766,-2.0776635640817886e-05,-0.00020110674489681735,3.871032394162904e-05
+0.22,6.083310972943089e-05,-5.560782674882313e-06,0.12574548316854894,1.5176092622285518e-05,0.00019247944176311945,2.4458176297022176e-05,0.999999981061574,0.0003191658105763353,-3.132235016220051e-05,0.9888069646673194,-3.647410731137493e-05,0.0002483195893832517,0.00038863983868462926,0.0,0.0,26.653277056500798,-1.7586204964503693e-05,-0.00016751672606418744,3.770076138167401e-05
+0.24,7.679912868170565e-05,-6.9510768667761574e-06,0.14694568146580123,1.4563870747744718e-05,0.00019275261391040672,2.8642100326515915e-05,0.9999999809069766,0.0003899482422690305,-3.802159791118977e-05,1.0600101453376,-6.13020498670971e-05,2.7325064132054024e-05,0.00041838019682400354,0.0,0.0,26.355361514911202,-1.4822605152176038e-05,-0.00013781976443549837,3.673075994254958e-05
+0.26,9.47664248860185e-05,-8.490135874145494e-06,0.169511578610433,1.3742514482432002e-05,0.00019120813676605711,3.311577712114471e-05,0.9999999810769683,0.0004651400441809517,-4.5057099434768435e-05,1.1282951207251761,-8.222628020747176e-05,-0.00015443884682139983,0.0004473540879359478,0.0,0.0,26.06128020191197,-1.248665044184527e-05,-0.00011237648127615518,3.579905749707245e-05
+0.28,0.0001146993832187328,-1.0167904779292288e-05,0.19338557813850527,1.2745013010223804e-05,0.0001881820159120577,3.787183555034989e-05,0.9999999814954086,0.0005437650023037907,-5.2324952084938885e-05,1.1937002704183637,-9.98511068502665e-05,-0.0003026022483523487,0.0004755909181486874,0.0,0.0,25.770832156275954,-1.0543995748291124e-05,-9.107345409031797e-05,3.4904418193991524e-05
+0.3,0.00013654796202310516,-1.1972741640004973e-05,0.21851081174630843,1.1598797380307315e-05,0.00018395549439779657,4.290318844542166e-05,0.99999998209258,0.0006249207221359291,-5.972899744876074e-05,1.2562620017647255,-0.00011473224925839966,-0.000422641401451132,0.0005031190949293762,0.0,0.0,25.48381676568331,-8.945026727102956e-06,-7.354870443132544e-05,3.4045630751051904e-05
+0.32,0.00016025078983072265,-1.3891732485642351e-05,0.24483109994552332,1.0326442259004211e-05,0.00017875986641732912,4.820302361314376e-05,0.9999999828073715,0.0007077952482184457,-6.718182979192059e-05,1.3160147550539718,-0.0001273552949626109,-0.0005195511959160284,0.0005299661248519798,0.0,0.0,25.20003370373022,-7.636417926670802e-06,-5.933180811457106e-05,3.32215071621932e-05
+0.34,0.00018573771765226005,-1.5910952505941744e-05,0.2722909128120399,8.946419825077345e-06,0.00017278271549180994,5.376479542160372e-05,0.9999999835877207,0.000791672693879034,-7.460486386955082e-05,1.3729910081731154,-0.0001381304756607396,-0.0005977027007750253,0.0005561586705364821,0.0,0.0,24.919282864910798,-6.567422575320947e-06,-4.7927735263297855e-05,3.2430881669300074e-05
+0.36,0.00021293189584760944,-1.801568139927669e-05,0.30083533081772834,7.4738170352390416e-06,0.00016617447059009947,5.9582216700792056e-05,0.9999999843900733,0.0008759314520642052,-8.192782837096444e-05,1.4272212807251576,-0.00014739632136483074,-0.0006608113712693733,0.0005817225820474456,0.0,0.0,24.64136429751868,-5.69289873331158e-06,-3.886411208742224e-05,3.167260989641808e-05
+0.38,0.00024175144651356973,-2.0190580482276536e-05,0.33041000573419105,5.920973576027082e-06,0.0001590546571491374,6.564925080213585e-05,0.9999999851783669,0.0009600377908456243,-8.908795302508147e-05,1.4787341376012537,-0.00015542758413341926,-0.0007119675595164921,0.0006066829129003706,0.0,0.0,24.366078134381194,-4.974373055688375e-06,-3.171515259203783e-05,3.0945568083510296e-05
+0.4,0.00027211079856612533,-2.2419838396331563e-05,0.36096112159726595,4.298025521235641e-06,0.0001515175281673838,7.196010372789714e-05,0.9999999859228544,0.0010435369191785571,-9.602903355600483e-05,1.527556191995093,-0.00016244466125909436,-0.0007536985076862657,0.0006310639273148624,0.0,0.0,24.093224521305874,-4.37997698465864e-06,-2.6111103254233825e-05,3.0248652376453937e-05
+0.42,0.00030392174605596784,-2.46872913318105e-05,0.3924353557207975,2.6133556236762282e-06,0.00014363695178116296,7.850921628435798e-05,0.9999999865989496,0.0011260429953360142,-0.00010270049204560422,1.5737121078472105,-0.00016862292692206813,-0.0007880427000593195,0.0006548891029633633,0.0,0.0,23.82260354308645,-3.883782088376364e-06,-2.1739039836821128e-05,2.95807781491162e-05
+0.44,0.00033709428250247906,-2.697652277570891e-05,0.42477983974792916,8.739596396764638e-07,0.00013547054576643254,8.52912562395986e-05,0.9999999871861844,0.001207229057748464,-0.00010905650013402395,1.6172246017060508,-0.00017410112519403655,-0.0008166251707300359,0.000678181131856761,0.0,0.0,23.55401514688409,-3.4648590918617828e-06,-1.833881567026019e-05,2.8940879342710454e-05
+0.46,0.00037153725622228717,-2.927094695306558e-05,0.45794212072790047,-9.142580286413045e-07,0.00012706311323446766,9.230111048273954e-05,0.9999999876673171,0.001286817480569591,-0.00011505519663431869,1.6581144439918098,-0.00017898843551235287,-0.0008407273843924232,0.0007009619209358499,0.0,0.0,23.28725906276722,-3.1062536953729847e-06,-1.5696584158342297e-05,2.8327907813671338e-05
+0.48,0.0004071588843600551,-3.1553879414390404e-05,0.4918701222060494,-2.746244927451395e-06,0.00011844946562191448,9.953387719043742e-05,0.9999999880275947,0.001364571279465591,-0.00012065800684957773,1.696400459648282,-0.0001833700943022203,-0.0008613485070478094,0.0007232525932345714,0.0,0.0,23.022134721159553,-2.7939881004357403e-06,-1.3637397384913541e-05,2.774083268700711e-05
+0.5,0.0004438671568986315,-3.380859763213172e-05,0.5265121053144176,-4.617603312147996e-06,0.00010965673109070238,0.00010698485801072983,0.9999999882541595,0.001440286399052448,-0.00012582905553707214,1.7320995281671026,-0.00018731161013678182,-0.0008792568651640862,0.0007450734900301386,0.0,0.0,22.758441166909613,-2.516144231856643e-06,-1.201777398018855e-05,2.7178639715342917e-05
+0.52,0.0004815701564426534,-3.6018394005446324e-05,0.5618166298500358,-6.524422171465399e-06,0.00010070624669678814,0.00011464955027574895,0.9999999883355821,0.001513784984536765,-0.00013053465678020834,1.7652265829678462,-0.0001908616887990171,-0.0008950315689687848,0.0007664441741152649,0.0,0.0,22.49597696965455,-2.2620524688932164e-06,-1.0718733671533087e-05,2.6640330642770012e-05
+0.54,0.0005201763150304116,-3.816662332264446e-05,0.5977325153276241,-8.463127112576458e-06,9.161512638575555e-05,0.00012252363925452665,0.9999999882615008,0.0015849095608419749,-0.0001347428601365797,1.7957946101163627,-0.00019405401818309043,-0.0009090949299985471,0.0007873834341614194,0.0,0.0,22.234540130106808,-2.0215909726228887e-06,-9.639544573558645e-06,2.612492257810989e-05
+0.56,0.0005595946255653072,-4.023674647932752e-05,0.6342088019930675,-1.0430338277520445e-05,8.239758734523268e-05,0.00013060299045586693,0.9999999880223522,0.0016535179997827969,-0.00013842303188087653,1.8238146463624958,-0.00019690807011360425,-0.0009217366257355021,0.0008079092900505667,0.0,0.0,21.973927981841808,-1.7845907109848014e-06,-8.692274912180942e-06,2.5631447378592437e-05
+0.58,0.000599734822569925,-4.22123720808402e-05,0.6711947117836287,-1.2422727964662252e-05,7.306610652389625e-05,0.0001388836419895804,0.9999999876091769,0.0017194791392778374,-0.000141545451874534,1.8492957764768965,-0.00019942906926489473,-0.0009331306833716197,0.0008280389990108131,0.0,0.0,21.713937088108732,-1.5403367776833932e-06,-7.797150893829041e-06,2.515895104800238e-05
+0.6,0.0006405075447391281,-4.4077297453914274e-05,0.7086396092204232,-1.4436870999247184e-05,6.3632468399211e-05,0.00014736179699255714,0.9999999870134931,0.0017826689201577296,-0.00014408090980208148,1.8722451298649925,-0.00020160726608800175,-0.0009433463554822843,0.0008477890623779501,0.0,0.0,21.45436313312252,-1.2771554757096126e-06,-6.8786745212046085e-06,2.4706493152370404e-05
+0.62,0.0006818244900890152,-4.581555053893739e-05,0.7464929622182092,-1.646908211867505e-05,5.410875496982907e-05,0.00015603381612440723,0.99999998622723,0.0018429669198775345,-0.00014600028869160502,1.8926678764342992,-0.00020341663579537278,-0.0009523538982479262,0.0008671752328109703,0.0,0.0,21.195000807223682,-9.820774751201355e-07,-5.862434029113572e-06,2.4273146256859216e-05
+0.64,0.0007235985732570138,-4.741143411702785e-05,0.784704302797023,-1.851523572320959e-05,4.450831973999729e-05,0.00016489621013581664,0.9999999852427176,0.0019002531840888656,-0.0001472741284450862,1.9105672216891183,-0.0002048131118278433,-0.0009600261730955736,0.0008862125218045072,0.0,0.0,20.935643685211176,-6.405692328792512e-07,-4.6725349894805035e-06,2.3857995387433176e-05
+0.66,0.0007657440936044365,-4.884957383224674e-05,0.8232231876796332,-2.0570564317383497e-05,3.484677921072753e-05,0.00017394563251140198,0.9999999840527353,0.0019544052835758583,-0.0001478721673405305,1.9259444010242788,-0.00020573245119390835,-0.0009661368970309222,0.0009049152073612103,0.0,0.0,20.676084097061988,-2.363272203487199e-07,-3.2295850930230685e-06,2.3460137520346756e-05
+0.68,0.00080817692211612,-5.01149714871848e-05,0.8619991587581588,-2.2629432864851066e-05,2.5143048028651446e-05,0.00018317887218771836,0.9999999826506182,0.002005295553490065,-0.00014776286501515064,1.9387986731868971,-0.00020608782138622535,-0.0009703562780524738,0.0009232968417082108,0.0,0.0,20.41611299014797,2.4886809246726926e-07,-1.4491755564449704e-06,2.3078681104287097e-05
+0.7000000000000001,0.0008508147146017904,-5.119306510922231e-05,0.9009817034125251,-2.4685087133546914e-05,1.5420437336278718e-05,0.00019259284634617788,0.9999999810304259,0.002052788502806585,-0.0001469129162397186,1.9491273128721744,-0.00020576719426255668,-0.0009722446963724164,0.0009413702589631916,0.0,0.0,20.15551978194459,8.352392587562818e-07,7.591859008115875e-07,2.2712745617763053e-05
+0.72,0.0008935771583092904,-5.2069797295202786e-05,0.9401202146626866,-2.6729374950802616e-05,5.707829857251853e-06,0.00020218459327994823,0.9999999791871755,0.002096738413607229,-0.0001452867708143035,1.9569256024160053,-0.0002046306311694576,-0.0009712450324566849,0.0009591475826746313,0.0,0.0,19.894092202096353,1.5450387185200753e-06,3.492723536835385e-06,2.23614611660418e-05
+0.74,0.0009363862586978747,-5.273169333505811e-05,0.9793639511357344,-2.875244014628547e-05,-3.959060321305788e-06,0.00021195126533338508,0.9999999771171418,0.0021369871816076104,-0.0001428461811272871,1.9621868225436647,-0.00020250754515343576,-0.0009666741990283394,0.0009766402331763623,0.0,0.0,19.631616122559407,2.4025127408622282e-06,6.855645507175545e-06,2.2023968121615554e-05
+0.76,0.0009791666727349104,-5.3165950581615595e-05,1.0186619968281212,-3.0742389856649796e-05,-1.3536340585725084e-05,0.00022189012191221576,0.999999974818223,0.0021733624808538874,-0.0001395498052787567,1.9649022421300921,-0.00019919402991304503,-0.0009577144048695558,0.0009938589347135492,0.0,0.0,19.367875374382386,3.433798598906357e-06,1.0959012031963589e-05,2.1699416811902633e-05
+0.78,0.0010218460946226015,-5.336054046655318e-05,1.0579632206422716,-3.2684936814569306e-05,-2.2970507674419493e-05,0.00023199852256245462,0.9999999722903677,0.002205676366337275,-0.00013535290011075399,1.965061106923386,-0.00019445035045097007,-0.0009434046610062072,0.0010108137223060576,0.0,0.0,19.10265154951177,4.666750477499981e-06,1.592022552687755e-05,2.138696725701853e-05
+0.8,0.0010643556992898497,-5.3304324453961666e-05,1.097216235675808,-3.456301925481214e-05,-3.2196938046545044e-05,0.00024227392011590381,0.9999999695360507,0.002233724457983445,-0.00013020714490609027,1.9626506271791535,-0.00018799869654038915,-0.0009226330318399689,0.0010275139483265133,0.0,0.0,18.835723785820342,6.130689457539695e-06,2.186214369204187e-05,2.1085788962501225e-05
+0.8200000000000001,0.00110663064826069,-5.29871850665428e-05,1.1363693582405012,-3.635640214352373e-05,-4.11383131962036e-05,0.000252713853900019,0.999999966560779,0.0022572858775527327,-0.00012406064277050528,1.9576559641495144,-0.00017952130650774088,-0.0008941301319079633,0.0010439682887754987,0.0,0.0,18.566868533358154,7.856073285572585e-06,2.89118140897604e-05,2.079506076738212e-05
+0.84,0.0011486106616029356,-5.2400172909227246e-05,1.1753705665868723,-3.804126456798197e-05,-4.9702997386950746e-05,0.00026331594300986575,0.9999999633735935,0.0022761241357934277,-0.00011685815261471801,1.9500602153671027,-0.00016865907463323052,-0.0008564643701474082,0.0010601847492395734,0.0,0.0,18.29585929962213,9.874082403826812e-06,3.7198828491680214e-05,2.0513970751809363e-05
+0.86,0.0011902406585421546,-5.153567034587691e-05,1.2141674593091312,-3.9589779301633556e-05,-5.778339031448902e-05,0.0002740778796398478,0.9999999599875218,0.002289989189943598,-0.00010854160996990479,1.939844398661747,-0.00015501076015683312,-0.000808039444918882,0.001076170670520538,0.0,0.0,18.022466371440913,1.2216119986252494e-05,4.685329691617168e-05,2.0241716201427698e-05
+0.88,0.0012314714679759241,-5.038757215276142e-05,1.2527072134038761,-4.0969691765205035e-05,-6.52542822288666e-05,0.00028499742247276624,0.9999999564199151,0.0022986209104575795,-9.905099933325376e-05,1.9269874348462328,-0.00013813291876485934,-0.0007470945920626131,0.0010919327339182971,0.0,0.0,17.746456510892813,1.4913225338308855e-05,5.8003444410648086e-05,1.9977503628474362e-05
+0.9,0.0012722606085263245,-4.895148307493164e-05,1.2909365419547318,-4.214390681563898e-05,-7.197124401809258e-05,0.00029607239012358256,0.9999999526925843,0.0023017542096006654,-8.83256440428264e-05,1.9114661290085264,-0.00011754067797342899,-0.000671708082966203,0.0011074769661466384,0.0,0.0,17.467592622540536,1.799540185531967e-05,7.077283768980314e-05,1.972054884087476e-05
+0.92,0.0013125731359159274,-4.7224931747085886e-05,1.3288016514139456,-4.307009298466488e-05,-7.77690896164731e-05,0.00030730065463488746,0.9999999488316204,0.002299126092177917,-7.63059814900356e-05,1.8932551503523074,-9.270947577901866e-05,-0.0005798044586913464,0.0011228087438456122,0.0,0.0,17.185633389213617,2.149086275560208e-05,8.527725273066444e-05,1.9470077049578047e-05
+0.9400000000000001,0.0013523825533415126,-4.520759990929865e-05,1.3662481984509973,-4.370031492894159e-05,-8.246045279824388e-05,0.000318680135020521,0.9999999448667624,0.00229048488895564,-6.293589145205672e-05,1.8723270105374077,-6.307787699590453e-05,-0.0004691659697845806,0.0011379327976393941,0.0,0.0,16.90033287365644,2.542519959525258e-05,0.00010162119719525631,1.922532299194753e-05
+0.96,0.00139167177815442,-4.290156525194739e-05,1.4032212463376617,-4.3980705918357205e-05,-8.583452490884345e-05,0.0003302087908528987,0.9999999408301413,0.0022756019251244596,-4.8165643120346525e-05,1.8486520404893387,-2.8051574194079355e-05,-0.0003374486708371048,0.0011528532156588086,0.0,0.0,16.61144008367137,2.9820479796827115e-05,0.00011989410108381726,1.8985531042157285e-05
+0.98,0.0014304341555470817,-4.031155556692856e-05,1.4396652208389147,-4.385119316005755e-05,-8.76560043733514e-05,0.00034188461588831516,0.9999999367542016,0.002254285858331002,-3.1955521697119344e-05,1.8221983656761673,1.2991329174000386e-05,-0.00018220359839085954,0.0011675734464123776,0.0,0.0,16.318698499049297,3.469427954361226e-05,0.00014016618274487191,1.874995525769601e-05
+1.0,0.001468674507108085,-3.744521116902669e-05,1.475523865579948,-4.324528963016294e-05,-8.766431297689452e-05,0.0003537056317226157,0.9999999326685673,0.002226399892413293,-1.4280187954796318e-05,1.7929318798967526,6.069366548599252e-05,-9.034475892517283e-07,0.0011820963008337853,0.0,0.0,16.021845559789746,4.005865649203688e-05,0.000162483981261341,1.8517859292026906e-05
+1.02,0.001506410199064022,-3.431335178872146e-05,1.510740196861797,-4.2089966886916746e-05,-8.55731381328223e-05,0.0003656698814669736,0.9999999285956027,0.002191882033266351,4.866186302216029e-06,1.7608162176942668,0.0001157110987402225,0.00020902483046624857,0.0011964239532602425,0.0,0.0,15.720612117152145,4.591906158624148e-05,0.00018686551438923393,1.828851606651494e-05
+1.04,0.0015436722117995088,-3.093024333341369e-05,1.5452564579024297,-4.030562413648687e-05,-8.107036503488823e-05,0.00037777542342974575,0.9999999245443881,0.0021507685017854773,2.5463977482170326e-05,1.7258127256136366,0.0001787024890274656,0.0004501610427606256,0.0012105579409895301,0.0,0.0,15.414721852380449,5.227317854250568e-05,0.0002132949640330506,1.8061207047117212e-05
+1.06,0.0015805061888614685,-2.7313859075896887e-05,1.579014072487553,-3.780616975755779e-05,-7.381846844056831e-05,0.0003900203247850978,0.9999999205028336,0.0021032203554712794,4.7461351404589304e-05,1.687880432676413,0.0002503186314450222,0.0007250457948897646,0.0012244991619171106,0.0,0.0,15.103890673156826,5.910965842026439e-05,0.00024171669211869862,1.7835220906830366e-05
+1.08,0.0016169734401299278,-2.348612896326248e-05,1.6119535980273325,-3.4499232844681466e-05,-6.345544208650764e-05,0.0004024026552017028,0.999999916427653,0.002049553295313277,7.076669328609663e-05,1.646976020673815,0.00033118923316386253,0.001036126732878039,0.0012382478695524273,0.0,0.0,14.78782610594287,6.64066752843473e-05,0.00027202821923844045,1.7609851258719636e-05
+1.1,0.001653151870182683,-1.9473169847869088e-05,1.644014678033785,-3.028652465444309e-05,-4.9596356511907836e-05,0.00041492048039454743,0.9999999122319575,0.0019902705487267456,9.524025551524635e-05,1.6030537952105652,0.000421907895064738,0.001385695709064288,0.0012518036644383974,0.0,0.0,14.466226714867066,7.413015154134062e-05,0.0003040715069428934,1.738439302001736e-05
+1.12,0.0016891367990783976,-1.5305488516725146e-05,1.6751359940618071,-2.5064374160441842e-05,-3.1835656975890705e-05,0.0004275718555487583,0.9999999077702839,0.0019260986238547363,0.00012068507625292176,1.5560656589159119,0.0005230146641069133,0.0017758147814023405,0.001265165480635101,0.0,0.0,14.138781597017049,8.223137033946948e-05,0.0003376213971820752,1.7158136790890644e-05
+1.1400000000000001,0.0017250416387769808,-1.1018148416065836e-05,1.705255217199113,-1.8724469977156014e-05,-9.750348221072232e-06,0.00044035481854513527,0.9999999028209746,0.0018580256272138162,0.00014683724664945182,1.5059610889437332,0.0006349753485708337,0.0022082275401313595,0.001278331565449354,0.0,0.0,13.80517003347499,9.064349858056708e-05,0.0003723692799050826,1.6930360377925884e-05
+1.16,0.0017609983841042724,-6.650889904001482e-06,1.7343089592532526,-1.1154855233176231e-05,1.7095728334155853e-05,0.00045326738289072003,0.9999998970659871,0.0017873417270810202,0.0001733556286872019,1.4526871219146964,0.0007581561602975608,0.002684249879721549,0.0012912994499198814,0.0,0.0,13.46506142074264,9.927628443806569e-05,0.00040789885681447036,1.670031621867823e-05
+1.18,0.0017971578724005456,-2.2481926851401403e-06,1.762232723877023,-2.2412459029954387e-06,4.914622215584608e-05,0.00046630753022079317,0.9999998900684505,0.0017156812285402596,0.00019981114923178423,1.396188350965737,0.0008927912849149188,0.0032046304939722476,0.0013040659065590764,0.0,0.0,13.118115676956961,0.00010800783408559944,0.00044364906755506306,1.646721287969829e-05
+1.2,0.001833689760589928,2.1407322408881417e-06,1.788960858011283,8.131218807916963e-06,8.684474576009318e-05,0.00047947320218536975,0.9999998812486538,0.0016450655951205997,0.0002256758217759048,1.3364069417998015,0.0010389395616226913,0.0037693653638232734,0.001316626889284294,0.0,0.0,12.763984424651412,0.00011667179169583264,0.000478856545860773,1.62301878957993e-05
+1.22,0.0018707821621939675,6.462035288327101e-06,1.8144265042277703,2.007562588286322e-05,0.0001306247818319582,0.0004927622914551494,0.9999998698597214,0.0015779465841785191,0.0002503116830248811,1.2732826779231874,0.0011964243020104487,0.004377441291169292,0.0013289774481265906,0.0,0.0,12.402313416022983,0.00012503722475576406,0.0005124658746931687,1.5988267781081184e-05
+1.24,0.0019086408796754083,1.0657421569920133e-05,1.8385615548557541,3.370007573149198e-05,0.00018089592585745625,0.0005061726314679754,0.9999998549651075,0.0015172484105198784,0.0002729598889154119,1.2067530501367558,0.0013647468382522916,0.00502646950588779,0.0013411116079679064,0.0,0.0,12.032746912951565,0.00013277445813686395,0.0005429815915139469,1.574066721378354e-05
+1.26,0.0019474881573701191,1.4664463389358336e-05,1.8612966092214678,4.9103164316624205e-05,0.00023802466053654363,0.0005197019870884877,0.9999998354214789,0.0014664074321276716,0.00029273030956777246,1.136753412536819,0.0015429556615896146,0.005712138928228991,0.001353022467232675,0.0,0.0,11.654935085781528,0.00013941147585438962,0.0005682745189826144,1.548561289576385e-05
+1.28,0.0019875608806281847,1.8417050222092967e-05,1.8825609359823987,6.636760491775683e-05,0.00030230733977743723,0.0005333480371492095,0.9999998098727242,0.0014294072396541454,0.0003085920845113855,1.0632172376737414,0.0017294598767311575,0.00642743724921852,0.0013647013982765962,0.0,0.0,11.268545938273384,0.00014425774471393815,0.0005852589371075356,1.5219210598408263e-05
+1.3,0.0020291081355949298,2.1845998030147914e-05,1.9022824434773342,8.555102188837693e-05,0.00037393152895423456,0.0005471083403370218,0.9999997767643242,0.001410804837338228,0.0003193661559216246,0.9860765177858607,0.0019217456318929294,0.007161486743645971,0.0013761364004810364,0.0,0.0,10.873283639653724,0.0001462384938636374,0.0005892247126642602,1.4942654591777711e-05
+1.32,0.002072388007795087,2.487985069578418e-05,1.9203876622961615,0.00010667180092793813,0.000452917697180744,0.0005609803385766387,0.9999997343938378,0.001415741883191005,0.00032372112876311417,0.9052623762082537,0.002115880478763752,0.007897579793742147,0.001387315871783246,0.0,0.0,10.468914841980165,0.0001437506023028723,0.0005732024893719823,1.4650799549180987e-05
+1.34,0.0021176635803512992,2.7445882312585917e-05,1.9368017458250475,0.00012968825184094904,0.0005390340026444215,0.0005749613117272527,0.9999996810213448,0.0014499332787079097,0.00032017408560315613,0.8207059664420872,0.0023058427848359295,0.00861048864799949,0.0013982237773624377,0.0,0.0,10.055301888741688,0.00013426943913319445,0.000526359640621421,1.4340426720078128e-05
+1.36,0.0021651980159363184,2.947133283936838e-05,1.9514484958240934,0.00015446617897862987,0.0006316653651406534,0.0005890483456810099,0.9999996150804822,0.001519610763744835,0.00030710201587349134,0.7323397237714716,0.0024823593884057793,0.009261894921148407,0.0014088410341568775,0.0,0.0,9.632433219709114,0.00011370376003089211,0.00043145575262482083,1.4003848675377564e-05
+1.3800000000000001,0.002215248683886902,3.0884893181499103e-05,1.9642504195523571,0.0001807269266487254,0.0007296074635374591,0.0006032382666609615,0.9999995355570525,0.001631396274747139,0.0002827698340219759,0.6400989419775581,0.002630947924088747,0.009792883411318763,0.0014191421648980706,0.0,0.0,9.200422135240945,7.500704300589245e-05,0.0002601256889447623,1.363326179399456e-05
+1.4000000000000001,0.002268060372989986,3.161845892188782e-05,1.9751288191422973,0.00020795880852435535,0.0008307315784578456,0.0006175275797488216,0.999999442648778,0.0017920539225420185,0.00024538725357845426,0.5439233838847661,0.0027282462416125123,0.010111029107925744,0.0014290955257488273,0.0,0.0,8.759416630676654,5.9224872971941345e-06,-3.32982957525485e-05,1.32180045953285e-05
+1.42,0.0023238581072786957,3.160906621504949e-05,1.9840038979909491,0.00023526477275006728,0.0009314521890410814,0.0006319124110505699,0.9999993388667869,0.002008026943918258,0.00019321616091891395,0.4437581279282786,0.002735621846094594,0.010070677011409535,0.00143866255877871,0.0,0.0,8.309340800579037,-0.00011341178930313056,-0.0005170911025723091,1.2749079277874663e-05
+1.44,0.0023828409309284695,3.080062606355726e-05,1.990794839685318,0.00026111948911970034,0.0010259459483275921,0.0006463885196668561,0.9999992307164067,0.0022845607830903956,0.00012478000431080736,0.3395521865473096,0.002590095273873209,0.009448086550713304,0.0014478018790237022,0.0,0.0,7.849355230911976,-0.0003122847767576542,-0.0012865142670025198,1.2225268111288317e-05
+1.46,0.0024451787348663654,2.9144725531188213e-05,1.9954197680886347,0.0002830195104950638,0.0011050902671257028,0.0006609514491073395,0.9999991309089423,0.0026240603154565586,3.9284204395819835e-05,0.23125254935531062,0.0021936247815752753,0.00791340071447829,0.001456478488285123,0.0,0.0,7.376521136508651,-0.0006310843157646406,-0.0024685852540669018,1.166883701943257e-05
+1.48,0.002511014658898052,2.6599542888705284e-05,1.9977953650908702,0.0002970166657636101,0.001155098277566318,0.0006755969367364152,0.999999060548483,0.003023196107316387,-6.25805391149617e-05,0.11878704176514422,0.0014014031305057738,0.005000293827670246,0.0014646836290186681,0.0,0.0,6.8788084060216255,-0.0010997820664007119,-0.004150182446593764,1.1194493756706028e-05
+1.5,0.0025804731708883883,2.3126550075375276e-05,1.9978349656399208,0.0002974216378151039,0.0011567521938394495,0.0006903219158655309,0.9999990484597392,0.003468099049434656,-0.00017721066242340516,0.00198815504816445,3.890838443533018e-05,0.00016580286494318787,0.0014724950005191343,0.0,0.0,18.51836419726961,0.44936437173582827,1.5059908207012014,0.18729616038545208
+1.52,0.002632454171935659,2.6333855787167334e-05,1.9978915257913066,0.006554129811626445,0.01970297929125427,0.0020671325510581004,0.9997822577703992,0.002540103204491672,0.00017277702006931588,0.0028806506197515407,0.6268379216853844,1.8543740638136705,0.13787688761162972,0.0,0.0,18.35563541311824,0.16091224654634234,0.8159975151196931,0.19808217197896544
+1.54,0.0028187739886145263,-1.5946538692406762e-05,1.9978524915164102,0.015149946190354174,0.04822693980054579,0.004944754335420059,0.9987092624044628,0.009411636232906376,-0.0022150896124153202,-0.0012683832223961329,0.8604528666549536,2.8544249649275857,0.2862054540021749,0.0,0.0,19.191367665457445,0.011210885121456404,0.32355979405103674,0.19482003920099808
+1.56,0.003380144406601722,-0.0001739690393038598,1.9978011617876794,0.02385733888117927,0.08074607573453739,0.009358437501834608,0.9964051978399677,0.02803450838551585,-0.008328033539651414,0.0013774651963374455,0.8680676475032548,3.2618387537912286,0.43474862077143833,0.0,0.0,20.274927578463746,-0.056706446510644604,-0.0052345729602146784,0.18679455893791014
+1.58,0.004616516196606919,-0.0005209055197172559,1.9978823393594416,0.031686343137567685,0.11311639950896364,0.015266723153315278,0.992959003677824,0.0593507681870581,-0.018265806430643988,0.016863159924160882,0.7710427819009685,3.260619615407251,0.5780302745717278,0.0,0.0,21.172913353059755,-0.08731583468965642,-0.19474447464449696,0.1791026230297918
+1.6,0.006840618088389445,-0.0011196806970899937,1.9981998146618434,0.03823152504933126,0.1428922284969528,0.022615347300577706,0.9887409709292172,0.10209851266974644,-0.03185210612482418,0.04561499316006298,0.6257124951315041,3.018127029367769,0.7159353064566114,0.0,0.0,21.83048250617594,-0.09642290465736414,-0.2919351071593176,0.17189076231246936
+1.62,0.010344357631584901,-0.002015588069609184,1.9988042245350242,0.04341722116894763,0.16880831704644922,0.03134820317332547,0.984193063966474,0.15425741472530255,-0.048997107848545815,0.0861581129691169,0.46400429358676815,2.649426590407973,0.8488694439158168,0.0,0.0,22.264530642361564,-0.09361185737058841,-0.33098390944337797,0.16498223091164343
+1.6400000000000001,0.015379749754567806,-0.003234121940764242,1.9997033349026891,0.04735209666365171,0.19035513069646604,0.041406862494033705,0.9796980018850134,0.2138275717194289,-0.0698129168436788,0.136156944405886,0.30456139558072237,2.2280628475955715,0.9770290681756331,0.0,0.0,22.510336759248982,-0.0852153306272262,-0.3344644062662644,0.1583077372849761
+1.6600000000000001,0.02215086714597756,-0.004782226984238618,2.0008757465191245,0.05023132364867353,0.2074853708442581,0.052730895821322926,0.9755234941486564,0.27912048138907036,-0.09460074387211115,0.19289565634355121,0.1572283512779582,1.7995439841171805,1.1004729523187637,0.0,0.0,22.60776267159686,-0.07513947028812651,-0.31718305561528937,0.15184414195950355
+1.68,0.030813202311342075,-0.00665133462147483,2.0022831622378066,0.05227389839270899,0.2204172081690369,0.06525923619117661,0.9718152735893486,0.3487830643354794,-0.12378620006100555,0.25362506955209546,0.026132339808656035,1.3907113955104233,1.2192144431004788,0.0,0.0,22.59421564446368,-0.06558044031957777,-0.28872050899653334,0.14557902716658427
+1.7,0.041477270300358554,-0.008820986272270027,2.0038797612190904,0.053686035037046616,0.22950664565101714,0.07893166681800802,0.968614629879887,0.421704232029634,-0.15784738983088725,0.31579050839185946,-0.08808645271562086,1.0161764668940894,1.3332693538492686,0.0,0.0,22.50143261200271,-0.057609109701168504,-0.25514507636413675,0.13950027551501365
+1.72,0.05421413854325978,-0.011262323893322747,2.0056186097318136,0.054642557636573,0.23516787308251325,0.0936898557260294,0.9658842960203963,0.4968965242501052,-0.19725601912162521,0.37715547192904525,-0.18672500937415926,0.6827280032146,1.4426727819948337,0.0,0.0,22.354641785310605,-0.0516020065519216,-0.22019154799743967,0.1335952058409851
+1.74,0.06906143490373187,-0.01394110448977473,2.0074555973494674,0.055279793129061755,0.23782567799453694,0.10947777479603517,0.9635339683600965,0.5733983712278709,-0.24243584538276577,0.4358499445520357,-0.27199972083153745,0.39230148312430146,1.5474809403585401,0.0,0.0,22.173016797065053,-0.04753488570386177,-0.18606468174316937,0.1278515537707896
+1.76,0.08602905511215939,-0.016820133267995675,2.009351534434174,0.05569528686516358,0.23788853331796986,0.12624157498872307,0.9614417015509567,0.6502116549023496,-0.29373538132388777,0.49037204185373157,-0.34641433636558183,0.14394386016183874,1.647767237666237,0.0,0.0,21.970670563979134,-0.04517207115899535,-0.15398009114971511,0.1222580623907702
+1.78,0.10510422870308238,-0.019861135438766796,2.0112729972974708,0.0559512282215487,0.23573463385805732,0.143929076882095,0.9594701992705015,0.7262730871635286,-0.351409947618638,0.5395642637748408,-0.412391039894985,-0.06490856978177012,1.7436169153486096,0.0,0.0,21.757747769971385,-0.0441837958143243,-0.12452520027081537,0.1168045475571143
+1.8,0.12625585150714486,-0.023026139373029483,2.0131923857178897,0.0560796261071442,0.23170582466534592,0.16248904437919826,0.9574783531801488,0.8004516416975213,-0.41560899419098046,0.5825777161064625,-0.4720736686196614,-0.23761468890620352,1.8351220357208426,0.0,0.0,21.541406171091396,-0.04421621961088518,-0.09789847179767674,0.11148170898713657
+1.82,0.14943811523968503,-0.026278457077641496,2.0150875286514895,0.05608805286230672,0.22610619836526186,0.18187038413495193,0.9553288861756086,0.8715633186744672,-0.4863661823163585,0.6188319012382629,-0.5272399198473546,-0.377820710817416,1.9223774397586602,0.0,0.0,21.326615438864245,-0.04493119313749805,-0.07406676187701333,0.10628088844635278
+1.84,0.17459351356559655,-0.02958334325070113,2.016941065040282,0.055965276591769975,0.2192033464077939,0.2020213771131345,0.9528930390809579,0.9383958501659577,-0.5635911384285928,0.6479741832567238,-0.5792802343569964,-0.48915491662940286,2.0054777594772166,0.0,0.0,21.11677154402731,-0.046027046199828114,-0.052867778703824975,0.10119386576526901
+1.86,0.2016553149519231,-0.03290840320669837,2.018739744178724,0.055686415852145565,0.21123102275862907,0.22288901280807388,0.9500531385576955,0.9997379089495261,-0.6470627607514706,0.6698411235773369,-0.6292155880594934,-0.5750543259776292,2.084515349868018,0.0,0.0,20.914152784235938,-0.047247706124085465,-0.03407582990411251,0.09621271703146293
+1.8800000000000001,0.2305495867872036,-0.03622380819404319,2.0204737318059,0.055217443248825424,0.20239246468422575,0.24441846593243266,0.946703722244369,1.0544092058690413,-0.7364244801457386,0.6844228892856822,-0.6777362535001098,-0.6386761023274937,2.1595789387727082,0.0,0.0,20.72025060188015,-0.048384828468953645,-0.01744309037004199,0.09132972979037955
+1.9000000000000001,0.26119684382776925,-0.03950236611031335,2.022135968870354,0.05451898062440699,0.19286391720816526,0.2665527314627648,0.9427517337775289,1.1012892867470387,-0.8311820528175327,0.691831402400733,-0.7252501405519753,-0.6828623225360153,2.23075279751247,0.0,0.0,20.53600609063089,-0.049275919248627176,-0.002724605915965239,0.08653736045869602
+1.92,0.2935133822349063,-0.042719486792036354,2.023721604656059,0.05354939682135803,0.18279809430532315,0.2892324168558906,0.9381161590432772,1.1393438396153468,-0.9307044175740529,0.6922725716847845,-0.7719336548893102,-0.7101373620133702,2.2981162672627446,0.0,0.0,20.361977639089986,-0.04980032575789306,0.010307451081012498,0.08182821839709863
+1.94,0.32741235026845816,-0.04585307373363848,2.025227510452367,0.05226725742014184,0.17232742510398372,0.3123956791722255,0.9327273620910514,1.1676479614529864,-1.034227987269286,0.6860227111943278,-0.8177808584673237,-0.7227242474267358,2.361743514433154,0.0,0.0,20.19845932901024,-0.049874260301392526,0.0218565647680109,0.07719506363583456
+1.96,0.3628045985035336,-0.04888336794716167,2.0266518710299803,0.05063319554674714,0.1615670048510245,0.3359782885898483,0.9265262932310289,1.1854062007857662,-1.1408645366560632,0.6734090574677675,-0.862648571917707,-0.7225710717518894,2.42170342308511,0.0,0.0,20.045564613539,-0.049445560327536886,0.03209919490294566,0.07263080860821092
+1.98,0.3995993459862607,-0.05179276442829603,2.0279938465782577,0.04861128080164744,0.15061721501848466,0.359913797133755,0.9194636787560785,1.1919693739475787,-1.2496126432460275,0.6547941401999132,-0.9062962534856027,-0.7113817403412606,2.478059560460135,0.0,0.0,19.90328575066265,-0.04848859216517876,0.041184935770003483,0.06812851743991019
+2.0,0.43770469360589875,-0.05456561715666478,2.029253295885615,0.04616996404409675,0.13956600485802323,0.38413379126076386,0.9114992567850886,1.186848218632537,-1.3593724587500886,0.6305636412280741,-0.9484202430374584,-0.6906474484319238,2.5308701728316096,0.0,0.0,19.771536409813095,-0.0469995178700382,0.049238361482462634,0.06368139905748146
+2.02,0.47702801172354936,-0.0571880446231592,2.0304305513247103,0.04328267206982489,0.1284908440492464,0.4085682083109475,0.9026010926492336,1.1697239510971427,-1.468963446272083,0.6011172963384981,-0.9886824215278178,-0.6616767272587817,2.5801881842112473,0.0,0.0,19.65018263442922,-0.04499202926681056,0.05636177435937184,0.05928279258355197
+2.04,0.5174762254513904,-0.05964774449233677,2.0315262369120015,0.0399281200514965,0.11746036654034765,0.4331456990613365,0.8927449864916781,1.1404557694689839,-1.5771446137379193,0.5668623535009829,-1.0267336075466356,-0.6256228637926649,2.6260611813371195,0.0,0.0,19.539065741147155,-0.042493585683585966,0.06263833053034984,0.05492614522889809
+2.06,0.5589560177182539,-0.061933823147729244,2.032541121868371,0.0360904022135889,0.10653573154864444,0.45779402120456686,0.8819139725170528,1.0990853183604177,-1.6826367035012657,0.528209095913379,-1.0622321622700728,-0.5835081446806505,2.6685313761290033,0.0,0.0,19.43801960159797,-0.03954215497146307,0.06813520015987296,0.05060498422117005
+2.08,0.6013739672746913,-0.06403664351347196,2.033476003406525,0.031758913376224475,0.09577173046988482,0.48244045120435347,0.8700979014488571,1.0458381125687886,-1.784145756345888,0.4855679624295228,-1.0948583484458312,-0.5362447939129511,2.7076355424300376,0.0,0.0,19.34688395463611,-0.03618343870103202,0.07290655257904932,0.04631288524536536
+2.1,0.6446366360345192,-0.06594769267382578,2.0343316137234146,0.026928146204371915,0.08521766915890622,0.5070122044606657,0.8572930935992983,0.9811219160541154,-1.8803874497676722,0.4393478440172762,-1.1243250171935408,-0.48465273021768923,2.7434049280636166,0.0,0.0,19.26551483749173,-0.032468552763394776,0.07699625346475547,0.04204344040324388
+2.12,0.6886506176110424,-0.06765946939103781,2.0351085472759376,0.02159740151741513,0.07491805441742172,0.531436855946959,0.8435020483147714,0.9055220878212734,-1.9701116107071566,0.38995519509834553,-1.1503851950258135,-0.42947440951385785,2.775865146444159,0.0,0.0,19.193792833379874,-0.028452127547503148,0.08044023019532154,0.037790228810578294
+2.14,0.7333225565961099,-0.06916539061060538,2.035807205315655,0.015770442016323546,0.06491311208907383,0.5556427554094134,0.8287331955451983,0.8197939371300909,-2.0521263179729208,0.33779366576423386,-1.172837120160253,-0.37138707449245756,2.8050360543894803,0.0,0.0,19.13162956106831,-0.024190783542597405,0.08326850714767675,0.03354679264083402
+2.16,0.7785591460672869,-0.07045971539307273,2.0364277553398624,0.009455113398653969,0.05523916230712617,0.5795594328235483,0.8130006762626171,0.7248521734931217,-2.1253210364859165,0.28326402880596124,-1.1915272336754368,-0.31101273614087294,2.830931624432916,0.0,0.0,19.078972645305395,-0.019741927269432247,0.08550693754118543,0.029306620725614692
+2.18,0.8242671090095999,-0.0715374843699232,2.036970103591316,0.002662951191561074,0.045928875443886026,0.6031179910854884,0.796324139983472,0.62175758857156,-2.188688261985791,0.22676423946696309,-1.2063515666141755,-0.24892618594647772,2.853559820776741,0.0,0.0,19.03580928443065,-0.015162799938915894,0.08717866433529964,0.025063140815918525
+2.2,0.8703531678142447,-0.07239447273265345,2.037433879028826,-0.004591213090961171,0.03701143035491061,0.6262514838903065,0.778728549438523,0.5117011637586697,-2.2413431994313067,0.1686895225808091,-1.2172558786301548,-0.18566130530854552,2.872922487988015,0.0,0.0,19.002168460443926,-0.010509700356831742,0.08830533178744482,0.02080972041734446
+2.22,0.9167240047703781,-0.07302715487359983,2.0378184273237943,-0.012289636468446377,0.028512594687542036,0.6488952774583459,0.760243984304982,0.3959858556142712,-2.282541050043142,0.10943242957742177,-1.2242348005309496,-0.12171592032350374,2.8890152605607065,0.0,0.0,18.978121810586327,-0.005837299033936927,0.08890804481387116,0.016539674887812335
+2.24,0.9632862154933962,-0.07343267905163599,2.038122813445268,-0.020412031982020445,0.020454745334950925,0.6709873952827357,0.7409054377687729,0.2760063682721834,-2.3116915403325584,0.04938284640364831,-1.2273301209377694,-0.05755546159854681,2.9018274995611977,0.0,0.0,18.963783193744366,-0.0011979674174460442,0.08900804030904019,0.01224628037898412
+2.2600000000000002,1.0099462565241257,-0.07360885080669652,2.0383458313138796,-0.028935912246382374,0.012856845475002146,0.6924688454920477,0.7207526014728023,0.15322727442134051,-2.3283703914470886,-0.011072036585020727,-1.2266282514510687,0.006384259917621288,2.911342259822887,0.0,0.0,18.959307034194502,0.0033589311097426133,0.08862700037193222,0.007922788329691233
+2.2800000000000002,1.0566103878749202,-0.07355412425127836,2.0384860188746434,-0.03783690142225711,0.005734392864123649,0.7132839308861469,0.6998296361035653,0.02915989124575047,-2.33232749873689,-0.0715477590049301,-1.222256825711033,0.06969475758739598,2.9175362877168793,0.0,0.0,18.964885599423166,0.0077878017537799105,0.08778691151007711,0.0035624376374518564
+2.3000000000000003,1.1031846110763892,-0.07326760078932705,2.0385416768089293,-0.04708900577347477,-0.0009006480734884934,0.7333805423619453,0.6781849264420526,-0.09466164865857256,-2.3234916692477423,-0.13166257877476653,-1.2143803581755919,0.1319925270224591,2.920380045606629,0.0,0.0,18.980745451754768,0.012047589169754347,0.0865093672915111,-0.0008415396677877284
+2.32,1.1495746032788137,-0.07274903522736513,2.0385108890338275,-0.056664829624198144,-0.007039920549868956,0.7527104374413166,0.6558708210991048,-0.2167046724481222,-2.301971847659963,-0.19103735308948772,-1.2031949274164044,0.19291599826646239,2.9198377549251133,0.0,0.0,19.007143383115928,0.016102457156686234,0.08481423492429863,-0.005295923491113993
+2.34,1.1956856481739717,-0.0719988496349004,2.038391543184878,-0.06653571954827828,-0.012678749783763966,0.7712295070455307,0.6329433582797288,-0.33546212832164457,-2.2680548464672348,-0.24929524578309886,-1.1889219647179292,0.2521210186899172,2.9158674456387756,0.0,0.0,19.04436217394774,0.019922223729990973,0.08271766530212832,-0.009807525571318108
+2.36,1.2414225649184374,-0.07101815567967254,2.0381813495088594,-0.07667181573065089,-0.017816215321456202,0.7888980355490557,0.6094619796599502,-0.4494761919330351,-2.222199679828662,-0.3060611974706926,-1.1718014124454625,0.3092740693869881,2.9084209959747374,0.0,0.0,19.092706490814983,0.02348237232515471,0.08022951959122576,-0.014383227828291963
+2.38,1.286689636855042,-0.06980878652051192,2.0378778570650717,-0.08704198649419173,-0.022455223816127173,0.8056809613989586,0.5854892347001456,-0.5573609157917748,-2.1650286771293366,-0.3609611074965148,-1.1520847362367506,0.3640424969477742,2.897444143097268,0.0,0.0,19.15249914409517,0.026763477790921142,0.07735040010858701,-0.019030041739526743
+2.4,1.331390542646094,-0.0683733397108963,2.0374784668927557,-0.09761362081723256,-0.026602589171738817,0.8215481479872931,0.5610904773910871,-0.6578235752185962,-2.0973156159729425,-0.41362070198808965,-1.1300284764737687,0.416081260623968,2.882876443533087,0.0,0.0,19.224077773921024,0.029749959526980008,0.07406858252655485,-0.023755189400776595
+2.42,1.3754282934547124,-0.06671523298087743,2.0369804428778946,-0.1083522548647662,-0.03026913019405473,0.8364746766856249,0.5363335565547763,-0.7496844304286061,-2.019971160136488,-0.4636641022735429,-1.105889137834062,0.4650160467098874,2.8646511623688315,0.0,0.0,19.30779183631702,0.03242820537587976,0.07035721371051867,-0.028566203908037213
+2.44,1.418705181046171,-0.06483877525266517,2.0363809224488802,-0.11922101218968466,-0.03346979685377036,0.8504411756105975,0.5112884995548983,-0.8318946910458271,-1.9340259135651021,-0.5107121630450934,-1.0799201679170152,0.5104230378486737,2.8426950734247685,0.0,0.0,19.403999537077862,0.034784286969907985,0.06617212673268302,-0.033471042753333055
+2.46,1.4611227431386187,-0.06274925582018379,2.0356769309193212,-0.13017984262467613,-0.03622383382627942,0.8634341984422005,0.48602718792019795,-0.9035525257305348,-1.8406114085232688,-0.5543807179651554,-1.0523715071508861,0.5518060492195194,2.8169281592925777,0.0,0.0,19.51306411034494,0.03680168420182195,0.06145050526948581,-0.0384782037189768
+2.48,1.5025817540272544,-0.060453055273018315,2.0348654052118897,-0.14118455037494523,-0.03855498314714198,0.875446667269548,0.4606230224126769,-0.9639169955829312,-1.740939341268325,-0.5942789468687285,-1.0234916734863286,0.5885720280736993,2.7872632101525876,0.0,0.0,19.635348554536463,0.03845961441521828,0.05611040149066865,-0.04359682928055733
+2.5,1.5429822504694695,-0.05795778242017516,2.033943234742308,-0.15218560505258524,-0.04049171591712979,0.8864783920519741,0.43515057506777044,-1.0124197996011826,-1.6362793634150659,-0.6300081552889407,-0.9935315956372213,0.6200059269582894,2.753605332575546,0.0,0.0,19.771207618671774,0.0397326718891049,0.050050804968409746,-0.048836783732026406
+2.52,1.5822236051149081,-0.055272442070195014,2.032907329315154,-0.1631267276519154,-0.04206746575625372,0.896536677200239,0.40968522732114543,-1.0486747044040283,-1.5279357455991855,-0.6611613239549987,-0.9627485089651028,0.6452456099313841,2.7158513922144594,0.0,0.0,19.920975478653006,0.04059247288397316,0.04315164009322303,-0.05420868787212786
+2.54,1.6202046625117603,-0.05240763889095527,2.0317547259198325,-0.1739432344763822,-0.04332081455782869,0.9056370235372369,0.38430279702931486,-1.0724844971116323,-1.417223265458484,-0.6873238345367491,-0.9314073293372066,0.6632567196459073,2.673889425118076,0.0,0.0,20.08494710258195,0.041011830164380036,0.03527283639490586,-0.05972390034652485
+2.56,1.6568239561144287,-0.04937582246761754,2.030482748329158,-0.1845601049734384,-0.04429555532605722,0.9138039319245774,0.35907916317955824,-1.0838452645842729,-1.3054427421878354,-0.7080758051444799,-0.8997762315113679,0.6728064279987355,2.6275980585043515,0.0,0.0,20.263350585570322,0.04097164747019244,0.02625160811044963,-0.06539444545996057
+2.58,1.6919800290593854,-0.0461915777747044,2.0290892354794248,-0.1948897161561863,-0.045040529597844145,0.9210718129297238,0.3340899050502105,-1.0829477812249588,-1.1938567423918194,-0.722996482128791,-0.8681128983186813,0.6724339905028098,2.5768459787927047,0.0,0.0,20.456306484631877,0.04047030452643963,0.01589745868772119,-0.07123290795233098
+2.6,1.7255718871952228,-0.04287196316195447,2.0275728568444698,-0.20482916250995453,-0.04560911034359113,0.927486003383553,0.3094099816856925,-1.0701757934934335,-1.0836661003320691,-0.7316711525424959,-0.8366382037481279,0.6604155041039053,2.5214914661554895,0.0,0.0,20.66376856918616,0.03953496855582328,0.003985297148613065,-0.07725234259912853
+2.62,1.7574996204723863,-0.039436894114472563,2.0259335354345844,-0.21425706605865022,-0.04605817624255098,0.9331038815396693,0.2851134867044212,-1.0461010211655115,-0.9759880008431984,-0.7337010874471241,-0.805494858450137,0.6347208440108438,2.4613819777484385,0.0,0.0,20.88543910810273,0.038234265994488315,-0.009751726444523878,-0.08346626714880817
+2.64,1.7876652387921972,-0.035909564862266125,2.0241730014648778,-0.22302978879836127,-0.04644639811423322,0.9379960512529079,0.2612735218975027,-1.0114747338749424,-0.8718364303112488,-0.7287171127182996,-0.774689370289588,0.5929628670960474,2.3963537138657522,0.0,0.0,21.12065231025711,0.03669209483956615,-0.025626490723679896,-0.08988878370403264
+2.66,1.8159737809321332,-0.032316890601445866,2.0222955017587974,-0.23097700436317883,-0.046831628816402085,0.9422475227144771,0.2379622405682166,-0.9672157759403388,-0.7721057822703622,-0.7163975078311863,-0.7440161274580801,0.5323430756530615,2.3262310797072683,0.0,0.0,21.368210799553516,0.03510341728199551,-0.04399542696503944,-0.0965349568686509
+2.68,1.8423347706935134,-0.028689939724392596,2.0203086883027144,-0.23789667101284762,-0.04726714713302481,0.9459587442767757,0.2152511204366089,-0.9143948726643335,-0.677558296007204,-0.6964911103788661,-0.7129615782570472,0.44960291129813906,2.2508258809590833,0.0,0.0,21.626154579105105,0.033752630068603295,-0.0652428608445556,-0.10342149596936245
+2.7,1.8666641127580135,-0.025064306657099363,2.018224701092986,-0.2435496074983671,-0.04779645813968536,0.9492462219960872,0.19321153511850062,-0.8542149759367336,-0.5888158268536412,-0.6688466268235318,-0.680586139081188,0.3409982115464734,2.1699360991166223,0.0,0.0,21.891435117424663,0.03303300962143774,-0.0897501044899478,-0.11056761388451725
+2.72,1.8888865401889312,-0.02148034918041821,2.0160614385983253,-0.24765406159793799,-0.048446348605075434,0.9522423077488523,0.17191568984249364,-0.7879873204819149,-0.5063561951738651,-0.6334491878535405,-0.6453842473274283,0.2023171501355999,2.0833442664254,0.0,0.0,22.159438320080397,0.03346353870062169,-0.11784198205936586,-0.11799607249426292
+2.74,1.9089387450909643,-0.017983178942515835,2.0138439637784833,-0.249880945047328,-0.04921795314534058,0.9550935289929737,0.15143796507542254,-0.7171030203708124,-0.4305140082294483,-0.5904651412106647,-0.605128787381582,0.028971510287616242,1.990815572611395,0.0,0.0,22.42332385133251,0.035692228586269525,-0.14969931045884705,-0.12573380367279904
+2.7600000000000002,1.926773334376618,-0.0146222502173627,2.011605920465755,-0.2498508564748846,-0.05007580133625938,0.9579565698397288,0.13185664162338928,-0.6430005362928132,-0.36148541716238375,-0.5402954442289131,-0.5567219368731042,-0.1837940389141244,1.892096491089437,0.0,0.0,22.67303498430932,0.04047669093924555,-0.18521391728429654,-0.13381347354442244
+2.7800000000000002,1.9423637518003736,-0.011450345963304064,2.009390703036828,-0.24713477040817936,-0.050935170817784536,0.96099069552389,0.11325589055572129,-0.5671303717482913,-0.2993357586532729,-0.48363808084334664,-0.4960882356892689,-0.44049992145139694,1.7869138942681622,0.0,0.0,22.894141314822633,0.04860195880597562,-0.22376925947361143,-0.14227268838628163
+2.8000000000000003,1.9557102425260775,-0.008521719051075724,2.0072519666577193,-0.24126133288261112,-0.05164901510231737,0.9643451465475594,0.09572767011074974,-0.4909199360351296,-0.24400882610976565,-0.4215565531383624,-0.41820159090834064,-0.7447409047877825,1.6749774653788683,0.0,0.0,23.06653654415096,0.0607396075915867,-0.26390612487007187,-0.1511509532879647
+2.82,1.966846788832946,-0.005889147534336558,2.005252846830219,-0.23173529247590458,-0.05199670179295619,0.9681399809983968,0.07937300810845699,-0.41574403644056446,-0.1953364711488981,-0.35554886721281403,-0.3173329552131142,-1.0980605235337122,1.555988227566219,0.0,0.0,23.162939769642296,0.07720623459431342,-0.3028608290746234,-0.16048769828038048
+2.84,1.975848676575367,-0.003599732370921817,2.003462966236383,-0.21807352472478242,-0.0516784918296396,0.9724394720046124,0.06430197962515714,-0.3429092497254102,-0.15304889688648352,-0.28760868912152243,-0.1876617684562123,-1.4982313526308793,1.4296532618046354,0.0,0.0,23.148111049324456,0.09752359929592203,-0.3358574514479608,-0.1703123780099518
+2.86,1.9828398434899595,-0.001689466023065938,2.001952157123667,-0.1998686186198475,-0.05032269069991764,0.9772188182596308,0.050631446068277915,-0.27366307148866587,-0.1167888992376677,-0.2202584377409787,-0.024516818246322514,-1.9366462664314896,1.2957122702276525,0.0,0.0,22.979269407220148,0.11974865306009175,-0.35512684180454523,-0.18063380579475993
+2.88,1.9879984512661804,-0.000176997630954233,2.0007798286621004,-0.17689344687262085,-0.047515938234874044,0.9823289921347144,0.038479803517936485,-0.20923716138338783,-0.08613646136639982,-0.15652824525350423,0.17347083228802476,-2.3945358647151282,1.1539723171205765,0.0,0.0,22.608693036182355,0.13974047895327435,-0.3487143078379126,-0.19144140324563055
+2.9,1.9915582813048527,0.0009423598068465961,1.9999793761366165,-0.14926135293089407,-0.04286803878246876,0.9874723586012881,0.027959269850856613,-0.15092503994538836,-0.060652700323925876,-0.0998462012210484,0.40130392211979793,-2.8379039915789552,1.0043302873459765,0.0,0.0,21.988919941787596,0.15057644754080624,-0.2991296490763341,-0.20275104135140887
+2.92,1.9938029324094697,0.0017006506415428784,1.9995383193344471,-0.11765414830264281,-0.03611955448603639,0.9922124046840809,0.019168285315690866,-0.10017206909381265,-0.03995089743831416,-0.05380518761364229,0.6439926558164626,-3.2113040929325414,0.8467346418796958,0.0,0.0,21.08165906339926,0.14217669649136666,-0.18167397130453686,-0.2147073846613809
+2.94,1.9950499032144968,0.0021571817595063816,1.9993779328305024,-0.08362474657997361,-0.027295975146196978,0.9960488453752359,0.012187252640068142,-0.05861273093353446,-0.023788716168320602,-0.021754910915748445,0.8726262997845966,-3.4306215962395155,0.6810684018114969,0.0,0.0,19.896291716409852,0.09893373389159876,0.03625064896362434,-0.22731449766016648
+2.96,1.9956227048422253,0.0023923716568491006,1.9993476381324697,-0.049945273015515115,-0.01694575966044795,0.998583155409602,0.0070705491398316235,-0.027923409720602862,-0.012144652355120675,-0.0058335605874832545,1.036327449373649,-3.377902944013586,0.5074670562903593,0.0,0.0,18.697940813302925,-0.013445577183156976,0.3767669620700846,-0.23742969144444365
+2.98,1.995813809595637,0.0024954545923018562,1.9992888173063397,-0.020731110965176533,-0.006639047375934434,0.9997558249366919,0.003799288518152399,-0.009264725753505725,-0.005178421440607834,-0.003729169208359941,1.035683468223422,-2.9218631763938996,0.32951662703800544,0.0,0.0,18.174161213549525,-0.23421966791604976,0.8466715146216237,-0.23639221462242882
+3.0,1.9958599205642589,0.0025512469418962733,1.999172728215246,-0.0016837428434832613,0.0006766879495327952,0.9999958101658875,0.002255383365704436,-0.0021580363748036476,-0.0027684259490047174,-0.005871472287899877,0.7356415296752087,-1.9031476159670109,0.15697564448800388,0.0,0.0,18.74430531537498,-0.560702769518466,1.5532105764106605,-0.22064962988222186
+3.02,1.9959231417371093,0.00262408184291302,1.9991808698905666,-0.0016439659322199573,0.0006824797624709441,0.999995879951269,0.0022520386042146467,-0.0031459583496519818,-0.003655409767583661,0.0004015488137856958,0.0005875821915860557,-0.003976153599771104,0.00033815733496064815,0.0,0.0,19.327437243861905,1.440540937842178,0.5846662049315385,-0.22158869647802074
+3.04,1.9959815456998506,0.002678154449175779,1.9993840091159523,-0.0083428248483166,0.019561133810259392,0.9997661909612813,0.003914425676667652,-0.0030055847756676407,-0.0025160979050515434,0.010180867285972468,1.886765058006247,0.6741043838848578,-0.16365436845735964,0.0,0.0,18.807736917421305,0.7551966654377452,0.37116774289734855,-0.23387940372630878
+3.06,1.9959798551528702,0.0028758184391177903,1.9996167047115874,-0.019197562915907913,0.04818010839963193,0.998627177680022,0.007341030685345519,-0.0001375673719190727,-0.009074130705905039,0.01227847445535776,2.8625193142116574,1.0908428348824561,-0.3404369302919102,0.0,0.0,19.213212686827635,0.3090457190372755,0.17663951348177087,-0.23390793239783206
+3.08,1.9958319157881081,0.0034550769686195037,1.999875910608885,-0.03201225817284407,0.08076446312825356,0.9961399162833942,0.012553246958157944,0.0071888130936963186,-0.026889601028777296,0.01694810945558025,3.2672305217668467,1.283562403591262,-0.5216287961607138,0.0,0.0,20.234383013059677,0.005848033573991884,-0.006467591766404435,-0.22299683827954345
+3.1,1.995429900734949,0.004713477805495539,2.0002748133955164,-0.04460094766435352,0.11335772320820921,0.9923605410450252,0.019527893724548773,0.019994917670618256,-0.05709624223956828,0.033211639016691163,3.2844959461542578,1.2509606670595705,-0.696999369169689,0.0,0.0,21.161813433225582,-0.17291482960984483,-0.1180138973765911,-0.2108914446031266
+3.12,1.9946687470207853,0.006967820391286942,2.0009105061173553,-0.05554500055626823,0.14353152182704015,0.9876820791475642,0.028241205462277263,0.038367269803040754,-0.09858609237738236,0.06307282603830486,3.065429800085171,1.0672626236526237,-0.8634543575665186,0.0,0.0,21.836833973269844,-0.26561732826590556,-0.16991989973350818,-0.19951289537760528
+3.14,1.9934658906829748,0.010517110775098894,2.001826051453918,-0.06421282751295937,0.16998758584196963,0.9825917477905123,0.03865734880195791,0.062072813429632806,-0.14931286555767745,0.10521487735905374,2.7198700946981997,0.809713341927912,-1.0207746814418273,0.0,0.0,22.27637964534187,-0.30412730270577804,-0.1847957899986799,-0.18872558509754225
+3.16,1.991769581882639,0.015622225856459011,2.0030205310069906,-0.07048585805944847,0.19215942156914279,0.9775142411954016,0.050718919310390305,0.09100430809286907,-0.20717698374257007,0.15725638659398591,2.3183698067337195,0.5297276214631967,-1.1691872023916328,0.0,0.0,22.520120057857582,-0.3093468615953483,-0.17841211228669807,-0.1784266675752959
+3.18,1.9895596656957741,0.022496855174831503,2.004464260620135,-0.07453990642392112,0.2099347055990227,0.9727438994725762,0.06434693290477182,0.1252866619325582,-0.27036133504488075,0.21634126639746457,1.9048941880140982,0.25821564021490745,-1.3090821490439264,0.0,0.0,22.61087122580693,-0.2948860766823249,-0.1611748327007823,-0.16857054259876475
+3.2,1.986843991509064,0.03130588538293424,2.006112025785828,-0.07669493917845387,0.2234693712349993,0.9684356905343707,0.07944582886132974,0.16522170211104317,-0.3373823039175164,0.2795146815529396,1.5058096690881657,0.011856898419976392,-1.4409165582330585,0.0,0.0,22.58789436630217,-0.269522084605756,-0.13971169838784891,-0.1591269441491662
+3.22,1.9836528687930863,0.04216814067947756,2.0079128663335757,-0.07731872275262583,0.23306705230923494,0.9646257471330021,0.09591002141530491,0.2111816872638898,-0.4070089059922761,0.34398124816687803,1.1360021659054926,-0.20205767042107056,-1.5651485043882387,0.0,0.0,22.484112952216794,-0.23882394263148954,-0.11801595429754003,-0.15006751529785506
+3.24,1.9800331441885293,0.05516123640931081,2.0098165153709178,-0.07676937374820711,0.2391034883516549,0.9612617845143547,0.11362995531397627,0.26351114827313743,-0.4781463298821981,0.4072569050902038,0.8030185774314127,-0.38210275231771007,-1.682199261176238,0.0,0.0,22.325581714425343,-0.20625787269320783,-0.09828927445873631,-0.14136471381211008
+3.2600000000000002,1.9760427537877436,0.07032712948779449,2.0117771485646156,-0.07536439533121587,0.241980470743774,0.9582328947345737,0.13249671370860872,0.32245929881394764,-0.5497347585113429,0.46724084764627105,0.5098107160143414,-0.530255420354942,-1.7924388498405148,0.0,0.0,22.132133090729177,-0.1739268072942699,-0.08155641133243119,-0.13299314662754472
+3.2800000000000002,1.9717461114478252,0.08767758748647567,2.0137552035818747,-0.07336686743416897,0.24209850941790098,0.9553943397199797,0.15240495441930887,0.38814358111494857,-0.6206827207704314,0.5222358338846421,0.25651964307640884,-0.650237928001396,-1.8961862368985232,0.0,0.0,21.918471930351807,-0.14305642470994662,-0.06810170691327727,-0.12493030099847996
+3.3000000000000003,1.9672104105563137,0.10719921422729232,2.0157179347504806,-0.07098222068217513,0.2398411036331386,0.9525863199103063,0.17325435773683864,0.46053606776658956,-0.6898364516801833,0.5709380651573437,0.04162235406210982,-0.7464475713664905,-1.9937163905554485,0.0,0.0,21.695301414424353,-0.11431181324191786,-0.05777049741637169,-0.11715649217608667
+3.3200000000000003,1.9625027792617633,0.1288579055088448,2.0176392095847273,-0.06836117288608486,0.2355661363151283,0.9496472320609552,0.19494994255492187,0.5394632382374756,-0.7559788043947607,0.6124098794027458,-0.13733680449982835,-0.8232993260110596,-2.0852692085670657,0.0,0.0,21.470296140669962,-0.08800336396472952,-0.05017165665169704,-0.10965443183528736
+3.34,1.9576881767385708,0.15260272526800375,2.01949889957862,-0.06560594681153521,0.22960164227738794,0.9464224606297871,0.21740163665885187,0.6246117962698875,-0.8178489809899752,0.6460429034148,-0.28328262896035955,-0.8848502663180438,-2.1710577846906243,0.0,0.0,21.248874301159198,-0.06422077665942072,-0.044809894584818655,-0.10240872446097843
+3.36,1.9528279081560145,0.1783692411708011,2.0212820935410747,-0.06277793825957134,0.2222442795822247,0.9427697944228413,0.24052344040606294,0.715536120289887,-0.8741748161718257,0.6715161656436542,-0.3993179671610074,-0.9346111336431692,-2.2512750519724984,0.0,0.0,21.03478378207714,-0.04292042834901744,-0.04116923666054462,-0.09540543237909892
+3.38,1.9479786441004445,0.20608237323696615,2.022978271726067,-0.05990570020212639,0.21375948306963605,0.9385624139176588,0.2642324462706123,0.8116654421264665,-0.9237108838151624,0.6887521159959775,-0.4885437988280906,-0.9754785134084792,-2.3260986772369825,0.0,0.0,20.830537809672165,-0.023982347034732676,-0.03876315554933368,-0.08863174544137316
+3.4,1.9431918439277052,0.23565880907100462,2.02458051807948,-0.05699256478076409,0.2043826624284112,0.933690198100309,0.2884478962131464,0.9123105450293898,-0.965277453234474,0.6978727412679588,-0.5539513025920698,-1.009741729356815,-2.395694448763764,0.0,0.0,20.637736769872152,-0.007247196859576421,-0.037162272933441765,-0.08207574263215871
+3.42,1.9385134966081814,0.2670090344517553,2.0260848088390215,-0.054023522302810816,0.19432103239813558,0.9280599142232138,0.3130903879356908,1.016670678845864,-0.9977969192515189,0.6991575135287423,-0.5983607041120749,-1.0391332846860113,-2.4602185078627197,0.0,0.0,20.457306607747046,0.00746004994218552,-0.03600722312544152,-0.075726216565426
+3.44,1.9339841053176092,0.30003902274156274,2.0274893918634724,-0.05097117011250198,0.18375579747588708,0.9215947002009933,0.33808128500821255,1.1238416863448335,-1.0203256266781167,0.6930045300182734,-0.6243904845716595,-1.0649019837007743,-2.5198187581225486,0.0,0.0,20.289676730876607,0.02031542936070463,-0.035011903473863515,-0.0695725318824224
+3.46,1.9296388531021778,0.33465162227201534,2.0287942572925717,-0.04780067009874683,0.1728444980271653,0.9142331297952239,0.3633423452127172,1.2328262312649523,-1.0320799830557281,0.6798958206074381,-0.6344460592119701,-1.0878949963577054,-2.5746357199287955,0.0,0.0,20.134913914879405,0.03148742547092098,-0.03396067804289715,-0.06360449617487732
+3.48,1.9255078966905341,0.3707476776921883,2.03000069278996,-0.04447373441569424,0.16172338159481459,0.9059280609250921,0.38879555452855,1.3425467021460367,-1.0324564361059325,0.6603674135739402,-0.6307205763203861,-1.1086400230028637,-2.624803022884201,0.0,0.0,19.992823909901325,0.0411325695399285,-0.03270195467918324,-0.05781223245798471
+3.5,1.921616744269279,0.4082269188909698,2.031110913236396,-0.04095170931404832,0.15050970448007164,0.8966454014392207,0.4143631383695682,1.4518609772094013,-1.0210453397802322,0.6349843905643365,-0.6152028340302363,-1.127422064909922,-2.6704476642198296,0.0,0.0,19.863029131063442,0.04939379380854092,-0.031139776742982572,-0.052186052435846286
+3.52,1.9179866810018977,0.4469886491091476,2.0321277537309963,-0.037197856275395715,0.13930390026133657,0.8863628789974183,0.4399677142653794,1.5595808678214298,-0.9976390050887078,0.6043208561432832,-0.589688946196925,-1.1443516009815113,-2.7116901141456657,0.0,0.0,19.745028624909416,0.05640049640644207,-0.029224552997017872,-0.04671633781192147
+3.54,1.9146352133437947,0.4869322619075963,2.0330544151375483,-0.033178944207022214,0.1281915764653655,0.8750688678425546,0.46553254871961586,1.664492764117775,-0.9622343834606044,0.5689445147680575,-0.5557955855995609,-1.1594225604130486,-2.7486443195255426,0.0,0.0,19.63824506668786,0.06226952658672236,-0.02694371150041608,-0.041393441433727814
+3.56,1.9115765097858126,0.5279576145554308,2.033894252585797,-0.028866270618187237,0.11724531918946934,0.862761301672353,0.4909818834594452,1.7653798080361782,-0.9150309049010494,0.529405393601132,-0.5149735393365373,-1.1725596040102584,-2.781417641920885,0.0,0.0,19.542062544456734,0.0671065006969418,-0.024312842920674926,-0.0362076206848458
+3.58,1.908821821498421,0.5699652830664655,2.0346505989464783,-0.02423622653728094,0.10652629862745051,0.84944668581596,0.5162413011182373,1.861044816909337,-0.8564240219547795,0.4862281670357889,-0.4685209936019384,-1.1836550382690405,-2.8101107601750286,0.0,0.0,19.455858085399242,0.07100704341353303,-0.02136774647025902,-0.031149012635896513
+3.6,1.9063798713963636,0.6128567215494618,2.0353266171135402,-0.01927051168164826,0.09608567677479714,0.8351392113479015,0.541238106351966,1.9503331623562152,-0.7869950133270806,0.4399075143425701,-0.4175964409679598,-1.192596277713374,-2.8348175665501802,0.0,0.0,19.379029121975854,0.07405773236495372,-0.01815767012865782,-0.026207655090571873
+3.62,1.904257204364968,0.6565343458039649,2.0359251767718374,-0.013956094689883868,0.08596582304772328,0.819859966971954,0.565901704579056,2.0321548550342037,-0.7074975888501651,0.390905958566629,-0.3632313675160737,-1.1992851863431295,-2.8556250851044553,0.0,0.0,19.311018314299915,0.07633670757307942,-0.014739918433709087,-0.021373551233223848
+3.64,1.902458494778999,0.7009015582705953,2.0364487530445445,-0.008284999245984564,0.07620134554704516,0.8036362402236945,0.5901639663038046,2.1055051741484787,-0.618841815516928,0.33965367671191343,-0.3063429234139347,-1.2036508786162818,-2.872613438944684,0.0,0.0,19.251336355683577,0.07791407491248525,-0.011175868571130065,-0.016636769558342167
+3.66,1.9009868100267426,0.7458627286244086,2.0368993458512588,-0.002253981537880646,0.06681994692355481,0.7865008968919915,0.6139595698809924,2.1694832908012907,-0.5220758552753012,0.28654982579758903,-0.24774662250234766,-1.2056576335617115,-2.885855888052627,0.0,0.0,19.199582664455008,0.07885235801218116,-0.007528283468320129,-0.011987566359682117
+3.68,1.8998438305770955,0.7913231415776745,2.037278419837563,0.004135851547693326,0.05784311594134308,0.7684918260174878,0.637226319304593,2.2233084491450574,-0.4183659720801786,0.2319649884344935,-0.18816880358012095,-1.2053094553305599,-2.895418951646008,0.0,0.0,19.155463305190562,0.07920730426319311,-0.0038596484487630756,-0.007416516100638813
+3.7,1.8990300282730441,0.8371889209285336,2.03758686527795,0.010879449955633735,0.04928667018631298,0.7496514371721963,0.6599054359953925,2.266333376934042,-0.3089752328162702,0.1762444009416986,-0.12825820973015387,-1.202652493271163,-2.9013626195556754,0.0,0.0,19.118805173494103,0.07902930213767034,-0.00023111066746855924,-0.0029146358362950424
+3.72,1.8985448051057243,0.8833669366423325,2.0378249803914854,0.0179682348049673,0.04116117246089278,0.7300261969760681,0.68194182460698,2.298054691715828,-0.19524130015836968,0.11971168509854987,-0.06859572839324933,-1.197776036589294,-2.9037406475644296,0.0,0.0,19.08956546678048,0.07836550982252832,0.0032984944674357974,0.0015265066546902377
+3.74,1.898386594853773,0.929764699856125,2.037992475103766,0.025390512680099474,0.03347225272887502,0.7096661931041375,0.7032843127272049,2.3181201511473297,-0.0785537013227894,0.06267286086503973,-0.00970122355304883,-1.190812188074706,-2.9026009239877393,0.0,0.0,19.06783573514644,0.0772625460054337,0.006674046698618374,0.005914706051615096
+3.7600000000000002,1.8985529298268984,0.9762902492026247,2.038088495566502,0.03313189098092105,0.026220877372361822,0.6886247165064557,0.723885863383348,2.326332671619155,0.03966904130012888,0.0054204710922715615,0.04796339802453699,-1.1819337113027217,-2.897985890617862,0.0,0.0,19.053840277427597,0.07576930006311355,0.009846365350763886,0.010257053417558285
+3.7800000000000002,1.8990404747163836,1.0228520307746611,2.0381116678741047,0.041175697952036565,0.01940361537456072,0.6669578561707327,0.7437037580251286,2.322651117338548,0.15800201475642076,-0.051762301204859466,0.10399515235074205,-1.1713490862683178,-2.8899330011771336,0.0,0.0,19.047929169199875,0.07393915750404167,0.012775385413633433,0.014559886634473183
+3.8000000000000003,1.8998450294178837,1.0693587733785936,2.0380601585616778,0.049503407213515684,0.013012953748026268,0.644724105205,0.7626997468706914,2.3071879505836606,0.27503582885028655,-0.10859759966156335,0.15805430605423945,-1.1592936581217463,-2.878475203703425,0.0,0.0,19.05056659926992,0.07183080203512737,0.015434048861225236,0.01882875367381997
+3.8200000000000003,1.9009615028160656,1.1157193604138311,2.0379317487569004,0.0580950567222355,0.007037709292112724,0.6219839816922,0.7808401638944278,2.280203936919618,0.38939739595859807,-0.16480898948933556,0.2098702796633018,-1.146016046367364,-2.863641437025039,0.0,0.0,19.062315301721164,0.06950683761703158,0.017812271210164266,0.023068391155451055
+3.84,1.9023838600056004,1.1618426997165798,2.03772391835241,0.06692963784480181,0.0014635690196200797,0.5987996717933837,0.798096005881388,2.2421002129059944,0.49976914607640155,-0.22011770559212618,0.25924631900021206,-1.1317597385850164,-2.8454571315822856,0.0,0.0,19.083817687877776,0.0670298200459789,0.01992000699311225,0.027282731353225752
+3.86,1.9041050462714493,1.2076375929825862,2.0374339362790956,0.07598541506250288,-0.0037262322208520848,0.5752347049597105,0.8144429790360589,2.193408135060934,0.6049063007283525,-0.2742390765187318,0.3060595468523597,-1.1167409429259791,-2.8239446976071476,0.0,0.0,19.115773999450678,0.06445586133338155,0.021788285297301258,0.03147496183148642
+3.88,1.9061168922550897,1.253012606921488,2.0370589529096903,0.0852401254175007,-0.008550118571865127,0.5513536709767297,0.829861522178068,2.1347774164522675,0.7036519372870653,-0.3268791622434826,0.350253164472006,-1.1011251012545136,-2.7991239682448654,0.0,0.0,19.158917767527115,0.0618266265489934,0.02346721487062753,0.035647666405760677
+3.9,1.908410005888542,1.297875949089256,2.036596090878483,0.09467100364059668,-0.013027318066665594,0.5272219853904858,0.8443368215200534,2.0669630968020454,0.7949497309273641,-0.3777316756600955,0.3918185782770094,-1.085005640818781,-2.7710125457010393,0.0,0.0,19.213989318387565,0.05916105928160631,0.02502040896612323,0.039803068034495126
+3.92,1.9109736576263594,1.3421353523658732,2.036042531292643,0.10425458628028908,-0.017177027549654684,0.5029057037102196,0.8578588369538505,1.9908118628991451,0.8778544431435137,-0.4264753016578866,0.43076690258685996,-1.0683891877303158,-2.739625983611573,0.0,0.0,19.281708807845575,0.05644834965056056,0.02651600988537972,0.04394337307499676
+3.94,1.91379566597858,1.385697973214388,2.0353955935093064,0.11396626463388129,-0.021017698728077572,0.47847137641281956,0.8704223623156736,1.907248130000698,0.951540399011849,-0.4727715758837632,0.46709119922519626,-1.0511912473364549,-2.704977737869871,0.0,0.0,19.36275052757119,0.05364344764840956,0.028015364869231063,0.048071189280270016
+3.96,1.9168622900918055,1.4284703100721694,2.0346528082333415,0.12377957698559024,-0.02456655126126312,0.4539859277079347,0.8820271414891732,1.8172601219814053,1.0153083225165402,-0.516263527458002,0.5007224665929652,-1.0332450976335055,-2.6670788355163464,0.0,0.0,19.457719185974,0.050665963728325436,0.029561209140215206,0.05218996753280114
+3.98,1.9201581349177939,1.4703581494457263,2.033811985272804,0.13366524991898082,-0.02783940450814913,0.4295165334352559,0.8926780584435179,1.7218859793251111,1.0685909503371345,-0.5565753155653704,0.5314834837679357,-1.0143244115072858,-2.6259372441649,0.0,0.0,19.567126518427187,0.047402800720109495,0.031167727315579152,0.05630440585911954
+4.0,1.923666072203913,1.5112665486215553,2.032871278410157,0.14359001313246791,-0.03085088285044722,0.4051304698780498,0.9023854129651899,1.6221997275833304,1.1109578045055062,-0.593313092193709,0.5590451297952006,-0.9941772617732566,-2.5815569658464437,0.0,0.0,19.691364172947516,0.04371441132941234,0.03281494484365832,0.060420753610960054
+4.0200000000000005,1.9273671771865055,1.551099865633995,2.031829250284301,0.15351522251281952,-0.03361499034823993,0.38089490801149656,0.9111652857366993,1.5192968092256063,1.142119374464721,-0.6260673028026873,0.5828898112337244,-0.9725661975312462,-2.533936922248691,0.0,0.0,19.830668228603397,0.039444033065552886,0.03444953344168855,0.06454696301701805
+4.04,1.931240676861849,1.5897618495192605,2.030684940168547,0.16339533285900149,-0.03614597401672837,0.3568766382334992,0.9190399876204703,1.414278866163851,1.1619307553793865,-0.6544166001196219,0.6022860016055731,-0.9493065626206572,-2.4830697362174403,0.0,0.0,19.985073246263394,0.034428384957936764,0.03599337012838921,0.0686926556540094
+4.0600000000000005,1.9352639018825224,1.62715580698309,2.029437937732323,0.1731762607288766,-0.03845931204743788,0.33314172895808286,0.9260385804401571,1.3082375854300463,1.1703945443219765,-0.6779334972227959,0.6162761424222124,-0.9242936184774532,-2.4289405385103615,0.0,0.0,20.154359287794982,0.02850708857172579,0.037360069926561934,0.07286889729141169
+4.08,1.9394122316937894,1.6631848650876488,2.0280884671735904,0.18279365912474177,-0.040572589371639295,0.3097551464719248,0.9321974535524243,1.2022376833279897,1.1676625621062922,-0.696191813969687,0.6236767353018539,-0.8975087874962373,-2.371525934606444,0.0,0.0,20.338001906510492,0.021526922927203707,0.038478059142033394,0.07708778861183205
+4.1,1.9436590228540702,1.6977523527823386,2.0266374894144765,0.19217107732129882,-0.042505977153074734,0.28678038808846656,0.9375609462617681,1.097299463942992,1.1540358196386222,-0.7087758577332737,0.623084388948623,-0.868997102218315,-2.310793263272562,0.0,0.0,20.53513888091909,0.013336834052587232,0.03931645895906488,0.08136187007546389
+4.12,1.947975514722397,1.730762326513846,2.0250868360446277,0.2012178987098018,-0.04428203585321441,0.2642791973587515,0.9421820229567749,0.9943817636192345,1.129962161566063,-0.7152911374479216,0.6128761050243834,-0.8388125432602376,-2.2467002852393323,0.0,0.0,20.74456379431066,0.003771432144474285,0.0399075459911471,0.08570333587195349
+4.14,1.9523307152885219,1.7621202669435327,2.023439397359248,0.2098268556697627,-0.04592463018194126,0.24231142837082523,0.9461230314622316,0.8943663597251496,1.0960312395494798,-0.7153763248101771,0.5911888483689683,-0.8069356747732502,-2.1791954520356502,0.0,0.0,20.96474523854291,-0.007374865767768357,0.04035854803268524,0.09012309763841964
+4.16,1.9566912820954347,1.7917339762586602,2.021699397242113,0.2178708567394381,-0.04745687462357882,0.22093509914083026,0.94945659026741,0.7980449388686985,1.0529668493876383,-0.7087162798322166,0.5558659922486013,-0.7731770212960062,-2.1082188811132885,0.0,0.0,21.193851149826457,-0.020375340205063708,0.040847552112474594,0.0946299707750386
+4.18,1.9610214269420398,1.8195147111235936,2.019872796986673,0.225198878684731,-0.0488981882196875,0.20020662584160803,0.9522666324076939,0.7061094011260808,1.001617049395746,-0.6950563886604456,0.5043681838126995,-0.7370862437338086,-2.033703952730188,0.0,0.0,21.429753821895602,-0.03558776131897512,0.04160578868654241,0.09923037548737285
+4.2,1.9652828854031505,1.8453785962942941,2.0179678764189197,0.2316308086367001,-0.050260642509154546,0.18018118314585074,0.9546495574529099,0.6191456687746498,0.9429426367885216,-0.6742190574511671,0.43366251806835854,-0.6978844605522705,-1.9555791258825144,0.0,0.0,21.669982096539798,-0.053449747992589614,0.04289794779221745,0.10392915986511131
+4.22,1.9694350011791537,1.8692483791741086,2.015996038137356,0.2369513379344722,-0.05154475235207299,0.16091312589648607,0.9567153013672608,0.537630523386323,0.8780043226648603,-0.6461238462777087,0.34011834945635844,-0.6544226489542391,-1.8737690645775853,0.0,0.0,21.91159847221541,-0.07444784251423564,0.04501318697905832,0.10873161482663421
+4.24,1.973434982546131,1.8910556040814401,2.013972866541247,0.24090326118690578,-0.05273470315071019,0.14245644176917352,0.9585879365147394,0.46193066448882547,0.8079484022375195,-0.6108129161982507,0.21944502302219004,-0.6051536240070969,-1.7881940638780611,0.0,0.0,22.150936448019994,-0.09906216905998555,0.04827406704976667,0.11364643528756103
+4.26,1.9772383979385042,1.9107433080986653,2.011919434515313,0.24318077155559895,-0.053792812982863016,0.12486526906007789,0.960405180223206,0.3923032852568968,0.7339902092030046,-0.5684835570508692,0.06670496680741914,-0.5480967871253712,-1.698766909722145,0.0,0.0,22.383132089906812,-0.127685271536504,0.05304558630080247,0.11868822446438534
+4.28,1.9807999966082106,1.9282693644917794,2.0098637835171926,0.24342366718960065,-0.054653085845848604,0.10819452326681382,0.9623159063397839,0.32889789507821277,0.6573946226646478,-0.5195291117845705,-0.12356162887689823,-0.4807987503465016,-1.6053873681852662,0.0,0.0,22.60127686658211,-0.16050172843058397,0.059723076010970044,0.12387954140134472
+4.3,1.9840749667864406,1.9436106269157438,2.0078423786458623,0.24121393608407254,-0.05521405139838153,0.09250062880437751,0.9644744056934847,0.2717593411464499,0.579453609323516,-0.46458990282214535,-0.35704548146900206,-0.40032128713068277,-1.5079344946918036,0.0,0.0,22.795187488568473,-0.1972828191381496,0.06865475405598753,0.12925069814801127
+4.32,1.987020770301245,1.9567680260996327,2.0059011761040617,0.23607742766724757,-0.05533197009800324,0.0778421899945068,0.9670296865598017,0.22083208282258465,0.5014622727601918,-0.4046140308356446,-0.6390923549986327,-0.3033456903652655,-1.4062583855315423,0.0,0.0,22.949670110448245,-0.23706894732640815,0.08000308386534283,0.13483923446151755
+4.34,1.989599694040637,1.9677727149071895,2.0040956806294985,0.2274949760032517,-0.05481642731791191,0.06428034960605199,0.9701078454707516,0.1759656707233199,0.42469682960444544,-0.34092701378320556,-0.9736875566740303,-0.1864784874439781,-1.3001716226446132,0.0,0.0,23.042534950394817,-0.2776667181886452,0.09350434910117333,0.14068685176709908
+4.36,1.9917822072499005,1.9766931611430083,2.00248907669432,0.21493020033627466,-0.053431875855886844,0.05187848999521409,0.9737857392166649,0.13692246219978854,0.3504009454483512,-0.2753031945240926,-1.361745842460857,-0.04689449184306793,-1.1894428490654465,0.0,0.0,23.04265475814403,-0.3149270216673352,0.10812656225912642,0.14683408494661948
+4.38,1.9935510611753366,1.9836426745215956,2.0011471988483054,0.19788479457695515,-0.05091042242401162,0.040701004215497945,0.9780558088471716,0.10339027220552924,0.279789771119854,-0.21002505763781454,-1.7984083876422756,0.1165509364884422,-1.0737957096861557,0.0,0.0,22.908821851229863,-0.3417365539827186,0.12159567794003967,0.15331287561413998
+4.4,1.9949057666360457,1.9887861318480826,2.0001289484150515,0.17599685555232916,-0.046983324045729184,0.0308110851072391,0.9827860149248264,0.07500513035468327,0.2140826475033013,-0.14790493344379452,-2.2689649438454937,0.3015843631894807,-0.9529182072258399,0.0,0.0,22.591082291203275,-0.34664465487338203,0.12978800559481085,0.1601373711155359
+4.42,1.9958666041099473,1.9923436058807211,1.9994711597857615,0.14920321798310499,-0.04144057908804521,0.022267936611432636,0.9876868011406261,0.051390326188925854,0.15457069963344017,-0.09222577782051387,-2.7429408720525505,0.5000265975387276,-0.8264901799927011,0.0,0.0,22.037263237456564,-0.31195983777972713,0.12606545632937238,0.16730057583939675
+4.44,1.9964767346197212,1.99458746782068,1.9991684786014448,0.11799236680685372,-0.03422789096689701,0.015124592723574064,0.9923091753834694,0.032214043298620654,0.1027063109971041,-0.04653690224574144,-3.1657427834413894,0.6945893846848327,-0.6942348856125349,0.0,0.0,21.20596813357938,-0.21079128950453352,0.10082613632226178,0.1748276669545097
+4.46,1.9968006189464822,1.995829190693021,1.999151864455269,0.08378163152155858,-0.025584971460723232,0.009428582530266424,0.9961110125314395,0.0172538977296628,0.06015683506975396,-0.01424303664842564,-3.446787830938415,0.8553756859119165,-0.5559738851385823,0.0,0.0,20.082682089984353,-0.0014404593330858972,0.041960524850413905,0.18299278404905622
+4.48,1.9969173761351182,1.996393714695754,1.9992741011782653,0.049463394455286545,-0.01621525662382466,0.005227236792058514,0.9986306194269751,0.006428685498570657,0.028687620396424758,0.002053960647023109,-3.442241728676518,0.9371331126641117,-0.4116626750998658,0.0,0.0,18.792002875268306,0.36497555083061706,-0.06357246083304169,0.19106805645819963
+4.5,1.9969088754821915,1.9965816661004083,1.9993415046422565,0.01985857176567676,-0.007467785917368671,0.00255112101835115,0.9997716544703943,-0.0002785022838532134,0.009612954857845607,0.0027195873338694684,-2.9623137591439335,0.8777782492723295,-0.2631467084182476,0.0,0.0,18.208173772876336,0.8398146106579514,-0.22879927545100892,0.18743185730308415
+4.5200000000000005,1.996846489569091,1.9966315954224705,1.999299214479492,0.0008845702548103091,-0.0015218746796181184,0.001306542784104257,0.9999975971865518,-0.003128806408322322,0.0024644968402238385,-0.0021380044434074694,-1.8969536348824176,0.5970325243738921,-0.12213347772566754,0.0,0.0,18.80181071960565,1.4808495555471888,-0.46399910944220824,0.16860912568120295
+4.54,1.9967809272265173,1.9967034557943861,1.999307880101362,0.000829557222930486,-0.0014956232227179688,0.0013016934528792944,0.9999976902674047,-0.0032674205364779823,0.0036023006506212627,0.0004369898286000532,-0.0054986063487407755,0.0026319122802268557,-0.0004788939507678197,0.0,0.0,19.1412382407069,-0.3032822128419834,-1.553573378463879,0.17018674028254116
+4.5600000000000005,1.9967291491099106,1.9967638522080373,1.9994305557607313,-0.0026834682794380374,-0.020425641644945015,0.002518372152075362,0.9997846017833487,-0.002442406877845522,0.0030180608137059055,0.006194923550100361,-0.3535881359942936,-1.8925885302383412,0.1237987760986273,0.0,0.0,18.522681033582508,-0.20222403116638965,-0.8705890510133236,0.18993210402680466
+4.58,1.9965350579091425,1.9968408554790207,1.9994942576033035,-0.00851351620817091,-0.04984836829459684,0.0051320477709610535,0.9987073256494197,-0.009427251962553776,0.0038840426135578095,0.003901724632506832,-0.5853279866149165,-2.943517766756258,0.26563550008553005,0.0,0.0,19.102027571001145,-0.09743425260697769,-0.371484632864333,0.19332285503777064
+4.6000000000000005,1.995955702301429,1.9969795373111787,1.9995322173254113,-0.015507653184835315,-0.08381862822871428,0.0091950722385489,0.9963179215998693,-0.02834868056292046,0.007253008661097198,0.005909835879309119,-0.6978664923486866,-3.405110475417108,0.41303181793389565,0.0,0.0,20.292188183550135,-0.002752855551881539,-0.005828362545412746,0.18592673540073318
+4.62,1.9946765876229324,1.997237153866804,1.9997029266654325,-0.0224411850635175,-0.11777582172067964,0.014683842132131785,0.9926780111452063,-0.06058682754551306,0.013802860414782412,0.021670682165158668,-0.6818040503520293,-3.4165703418279483,0.5557744657255824,0.0,0.0,21.296509913838808,0.0471972965012909,0.2049924237780021,0.17850942049148114
+4.64,1.9923657314327514,1.9976680255151864,2.0001255957233046,-0.028635238036224836,-0.14899178776859529,0.021570209219963884,0.9881883405476577,-0.10488679455821662,0.02369774858195292,0.05199217475441854,-0.5897838514339061,-3.1585451075622917,0.6932105471981281,0.0,0.0,22.02719243960943,0.06833431491839692,0.31240714899156774,0.1715773279525086
+4.66,1.9887133528553913,1.998313903239932,2.0008587303717316,-0.0338381260785308,-0.1760697334842086,0.02982036157925228,0.9833438748523203,-0.15909562520358783,0.0370301669194022,0.09520345930308907,-0.46356858801639894,-2.760783313090531,0.8258177123073767,0.0,0.0,22.502426707960996,0.07380856076567133,0.35550372747414816,0.16493434971366785
+4.68,1.9834537274451127,1.9992002558735722,2.0019118095865087,-0.03804472331410468,-0.19845739503137125,0.03939186838306774,0.9785783270084243,-0.2211039446198442,0.054027355355459036,0.1486777147299758,-0.328049056780377,-2.3054711392654625,0.9538297561143201,0.0,0.0,22.762247829152095,0.07147789877978006,0.3595074631006987,0.15852372385480468
+4.7,1.9763742771851907,2.0003363878099716,2.0032608877975955,-0.04137379577308445,-0.21611307363870993,0.05023507701084708,0.9741969952034706,-0.2891762484213805,0.07507694768441867,0.2093652429325756,-0.19681707001531357,-1.8429772289876136,1.0773316652033895,0.0,0.0,22.853429944451232,0.065973681945387,0.3408071892990614,0.15232140073579958
+4.72,1.967315986044041,2.0017176989206567,2.0048627649784225,-0.043991206989512976,-0.22928404216012035,0.062295993749855734,0.9703673587260857,-0.36196546149583064,0.10067919634690578,0.27419773064784986,-0.07634868932813423,-1.4027068464325072,1.1963493601516735,0.0,0.0,22.820007326427508,0.059916221461945085,0.30996530884930706,0.146309649591493
+4.74,1.956168936184962,2.003328785601236,2.0066658343122215,-0.0460650610450782,-0.23836438573939311,0.07551903832033079,0.9671387204600834,-0.4383921587546352,0.1313818510670862,0.3403469524447976,0.03117462393872708,-1.0004502724888487,1.3108982767409947,0.0,0.0,22.69901148985131,0.0546660180571269,0.2736585079563024,0.1404728087513657
+4.76,1.942865748899982,2.005146574729033,2.0086174318021484,-0.047741960936341206,-0.24380731588659266,0.0898490186628928,0.9644717993448244,-0.5174997758577407,0.1677235715048886,0.40535743612336705,0.1260116846327885,-0.6433503658158448,1.4210046481064338,0.0,0.0,22.519533971438076,0.05080933792099418,0.23598235610761142,0.13479693622132405
+4.78,1.927374631397543,2.0071430879097387,2.010668271942741,-0.04913658200752112,-0.24607352276918973,0.10523219924658211,0.9622679470616834,-0.5983388834994041,0.21019274717794734,0.46719256548683613,0.20967134473962634,-0.33322638681248584,1.526711600322581,0.0,0.0,22.30342526540423,0.04847424715948079,0.19933583029162594,0.12926976706960733
+4.8,1.9096929035181345,2.009287694648079,2.012774748157931,-0.050329221639419754,-0.24560257781134068,0.12161658751183982,0.9603935385369736,-0.679895915136933,0.25919962342264413,0.5242298096128166,0.2841711380078193,-0.06876671758509917,1.6280779878346339,0.0,0.0,22.06658986252172,0.04753403981093629,0.16501990827423163,0.12388045821298588
+4.82,1.8898413552349898,2.0115488527449616,2.014899806378808,-0.05136782168584935,-0.24279862641543665,0.13895165369766788,0.9586983946167383,-0.7610627516025629,0.31505715239049814,0.575229811125249,0.3515946912298581,0.1530495359588072,1.7251744730797058,0.0,0.0,21.820337402195626,0.047735446966365114,0.13363995700708775,0.1186192495961987
+4.84,1.8678595008204886,2.013895394936292,2.017012941588341,-0.052272247588728886,-0.23802466983035375,0.15718770507969385,0.9570286798502712,-0.840636765119334,0.37796661575993573,0.6192940390438317,0.4138443702915695,0.33605063462816276,1.8180791608985647,0.0,0.0,21.57256122720397,0.04877722478932156,0.10537380064265955,0.11347714517625379
+4.86,1.8438016635894392,2.016297438508596,2.01908970617633,-0.05303946946783751,-0.2316018287826931,0.17627510263992208,0.9552353091109893,-0.9173404353406748,0.44800563447621017,0.6558189451643034,0.4725188694246506,0.4843163744772079,1.906873788984657,0.0,0.0,21.328679008610926,0.05035650052452645,0.08014830732659906,0.10844566088440497
+4.88,1.8177337787677845,2.0187269925243885,2.021110987024154,-0.05364884795782621,-0.22381135663746382,0.1961634622003125,0.9531789831141276,-0.9898516862784243,0.5251176422742223,0.6844506755723058,0.5288686643487565,0.6018497688484927,1.991640778088311,0.0,0.0,21.092345375811664,0.052194488220024926,0.057754084056215294,0.10351664743037521
+4.9,1.7897307975587033,2.021158327997722,2.0230622109478476,-0.05406708022696752,-0.21489804615908104,0.21680093633747816,0.9507328408106732,-1.0568386705456096,0.6091028993587907,0.7050423862680705,0.5837981708892332,0.6923961258626226,2.0724611166719624,0.0,0.0,20.86597428440502,0.05404921784617445,0.0379185950648645,0.09868217822134764
+4.92,1.7598745880222078,2.0235681657836664,2.024932570919954,-0.05425258117406928,-0.2050742202744644,0.2381336340570893,0.9477835163926073,-1.1169949942911834,0.6996116443125376,0.717615238194305,0.6378942231143464,0.7593559092016203,2.1494129292454747,0.0,0.0,20.65111377634629,0.05572026094658045,0.020351556199846188,0.09393448704222132
+4.94,1.7282522453786222,2.025935727330237,2.026714321276016,-0.05415920956455149,-0.19452383277466748,0.2601052044286974,0.9442311905095604,-1.1690730761228592,0.7961401363480228,0.7223236569142687,0.6914677192060202,0.8057558331665859,2.222570553502575,0.0,0.0,20.448711176481375,0.05704868464475421,0.004771990774047552,0.08926593999851382
+4.96,1.6949547403102294,2.028242685310419,2.0284021622994732,-0.05373932994676419,-0.18340640909460607,0.28265658825104756,0.9399890566587319,-1.2119144774145012,0.8980302589063541,0.7194251482037175,0.7446001271818616,0.8342548324893665,2.292003970716829,0.0,0.0,20.259299257602713,0.057914322367599746,-0.009076727681925134,0.08466902834242375
+4.98,1.6600758469728463,2.030473044364517,2.0299927179649324,-0.052946248161685625,-0.17186068108146182,0.30572592734735093,0.934982490986553,-1.2444767372094128,1.004473151650003,0.7092547377190569,0.7971897821743501,0.8471697086753869,2.357778466703891,0.0,0.0,20.08312645770313,0.05823170184679349,-0.021419600517034065,0.0801363716983255
+5.0,1.6237113026390892,2.0326129764403666,2.031484101402645,-0.05173608338705181,-0.16000784465478718,0.32924861282076934,0.9291481142854979,-1.2658566171211436,1.1145170913709233,0.69220391079586,0.8489950667412615,0.846510685761854,2.4199544332676814,0.0,0.0,19.920247952545775,0.05794547762687907,-0.032447657741329516,0.07566072416215083
+5.0200000000000005,1.5859581587099643,2.034650630264965,2.032875558166497,-0.05006915072551226,-0.14795441473715187,0.3531574499686031,0.9224328630513369,-1.2753098203181694,1.2270796029755364,0.6687037640609849,0.8996729920447157,0.8340207056393639,2.4785872472846036,0.0,0.0,19.770589506803823,0.0570258861747176,-0.042318648455439534,0.07123497840901291
+5.04,1.5469142889039476,2.0365759301528157,2.0341671758315143,-0.04791093043098995,-0.13579467934290687,0.3773829171092331,0.9147931360048126,-1.2722672800571702,1.340963575214867,0.6392119512165209,0.9488126205144527,0.8112146662377602,2.533727185456412,0.0,0.0,19.633992418910346,0.055464520358510905,-0.05115951285160247,0.06685216513396962
+5.0600000000000005,1.5066780253232657,2.038380375571236,2.03535964860354,-0.04523269638451766,-0.12361277113056109,0.401853497099657,0.9061940481265508,-1.2563480797176203,1.4548769913296904,0.6042029172669303,0.9959633449054013,0.7794163649367644,2.5854193480123806,0.0,0.0,19.510245294509915,0.053270571485966145,-0.059069808923460365,0.06250544682032641
+5.08,1.465347897212652,2.0400568496083156,2.0364540866988037,-0.04201186974111234,-0.11148438415552987,0.426496062870559,0.8966088016778724,-1.227369010149265,1.567455761486095,0.5641608699686582,1.0406583718206968,0.7397919280778936,2.633703575497193,0.0,0.0,19.399106581777485,0.050467589060048446,-0.06612555490451644,0.05818810600217915
+5.1000000000000005,1.4230224508280842,2.0415994417134566,2.0374518617351276,-0.0382321554213294,-0.09947816849008725,0.45123630120982194,0.8860181695406567,-1.18535071791849,1.6772890562081606,0.5195749309985825,1.082433926039936,0.6933791671195286,2.6786143504279507,0.0,0.0,19.300320565873946,0.04709074631652166,-0.07238314187793682,0.05389352892899342
+5.12,1.3798001320708768,2.043003287793272,2.038354480953857,-0.033883510453221966,-0.08765683735390545,0.47599916185304086,0.8744100779952618,-1.1305203646896667,1.7829464811001394,0.47093593565967223,1.1208447505812997,0.641112723309559,2.720180680985433,0.0,0.0,19.213628682643805,0.04318456328687581,-0.07788310065850856,0.04961518593219737
+5.14,1.335779216488717,2.044264428927852,2.0391634845772137,-0.02896198483867846,-0.07607802142359926,0.5007093214516932,0.8617792719147168,-1.063310710309204,1.8830063968701973,0.41873440077062707,1.1554764666028425,0.5838451257937768,2.7584259676941723,0.0,0.0,19.138777449408007,0.03880102115713513,-0.08265359352419888,0.045346609918390075
+5.16,1.2910577739268794,2.045379688593019,2.039880361901701,-0.02346946808759086,-0.06479490374039072,0.5252916540885171,0.8481270439397087,-0.9843555514855786,1.9760846705707928,0.3634592475175918,1.1859553061983155,0.5223640461626728,2.793367856684612,0.0,0.0,19.07552393453889,0.033997996444309564,-0.08671355852602103,0.041081374341834745
+5.18,1.2457336575338112,2.046346567291777,2.0405064827936896,-0.01741336807928201,-0.053856666585963615,0.5496717016929351,0.8334610095465634,-0.8944814920629844,2.060863141422319,0.3055969420693309,1.2119556605848516,0.4574061212622324,2.8250180848876383,0.0,0.0,19.023639435430148,0.028837946683900717,-0.09007547130751725,0.03681307179770284
+5.2,1.1999045089807792,2.0471631528647043,2.041043042084227,-0.01080624355766566,-0.04330877918708477,0.5737761390312308,0.8177949113463048,-0.7946960854285485,2.1361170964558207,0.24563079549663352,1.2332058103101402,0.38966775968674994,2.8533823235400857,0.0,0.0,18.982911864107354,0.023386787641272888,-0.09274771299338297,0.0325352940713073
+5.22,1.1536677736402476,2.0478280444139494,2.041491014975066,-0.003665407318409133,-0.03319315238145191,0.5975332289876945,0.8011484379811966,-0.6861724690812737,2.2007410773466414,0.18404024115349576,1.2494921321288335,0.31981336201907407,2.878460026762646,0.0,0.0,18.95314722253732,0.01771291307755707,-0.09473654710967438,0.028241614111676847
+5.24,1.1071207210807144,2.0483402876890517,2.041851121998734,0.003987486092648987,-0.02354818359778068,0.6208732647010606,0.7835470452904233,-0.5702306998521411,2.253772380479624,0.121299978563568,1.2606620145434424,0.24848138170450662,2.900244291757431,0.0,0.0,18.934170466666593,0.011886320023953956,-0.09604771665805117,0.023925569999496632
+5.26,1.0603604675804157,2.0486993198987298,2.042123802360951,0.012126856711306986,-0.01440871278131149,0.6437289958546221,0.7650217697806311,-0.4483160897092641,2.294411667673853,0.057878935432373324,1.2666256633061697,0.17628864007724315,2.91872173640097,0.0,0.0,18.925825996663626,0.0059778140446064,-0.09668767644568416,0.01958065057889999
+5.28,1.013483998446879,2.048904922189346,2.042309194675129,0.020723609680518913,-0.005805907296197038,0.6660360371031014,0.7456090267172693,-0.32197493083115303,2.322040174833641,-0.005760948083332232,1.267356939998972,0.10383329075533647,2.9338723987295894,0.0,0.0,18.927977962165777,5.827915767830927e-05,-0.09666447701697867,0.015200282053826435
+5.3,0.9665881887657622,2.048957178431498,2.042407124216687,0.029745531144901072,0.0022329086075847378,0.6877332573022992,0.7253503872587561,-0.19282807955799727,2.3362330891332364,-0.06916606737721086,1.2628933501920214,0.031696808946968635,2.9456696611030044,0.0,0.0,18.94051052952844,-0.005801994764208285,-0.09598831607741298,0.010777814554294716
+5.32,0.9197698218053258,2.0488564394496276,2.042417095917238,0.03915766119543105,0.009684466445480782,0.7087631489203708,0.7042923309335763,-0.06254293753953899,2.3367687604687464,-0.1318925585849337,1.2533352805180855,-0.039554637514180244,2.9540801997996513,0.0,0.0,18.963328218719674,-0.01153392872976955,-0.09467177187990228,0.006306507504580654
+5.34,0.8731256046879892,2.048603292388864,2.042338292418571,0.04892263561874492,0.016529797030925202,0.7290721777713,0.6824859713825961,0.06719557864279618,2.3236335173787657,-0.19350766808409914,1.2388445732849278,-0.10936909498993104,2.959063958597485,0.0,0.0,18.99635637716179,-0.017070214605256595,-0.09272973114221678,0.001779512624416744
+5.36,0.8267521811233117,2.0481985355160117,2.0421695766417267,0.05900098788603061,0.022754495513004215,0.7486111140321333,0.6599867546473863,0.19471445346722635,2.2970219684194864,-0.2535909870594804,1.2196425192324023,-0.17720768169688844,2.9605741427226127,0.0,0.0,19.039541813684192,-0.022345818390737016,-0.09017902188333105,-0.002810146392331256
+5.38,0.780746140991301,2.047643159378873,2.0419094985197765,0.0693514021136024,0.02834900012715859,0.7673353463916356,0.6368541303899161,0.3183817334216543,2.257332783689201,-0.3117354561957698,1.1960073393424382,-0.24254307927847424,2.9585572276332006,0.0,0.0,19.09285356542319,-0.027298431650515376,-0.08703775894660881,-0.007469598031441239
+5.4,0.7352040263816885,2.046938335884741,2.041556305811978,0.0799309066225682,0.033308878081007194,0.7852051821332138,0.6131511973066882,0.4366347979606421,2.2051600643317686,-0.36754805943232555,1.1682712102982515,-0.3048573478181833,2.952952975684599,0.0,0.0,19.15628371184284,-0.031868884896376404,-0.08332440630088345,-0.012206150798816146
+5.42,0.6902223333390551,2.0460854174921645,2.0411079592762076,0.09069499617563713,0.03763511270528821,0.8021861369830048,0.588944324696864,0.5480071312440128,2.141280517068467,-0.42065014784556043,1.1368168598838777,-0.36363875570067616,2.9436944530033404,0.0,0.0,19.229848080986734,-0.03600155001638672,-0.07905655432833991,-0.017027323811472678
+5.44,0.6458975080334739,2.045085949328916,2.0405621529389135,0.10159766890653338,0.04133438602136294,0.8182492196584336,0.564302752725308,0.6511530298728894,2.066636753177132,-0.47067736374549296,1.10207371263607,-0.41837731080518237,2.930708039018837,0.0,0.0,19.31358661136595,-0.03964477646768971,-0.07424940060771268,-0.021940878408268846
+5.46,0.6023259353602554,2.043941697641517,2.0399163407804437,0.1125913612819924,0.04441934999037784,0.8333712172360268,0.539298174476461,0.7448697429367869,1.9823171245237405,-0.5172791697055555,1.0645134966986276,-0.468558663654563,2.9139134220779557,0.0,0.0,19.407563035826488,-0.04275142747041494,-0.06891390738907294,-0.0269548484233431
+5.48,0.5596039170470759,2.042654698532062,2.03916777186373,0.12362676100888896,0.04690887765882368,0.847534988720557,0.5140043035361531,0.8281166158160842,1.8895325916496322,-0.5601180264254231,1.024645124883348,-0.5136560199952427,2.893223576259547,0.0,0.0,19.511863437693805,-0.04527960663912245,-0.06305458672262099,-0.03207756649290881
+5.5,0.5178276351740567,2.041227331422269,2.0383135367944663,0.13465247343796666,0.04882828191921183,0.8607297755371673,0.4884964317020066,0.9000308953720358,1.7895911896793546,-0.5988683062849557,0.9830085327479618,-0.5531196418136844,2.868544716609009,0.0,0.0,19.626593089141274,-0.04719368988293626,-0.056666835380565383,-0.03731768401151545
+5.5200000000000005,0.47709309553584245,2.039662422064362,2.0373506294344246,0.14561451151170565,0.05020948437041197,0.8729515390692123,0.4628509826641379,0.9599399482854384,1.683870716753691,-0.6332150757541183,0.9401669936917569,-0.5863634242501878,2.8397762320800006,0.0,0.0,19.751870808548684,-0.04846579775834425,-0.049733710060780034,-0.042684183232928274
+5.54,0.4374960434155236,2.0379633801335526,2.036276029025951,0.15645557247421135,0.051091109510179304,0.884203336781856,0.4371450692786293,1.0073697432703075,1.5737903170384733,-0.6628529294547028,0.8966972411000053,-0.6127479079922,2.8068105969609487,0.0,0.0,19.887819853365862,-0.04907785371935258,-0.042222008457437676,-0.04818638158260186
+5.5600000000000005,0.3991318419920644,2.0361343763963364,2.0350868093614247,0.16711405636502946,0.051518469991736406,0.894495749784625,0.41145606452229844,1.0420495486022239,1.4607816663303925,-0.6874851121361558,0.8531765245204801,-0.6315589309817126,2.7695332619089332,0.0,0.0,20.03455407458581,-0.04902436708091984,-0.03407752165938349,-0.053833930515043815
+5.58,0.36209530062655876,2.034180564019471,2.033780283388139,0.17752277205689182,0.05154339679043123,0.9038473756118505,0.3858611994592244,1.0639128955501846,1.3462604924881794,-0.7068232233748823,0.8101655350599595,-0.6419809704590863,2.727822524393751,0.0,0.0,20.19215766383374,-0.0483160514131632,-0.025219376361509338,-0.05963681412477881
+5.6000000000000005,0.32648043649154707,2.032108347577936,2.032354193713683,0.1876072671989567,0.05122385385134656,0.9122853999405067,0.3604372055520139,1.073094951790294,1.2315991736775835,-0.7205878655595104,0.768185987123694,-0.643064142742276,2.6815493750194768,0.0,0.0,20.36065627544054,-0.04698434226275028,-0.015533529557241965,-0.06560535594754191
+5.62,0.2923801482064158,2.0299257014797214,2.030806961876912,0.19728371080486728,0.050623260250760374,0.9198462587783036,0.3352600232419656,1.0699265309916144,1.1181011511621963,-0.7285106715196844,0.7276915711101783,-0.6336839278570139,2.6305773108615154,0.0,0.0,20.53997652802045,-0.04508682757578775,-0.004865756075071503,-0.07175024548315612
+5.64,0.259885774071692,2.0276425365128827,2.0291380118987257,0.20645625463861308,0.04980942429247553,0.9265763963486032,0.31040460354492866,1.0549250273210877,1.0069778644773684,-0.7303382367356319,0.6890310103902176,-0.6124931373117065,2.574762100300284,0.0,0.0,20.729889733104127,-0.04271357477343107,0.006985082998201442,-0.07808259852774752
+5.66,0.22908649988742938,2.025271108606324,2.0273481862621696,0.21501380691405947,0.04885297344700438,0.9325331102798743,0.28594483393384373,1.0287825957925836,0.8993288582387484,-0.7258385835739649,0.6524020529262844,-0.5778666614954622,2.513951477210349,0.0,0.0,20.92993383644941,-0.03999434706276017,0.02027206731502673,-0.0846140682133701
+5.68,0.20006857192998947,2.022826457150304,2.0254402744616526,0.22282617590719575,0.04782514191351383,0.937785450063387,0.26195362333996897,0.9923518892246176,0.7961256146165512,-0.7148108991944944,0.6177953724648431,-0.527841358812956,2.4479847370386376,0.0,0.0,21.13930443109181,-0.037106734239650496,0.03530130341711679,-0.09135702968842911
+5.7,0.1729142592216093,2.0203268507112195,2.0234196743241775,0.2297395905579262,0.046794754826817955,0.9424150907051176,0.238503182918466,0.9466296013977282,0.6981995288515499,-0.6970994069457337,0.5849275387321019,-0.46005631046299883,2.3766922029418422,0.0,0.0,21.35670090179721,-0.034285223394837194,0.05242588120042152,-0.09832487102361498
+5.72,0.14770049568519264,2.017794204969387,2.0212952021781896,0.23557169051245846,0.04582422749300774,0.9465170333331453,0.2156655383087271,0.8927379567336458,0.6062342662945551,-0.672612333953737,0.5531625112362023,-0.3717027458767676,2.299894527081041,0.0,0.0,21.580108613381093,-0.031830942557951714,0.07202850357898277,-0.10553241143267444
+5.74,0.1244971176025976,2.015254420411483,2.019080056287272,0.2401062117039739,0.04496438758027576,0.9501998802108411,0.19351330341348694,0.8319041560314347,0.5207625231846071,-0.641346974137237,0.5214219401679777,-0.25949831857117966,2.2174018108163938,0.0,0.0,21.80648606234112,-0.030119995904925867,0.0944882076887597,-0.11299655337015832
+5.76,0.10336459593390626,2.012737564889002,2.0167929128304363,0.24308778470613285,0.044247958888987735,0.9535852881773237,0.1721207286546094,0.7654376998521049,0.4421669696040866,-0.6034218087925647,0.48808801339167873,-0.11970627560323568,2.129012497224006,0.0,0.0,22.031451380696335,-0.029601752642732126,0.1201206544737955,-0.1207350405990617
+5.78,0.08435116158221188,2.0102777981465576,2.014459111985072,0.24421759787696332,0.043681692896926444,0.9568060063469387,0.1515649722438736,0.6947055957876909,0.3706848817608944,-0.5591151378704142,0.4509150980053646,0.051766566531574604,2.034513691089236,0.0,0.0,22.248615823258532,-0.03079107779122059,0.14908524438644027,-0.12876871006458587
+5.8,0.06748920111002012,2.0079129081493834,2.0121117862501,0.24315107360769653,0.04323727885882803,0.9600016777057218,0.13192752526485063,0.6211057789116033,0.3064155850570947,-0.5089121832080591,0.40696214491798277,0.2591506886391277,1.933680524098913,0.0,0.0,22.448836730861263,-0.03423509135295966,0.18123955931700125,-0.13712204307848364
+5.82,0.052790828216775235,2.005683301108188,2.0097926955946956,0.23949940113202653,0.04284157316517555,0.9633113415591502,0.11329561196111496,0.5460398643574069,0.2493298131234022,-0.45355849108586394,0.35258529882653,0.5064062413155793,1.8262762742789167,0.0,0.0,22.619229287365073,-0.04044019791902009,0.2159314638189232,-0.14582456518225315
+5.84,0.040242593598036565,2.0036302731956526,2.007552375259223,0.23283763095917911,0.04236736773168452,0.966861438295891,0.09576326486292351,0.47088767967416134,0.19928009685705347,-0.39411722465630067,0.28355104026661004,0.7965476769611324,1.7120530732147425,0.0,0.0,22.742154662068145,-0.04975265658133088,0.2516851377293202,-0.15490998685699478
+5.86,0.029799441316327235,2.0017934142990383,2.005449035755677,0.22272350494666968,0.04162673109018765,0.9707481984664464,0.07943167363945401,0.3969880425540927,0.156011807746212,-0.3320234327232464,0.19534077146274403,1.1305668039948626,1.5907553686406377,0.0,0.0,22.793921948296013,-0.06209225853862099,0.28585656261074643,-0.16442013993978
+5.88,0.021378252238055406,2.00020706487295,2.003545399366091,0.2087319725126824,0.04037133394014528,0.9750142132534833,0.06440809732129225,0.32563198544414795,0.11917596049128952,-0.2691281843787391,0.0838539632184514,1.505916822620113,1.4621226309632516,0.0,0.0,22.743881691273092,-0.07665795623059624,0.3139889396907984,-0.17440615928917957
+5.9,0.014851721534236549,1.9988959514579288,2.001902550413921,0.1905142781113701,0.03830481390490193,0.9796203894979043,0.05080298758329584,0.25807699943856655,0.08834586644893719,-0.2077161064272749,-0.05344378481503345,1.9141853712839498,1.325892537905797,0.0,0.0,22.554604216117035,-0.09123988326481736,0.3290296405191221,-0.18492759010349147
+5.92,0.010043926041649949,1.9978704229605457,2.0005699765898584,0.16789278268655566,0.035118083308032164,0.9844181764617261,0.03872450901308811,0.19558659346488566,0.06304331769822005,-0.15047286773331942,-0.21539295242877923,2.3377985148598364,1.1818031972040086,0.0,0.0,22.1856972372209,-0.10142307682501106,0.31954271064891004,-0.19602529933771368
+5.94,0.006729695614413436,1.99712222736846,1.999571829742075,0.14101419056187517,0.030560011232449405,0.9891318604096482,0.02827094791831651,0.1394919015071433,0.04277760563931368,-0.10035794085197994,-0.39389416370080177,2.744576947809456,1.0296054503231842,0.0,0.0,21.605752202254802,-0.09955461387159992,0.2693906565984273,-0.20764176752623678
+5.96,0.004640712482444695,1.9966223089423412,1.998891589488236,0.1105626164017985,0.02455862529252359,0.993373887368264,0.019521825554842404,0.09124245275105462,0.02709711093192845,-0.06030143304647963,-0.569394682245811,3.082195100171662,0.8691174326288605,0.0,0.0,20.79239299183078,-0.07347083868008131,0.15352449747944769,-0.21979680865097811
+5.98,0.003479828069386919,1.9963222156572584,1.9984572181240863,0.07807202093768191,0.017402983586605924,0.99671700769327,0.01253404499610986,0.0523901787848524,0.015636672987310488,-0.032837641570781756,-0.7053390251328062,3.268320864524301,0.7001326292021245,0.0,0.0,19.767893089586973,-0.006628627618305849,-0.06363908696639775,-0.2323636231095203
+6.0,0.0029454047149399037,1.9961605855431919,1.9981403429141085,0.04630372681097297,0.009947081741077711,0.9988508613061422,0.007346925631106359,0.024377025221913436,0.00811116587081736,-0.019253632177605354,-0.7463139591028505,3.1833228608207564,0.5227025210296768,0.0,0.0,18.859639173855093,0.09940229662978983,-0.39467972505209825,-0.24119712919628844
+6.0200000000000005,0.002765424143353381,1.9960751857704766,1.9978323095861112,0.019167908292807827,0.0034557213502329075,0.9998024200843382,0.0039711553896272485,0.007923309129780159,0.004155669722973686,-0.016014584455310135,-0.6537281875362669,2.713816080364414,0.3409983461056387,0.0,0.0,18.782387156127587,0.21210093808219788,-0.8173661686586461,-0.23690746824789427
+6.04,0.0027187462652102476,1.9960130255143576,1.9975619630208303,0.001709417558514515,-0.0007187356894446683,0.9999955264665803,0.002346989017082791,0.002031656295942921,0.003084830749777505,-0.013572118867902865,-0.4212726201881396,1.744872284057725,0.1644385300357949,0.0,0.0,19.625641231141206,0.3607931153740418,-1.4418920901564056,-0.22164576978643474
+6.0600000000000005,0.0026554328530712648,1.9959377533912808,1.9975247571510324,0.0016602319389490128,-0.0007389451312452054,0.9999956048248881,0.002342631193352069,0.003141702396159369,0.003781111275874072,-0.0018654214367929498,-0.002031728385216758,0.004913497004097822,0.0004427885978891821,0.0,0.0,10.594326710729092,-1.37727847924261,-0.8501082672700033,-0.1711354472147628
+6.08,0.002575933387592718,1.9959057634491715,1.9962142553422206,0.010572640157522305,-0.01755365540745269,0.9997838403007046,0.003515840654989765,0.003160543952008314,0.002819353185108936,-0.06553229069961815,-1.679688003551399,-0.8951786817697706,-0.11524396891225017,0.0,0.0,9.51659064497822,-0.8488065448193299,-0.47830024606567856,-0.18421470053461825
+6.1000000000000005,0.0025092722473512635,1.9958629824284955,1.9933372181480584,0.024716591661040017,-0.044832634980182785,0.9986710389078143,0.005939779809211699,-0.0018023968661845884,0.01111785501653128,-0.14348776065091148,-2.727284052752327,-1.4208271764309885,-0.23875734094594275,0.0,0.0,9.970145331246004,-0.4713539704680362,-0.1946567907327227,-0.19691338248080403
+6.12,0.0025260119646469434,1.9956780759396011,1.9889362556878687,0.04096994881727714,-0.07791462172590469,0.9960700933299481,0.009754188274127893,-0.015575247004524844,0.036003438940809616,-0.2167732817541586,-3.315999374934077,-1.6325288049320852,-0.37366545897266473,0.0,0.0,10.739825128106176,-0.22156298748320247,-0.021952418344009045,-0.20352396797030284
+6.140000000000001,0.002718686413776057,1.9951619789217871,1.983089063110715,0.05731615591429593,-0.11362097116431545,0.9917547390837336,0.015088760314937343,-0.03924695074113438,0.08034160394949857,-0.2797903956483685,-3.59708973004568,-1.6395756142682263,-0.5167790548798193,0.0,0.0,11.600879958867713,-0.04490513213399272,0.09328140886570555,-0.206955518997323
+6.16,0.0031863083880545283,1.9940867829398403,1.975863141456467,0.07240444818210395,-0.14969509060107195,0.9858306766588729,0.022064738649949966,-0.0723575374372421,0.1438806022465055,-0.3287918536737266,-3.663179616837401,-1.5059174172235332,-0.6656352174542737,0.0,0.0,12.527542972875622,0.08709189264195538,0.1706152339314256,-0.2063344849236709
+6.18,0.004022451514262627,1.9921993799790456,1.9673139258113204,0.08536988669312585,-0.18448326536517098,0.978637215714489,0.03077510787632606,-0.11368764792401204,0.22433953988463418,-0.36085254149281115,-3.572563332404235,-1.276946322467716,-0.816976554675675,0.0,0.0,13.526090490902035,0.19553082831633029,0.22134765968575137,-0.2007237425998143
+6.2,0.00530778056989916,1.9892282284535976,1.9574959625890966,0.09570053579094866,-0.21663790843094582,0.9706736337672938,0.041256766584260245,-0.16184162454740558,0.31822786224032484,-0.3739308560686301,-3.352111017753502,-0.9847836393441572,-0.9665731801922017,0.0,0.0,14.414461793886016,0.27316480387538966,0.2466436979140057,-0.1929000796450307
+6.22,0.007099960228508782,1.9849031701634834,1.9464495982123147,0.10319510750596432,-0.24512987710387707,0.9624969392599908,0.05349537412730255,-0.21560866501301162,0.42136099931334237,-0.36868398156715215,-3.029530085654834,-0.6582627331947949,-1.111595877849576,0.0,0.0,15.111246642626774,0.31663020133747555,0.25157455169875526,-0.18436037877042333
+6.24,0.009426531381034525,1.9789786549152277,1.934191868186672,0.10789622371726294,-0.2693272803977547,0.9546057323860846,0.06744713957411855,-0.27425216012217646,0.5295999076725992,-0.3483193769547017,-2.6416369578618846,-0.32140616252082493,-1.2507807305873144,0.0,0.0,15.604616079386984,0.3320014523582986,0.2409789731462627,-0.1757166772524511
+6.26,0.012283440037862085,1.97124877584085,1.9207186797659308,0.11002861175805138,-0.2889507147711626,0.9473671818498283,0.08304584144762847,-0.3375774216024227,0.6394793957921519,-0.31749330519327873,-2.222393818421021,0.006101629043159917,-1.3837426159606854,0.0,0.0,15.924854582131811,0.3275653702134703,0.2203005967936237,-0.16702558123343508
+6.28,0.015638635859189738,1.9615522501551594,1.9060135875726931,0.10993261865511776,-0.30397634929217404,0.940995529076999,0.10020285766249042,-0.405827360119001,0.7483967942216074,-0.2812326724142669,-1.797330091510294,0.3099881952834179,-1.5103557938266579,0.0,0.0,16.13661970264307,0.3075837257015961,0.19018980676847946,-0.15780965217654536
+6.3,0.019438265417655066,1.9497699515906377,1.8900586628803469,0.10803947254014706,-0.3145796622596411,0.9355502966788394,0.11879709950159824,-0.47947484378783034,0.8544799493018166,-0.2441364327291432,-1.3867840128492979,0.5773033694875715,-1.6302939940017098,0.0,0.0,16.226474939946293,0.28125617653014345,0.16193233473870364,-0.14888938936604212
+6.32,0.023612812993445413,1.9358240833709601,1.8728354965861969,0.10476317740292924,-0.32102861817781386,0.9309820193591172,0.13870033381829405,-0.5590485717975952,0.9563679644486135,-0.21049147107814714,-1.0010742126606913,0.806725113201463,-1.743714123819272,0.0,0.0,16.237709449743164,0.2503784937307304,0.13520613674743476,-0.14006353975004326
+6.34,0.02808315815563785,1.9196728779190193,1.8543298073391252,0.10048986585148005,-0.32365685385942033,0.9271565930904436,0.1597769686153008,-0.6449177234752063,1.0529457499709818,-0.18383052326386773,-0.6476198891381584,0.9980083303366256,-1.8506851295799671,0.0,0.0,16.191409017452514,0.21744523863746001,0.11165386837887706,-0.1314304943057376
+6.36,0.03276563127888043,1.9013059297330233,1.8345327870648647,0.0955555415916126,-0.32283195400153275,0.9238854418251964,0.18189106171042993,-0.7372076908823693,1.1431711320447377,-0.16695852658259214,-0.33048418534753965,1.153636050472468,-1.9513454738275868,0.0,0.0,16.106182202064282,0.1840779965848216,0.09190253802266121,-0.1230549290723171
+6.38,0.03757611115893462,1.8807394861162061,1.8134416185308961,0.09023634979124004,-0.31893487581409335,0.9209503016539552,0.20491092711051476,-0.8357557232341175,1.2259621829157212,-0.16194914876384758,-0.05143422764519423,1.2775486043031898,-2.0458761185491547,0.0,0.0,15.998481839330568,0.15131582130777577,0.07592211580728683,-0.11496789129295881
+6.4,0.04243319965275134,1.8580119147476464,1.7910595740815234,0.08474743426484148,-0.3123468913856655,0.9181222042777443,0.22871141169476786,-0.9401090054801222,1.300151418023285,-0.17017139547365306,0.189302418785689,1.3742059971690206,-2.134474583832632,0.0,0.0,15.882480297564882,0.11983183109933904,0.0633298863914287,-0.10717674229970116
+6.42,0.04726056734667027,1.8331795119952,1.767395911413976,0.07924678095571819,-0.3034411336751331,0.9151754354656116,0.2531745414184345,-1.0495478280760138,1.364493342042882,-0.19233720868124546,0.3925379056450891,1.448010147018265,-2.2173379060379026,0.0,0.0,15.769951111908798,0.09007077745653502,0.05359287055195991,-0.09967525143387874
+6.44,0.05198858812439886,1.806312753358189,1.7424656830966585,0.07384156274688068,-0.29257683078064023,0.9118974518580697,0.27818925032469305,-1.1631208289256922,1.4177091972737652,-0.2285645686060699,0.5598000511343091,1.5029973103529048,-2.2946541998628254,0.0,0.0,15.670288674614802,0.06232786653825084,0.04614452497404892,-0.09245072161601756
+6.46,0.0565553832193662,1.7774930312663884,1.7162895202951929,0.06859550488068929,-0.280095151789342,0.9080955966530451,0.3036507039241285,-1.2796844147233422,1.45855343315968,-0.27845241993793457,0.6931143471757581,1.5427007599476625,-2.366599881471663,0.0,0.0,15.5906571982717,0.03679208426971092,0.04044515333442744,-0.08548806303417306
+6.48,0.060907386169347114,1.7468098841398383,1.6888934155232858,0.06353645444234068,-0.26631613527492826,0.9036012725144753,0.3294595200617875,-1.3979424096029618,1.48588851337817,-0.3411641809150079,0.7948467444344854,1.570110314436772,-2.4333397285540825,0.0,0.0,15.536211845791648,0.01357132163500907,0.036010702667027994,-0.07877186388917076
+6.5,0.06499952596912931,1.7143586949755794,1.6603085055683429,0.058663707213379684,-0.2515364645377675,0.8982720986878389,0.35552104464119805,-1.516484449598961,1.4987573830557668,-0.4155164200371329,0.8675827881208772,1.58768448655003,-2.4950280185217704,0.0,0.0,15.510352544397591,-0.007291032884172877,0.03242451557717008,-0.07228730158561208
+6.5200000000000005,0.06879511105492116,1.6802388229275602,1.6305708428079986,0.05395484204970933,-0.23602798699850067,0.8919924941614576,0.3817447507595204,-1.6338229335281382,1.4964458208312443,-0.5000689095109747,0.9140310565654364,1.5973901026059765,-2.5518098366956807,0.0,0.0,15.51498917179968,-0.025803360541772014,0.02933985201938283,-0.06602041212616291
+6.54,0.0722654836421191,1.6445521232769305,1.5997211393058985,0.04937192116158949,-0.2200369221041412,0.8846730694534242,0.40804378012121023,-1.7484288355203608,1.4785297284198733,-0.5932120797856582,0.9369450687341323,1.6007551198587824,-2.6038221569457916,0.0,0.0,15.550802981221882,-0.042014683462958446,0.02647786618074499,-0.05995807346715987
+6.5600000000000005,0.07538950302103724,1.6074018073958234,1.567804470469187,0.04486697552181615,-0.20378370155643458,0.8762491618435855,0.4343346219862013,-1.858766701197733,1.4449049522156556,-0.6932481254032267,0.9390609530990339,1.5989270304511243,-2.651194609762185,0.0,0.0,15.617497068811112,-0.05600452821471802,0.023623343192872015,-0.05408786290687845
+6.58,0.07815290749693016,1.5688915931896812,1.5348699312982474,0.0403867348974717,-0.18746336807806074,0.8666788028136341,0.4605369149445862,-1.9633289159336293,1.3957993251175638,-0.798462627694587,0.9230491909196463,1.5927326227817455,-2.6940499711593535,0.0,0.0,15.714030383905818,-0.06787382858009627,0.020619427172984484,-0.04839787389001608
+6.6000000000000005,0.08054759726588546,1.5291250974190307,1.5009702458966767,0.03587659222458554,-0.17124643953350097,0.8559403592285162,0.48657335369007676,-2.060669012620683,1.3317681812189506,-0.9071844735547925,0.8914789051385368,1.5827367021859324,-2.7325044428135965,0.0,0.0,15.838830555342003,-0.07773715870033548,0.01736193655194518,-0.042876538197464543
+6.62,0.08257087417050578,1.4882054239123383,1.4661613380234628,0.03128381675513632,-0.1552801323124122,0.8440300418665139,0.5123696826951041,-2.1494334834270625,1.2536756237784161,-1.0178329036100155,0.8467929729006433,1.5692982961715565,-2.7666677928719987,0.0,0.0,15.989981467689864,-0.08571658107944091,0.013793503056486467,-0.03751248061342274
+6.640000000000001,0.08422466862483638,1.4462349056981605,1.4305018758021932,0.02656005102812518,-0.13968983483824465,0.8309594283744067,0.5378547593244168,-2.2283913377865336,1.1626643737396212,-1.1289515566822848,0.7912919919976301,1.552623292509954,-2.796643414020315,0.0,0.0,16.16538261833279,-0.0919372518256063,0.009897585535229791,-0.03229442171640172
+6.66,0.08551477882640704,1.4033149641418674,1.3940528066722429,0.021663143358580432,-0.12458072888706792,0.81675310499473,0.5629606697552327,-2.2964605374141525,1.0601171881988987,-1.2392302608559447,0.7271249913702487,1.5328126331427494,-2.822528341648367,0.0,0.0,16.362878872997904,-0.09652472731967476,0.0056923433972813076,-0.027211134446703034
+6.68,0.08645014258720729,1.3595460538202715,1.3568768992815714,0.016558379965278792,-0.11003947090110808,0.8014464944270909,0.5876228820146953,-2.3527304443029093,0.9476127271176271,-1.3475159750964796,0.6562848231989489,1.5099052754125155,-2.844413262903763,0.0,0.0,16.58036105838654,-0.0996037249892,0.0012243651405514089,-0.022251450523020737
+6.7,0.08704215768585734,1.3150276676360322,1.3190383077042007,0.011219188999814604,-0.09613586438243177,0.7850839070494533,0.611780421613502,-2.396479527865723,0.826878467281136,-1.452814683125362,0.5806064232945933,1.483915232383146,-2.8623825364910647,0.0,0.0,16.81583951548521,-0.10129795348338677,-0.0034377015067034167,-0.017404305089740887
+6.72,0.08730406257363038,1.2698583821528664,1.2806021707482593,0.005627392866438887,-0.0829244744141636,0.7677168296459258,0.6353760567341753,-2.4271877695370847,0.6997428995188191,-1.5542862010555956,0.5017665349887067,1.4548621614565131,-2.876514232702265,0.0,0.0,17.067493976354847,-0.10173056973257233,-0.008209510767947736,-0.012658803871849492
+6.74,0.08725038557349722,1.2241359280310067,1.2416342558486742,-0.00022691410757066601,-0.07044615372984606,0.7494024492412767,0.6583564817071632,-2.444543447944236,0.5680888572524863,-1.6512338278564929,0.42128397951042634,1.422795190386669,-2.886880193933437,0.0,0.0,17.3337037766796,-0.10102484199843231,-0.012997215915440563,-0.008004297088631982
+6.76,0.08689646741485195,1.1779572746037485,1.2022006536453913,-0.006345787680399134,-0.05872946492754861,0.7302023986536947,0.6806724894752467,-2.44844425453671,0.4338094465410452,-1.7430905995714545,0.3405200341071961,1.387809931944069,-2.89354610911944,0.0,0.0,17.613062506002905,-0.09930467851864311,-0.017703321918504433,-0.003430447506237564
+6.78,0.08625805906842667,1.1314187210198374,1.1623675262006237,-0.012724084565922838,-0.047791994801959406,0.7101817038024825,0.7022791257676764,-2.4389929502916203,0.29876769729822394,-1.8294036587813893,0.26067887705165294,1.3500589119589108,-2.896571592111609,0.0,0.0,17.904380869596476,-0.09669479115107715,-0.02223201377271783,0.001072715385126313
+6.8,0.08535099440303562,1.0846159890216225,1.1222009091128002,-0.01935027695089022,-0.03764156447803508,0.6894079095455665,0.7231358196504871,-2.416488006186681,0.1647607387947594,-1.9098179652363942,0.18280833771734037,1.3097558900868944,-2.8960102541531807,0.0,0.0,18.20668096590283,-0.09332037525935866,-0.026493802545656808,0.005514758587804365
+6.82,0.08419093518461665,1.037644314416583,1.0817665656199762,-0.026207371978042286,-0.028277343612172587,0.6679503599289105,0.7432064868951849,-2.381409857807629,0.0334890210294945,-1.9840602854743117,0.1078013440630497,1.2671747655831278,-2.891909763479114,0.0,0.0,18.519184524309047,-0.08930628250128059,-0.030409367672337484,0.009904826321709654
+6.84,0.08279318436963826,0.9905985357263255,1.0411298891460643,-0.033273884546902194,-0.01969087905731116,0.6458796094515841,0.7624596041652166,-2.334403539000229,-0.09346914384559643,-2.0519241313428913,0.036398506166521086,1.2226439183511442,-2.8843118893611144,0.0,0.0,18.841297007223968,-0.08477573358660895,-0.033912518059224905,0.014251636363110932
+6.86,0.0811725624835392,0.9435731794573713,1.0003558495751914,-0.040524818948546225,-0.01186704871793144,0.6232669437062985,0.7808682533481069,-2.276258538391478,-0.21467766042718453,-2.11325608335866,-0.030807758782772296,1.1765369339309115,-2.873252532275698,0.0,0.0,19.17258892186386,-0.0798486623207929,-0.036952240215689214,0.018563489309469168
+6.88,0.0793433410799877,0.8966625420177264,0.9595089777662181,-0.04793262183088218,-0.004784950404634388,0.6001839901075744,0.7984101364791117,-2.2078867483240447,-0.32885747253168973,-2.167943741465588,-0.09336723711877327,1.1292607017501735,-2.8587617452502165,0.0,0.0,19.512775242171106,-0.07463980340422571,-0.039493844645834164,0.022848288108916155
+6.9,0.07731922679805435,0.8499607685761158,0.9186533823601216,-0.0554680755136796,0.0015812662185624203,0.5767024020404828,0.8150675626453872,-2.130299354191743,-0.43489640458227563,-2.215905394449681,-0.15096384703841106,1.0812418648847186,-2.8408637532687413,0.0,0.0,19.861693520488526,-0.06925663914224456,-0.041519259308549494,0.027113565626148944
+6.92,0.07511338931766182,0.8035619291885656,0.8778527927143911,-0.06310110650359319,0.00726161866895084,0.5528936024100916,0.8308274090726188,-2.0445834513973966,-0.5318544195326239,-2.2570813877386815,-0.2034053730152132,1.0329125450500334,-2.8195769777892337,0.0,0.0,20.21928107037264,-0.06379730910854908,-0.043026549486603116,0.0313665185380327
+6.94,0.07273852650641552,0.7575600923499782,0.8371706217632977,-0.07080148899983811,0.012289558083867487,0.5288285750722024,0.8456810593271041,-1.9518790862822581,-0.6189647844948932,-2.2914270928547893,-0.25061098672527987,0.9846961778650611,-2.7949140721827486,0.0,0.0,20.58555151770903,-0.05834856597007121,-0.04402876629042651,0.03561404627189422
+6.96,0.07020696020686132,0.7120493958059881,0.7966700427139974,-0.07853942706264966,0.01670128179786731,0.5045776948633608,0.8596243222648811,-1.8533573056959678,-0.695631701492935,-2.318907334364538,-0.2925966021242322,0.9369941798670995,-2.7668819717119586,0.0,0.0,20.96057103434919,-0.052983833370966016,-0.044552241633037615,0.039862793944137206
+6.98,0.06753075639525774,0.6671241140119876,0.7564140737323485,-0.08628600179297588,0.020535092895574054,0.48021058885379153,0.8726573360758778,-1.7501996762773524,-0.7614250015929007,-2.3394921053680027,-0.32945814778724675,0.8901740386033208,-2.735481959048447,0.0,0.0,21.34443467063296,-0.04776139030958748,-0.044634457313956655,0.04411919833735604
+7.0,0.06472186382471481,0.6228787210707816,0.716465665159449,-0.0940134712051027,0.023830809313793704,0.45579602299287497,0.8847844625575743,-1.643579607095941,-0.8160725184160638,-2.3531533943070686,-0.3613528390855522,0.8445592765091747,-2.700709743776182,0.0,0.0,21.737243369932827,-0.042722672006471174,-0.0443216224400812,0.04838953592757202
+7.0200000000000005,0.061792265731676566,0.5794079473259311,0.6768877843631115,-0.10169541031267951,0.026629226270300645,0.4314018095108797,0.8960141776670275,-1.5346456851770303,-0.8594507545108798,-2.359862948914612,-0.38847849302613413,0.8004215908951288,-2.662555552224922,0.0,0.0,22.139082453167195,-0.03789063959822392,-0.04366610031364748,0.052679971908064185
+7.04,0.05875413973000006,0.5368068270296222,0.6377434941199756,-0.10930667709642096,0.028971636259212348,0.40709473132443186,0.9063589655111771,-1.4245071187154768,-0.8915744344494123,-2.359590813418539,-0.41105084566098027,0.7579753143604469,-2.6210042225688897,0.0,0.0,22.550002569272635,-0.03326812816742164,-0.04272383697870408,0.05699660910553876
+7.0600000000000005,0.055620021643760106,0.4951707336199877,0.5990960215104708,-0.11682318619581455,0.030899410286494823,0.38294048034188855,0.915835224331609,-1.314221282702535,-0.912585505356861,-2.3523044899818277,-0.4292786989560142,0.7173741655899749,-2.5760352995320885,0.0,0.0,22.970004274244125,-0.028836031044777055,-0.04155196196421507,0.061345535666913774
+7.08,0.052402969743688646,0.4545953980868925,0.5610088157663261,-0.12422146595040173,0.03245364202734964,0.3590036070881786,0.9244631948401031,-1.2047832807436176,-0.9227421027358493,-2.3379685936331702,-0.4433365288393074,0.6787100590411651,-2.5276231232888975,0.0,0.0,23.399027471606487,-0.024551113492623733,-0.04020675858460909,0.06573287038482856
+7.1000000000000005,0.04911672665258993,0.4151769045943282,0.5235455954188738,-0.1314779652114566,0.033674852649555484,0.3353474796096905,0.9322669236234405,-1.0971173788887612,-0.9224079510651599,-2.3165448920695613,-0.4533339103789979,0.6420135012815221,-2.475736908222483,0.0,0.0,23.836946855597063,-0.02034316696813243,-0.03874223439858393,0.0701648044566145
+7.12,0.045775877053202574,0.3770116558236555,0.48677038753674573,-0.1385680633474109,0.03460274753101152,0.31203425034658633,0.9392742774579196,-0.9920701354333523,-0.9120426191132504,-2.28799264989466,-0.45928073566493255,0.6072548122658596,-2.4203408090995904,0.0,0.0,24.28357416850919,-0.016111116145406168,-0.037209556027590907,0.07464763823964145
+7.140000000000001,0.04239600021132925,0.34019629818511754,0.4507475648938202,-0.14546471908497072,0.03527600630721522,0.28912483074744744,0.9455170284695055,-0.8904050467933093,-0.8921930034934097,-2.2522692352386615,-0.46104669041761487,0.5743450704387203,-2.3613939749711714,0.0,0.0,24.738667425898736,-0.011717583387809372,-0.03565763636810848,0.07918781111002376
+7.16,0.03899381709902338,0.30482759377695534,0.41554189065264413,-0.15213667020440313,0.035732073932638865,0.2666788749987513,0.9510310352985277,-0.792798553680468,-0.8634863740672162,-2.209331001575699,-0.4583128031319693,0.5431353040761085,-2.2988505947500295,0.0,0.0,25.201946171117218,-0.006981323905868919,-0.03413516295135188,0.08379192192022486
+7.18,0.03558733235663311,0.2710022212466584,0.38121858466538244,-0.15854606581366523,0.03600690218115102,0.24475477642617752,0.9558565516995203,-0.699837300879707,-0.8266252821782569,-2.1591345374522124,-0.4505121048801155,0.5134120699308378,-2.2326599428391347,0.0,0.0,25.67311016749108,-0.0016669197635188482,-0.032694318635738906,0.08846673714466219
+7.2,0.032195971131966594,0.23881648079404708,0.3478434308572096,-0.16464537595595827,0.03613456799221372,0.22340968284175855,0.9600387006703855,-0.6120166108953359,-0.782384602831966,-2.1016384922813636,-0.43675562630599746,0.48488722962926456,-2.1627664378063653,0.0,0.0,26.151856577750202,0.004528748149920325,-0.03139635371432847,0.09321918443465882
+7.22,0.02884070948004772,0.2083658684203789,0.3154829514046568,-0.1703733815331016,0.03614666756447568,0.20269954011014654,0.963628157396874,-0.5297402047312813,-0.7316109409402016,-2.0368063486566146,-0.41573932152965476,0.45717953730403793,-2.0891097303238557,0.0,0.0,26.63788746169556,0.011993297220316057,-0.03031900251330788,0.09805633240411316
+7.24,0.02554419379617087,0.17974446977836014,0.2842046804806223,-0.17565000646801104,0.03607135428379985,0.18267917597949085,0.9666820838783423,-0.45332126535104356,-0.675224551429793,-1.9646107299139104,-0.38562746079649457,0.4297857061830629,-2.011624835032235,0.0,0.0,27.130895192777196,0.021217991269687708,-0.029565467402974388,0.10298536497335459
+7.26,0.022330838748098863,0.15304410282805875,0.2540775778452098,-0.18036972563772363,0.035931857221371176,0.1634024379462852,0.9692653439503579,-0.38298497010736204,-0.614223756049982,-1.885040106793974,-0.3439093072857197,0.40203909743266,-1.9302423086236384,0.0,0.0,27.630508004716297,0.03281648260268133,-0.02927426209801176,0.1080135724607052
+7.28,0.019226883171777016,0.12835310883659223,0.22517262949129924,-0.18439328909388578,0.03574429069059168,0.1449223984372455,0.9714519849438579,-0.31887259451453775,-0.5496915171453229,-1.7981090810013638,-0.28722970135871745,0.3730553325249174,-1.8448884438910933,0.0,0.0,28.136171151773418,0.04753717557483195,-0.029628509430914884,0.11314840269919069
+7.3,0.01626036797773351,0.10575465090243959,0.19756368650003,-0.1875375854706562,0.035514554920435815,0.12729163643198624,0.9733268771173981,-0.26104718845802166,-0.4828032635325443,-1.7038737229957293,-0.21120243540709266,0.3416663878654833,-1.7554853840004447,0.0,0.0,28.646930778056067,0.06625793255504964,-0.030862160085625145,0.11839764184669722
+7.32,0.013460977820069645,0.08532432660703418,0.17132858915990945,-0.18956369247582122,0.035234152447580096,0.11056259529149995,0.9749872171052505,-0.20950062909665837,-0.41483416655615696,-1.6024536168080705,-0.11023093820677701,0.30634873968258647,-1.6619509479451808,0.0,0.0,29.161076831940896,0.08994346164126732,-0.0332588366471579,0.12376981927950079
+7.34,0.010859658103909348,0.06712683721683495,0.14655060143898196,-0.1901636392525086,0.034874836261022175,0.09478800115355748,0.9765432765317275,-0.16416155580919428,-0.3471627786907573,-1.4940621294938308,0.02261189445052203,0.26515781168503894,-1.5641977839514087,0.0,0.0,29.675586677724134,0.11953299974155462,-0.037136453213837184,0.1292749377754447
+7.36,0.008487881392965862,0.05121138586477306,0.1233201234554594,-0.18894730774828086,0.03438221304836533,0.080021306321592,0.9781172572116081,-0.12490331100671878,-0.28126631069367614,-1.3790457103529332,0.195547797769294,0.21569144903158932,-1.4621312311664987,0.0,0.0,30.185289277874173,0.1557074235525986,-0.04280700489863343,0.13492557677250863
+7.38,0.006376396374868791,0.037605417995795046,0.10173652759343209,-0.1854324975461197,0.033668822429188755,0.06631710311064945,0.9798383749808187,-0.0915506168563724,-0.2187010593271698,-1.257931438167146,0.41773332414236103,0.15512097300163355,-1.3556450119862669,0.0,0.0,30.681635294214942,0.19845999492870314,-0.05049175101837455,0.14073817041563977
+7.4,0.004553257659095049,0.026306306981120964,0.08190973433459264,-0.1790438717929125,0.03260795850621367,0.05373143607058676,0.9818314243256082,-0.0638834524008726,-0.16106011737281442,-1.1314794091171179,0.6983025747396674,0.08035650759473473,-1.2446137757406075,0.0,0.0,31.15090624772478,0.2463664069450049,-0.060135643331768907,0.14673352888917127
+7.42,0.0030409219366444172,0.017270703327813733,0.06396075387811843,-0.16913082256368803,0.031031119756319173,0.04232186661369148,0.9841954552180843,-0.04163655965025201,-0.10990037742796584,-1.0007331855505253,1.044393715163259,-0.01151094745493321,-1.1288820634971297,0.0,0.0,31.57165681478071,0.29542256648397247,-0.07097111808088441,0.15293467091015833
+7.44,0.0018522436913296879,0.010401651061489606,0.04801983372726073,-0.15502082436941558,0.02873574843663843,0.03214686003404414,0.9869697969864957,-0.024492693591281423,-0.06663321359475635,-0.8670584004841498,1.4575586906199884,-0.12215034310361189,-1.0082509876715249,0.0,0.0,31.911378125076965,0.3372628317864961,-0.0806944993663734,0.15936130702211337
+7.46,0.0009854542324235398,0.005534466020767472,0.034220151114019506,-0.13613441146598584,0.025515702226768022,0.02326383873687466,0.9900884630990856,-0.012065753229312665,-0.03237929532715097,-0.7321574096717962,1.9276987298961525,-0.2497980870979821,-0.8824658299355848,0.0,0.0,32.12348338909857,0.35643157558606364,-0.08458326810668138,0.1660239805821349
+7.48,0.00041891967950733226,0.0024240926235593934,0.022684622590646755,-0.11220214904872222,0.021228444203215242,0.015726362596093946,0.9933341393635152,-0.003869497315732549,-0.0077970861706913065,-0.5980432408688401,2.4232150523822904,-0.3858650316072227,-0.7512094371042786,0.0,0.0,32.14875386121323,0.32634356109616824,-0.07512063240450484,0.17291961038361758
+7.5,0.000107599543358684,0.0007394937803025972,0.013504567661559979,-0.08364266861489082,0.015906427905240394,0.009583067165034334,0.9963227661566232,0.0007197147052641299,0.007097434508565469,-0.46693858693088486,2.8755422492985363,-0.5110588809514813,-0.6141125252964661,0.0,0.0,31.92977526965268,0.2038459682312732,-0.04214149517287891,0.1800308823937404
+7.5200000000000005,-1.5865370229633407e-05,7.418337530198168e-05,0.006713580620871101,-0.05217776311495121,0.009911221052816539,0.004881165354838409,0.9985766985857125,0.0024796531555402106,0.013097491291318689,-0.341029033641626,3.156060457945725,-0.5918302643488129,-0.4707998669600896,0.0,0.0,31.448645474327186,-0.07776553719173873,0.026240217259710485,0.18734332058430725
+7.54,-3.231287106677665e-05,-1.583458595723645e-05,0.002269336442746955,-0.02177314993565547,0.00411190483191998,0.0016742417767014477,0.9997530790625283,0.0023154611040718635,0.011932777921510854,-0.2219622720900245,3.043111480054994,-0.5784736237906646,-0.3209868579517091,0.0,0.0,30.79332731819349,-0.6192137519227473,0.13896838837481607,0.1949308349353997
+7.5600000000000005,-1.7778054681255422e-05,6.276378069349673e-05,6.81847947490953e-05,-2.9101743477082726e-05,5.248989948503081e-06,2.8442939555858607e-05,0.9999999991582679,0.0011878576877215187,0.006327468028326785,-0.10995079009844967,2.1745731959410657,-0.4107564829246999,-0.16459463253847403,0.0,0.0,30.258807463117797,-1.5596038365875737,0.3263175708326902,0.20383953892364842
+7.58,-1.8464380976886724e-05,6.155953276224476e-05,3.178314290691738e-05,-2.145215392413631e-05,-3.2064688274261667e-06,1.285953355003455e-05,0.999999999682078,-3.431508225130802e-05,-6.011896685547205e-05,-0.0018200857038479373,0.0007649430861267348,-0.0008455222844616084,-0.0015583611924547824,0.0,0.0,19.533005621610283,-0.000548900290262555,0.000649674610254003,0.001990017664952188
diff --git a/test/gtest_processor_force_torque_inertial_dynamics.cpp b/test/gtest_processor_force_torque_inertial_dynamics.cpp
index 25df6c6d1d00683e5a77bc16f1a90511f41d991a..6e3ee1a73fdbcc8c4f8d39b79175ffd677b3ab31 100644
--- a/test/gtest_processor_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_processor_force_torque_inertial_dynamics.cpp
@@ -129,7 +129,7 @@ TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, not_moving_test)
betw_states << dpi, dvi, dpd, dvd, dL, dq.coeffs();
- VectorXd delta_preintegrated = C_KF1->getFeatureList().back()->getMeasurement();
+ VectorXd delta_preintegrated = C_KF1->getFeatureList().front()->getMeasurement();
//Cheking that the preintegrated delta calculated by the processor is the same as the one that is correct for sure
ASSERT_MATRIX_APPROX(betw_states, delta_preintegrated, 1e-8);
@@ -197,7 +197,7 @@ TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, 1m_x_moving_test__com_zer
betw_states << dpi, dvi, dpd, dvd, dL, dq.coeffs();
- VectorXd delta_preintegrated = C_KF1->getFeatureList().back()->getMeasurement();
+ VectorXd delta_preintegrated = C_KF1->getFeatureList().front()->getMeasurement();
//Cheking that the preintegrated delta calculated by the processor is the same as the one that is correct for sure
ASSERT_MATRIX_APPROX(betw_states, delta_preintegrated, 1e-8);
diff --git a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..64ab43627674118f196b43bccc4b05c15fed4ba8
--- /dev/null
+++ b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
@@ -0,0 +1,560 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
+#include "bodydynamics/factor/factor_angular_momentum.h"
+#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
+#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
+#include "bodydynamics/internal/config.h"
+#include "bodydynamics/capture/capture_force_torque_inertial.h"
+
+#include <core/sensor/factory_sensor.h>
+
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+#include <core/utils/params_server.h>
+#include <core/yaml/parser_yaml.h>
+#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 <core/tree_manager/tree_manager_sliding_window.h>
+#include <core/capture/capture_pose.h>
+
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+#include <iostream>
+#include <sstream>
+#include <fstream>
+#include <string>
+
+using namespace wolf;
+using namespace bodydynamics::fti;
+
+WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
+
+class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::Test
+{
+ public:
+ ProblemPtr P;
+ SolverCeresPtr solver;
+
+ // FTI sensor
+ SensorForceTorqueInertialPtr S;
+ ProcessorForceTorqueInertialDynamicsPtr p;
+ Vector6d bias_true;
+ Vector3d cdm_true;
+ Vector3d inertia_true;
+ double mass_true;
+ Matrix<double, 12, 1> data_fti;
+
+ // Pose sensor
+ SensorBasePtr SP;
+ Vector7d data_pose;
+
+ double dt;
+
+ // Reading csv data_fti
+ std::vector<double> time_stamp;
+ std::vector<Vector3d> position, vlin, vang, force, torque, a_meas;
+ std::vector<Quaterniond> quaternion;
+
+ protected:
+ void extractAndCompleteData(const std::string& file_path_name)
+ {
+ Vector3d position_i, vlin_i, vang_i, force_i, torque_i, a_meas_i;
+ std::ifstream data_simulation;
+ string data_file = file_path_name;
+ data_simulation.open(data_file, std::ios::in);
+ string line_data;
+ char delimiter = ',';
+ std::getline(data_simulation, line_data);
+
+ int counter = 0;
+
+ while (std::getline(data_simulation, line_data))
+ {
+ std::stringstream data_str(line_data);
+
+ // time stamp
+ string string_time_stamp;
+ std::getline(data_str, string_time_stamp, delimiter);
+ time_stamp.push_back(std::stod(string_time_stamp));
+
+ // position
+ string string_pos_x;
+ string string_pos_y;
+ string string_pos_z;
+
+ std::getline(data_str, string_pos_x, delimiter);
+ std::getline(data_str, string_pos_y, delimiter);
+ std::getline(data_str, string_pos_z, delimiter);
+
+ position_i(0) = std::stod(string_pos_x);
+ position_i(1) = std::stod(string_pos_y);
+ position_i(2) = std::stod(string_pos_z);
+
+ position.push_back(position_i);
+
+ // quaternion
+ string string_quat_x;
+ string string_quat_y;
+ string string_quat_z;
+ string string_quat_w;
+
+ std::getline(data_str, string_quat_x, delimiter);
+ std::getline(data_str, string_quat_y, delimiter);
+ std::getline(data_str, string_quat_z, delimiter);
+ std::getline(data_str, string_quat_w, delimiter);
+
+ Quaterniond quaternion_i(std::stod(string_quat_w),
+ std::stod(string_quat_x),
+ std::stod(string_quat_y),
+ std::stod(string_quat_z));
+
+ quaternion.push_back(quaternion_i);
+
+ // linear velocity
+ string string_vlin_x;
+ string string_vlin_y;
+ string string_vlin_z;
+
+ std::getline(data_str, string_vlin_x, delimiter);
+ std::getline(data_str, string_vlin_y, delimiter);
+ std::getline(data_str, string_vlin_z, delimiter);
+
+ vlin_i(0) = std::stod(string_vlin_x);
+ vlin_i(1) = std::stod(string_vlin_y);
+ vlin_i(2) = std::stod(string_vlin_z);
+
+ vlin.push_back(vlin_i);
+
+ // angular velocity
+ string string_vang_x;
+ string string_vang_y;
+ string string_vang_z;
+
+ std::getline(data_str, string_vang_x, delimiter);
+ std::getline(data_str, string_vang_y, delimiter);
+ std::getline(data_str, string_vang_z, delimiter);
+
+ vang_i(0) = std::stod(string_vang_x);
+ vang_i(1) = std::stod(string_vang_y);
+ vang_i(2) = std::stod(string_vang_z);
+
+ vang.push_back(vang_i);
+
+ // force
+ string string_force_x;
+ string string_force_y;
+ string string_force_z;
+
+ std::getline(data_str, string_force_x, delimiter);
+ std::getline(data_str, string_force_y, delimiter);
+ std::getline(data_str, string_force_z, delimiter);
+
+ force_i(0) = std::stod(string_force_x);
+ force_i(1) = std::stod(string_force_y);
+ force_i(2) = std::stod(string_force_z);
+
+ force.push_back(force_i);
+
+ // torque
+ string string_torque_x;
+ string string_torque_y;
+ string string_torque_z;
+
+ std::getline(data_str, string_torque_x, delimiter);
+ std::getline(data_str, string_torque_y, delimiter);
+ std::getline(data_str, string_torque_z, delimiter);
+
+ torque_i(0) = std::stod(string_torque_x);
+ torque_i(1) = std::stod(string_torque_y);
+ torque_i(2) = std::stod(string_torque_z);
+
+ torque.push_back(torque_i);
+
+ // acceleration -- need to compute from other data
+ a_meas_i = force_i/mass_true;
+
+ a_meas.push_back(a_meas_i);
+
+ counter++;
+ }
+ }
+
+ void SetUp() override
+ {
+ bias_true = Vector6d::Zero();
+ cdm_true = {0, 0, 0};
+ inertia_true = {0.0134943, 0.0141622, 0.0237319};
+ mass_true = 1.9828;
+
+ std::string wolf_bodydynamics_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+
+ extractAndCompleteData(wolf_bodydynamics_root + "/test/dades_simulacio_pep.csv");
+
+
+ ParserYaml parser = ParserYaml("problem_force_torque_inertial_dynamics_simulation_test.yaml",
+ wolf_bodydynamics_root + "/test/yaml/");
+ ParamsServer server = ParamsServer(parser.getParams());
+ P = Problem::autoSetup(server);
+
+ solver = std::make_shared<SolverCeres>(P);
+ auto options = solver->getSolverOptions();
+ // solver->getSolverOptions().max_num_iterations = 1e4;
+ // solver->getSolverOptions().function_tolerance = 1e-15;
+ // solver->getSolverOptions().gradient_tolerance = 1e-15;
+ // solver->getSolverOptions().parameter_tolerance = 1e-15;
+
+ // FTI sensor and processor
+ S = std::static_pointer_cast<SensorForceTorqueInertial>(P->findSensor("sensor FTI"));
+ p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
+
+ // Pose sensor
+ SP = P->findSensor("sensor Pose");
+ }
+};
+
+TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, force_registration)
+{
+ FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
+}
+
+//this test checks the pre-integration evolution along the time
+TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, preintegration_and_csv)
+{
+ std::string wolf_bodydynamics_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+ // creem un nou arxiu CSV per imprimir els estats que estem rebent ara
+ std::fstream fout;
+ fout.open(wolf_bodydynamics_root + "/test/dades_simulacio_pep_estimador.csv",
+ std::fstream::out | std::fstream::trunc);
+
+ S->getStateBlock('I')->setState(bias_true);
+ S->getStateBlock('C')->setState(cdm_true);
+ S->getStateBlock('i')->setState(inertia_true);
+ S->getStateBlock('m')->setState(Vector1d(mass_true));
+
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->fix();
+ S->getStateBlock('C')->fix();
+ S->getStateBlock('i')->fix();
+ S->getStateBlock('m')->fix();
+ S->setStateBlockDynamic('I', false);
+
+ //Writing the first lines to know how the data will be distributed in the csv
+ fout << "time stamp"
+ << ","
+ << "position_x_true"
+ << ","
+ << "position_y_true"
+ << ","
+ << "position_z_true"
+ << ","
+ << "quaternion_x_true"
+ << ","
+ << "quaternion_y_true"
+ << ","
+ << "quaternion_z_true"
+ << ","
+ << "quaternion_w_true"
+ << ","
+ << "position_x_est"
+ << ","
+ << "position_y_est"
+ << ","
+ << "position_z_est"
+ << ","
+ << "quaternion_x_est"
+ << ","
+ << "quaternion_y_est"
+ << ","
+ << "quaternion_z_est"
+ << ","
+ << "quaternion_w_est"
+ << "\n";
+
+ // Force processor to make a KF at t=0
+ CaptureMotionPtr C0 =
+ std::make_shared<CaptureForceTorqueInertial>(0, S, VectorXd::Zero(12), MatrixXd::Identity(12, 12), nullptr);
+ C0->process();
+
+ int i_init = 0;
+
+ // fix measured position and orientation
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i_init]);
+ p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i_init].coeffs());
+ p->getOrigin()->getFrame()->getStateBlock('V')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('V')->setState(quaternion[i_init].conjugate() * vlin[i_init]);
+ Vector3d ang_mom_init(
+ vang[i_init](0) * inertia_true(0), vang[i_init](1) * inertia_true(1), vang[i_init](2) * inertia_true(2));
+ p->getOrigin()->getFrame()->getStateBlock('L')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('L')->setState(ang_mom_init);
+
+
+ for (int i = i_init + 1; i < time_stamp.size(); i++) // start with second data_fti
+ {
+ // SIMULATOR
+
+ TimeStamp t = time_stamp[i];
+
+ // Data
+ data_fti << a_meas[i], vang[i], force[i], torque[i];
+
+ // ESTIMATOR
+
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data_fti, S->getNoiseCov(), nullptr);
+ C->process();
+
+ // results printed in the csv in the order established before
+ Vector3d position_est(p->getLast()->getFrame()->getStateBlock('P')->getState());
+ Vector4d quaternion_coeffs_est(p->getLast()->getFrame()->getStateBlock('O')->getState());
+
+ fout << time_stamp[i] << "," << position[i](0) << "," << position[i](1) << "," << position[i](2) << ","
+ << quaternion[i].coeffs()(0) << "," << quaternion[i].coeffs()(1) << "," << quaternion[i].coeffs()(2)
+ << "," << quaternion[i].coeffs()(3) << "," << position_est(0) << "," << position_est(1) << ","
+ << position_est(2) << "," << quaternion_coeffs_est(0) << "," << quaternion_coeffs_est(1) << ","
+ << quaternion_coeffs_est(2) << "," << quaternion_coeffs_est(3)
+ << "\n";
+
+ }
+ fout.close();
+}
+
+TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online)
+{
+ // Calibration params
+ Vector6d bias_guess = S->getStateBlock('I')->getState();
+ Vector3d cdm_guess = S->getStateBlock('C')->getState();
+ Vector3d inertia_guess = S->getStateBlock('i')->getState();
+ double mass_guess = S->getStateBlock('m')->getState()(0);
+
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->unfix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->unfix();
+ S->getStateBlock('m')->unfix();
+ S->setStateBlockDynamic('I', false);
+
+
+ // add regularization, uncomment if the parameter is not fixed
+
+ S->addPriorParameter('I', // cdm
+ bias_guess, // cdm
+ 0.1*0.1*Matrix6d::Identity()); // cov
+
+ S->addPriorParameter('C', // cdm
+ cdm_guess, // cdm
+ 0.1*0.1*Matrix3d::Identity()); // cov
+
+ S->addPriorParameter('i', // inertia
+ inertia_guess, // inertia
+ 0.01*0.01*Matrix3d::Identity()); // cov
+
+ S->addPriorParameter('m', // mass
+ Vector1d(mass_guess), // mass
+ 0.1 * 0.1 * Matrix1d::Identity()); // cov
+
+ std::string report;
+
+ // Force processor to make a KF at t=0
+ CaptureMotionPtr C0 =
+ std::make_shared<CaptureForceTorqueInertial>(0, S, VectorXd::Zero(12), MatrixXd::Identity(12, 12), nullptr);
+ C0->process();
+
+ int i_init = 0;
+
+ // Pose
+ data_pose << position[i_init] , quaternion[i_init].coeffs();
+ CaptureBasePtr CP = std::make_shared<CapturePose>(time_stamp[i_init], SP, data_pose, SP->getNoiseCov());
+ CP->process();
+
+ unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
+
+ for (int i = i_init + 1; i < time_stamp.size(); i++) // start with second data
+ {
+ // SIMULATOR
+
+ TimeStamp t = time_stamp[i];
+
+ // Data
+ data_fti << a_meas[i], vang[i], force[i], torque[i];
+ data_pose << position[i] , quaternion[i].coeffs();
+
+ // ESTIMATOR
+
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data_fti, S->getNoiseCov(), nullptr);
+ C->process();
+
+ CaptureBasePtr CP = std::make_shared<CapturePose>(t, SP, data_pose, SP->getNoiseCov());
+ CP->process();
+
+ // check if new KF and solve
+ if (last_kf_id != p->getOrigin()->getFrame()->id())
+ {
+ last_kf_id = p->getOrigin()->getFrame()->id();
+
+ // solve!
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+
+ // results of this iteration
+ WOLF_INFO("Time : ", t, " s.");
+ WOLF_INFO(report);
+ WOLF_INFO("Estimated bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+
+ }
+ }
+
+ // FINAL RESULTS
+ WOLF_INFO("True bias * : ", bias_true.transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("Guess bias : ", bias_guess.transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("Estimated bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("True center of mass * : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess center of mass : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("True inertia * : ", inertia_true.transpose(), " m^2 Kg.");
+ WOLF_INFO("Guess inertia : ", inertia_guess.transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("True mass * : ", mass_true, " Kg.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+
+ ASSERT_MATRIX_APPROX(bias_true , S->getStateBlock('I')->getState(), 0.11);
+ ASSERT_MATRIX_APPROX(cdm_true , S->getStateBlock('C')->getState(), 1e-2);
+ ASSERT_MATRIX_APPROX(inertia_true, S->getStateBlock('i')->getState(), 1e-2);
+ ASSERT_NEAR (mass_true , S->getStateBlock('m')->getState()(0), 2e-2);
+}
+
+TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
+{
+ // Calibration params
+ Vector6d bias_guess = S->getStateBlock('I')->getState();
+ Vector3d cdm_guess = S->getStateBlock('C')->getState();
+ Vector3d inertia_guess = S->getStateBlock('i')->getState();
+ double mass_guess = S->getStateBlock('m')->getState()(0);
+
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->unfix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->unfix();
+ S->getStateBlock('m')->unfix();
+ S->setStateBlockDynamic('I', false);
+
+
+ // // add regularization, uncomment if the parameter is not fixed
+
+ // S->addPriorParameter('I', // bias
+ // bias_guess, // bias
+ // 0.1 * 0.1 * Matrix3d::Identity()); // cov
+
+ // S->addPriorParameter('C', // cdm
+ // cdm_guess, // cdm
+ // 0.01 * 0.01 * Matrix3d::Identity()); // cov
+
+ // S->addPriorParameter('i', // inertia
+ // inertia_guess, // inertia
+ // 0.01 * 0.01 * Matrix3d::Identity()); // cov
+
+ // S->addPriorParameter('m', // mass
+ // Vector1d(mass_guess), // mass
+ // 0.01 * 0.01 * Matrix1d::Identity()); // cov
+
+ int i_init = 0;
+
+ // Force FTI processor to make a KF at t=0
+ CaptureMotionPtr C0 =
+ std::make_shared<CaptureForceTorqueInertial>(0, S, VectorXd::Zero(12), S->getNoiseCov(), nullptr);
+ C0->process();
+
+ // Pose
+ data_pose << position[i_init] , quaternion[i_init].coeffs();
+ CaptureBasePtr CP = std::make_shared<CapturePose>(time_stamp[i_init], SP, data_pose, SP->getNoiseCov());
+ CP->process();
+
+ for (int i = i_init + 1; i < time_stamp.size(); i++) // start with second data
+ {
+ // SIMULATOR
+ TimeStamp t = time_stamp[i];
+
+ // Data
+ data_fti << a_meas[i], vang[i], force[i], torque[i];
+ data_pose << position[i] , quaternion[i].coeffs();
+
+ // ESTIMATOR
+
+ // fti
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data_fti, S->getNoiseCov(), nullptr);
+ C->process();
+
+ // Pose
+ CaptureBasePtr CP = std::make_shared<CapturePose>(t, SP, data_pose, SP->getNoiseCov());
+ CP->process();
+ }
+
+ std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+
+
+ // FINAL RESULTS
+ WOLF_INFO("-----------------------------");
+ WOLF_INFO(report);
+ WOLF_INFO("True bias * : ", bias_true.transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("Guess bias : ", bias_guess.transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("Estimated bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("True center of mass * : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess center of mass : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("True inertia * : ", inertia_true.transpose(), " m^2 Kg.");
+ WOLF_INFO("Guess inertia : ", inertia_guess.transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("True mass * : ", mass_true, " Kg.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+
+ ASSERT_MATRIX_APPROX(bias_true , S->getStateBlock('I')->getState(), 0.21);
+ ASSERT_MATRIX_APPROX(cdm_true , S->getStateBlock('C')->getState(), 2e-3);
+ ASSERT_MATRIX_APPROX(inertia_true, S->getStateBlock('i')->getState(), 1e-2);
+ ASSERT_NEAR (mass_true , S->getStateBlock('m')->getState()(0), 4e-2);
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ // ::testing::GTEST_FLAG(filter) =
+ // "Test_SimulationProblemForceTorqueInertialDynamics_yaml.preintegration_and_csv";
+ ::testing::GTEST_FLAG(filter) =
+ "Test_SimulationProblemForceTorqueInertialDynamics_yaml.simulation*";
+
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index cc97b1c52db99beb3495c61311a48976c171352e..43629f400a4f763490ea6bd6210deaae1104ae80 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -21,6 +21,7 @@
//--------LICENSE_END--------
#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
+#include "bodydynamics/factor/factor_angular_momentum.h"
#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
#include "bodydynamics/internal/config.h"
@@ -35,6 +36,8 @@
#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 <core/tree_manager/tree_manager_sliding_window.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
@@ -51,6 +54,7 @@ class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test
SensorForceTorqueInertialPtr S;
ProcessorForceTorqueInertialDynamicsPtr p;
SolverCeresPtr solver;
+ Vector6d bias_true;
Vector3d cdm_true;
Vector3d inertia_true;
double mass_true;
@@ -73,8 +77,9 @@ class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test
S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
+ bias_true = Vector6d::Zero();
cdm_true = {0, 0, 0.0341};
- inertia_true = {0.017598, 0.017957, 0.029599};
+ inertia_true = {0.0176, 0.0180, 0.0296}; // rounded {0.017598, 0.017957, 0.029599}
mass_true = 1.952;
}
};
@@ -135,8 +140,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_only_hovering)
WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
WOLF_INFO("-----------------------------");
-
- for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 7.0 ; t += 1.0)
+ for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0; t <= 7.0; t += 1.0)
{
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->process();
@@ -148,15 +152,14 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_only_hovering)
P->print(4, 1, 1, 1);
-
ASSERT_NEAR(S->getStateBlock('m')->getState()(0), mass_true, 1e-3);
}
TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
{
- S->getStateBlock('C')->setState(Vector3d(0.01, 0.01, 0.033));
+ Vector3d cdm_guess(0.01, 0.01, 0.033);
+ S->getStateBlock('C')->setState(cdm_guess);
S->getStateBlock('m')->setState(Vector1d(mass_true));
- Vector3d cdm_guess = S->getStateBlock('C')->getState();
// Data
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
@@ -173,12 +176,12 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
S->getStateBlock('m')->fix();
// S->setStateBlockDynamic('I');
- CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>( 0.0, S, data, data_cov, nullptr);
- CaptureMotionPtr C1_0 = std::make_shared<CaptureForceTorqueInertial>( 0.2, S, data, data_cov, nullptr);
- CaptureMotionPtr C2_0 = std::make_shared<CaptureForceTorqueInertial>( 0.4, S, data, data_cov, nullptr);
- CaptureMotionPtr C3_0 = std::make_shared<CaptureForceTorqueInertial>( 0.6, S, data, data_cov, nullptr);
- CaptureMotionPtr C4_0 = std::make_shared<CaptureForceTorqueInertial>( 0.8, S, data, data_cov, nullptr);
- CaptureMotionPtr C5_1 = std::make_shared<CaptureForceTorqueInertial>( 1.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C1_0 = std::make_shared<CaptureForceTorqueInertial>(0.2, S, data, data_cov, nullptr);
+ CaptureMotionPtr C2_0 = std::make_shared<CaptureForceTorqueInertial>(0.4, S, data, data_cov, nullptr);
+ CaptureMotionPtr C3_0 = std::make_shared<CaptureForceTorqueInertial>(0.6, S, data, data_cov, nullptr);
+ CaptureMotionPtr C4_0 = std::make_shared<CaptureForceTorqueInertial>(0.8, S, data, data_cov, nullptr);
+ CaptureMotionPtr C5_1 = std::make_shared<CaptureForceTorqueInertial>(1.0, S, data, data_cov, nullptr);
C0_0->process();
CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
@@ -191,7 +194,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
C_KF0->getFrame()->fix();
- C_KF1->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
+ C_KF1->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
C_KF1->getFrame()->fix();
P->print(4, 1, 1, 1);
@@ -205,27 +208,31 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("-----------------------------");
- for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 11.0 ; t += 1.0)
+ double dt = 0.2;
+ double time_last = 5.0;
+
+ for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
{
- CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>( t, S, data, data_cov, nullptr);
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->process();
- p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
+ p->getOrigin()->getFrame()->getStateBlock('L')->setState(
+ Vector3d::Zero()); // Make sure L is not moving due to torques
p->getOrigin()->getFrame()->fix();
report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("-----------------------------");
}
-
- ASSERT_MATRIX_APPROX(S->getStateBlock('C')->getState().head(2), cdm_true.head(2), 1e-5); // cdm_z is not observable while hovering
+ ASSERT_MATRIX_APPROX(
+ S->getStateBlock('C')->getState().head(2), cdm_true.head(2), 1e-5); // cdm_z is not observable while hovering
}
TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
{
- S->getStateBlock('C')->setState(Vector3d(0.005, 0.005, 0.05));
- S->getStateBlock('m')->setState(Vector1d(1.900));
- Vector3d cdm_guess = S->getStateBlock('C')->getState();
- double mass_guess = S->getStateBlock('m')->getState()(0);
+ Vector3d cdm_guess(0.005, 0.005, 0.05);
+ double mass_guess = 1.9;
+ S->getStateBlock('C')->setState(cdm_guess);
+ S->getStateBlock('m')->setState(Vector1d(mass_guess));
// Data
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
@@ -260,7 +267,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
C_KF0->getFrame()->fix();
- C_KF1->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
+ C_KF1->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
C_KF1->getFrame()->fix();
P->print(4, 1, 1, 1);
@@ -277,37 +284,883 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("-----------------------------");
+ // Do a number of iterations with more keyframes, solving so that the calibration gets better and better
+ // Here we take advantage of the sliding window, thereby getting rid progressively
+ // of the old factors, which contained calibration parameters far from the converged values,
+ // which if not eliminated contaminate the overall solution.
+ // This is due to these first factors relying on the linearization Jacobian to correct the
+ // poorly preintegrated delta.
+
+ double dt = 0.2;
+ double time_last = 5.0;
+
+ for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
+ {
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->setTimeStamp(t);
+ C->process();
+ p->getOrigin()->getFrame()->getStateBlock('L')->setState(
+ Vector3d::Zero()); // Make sure L is not moving due to torques
+ p->getOrigin()->getFrame()->fix();
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
+ WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("-----------------------------");
+ }
+
+ auto cdm_estimated = S->getStateBlock('C')->getState();
+ auto mass_estimated = S->getStateBlock('m')->getState()(0);
+
+ ASSERT_NEAR(mass_estimated, mass_true, 1e-6);
+ ASSERT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-6); // cdm_z is not observable while hovering
+}
+
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_and_ang_mom_hovering)
+{
+ Vector3d cdm_guess(0.02, 0.02, 0.033);
+ S->getStateBlock('C')->setState(cdm_guess);
+ S->getStateBlock('m')->setState(Vector1d(mass_true));
+
+ // Data
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ data.segment<3>(6) = -mass_true * gravity();
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ // We fix the parameters of the sensor except for the cdm
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->fix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->fix();
+ S->getStateBlock('m')->fix();
+ // S->setStateBlockDynamic('I');
+
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C1_0 = std::make_shared<CaptureForceTorqueInertial>(0.2, S, data, data_cov, nullptr);
+ CaptureMotionPtr C2_0 = std::make_shared<CaptureForceTorqueInertial>(0.4, S, data, data_cov, nullptr);
+ CaptureMotionPtr C3_0 = std::make_shared<CaptureForceTorqueInertial>(0.6, S, data, data_cov, nullptr);
+ CaptureMotionPtr C4_0 = std::make_shared<CaptureForceTorqueInertial>(0.8, S, data, data_cov, nullptr);
+ CaptureMotionPtr C5_1 = std::make_shared<CaptureForceTorqueInertial>(1.0, S, data, data_cov, nullptr);
+
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+ C1_0->process();
+ C2_0->process();
+ C3_0->process();
+ C4_0->process();
+ C5_1->process();
+
+ CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ C_KF0->getFrame()->fix();
+ C_KF0->getFrame()->getStateBlock('L')->unfix();
+ C_KF1->getFrame()->fix();
+ C_KF1->getFrame()->getStateBlock('L')->unfix();
+
+ P->print(4, 1, 1, 1);
+
+ 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)
+
+ WOLF_INFO("True cdm : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess cdm : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("-----------------------------");
+
+ double dt = 0.2;
+ double time_last = 10.0;
+
+ for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
+ {
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->process();
+ p->getOrigin()->getFrame()->fix();
+ p->getOrigin()->getFrame()->getStateBlock('L')->unfix();
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("Estimated ang mom : ",
+ p->getOrigin()->getFrame()->getStateBlock('L')->getState().transpose(),
+ " m^2 kg/s.");
+ WOLF_INFO("-----------------------------");
+ }
+
+ ASSERT_MATRIX_APPROX(
+ S->getStateBlock('C')->getState().head(2), cdm_true.head(2), 1e-5); // cdm_z is not observable while hovering
+}
+
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_mom_hovering)
+{
+ Vector3d cdm_guess(0.05, 0.05, 0.05);
+ double mass_guess = 1.50;
+ S->getStateBlock('C')->setState(cdm_guess);
+ S->getStateBlock('m')->setState(Vector1d(mass_guess));
+
+ // Data
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ data.segment<3>(6) = -mass_true * gravity();
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ // We fix the parameters of the sensor except for the cdm and the mass
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->fix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->fix();
+ S->getStateBlock('m')->unfix();
+ S->setStateBlockDynamic('I', false);
+
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C1_0 = std::make_shared<CaptureForceTorqueInertial>(0.2, S, data, data_cov, nullptr);
+ CaptureMotionPtr C2_0 = std::make_shared<CaptureForceTorqueInertial>(0.4, S, data, data_cov, nullptr);
+ CaptureMotionPtr C3_0 = std::make_shared<CaptureForceTorqueInertial>(0.6, S, data, data_cov, nullptr);
+ CaptureMotionPtr C4_0 = std::make_shared<CaptureForceTorqueInertial>(0.8, S, data, data_cov, nullptr);
+ CaptureMotionPtr C5_1 = std::make_shared<CaptureForceTorqueInertial>(1.0, S, data, data_cov, nullptr);
+
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+ C1_0->process();
+ C2_0->process();
+ C3_0->process();
+ C4_0->process();
+ C5_1->process();
+
+ CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ C_KF0->getFrame()->fix();
+ C_KF0->getFrame()->getStateBlock('L')->unfix();
+ C_KF1->getFrame()->fix();
+ C_KF1->getFrame()->getStateBlock('L')->unfix();
+
+ P->print(4, 1, 1, 1);
+
+ 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)
+
+ WOLF_INFO("True mass : ", mass_true, " Kg.");
+ WOLF_INFO("True cdm : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Guess cdm : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
+ WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("-----------------------------");
// Do a number of iterations with more keyframes, solving so that the calibration gets better and better
- // Here we take advantage of the sliding window, thereby getting rid progressively
+ // Here we take advantage of the sliding window, thereby getting rid progressively
// of the old factors, which contained calibration parameters far from the converged values,
// which if not eliminated contaminate the overall solution.
// This is due to these first factors relying on the linearization Jacobian to correct the
// poorly preintegrated delta.
- for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 7.0 ; t += 1.0)
+
+ double dt = 0.2;
+ double time_last = 10.0;
+
+ for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
{
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->setTimeStamp(t);
C->process();
- p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
p->getOrigin()->getFrame()->fix();
+ p->getOrigin()->getFrame()->getStateBlock('L')->unfix();
report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("Estimated ang mom : ",
+ p->getOrigin()->getFrame()->getStateBlock('L')->getState().transpose(),
+ " m^2 kg/s.");
+
WOLF_INFO("-----------------------------");
}
auto cdm_estimated = S->getStateBlock('C')->getState();
auto mass_estimated = S->getStateBlock('m')->getState()(0);
+ ASSERT_NEAR(mass_estimated, mass_true, 1e-6);
+ ASSERT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-6); // cdm_z is not observable while hovering
+}
+
+// Here we only fix P and O from each key frame
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_fixing)
+{
+ Vector3d cdm_guess(0.05, 0.05, 0.05);
+ double mass_guess = 1.50;
+ S->getStateBlock('C')->setState(cdm_guess);
+ S->getStateBlock('m')->setState(Vector1d(mass_guess));
+
+ // Data
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ data.segment<3>(6) = -mass_true * gravity();
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ // We fix the parameters of the sensor except for the cdm and the mass
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->fix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->fix();
+ S->getStateBlock('m')->unfix();
+ S->setStateBlockDynamic('I', false);
+
+ // add regularization to unobservable states
+ S->addPriorParameter('C', // cdm
+ Vector1d(cdm_true(2)), // cdm Z
+ Matrix1d::Identity() * 1, // cov Z
+ 2, // start: Z coordinate
+ 1); // size
+ S->addPriorParameter('i', // inertia
+ Vector2d(inertia_true.segment<2>(0)), // inertia XY
+ Matrix2d::Identity() * 0.01, // cov XY
+ 0, // start: X coordinate
+ 2); // size: 2
+
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C1_0 = std::make_shared<CaptureForceTorqueInertial>(0.2, S, data, data_cov, nullptr);
+ CaptureMotionPtr C2_0 = std::make_shared<CaptureForceTorqueInertial>(0.4, S, data, data_cov, nullptr);
+ CaptureMotionPtr C3_0 = std::make_shared<CaptureForceTorqueInertial>(0.6, S, data, data_cov, nullptr);
+ CaptureMotionPtr C4_0 = std::make_shared<CaptureForceTorqueInertial>(0.8, S, data, data_cov, nullptr);
+ CaptureMotionPtr C5_1 = std::make_shared<CaptureForceTorqueInertial>(1.0, S, data, data_cov, nullptr);
+
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+ C1_0->process();
+ C2_0->process();
+ C3_0->process();
+ C4_0->process();
+ C5_1->process();
+
+ CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ C_KF0->getFrame()->unfix();
+ C_KF0->getFrame()->getStateBlock('P')->fix();
+ C_KF0->getFrame()->getStateBlock('O')->fix();
+ C_KF1->getFrame()->unfix();
+ C_KF1->getFrame()->getStateBlock('P')->fix();
+ C_KF1->getFrame()->getStateBlock('O')->fix();
+
+ P->print(4, 1, 1, 1);
+
+ WOLF_INFO("======== SOLVE PROBLEM =======")
+ std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO(report);
+
+ WOLF_INFO("True mass : ", mass_true, " Kg.");
+ WOLF_INFO("True cdm : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Guess cdm : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
+ WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("-----------------------------");
+
+ // Do a number of iterations with more keyframes, solving so that the calibration gets better and better
+ // Here we take advantage of the sliding window, thereby getting rid progressively
+ // of the old factors, which contained calibration parameters far from the converged values,
+ // which if not eliminated contaminate the overall solution.
+ // This is due to these first factors relying on the linearization Jacobian to correct the
+ // poorly preintegrated delta.
+
+ double dt = 0.2;
+ double time_last = 10.0;
+
+ for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
+ {
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->setTimeStamp(t);
+ C->process(); // En aquesta lÃnia el programa s'aborta
+ // p->getOrigin()->getFrame()->unfix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
+ WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("Estimated ang mom : ",
+ p->getOrigin()->getFrame()->getStateBlock('L')->getState().transpose(),
+ " m^2 kg/s.");
+
+ WOLF_INFO("-----------------------------");
+ }
+
+ auto cdm_estimated = S->getStateBlock('C')->getState();
+ auto mass_estimated = S->getStateBlock('m')->getState()(0);
ASSERT_NEAR(mass_estimated, mass_true, 1e-6);
- ASSERT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-6); // cdm_z is not observable while hovering
+ ASSERT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-6); // cdm_z is not observable while hovering
+}
+
+// Here we only fix P and O from each key frame
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant)
+{
+ Vector3d inertia_guess(0.01, 0.01, 0.02);
+ S->getStateBlock('i')->setState(inertia_guess);
+
+ // Data
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ Vector3d ang_vel_measure_initial(0, 0, 0);
+ data.segment<3>(3) = ang_vel_measure_initial;
+ data.segment<3>(6) = -mass_true * gravity();
+ Vector3d torque_measure_constant(0, 0, 1);
+ data.segment<3>(9) = torque_measure_constant;
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->fix();
+ S->getStateBlock('C')->fix();
+ S->getStateBlock('i')->unfix();
+ S->getStateBlock('m')->fix();
+ // S->setStateBlockDynamic('I');
+
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+ 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('P')->setState(Vector3d(0, 0, 0));
+ C_KF0->getFrame()->getStateBlock('O')->fix();
+ C_KF0->getFrame()->getStateBlock('O')->setState((Quaterniond(1, 0, 0, 0)).coeffs());
+
+ // initialiation of all the variables needed in the following iteration process
+ 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 quat_true(1, 0, 0, 0);
+ double dt = 0.2; // time increment
+ Vector3d position_data(0, 0, 0);
+ Quaterniond q_data;
+ std::string report;
+
+ for (TimeStamp t = dt; t <= 5.0; t += dt)
+ {
+ // SIMULATOR
+
+ // calculate dL = torque*dt
+ dL = torque_true * dt;
+ // actualize L = L + dt
+ L_true = L_true + dL; // L = L + dt
+ // w = L*i_inv we actualize the data
+ 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;
+ quat_true = quat_true * exp_q(angle_true);
+
+ // ESTIMATOR
+
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->setTimeStamp(t);
+ C->process();
+
+ p->getOrigin()->getFrame()->unfix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->setState(position_data);
+ q_data = quat_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.transpose(), " m^2 Kg.");
+ WOLF_INFO("Guess inertia : ", inertia_guess.transpose(), " 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);
+}
+
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__unfix_all__zero_bias)
+{
+ // Simulation data -- hovering and constant torque in yaw
+ Vector3d acc_true = -gravity();
+ Vector3d ang_vel_true(0, 0, 0);
+ Vector3d force_true = -mass_true * gravity();
+ Vector3d torque_true(0, 0, 0.01);
+ Vector3d L_true(0, 0, 0);
+ Vector3d position_true(0, 0, 0);
+ Quaterniond quat_true(1, 0, 0, 0);
+ Vector3d
+ ang_acc_true; // = inertia_true.asDiagonal().inverse() * torque_true; // alpha = I.inv * torque is constant
+ // Vector3d ang_acc_true = (torque_true.array() / inertia_true.array()).matrix();
+ 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);
+ Vector3d angle_true(0, 0, 0);
+
+ // Data -- hovering and constant torque in yaw
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ data.segment<3>(3) = ang_vel_true;
+ data.segment<3>(6) = -mass_true * gravity();
+ data.segment<3>(9) = torque_true;
+ MatrixXd data_cov = S->getNoiseCov();
+
+ Vector3d position_data = position_true;
+ Vector4d orient_data = quat_true.coeffs();
+
+ // Calibration params
+ Vector6d bias_guess;
+ bias_guess << 0, 0, 0, 0, 0, 0;
+ S->getStateBlock('I')->setState(bias_guess);
+ Vector3d cdm_guess(0.005, 0.005, 0.1);
+ S->getStateBlock('C')->setState(cdm_guess);
+ Vector3d inertia_guess(0.01, 0.01, 0.02);
+ S->getStateBlock('i')->setState(inertia_guess);
+ double mass_guess = 2.0;
+ S->getStateBlock('m')->setState(Vector1d(mass_guess));
+
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->unfix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->unfix();
+ S->getStateBlock('m')->unfix();
+ S->setStateBlockDynamic('I', false);
+
+ // Process origin
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ // UAV states
+ C_KF0->getFrame()->unfix(); // JS: this is unnecessary, all states are unfixed by default
+ C_KF0->getFrame()->getStateBlock('P')->fix();
+ C_KF0->getFrame()->getStateBlock('P')->setState(position_data);
+ C_KF0->getFrame()->getStateBlock('O')->fix();
+ C_KF0->getFrame()->getStateBlock('O')->setState(orient_data);
+
+ // initialiation of all the variables needed in the following iteration process
+ // Vector3d dL;
+ double dt = 0.1; // time increment
+ unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
+ std::string report;
+
+ for (TimeStamp t = dt; t <= 10.0; t += dt)
+ {
+ // SIMULATOR
+
+ // angular momentum
+ // L_true += torque_true * dt; // FIXME JS: it seems this is not needed!
+
+ // ang velocity
+ ang_vel_true += ang_acc_true * dt;
+
+ // orientation (quaternion and total angle)
+ // FIXME JS: this should be on top but it works better here, weird...
+ quat_true *= exp_q(ang_vel_true * dt /*+ 0.5 * ang_acc_true * dt * dt*/);
+ angle_true += ang_vel_true * dt /*+ 0.5 * ang_acc_true * dt * dt*/;
+
+ // // w = L*i_inv we actualize the data
+ // 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);
+
+ // FTI measurements
+ data.segment<3>(0) = acc_true; // TODO: add acc bias
+ data.segment<3>(3) = ang_vel_true; // TODO: add gyro bias
+ data.segment<3>(6) = force_true;
+ data.segment<3>(9) = torque_true;
+
+ // Pose measurements (simulate motion capture)
+ position_data = position_true;
+ orient_data = quat_true.coeffs();
+
+ // ESTIMATOR
+
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->process();
+
+ // check if new KF
+ if (last_kf_id != p->getOrigin()->getFrame()->id())
+ {
+ last_kf_id = p->getOrigin()->getFrame()->id();
+
+ // fix measured position and orientation
+ p->getOrigin()->getFrame()->unfix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->setState(position_data);
+ p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('O')->setState(orient_data);
+
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+
+ WOLF_INFO("Total angle turned : ", angle_true.transpose(), " rad.");
+ WOLF_INFO("Estimated IMU bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+ }
+ }
+
+ WOLF_INFO("True IMU bias : ", bias_true.transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Guess IMU bias : ", bias_guess.transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Estimated IMU bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("True center of mass : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess center of mass : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("True inertia : ", inertia_true.transpose(), " m^2 Kg.");
+ WOLF_INFO("Guess inertia : ", inertia_guess.transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("True mass : ", mass_true, " Kg.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+
+ auto bias_estimated = S->getStateBlock('I')->getState();
+ auto cdm_estimated = S->getStateBlock('C')->getState();
+ auto inertia_estimated = S->getStateBlock('i')->getState();
+ auto mass_estimated = S->getStateBlock('m')->getState()(0);
+
+ ASSERT_MATRIX_APPROX(bias_estimated, bias_true, 1e-4);
+ ASSERT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-6); // cdm_z is not observable while hovering
+ ASSERT_NEAR(inertia_estimated(2), inertia_true(2), 1e-6); // Ix and Iy not observable in this test
+ ASSERT_NEAR(mass_estimated, mass_true, 1e-5);
+}
+
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__unfix_all__nonzero_bias)
+{
+ // Simulation data -- hovering and constant torque in yaw
+ Vector3d acc_true = -gravity();
+ Vector3d ang_vel_true = Vector3d(0, 0, 0);
+ Vector3d force_true = -mass_true * gravity();
+ Vector3d torque_true = Vector3d(0, 0, 0.01);
+ Vector3d position_true = Vector3d(0, 0, 0);
+ Quaterniond quat_true = Quaterniond(1, 0, 0, 0);
+ Vector3d ang_acc_true = inertia_true.asDiagonal().inverse() * torque_true;
+ Vector3d angle_true = Vector3d(0, 0, 0);
+ Vector3d acc_bias_true = 0.001 * Vector3d(1, 1, 1);
+ Vector3d gyro_bias_true = 0.001 * Vector3d(1, 1, 1);
+ bias_true << acc_bias_true, gyro_bias_true;
+ // bias_true.setZero();
+
+ // Data -- hovering and constant torque in yaw
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity() + acc_bias_true;
+ 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 = S->getNoiseCov();
+
+ Vector3d position_data = position_true;
+ Vector4d orient_data = quat_true.coeffs();
+
+ // Calibration params
+ Vector6d bias_guess = Vector6d::Zero();
+ // Vector6d bias_guess = bias_true;
+ Vector3d cdm_guess = Vector3d(0.005, 0.005, 0.1);
+ Vector3d inertia_guess = Vector3d(0.01, 0.01, 0.02);
+ double mass_guess = 2.0;
+ S->getStateBlock('I')->setState(bias_guess);
+ S->getStateBlock('C')->setState(cdm_guess);
+ S->getStateBlock('i')->setState(inertia_guess);
+ S->getStateBlock('m')->setState(Vector1d(mass_guess));
+
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->unfix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->unfix();
+ S->getStateBlock('m')->unfix();
+ S->setStateBlockDynamic('I', false);
+
+ // add regularization to unobservable states
+ S->addPriorParameter('C', // cdm
+ Vector1d(cdm_true(2)), // cdm Z
+ Matrix1d::Identity() * 1, // cov Z
+ 2, // start: Z coordinate
+ 1); // size
+ S->addPriorParameter('i', // inertia
+ Vector2d(inertia_true.segment<2>(0)), // inertia XY
+ Matrix2d::Identity() * 0.01, // cov XY
+ 0, // start: X coordinate
+ 2); // size: 2
+
+ // Process origin
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ // UAV states
+ // C_KF0->getFrame()->unfix(); // JS: this is unnecessary, all states are unfixed by default
+ C_KF0->getFrame()->getStateBlock('P')->fix();
+ C_KF0->getFrame()->getStateBlock('P')->setState(position_data);
+ C_KF0->getFrame()->getStateBlock('O')->fix();
+ C_KF0->getFrame()->getStateBlock('O')->setState(orient_data);
+
+ // initialiation of all the variables needed in the following iteration process
+ double t_final = 8.0; // total run time
+ double dt = 0.1; // time increment
+ unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
+ std::string report;
+
+ for (TimeStamp t = dt; t <= t_final; t += dt)
+ {
+ // SIMULATOR
+
+ // ang velocity
+ ang_vel_true += ang_acc_true * dt;
+
+ // orientation (quaternion and total angle)
+ quat_true *= exp_q(ang_vel_true * dt /*+ 0.5 * ang_acc_true * dt * dt*/);
+ angle_true += ang_vel_true * dt /*+ 0.5 * ang_acc_true * dt * dt*/;
+
+ // FTI measurements
+ data.segment<3>(0) = acc_true + acc_bias_true;
+ data.segment<3>(3) = ang_vel_true + gyro_bias_true;
+ data.segment<3>(6) = force_true;
+ data.segment<3>(9) = torque_true;
+
+ // Pose measurements (simulate motion capture)
+ position_data = position_true;
+ orient_data = quat_true.coeffs();
+
+ // ESTIMATOR
+
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->process();
+
+ // check if new KF
+ if (last_kf_id != p->getOrigin()->getFrame()->id())
+ {
+ last_kf_id = p->getOrigin()->getFrame()->id();
+
+ // fix measured position and orientation
+ // p->getOrigin()->getFrame()->unfix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->setState(position_data);
+ p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('O')->setState(orient_data);
+
+ // solve!
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+
+ // results of this iteration
+ WOLF_INFO("Total angle turned : ", angle_true.transpose(), " rad.");
+ WOLF_INFO("Estimated IMU bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+ }
+ }
+
+ // Final results
+ WOLF_INFO("True IMU bias * : ", bias_true.transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Guess IMU bias : ", bias_guess.transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Estimated IMU bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("True center of mass * : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess center of mass : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("True inertia * : ", inertia_true.transpose(), " m^2 Kg.");
+ WOLF_INFO("Guess inertia : ", inertia_guess.transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("True mass * : ", mass_true, " Kg.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+
+ auto bias_estimated = S->getStateBlock('I')->getState();
+ auto cdm_estimated = S->getStateBlock('C')->getState();
+ auto inertia_estimated = S->getStateBlock('i')->getState();
+ auto mass_estimated = S->getStateBlock('m')->getState()(0);
+
+ ASSERT_MATRIX_APPROX(bias_estimated, bias_true, 1e-4);
+ ASSERT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-6); // cdm_z is not observable while hovering
+ ASSERT_NEAR(inertia_estimated(2), inertia_true(2), 1e-6); // Ix and Iy not observable in this test
+ ASSERT_NEAR(mass_estimated, mass_true, 1e-5);
+}
+
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml,
+ torque_yaw_constant__unfix_all__nonzero_bias_and_nonzero_noise)
+{
+ // Simulation data -- hovering and constant torque in yaw
+ Vector3d acc_true = -gravity();
+ Vector3d ang_vel_true = Vector3d(0, 0, 0);
+ Vector3d force_true = -mass_true * gravity();
+ Vector3d torque_true = Vector3d(0, 0, 0.1);
+ Vector3d position_true = Vector3d(0, 0, 0);
+ Quaterniond quat_true = Quaterniond(1, 0, 0, 0);
+ Vector3d ang_acc_true = inertia_true.asDiagonal().inverse() * torque_true;
+ Vector3d angle_true = Vector3d(0, 0, 0);
+ Vector3d acc_bias_true = 0.001 * Vector3d(1, 1, 1);
+ Vector3d gyro_bias_true = 0.001 * Vector3d(1, 1, 1);
+ bias_true << acc_bias_true, gyro_bias_true;
+ // bias_true.setZero();
+
+ // Noise
+ Vector3d acc_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->acc_noise_std;
+ Vector3d gyro_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->gyro_noise_std;
+ Vector3d force_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->force_noise_std;
+ Vector3d torque_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->torque_noise_std;
+
+ // Data -- hovering and constant torque in yaw
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity() + acc_bias_true + acc_noise;
+ data.segment<3>(3) = ang_vel_true + gyro_bias_true + gyro_noise;
+ data.segment<3>(6) = -mass_true * gravity() + force_noise;
+ data.segment<3>(9) = torque_true + torque_noise;
+ MatrixXd data_cov = S->getNoiseCov();
+
+ Vector3d position_data = position_true;
+ Vector4d orient_data = quat_true.coeffs();
+
+ // Calibration params
+ Vector6d bias_guess = Vector6d::Zero();
+ // Vector6d bias_guess = bias_true;
+ Vector3d cdm_guess = Vector3d(0.005, 0.005, 0.1);
+ Vector3d inertia_guess = Vector3d(0.01, 0.01, 0.02);
+ double mass_guess = 2.0;
+ S->getStateBlock('I')->setState(bias_guess);
+ S->getStateBlock('C')->setState(cdm_guess);
+ S->getStateBlock('i')->setState(inertia_guess);
+ S->getStateBlock('m')->setState(Vector1d(mass_guess));
+
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->unfix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->unfix();
+ S->getStateBlock('m')->unfix();
+ S->setStateBlockDynamic('I', false);
+
+ // add regularization to unobservable states
+ S->addPriorParameter('C', // cdm
+ Vector1d(cdm_true(2)), // cdm Z
+ Matrix1d::Identity() * 1, // cov Z
+ 2, // start: Z coordinate
+ 1); // size
+ S->addPriorParameter('i', // inertia
+ Vector2d(inertia_true.segment<2>(0)), // inertia XY
+ Matrix2d::Identity() * .001, // cov XY
+ 0, // start: X coordinate
+ 2); // size: 2
+
+ // Process origin
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ // UAV states
+ // C_KF0->getFrame()->unfix(); // JS: this is unnecessary, all states are unfixed by default
+ C_KF0->getFrame()->getStateBlock('P')->fix();
+ C_KF0->getFrame()->getStateBlock('P')->setState(position_data);
+ C_KF0->getFrame()->getStateBlock('O')->fix();
+ C_KF0->getFrame()->getStateBlock('O')->setState(orient_data);
+
+ // initialiation of all the variables needed in the following iteration process
+ double t_final = 2.40; // total run time
+ double dt = 0.01; // time increment
+ unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
+ std::string report;
+
+ for (TimeStamp t = dt; t <= t_final; t += dt)
+ {
+ // SIMULATOR
+
+ // ang velocity
+ ang_vel_true += ang_acc_true * dt;
+
+ // orientation (quaternion and total angle)
+ quat_true *= exp_q(ang_vel_true * dt /*+ 0.5 * ang_acc_true * dt * dt*/);
+ angle_true += ang_vel_true * dt /*+ 0.5 * ang_acc_true * dt * dt*/;
+
+ // noises
+ acc_noise = Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->acc_noise_std;
+ gyro_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->gyro_noise_std;
+ force_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->force_noise_std;
+ torque_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->torque_noise_std;
+
+ // FTI measurements
+ data.segment<3>(0) = acc_true + acc_bias_true + acc_noise;
+ data.segment<3>(3) = ang_vel_true + gyro_bias_true + gyro_noise;
+ data.segment<3>(6) = force_true + force_noise;
+ data.segment<3>(9) = torque_true + torque_noise;
+ data_cov = S->getNoiseCov();
+
+ // Pose measurements (simulate motion capture)
+ position_data = position_true;
+ orient_data = quat_true.coeffs();
+
+ // ESTIMATOR
+
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->process();
+
+ // check if new KF
+ if (last_kf_id != p->getOrigin()->getFrame()->id())
+ {
+ last_kf_id = p->getOrigin()->getFrame()->id();
+
+ // fix measured position and orientation
+ // p->getOrigin()->getFrame()->unfix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->setState(position_data);
+ p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('O')->setState(orient_data);
+
+ // solve!
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+
+ // WOLF_INFO(report);
+ // P->print(4,1,1,1);
+
+ // results of this iteration
+ WOLF_INFO("Total angle turned : ", angle_true.transpose(), " rad.");
+ WOLF_INFO("Estimated IMU bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+ }
+ }
+
+ // Final results
+ WOLF_INFO("True IMU bias * : ", bias_true.transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Guess IMU bias : ", bias_guess.transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Estimated IMU bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("True center of mass * : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess center of mass : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("True inertia * : ", inertia_true.transpose(), " m^2 Kg.");
+ WOLF_INFO("Guess inertia : ", inertia_guess.transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("True mass * : ", mass_true, " Kg.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+
+ auto bias_estimated = S->getStateBlock('I')->getState();
+ auto cdm_estimated = S->getStateBlock('C')->getState();
+ auto inertia_estimated = S->getStateBlock('i')->getState();
+ auto mass_estimated = S->getStateBlock('m')->getState()(0);
+
+ EXPECT_MATRIX_APPROX(bias_estimated, bias_true, 1e-3);
+ EXPECT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-3); // cdm_z is not observable while hovering
+ EXPECT_NEAR(inertia_estimated(2), inertia_true(2), 1e-3); // Ix and Iy not observable in this test
+ EXPECT_NEAR(mass_estimated, mass_true, 1e-3);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- // ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.cdm_only_hovering";
+ // ::testing::GTEST_FLAG(filter) =
+ // "Test_SolveProblemForceTorqueInertialDynamics_yaml.torque_yaw_constant__unfix_all__nonzero_bias_and_nonzero_noise";
return RUN_ALL_TESTS();
}
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
index 233368f38accb87f4bbd4500e37bede35dd1378a..2df498f9c582fcad2918a0ae5fc714951ebb6c26 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
@@ -31,13 +31,19 @@ config:
torque_noise_std: 0.1 # std dev of torque noise in N/m
acc_noise_std: 0.01 # std dev of acc noise in m/s2
gyro_noise_std: 0.01 # std dev of gyro noise in rad/s
+ accb_initial_std: 0.01 # m/s2 - initial bias
+ gyrob_initial_std: 0.01 # rad/sec - initial bias
acc_drift_std: 0.00001 # std dev of acc drift m/s2/sqrt(s)
gyro_drift_std: 0.00001 # std dev of gyro drift rad/s/sqrt(s)
com: [0,0,0.0341] # center of mass [m]
inertia: [0.017598,0.017957,0.029599] # moments of inertia i_xx, i_yy, i_zz [kg m2]
mass: 1.952 # mass [kg]
-
+ imu_bias_fix: true
+ com_fix: true
+ inertia_fix: true
+ mass_fix: true
+
processors:
-
name: "proc FTID"
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f3e25c03075b12b8df8c8f0026df1a88aa055e9e
--- /dev/null
+++ b/test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml
@@ -0,0 +1,97 @@
+config:
+ problem:
+ frame_structure: "POVL"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ V: [0,0,0]
+ L: [0,0,0]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ V: [100,100,100]
+ L: [100,100,100]
+ time_tolerance: 0.01
+ tree_manager:
+ type: "TreeManagerSlidingWindow"
+ plugin: "core"
+ n_frames: 1000
+ n_fix_first_frames: 0
+ viral_remove_empty_parent: true
+ map:
+ type: "MapBase"
+ plugin: "core"
+ sensors:
+ -
+ name: "sensor FTI"
+ type: "SensorForceTorqueInertial"
+ plugin: "bodydynamics"
+ extrinsic:
+ pose: [0,0,0, 0,0,0,1]
+
+ # IMU
+ acc_noise_std: 0.001 # std dev of acc noise in m/s2
+ gyro_noise_std: 0.001 # std dev of gyro noise in rad/s
+ accb_initial_std: 0.01 # m/s2 - initial bias
+ gyrob_initial_std: 0.01 # rad/sec - initial bias
+ acc_drift_std: 0.0001 # std dev of acc drift m/s2/sqrt(s)
+ gyro_drift_std: 0.0001 # std dev of gyro drift rad/s/sqrt(s)
+
+ #FT
+ force_noise_std: 0.001 # std dev of force noise in N
+ torque_noise_std: 0.0001 # std dev of torque noise in N/m
+
+ # Dynamics
+ com: [0.005,0.005,0.01] # center of mass [m]
+ inertia: [0.015,0.015,0.030] # moments of inertia i_xx, i_yy, i_zz [kg m2]
+ mass: 2.00 # mass [kg]
+ imu_bias_fix: true
+ com_fix: true
+ inertia_fix: true
+ mass_fix: true
+
+ -
+ name: sensor Pose
+ type: SensorPose
+ plugin: core
+ extrinsic:
+ pose: [0,0,0, 0,0,0,1]
+ std_px: 0.001
+ std_py: 0.001
+ std_pz: 0.001
+ std_ox: 0.001
+ std_oy: 0.001
+ std_oz: 0.001
+
+ processors:
+ -
+ name: "proc FTID"
+ type: "ProcessorForceTorqueInertialDynamics"
+ sensor_name: "sensor FTI"
+ plugin: "bodydynamics"
+ time_tolerance: 0.01
+ apply_loss_function: false
+ unmeasured_perturbation_std: 0.001
+ state_getter: true
+ state_priority: 1
+ # n_propellers: 6
+ keyframe_vote:
+ voting_active: true
+ max_time_span: 0.0995 # De moment el que funciona millor és 1 s o 2s
+ max_buff_length: 999 # motion deltas
+ dist_traveled: 999 # meters
+ angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ -
+ name: prc Pose
+ type: ProcessorPose
+ plugin: core
+ sensor_name: sensor Pose
+ time_tolerance: 0.01
+ keyframe_vote:
+ voting_active: true
+ apply_loss_function: false
+
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
index 8eb8204126f94bc008bcb01587ae2fa5482fec6b..79aaa00d0706000e43bd4641d1658d8ab1b77236 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
@@ -18,8 +18,8 @@ config:
tree_manager:
type: "TreeManagerSlidingWindow"
plugin: "core"
- n_frames: 3
- n_fix_first_frames: 1
+ n_frames: 6
+ n_fix_first_frames: 0
viral_remove_empty_parent: true
map:
type: "MapBase"
@@ -33,17 +33,25 @@ config:
pose: [0,0,0, 0,0,0,1]
# IMU
- force_noise_std: 0.1 # std dev of force noise in N
- torque_noise_std: 0.1 # std dev of torque noise in N/m
- acc_noise_std: 0.01 # std dev of acc noise in m/s2
- gyro_noise_std: 0.01 # std dev of gyro noise in rad/s
- acc_drift_std: 0.00001 # std dev of acc drift m/s2/sqrt(s)
- gyro_drift_std: 0.00001 # std dev of gyro drift rad/s/sqrt(s)
+ acc_noise_std: 0.001 # std dev of acc noise in m/s2
+ gyro_noise_std: 0.001 # std dev of gyro noise in rad/s
+ accb_initial_std: 0.01 # m/s2 - initial bias
+ gyrob_initial_std: 0.01 # rad/sec - initial bias
+ acc_drift_std: 0.0001 # std dev of acc drift m/s2/sqrt(s)
+ gyro_drift_std: 0.0001 # std dev of gyro drift rad/s/sqrt(s)
+
+ #FT
+ force_noise_std: 0.001 # std dev of force noise in N
+ torque_noise_std: 0.0001 # std dev of torque noise in N/m
# Dynamics
com: [0,0,0.0341] # center of mass [m]
inertia: [0.017598,0.017957,0.029599] # moments of inertia i_xx, i_yy, i_zz [kg m2]
- mass: 1.9 # mass [kg]
+ mass: 1.952 # mass [kg]
+ imu_bias_fix: true
+ com_fix: true
+ inertia_fix: true
+ mass_fix: true
processors:
-
@@ -59,7 +67,7 @@ config:
n_propellers: 3
keyframe_vote:
voting_active: true
- max_time_span: 0.995
+ max_time_span: 0.0995
max_buff_length: 999 # motion deltas
dist_traveled: 999 # meters
angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
diff --git a/test/yaml/processor_force_torque_inertial.yaml b/test/yaml/processor_force_torque_inertial.yaml
index beafed9ea44a627f2c5b2ea30490bfd11db7868b..84aca5a4b193784435a6c073d95770f1a255deea 100644
--- a/test/yaml/processor_force_torque_inertial.yaml
+++ b/test/yaml/processor_force_torque_inertial.yaml
@@ -10,4 +10,5 @@
max_buff_length: 5 # motion deltas
dist_traveled: 1 # meters
angle_turned: 1 # radians (1 rad approx 57 deg, approx 60 deg)
+
diff --git a/test/yaml/sensor_force_torque_inertial.yaml b/test/yaml/sensor_force_torque_inertial.yaml
index e6d323ba4b29ca53840bd4ae4618ca6348d2d057..21d169c79bfe81bc609e966bf3dfbd57275e7e97 100644
--- a/test/yaml/sensor_force_torque_inertial.yaml
+++ b/test/yaml/sensor_force_torque_inertial.yaml
@@ -2,9 +2,15 @@ force_noise_std: 0.1 # std dev of force noise in N
torque_noise_std: 0.1 # std dev of torque noise in N/m
acc_noise_std: 0.01 # std dev of acc noise in m/s2
gyro_noise_std: 0.01 # std dev of gyro noise in rad/s
+accb_initial_std: 0 # m/s2 - initial bias
+gyrob_initial_std: 0 # rad/sec - initial bias
acc_drift_std: 0.00001 # std dev of acc drift m/s2/sqrt(s)
gyro_drift_std: 0.00001 # std dev of gyro drift rad/s/sqrt(s)
com: [0,0,0.0341] # center of mass [m]
inertia: [0.017598,0.017957,0.029599] # moments of inertia i_xx, i_yy, i_zz [kg m2]
-mass: 1.952 # mass [kg]
\ No newline at end of file
+mass: 1.952 # mass [kg]
+imu_bias_fix: true
+com_fix: true
+inertia_fix: true
+mass_fix: true
\ No newline at end of file