Skip to content
Snippets Groups Projects
Commit 75cbd856 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Merge branch 'uav-identification' into 'devel' 

Complete UAV identification setup

See merge request !30
parents 140b60f5 3088c557
No related branches found
No related tags found
2 merge requests!31devel->main,!30Complete UAV identification setup
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 2135 additions and 173 deletions
include/bodydynamics/factor/factor_angular_momentum.h
+ 57
− 49
View file @ 75cbd856
......@@ -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
include/bodydynamics/math/force_torque_inertial_delta_tools.h
+ 2
− 2
View file @ 75cbd856
......@@ -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)
......
include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
+ 0
− 1
View file @ 75cbd856
......@@ -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.
......
include/bodydynamics/sensor/sensor_force_torque_inertial.h
+ 41
− 27
View file @ 75cbd856
......@@ -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
......
src/processor/processor_force_torque.cpp
+ 3
− 3
View file @ 75cbd856
......@@ -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;
}
......
src/processor/processor_force_torque_inertial_dynamics.cpp
+ 64
− 45
View file @ 75cbd856
......@@ -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;
}
......
src/sensor/sensor_force_torque_inertial.cpp
+ 12
− 4
View file @ 75cbd856
......@@ -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
......
test/CMakeLists.txt
+ 2
− 0
View file @ 75cbd856
......@@ -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)
test/dades_simulacio_pep.csv 0 → 100644
+ 381
− 0
View file @ 75cbd856
This diff is collapsed.
test/gtest_processor_force_torque_inertial_dynamics.cpp
+ 2
− 2
View file @ 75cbd856
......@@ -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);
......
test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp 0 → 100644
+ 560
− 0
View file @ 75cbd856
This diff is collapsed.
test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
+ 7
− 1
View file @ 75cbd856
......@@ -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"
......
test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml 0 → 100644
+ 97
− 0
View file @ 75cbd856
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
test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
+ 18
− 10
View file @ 75cbd856
......@@ -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)
test/yaml/processor_force_torque_inertial.yaml
+ 1
− 0
View file @ 75cbd856
......@@ -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)
test/yaml/sensor_force_torque_inertial.yaml
+ 7
− 1
View file @ 75cbd856
......@@ -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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment