diff --git a/.clang-format copy b/.clang-format copy
new file mode 100644
index 0000000000000000000000000000000000000000..bc951c7539c78c4fa00efde0030dcf12b126686a
--- /dev/null
+++ b/.clang-format copy
@@ -0,0 +1,115 @@
+---
+Language: Cpp
+BasedOnStyle: Google
+IndentAccessModifiers: false
+AccessModifierOffset: -2
+AlignAfterOpenBracket: Align
+AlignConsecutiveAssignments: true
+AlignConsecutiveDeclarations: true
+AlignEscapedNewlines: Left
+AlignOperands: true
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortBlocksOnASingleLine: false
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: Empty
+AllowShortIfStatementsOnASingleLine: true
+AllowShortLoopsOnASingleLine: true
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: true
+AlwaysBreakTemplateDeclarations: true
+BinPackArguments: true
+BinPackParameters: false
+BreakBeforeBraces: Custom
+BraceWrapping:
+ AfterClass: true
+ AfterControlStatement: Always
+ AfterEnum: true
+ AfterFunction: true
+ AfterNamespace: true
+ AfterObjCDeclaration: true
+ AfterStruct: true
+ AfterUnion: true
+ AfterExternBlock: false
+ BeforeCatch: true
+ BeforeElse: true
+ IndentBraces: false
+ SplitEmptyFunction: true
+ SplitEmptyRecord: true
+ SplitEmptyNamespace: true
+BreakBeforeBinaryOperators: None
+BreakBeforeInheritanceComma: false
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: false
+BreakConstructorInitializers: BeforeColon
+BreakAfterJavaFieldAnnotations: false
+BreakStringLiterals: true
+ColumnLimit: 119
+# CommentPragmas: "^ IWYU pragma: ^\\.+"
+CommentPragmas: '^\\.+'
+CompactNamespaces: false
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 4
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DerivePointerAlignment: true
+DisableFormat: false
+ExperimentalAutoDetectBinPacking: false
+FixNamespaceComments: true
+ForEachMacros:
+ - foreach
+ - Q_FOREACH
+ - BOOST_FOREACH
+IncludeBlocks: Preserve
+# IncludeCategories:
+# - Regex: '^<pinocchio/fwd\.hpp>'
+# Priority: 1
+# - Regex: '^<ext/.*\.h>'
+# Priority: 3
+# - Regex: '^<.*\.h>'
+# Priority: 2
+# - Regex: "^<.*"
+# Priority: 3
+# - Regex: ".*"
+# Priority: 4
+IncludeIsMainRegex: "([-_](test|unittest))?$"
+IndentCaseLabels: true
+IndentPPDirectives: None
+IndentWidth: 4
+IndentWrappedFunctionNames: false
+JavaScriptQuotes: Leave
+JavaScriptWrapImports: true
+KeepEmptyLinesAtTheStartOfBlocks: false
+MacroBlockBegin: ""
+MacroBlockEnd: ""
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCBlockIndentWidth: 2
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakAssignment: 2
+PenaltyBreakBeforeFirstCallParameter: 1
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyExcessCharacter: 1000000
+PenaltyReturnTypeOnItsOwnLine: 200
+PointerAlignment: Left
+ReflowComments: true
+SortIncludes: false
+SortUsingDeclarations: true
+SpaceAfterCStyleCast: false
+SpaceAfterTemplateKeyword: true
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeParens: ControlStatements
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 2
+SpacesInAngles: false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard: Auto
+TabWidth: 4
+UseTab: Never
diff --git a/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h b/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h
new file mode 100644
index 0000000000000000000000000000000000000000..f33b5d10d34d1bd659fbe5aca49e92dfac05a7b8
--- /dev/null
+++ b/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h
@@ -0,0 +1,285 @@
+//--------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--------
+#ifndef FACTOR_FORCE_TORQUE_INERTIAL_DYNAMICS_H
+#define FACTOR_FORCE_TORQUE_INERTIAL_DYNAMICS_H
+
+#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
+#include <core/feature/feature_motion.h>
+#include <core/factor/factor_autodiff.h>
+
+namespace wolf
+{
+using namespace Eigen;
+using namespace bodydynamics;
+
+WOLF_PTR_TYPEDEFS(FactorForceTorqueInertialDynamics);
+
+/**
+ * @brief Pre-integrated factor taking IMU and force-torque measurements (FTI)
+ *
+ * This factor evaluates the preintegrated FTI against pose, linear velocity, IMU bias and angular momentum.
+ *
+ * State blocks considered:
+ * - Frame 1 (origin)
+ * 'P' position
+ * 'V' velocity
+ * 'O' orientation
+ * 'L' angular momentum
+ * - Capture 1 (origin)
+ * 'I' IMU biases
+ * - Frame 2
+ * 'P' position
+ * 'V' velocity
+ * 'O' orientation
+ * 'L' angular momentum
+ * - Sensor Force Torque Inertial
+ * 'C' position of the center of mass
+ * 'i' inertia matrix components (we are assuming that it is a diagonal matrix)
+ * 'm' mass
+ *
+ * The residual computed has the following components, in this order
+ * - position delta error according to IMU preintegration
+ * - velocity delta error according to IMU preintegration
+ * - position delta error according to FT preintegration
+ * - velocity delta error according to FT preintegration
+ * - angular momentum delta error according to FT preintegration
+ * - orientation delta error according to IMU preintegration
+ */
+class FactorForceTorqueInertialDynamics
+ : public FactorAutodiff<FactorForceTorqueInertialDynamics, 18, 3, 4, 3, 3, 6, 3, 4, 3, 3, 3, 3, 1> // POVLB - POVL
+ // - Cim
+ // // TODO: add
+ // FT bias?
+{
+ public:
+ FactorForceTorqueInertialDynamics(const FeatureMotionPtr& _ftr, // gets measurement and access to parents
+ const CaptureBasePtr& _capture_origin, // gets biases
+ const ProcessorBasePtr& _processor, // gets access to processor and sensor
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorForceTorqueInertialDynamics() override = default;
+
+ template <typename T>
+ bool operator()(const T* const _p1, // position 1
+ const T* const _q1, // orientation
+ const T* const _v1, // velocity
+ const T* const _L1, // ang momentum
+ const T* const _b1, // IMU bias (acc and gyro)
+ const T* const _p2, // position 2
+ const T* const _q2, // orientation
+ const T* const _v2, // velocity
+ const T* const _L2, // ang momentum
+ const T* const _C, // center of mass
+ const T* const _i, // inertia matrix
+ const T* const _m, // mass
+ T* _res) const; // residual
+
+ template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8>
+ bool residual(const MatrixBase<D1>& _p1,
+ const QuaternionBase<D4>& _q1,
+ const MatrixBase<D1>& _v1,
+ const MatrixBase<D1>& _L1,
+ const MatrixBase<D2>& _b1,
+ const MatrixBase<D3>& _p2,
+ const QuaternionBase<D5>& _q2,
+ const MatrixBase<D3>& _v2,
+ const MatrixBase<D3>& _L2,
+ const MatrixBase<D6>& _C,
+ const MatrixBase<D6>& _i,
+ const D7& _m,
+ MatrixBase<D8>& _res) const;
+
+ VectorXd residual() const;
+
+ VectorXd getCalibPreint() const
+ {
+ return calib_preint_;
+ }
+
+ private:
+ double dt_;
+ Matrix<double, 19, 1> delta_preint_;
+ Matrix<double, 13, 1> calib_preint_;
+ Matrix<double, 18, 13> J_delta_calib_;
+ Matrix<double, 18, 18> sqrt_info_upper_;
+};
+
+inline FactorForceTorqueInertialDynamics::FactorForceTorqueInertialDynamics(const FeatureMotionPtr& _ftr,
+ const CaptureBasePtr& _capture_origin,
+ const ProcessorBasePtr& _processor,
+ bool _apply_loss_function,
+ FactorStatus _status)
+ : FactorAutodiff<FactorForceTorqueInertialDynamics, 18, 3, 4, 3, 3, 6, 3, 4, 3, 3, 3, 3, 1>(
+ "FactorForceTorqueInertialDynamics",
+ TOP_MOTION,
+ _ftr, // parent feature
+ _capture_origin->getFrame(), // frame other
+ _capture_origin, // capture other
+ nullptr, // feature other
+ nullptr, // landmark other
+ _processor, // processor
+ _apply_loss_function,
+ _status,
+ _capture_origin->getFrame()->getStateBlock('P'), // previous frame P
+ _capture_origin->getFrame()->getStateBlock('O'), // previous frame O
+ _capture_origin->getFrame()->getStateBlock('V'), // previous frame V
+ _capture_origin->getFrame()->getStateBlock('L'), // previous frame L
+ _capture_origin->getStateBlock('I'), // previous frame IMU bias
+ _ftr->getFrame()->getStateBlock('P'), // current frame P
+ _ftr->getFrame()->getStateBlock('O'), // current frame O
+ _ftr->getFrame()->getStateBlock('V'), // current frame V
+ _ftr->getFrame()->getStateBlock('L'), // current frame L
+ _capture_origin->getSensor()->getStateBlock('C'), // sensor C
+ _capture_origin->getSensor()->getStateBlock('i'), // sensor i
+ _capture_origin->getSensor()->getStateBlock('m')), // sensor m
+ dt_(_ftr->getFrame()->getTimeStamp() - _capture_origin->getFrame()->getTimeStamp()),
+ delta_preint_(_ftr->getDeltaPreint()),
+ calib_preint_(_ftr->getCalibrationPreint()),
+ J_delta_calib_(_ftr->getJacobianCalibration()),
+ sqrt_info_upper_(_ftr->getMeasurementSquareRootInformationUpper())
+{
+ //
+}
+
+template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8>
+inline bool FactorForceTorqueInertialDynamics::residual(const MatrixBase<D1>& _p1,
+ const QuaternionBase<D4>& _q1,
+ const MatrixBase<D1>& _v1,
+ const MatrixBase<D1>& _L1,
+ const MatrixBase<D2>& _b1,
+ const MatrixBase<D3>& _p2,
+ const QuaternionBase<D5>& _q2,
+ const MatrixBase<D3>& _v2,
+ const MatrixBase<D3>& _L2,
+ const MatrixBase<D6>& _C,
+ const MatrixBase<D6>& _i,
+ const D7& _m,
+ MatrixBase<D8>& _res) const
+{
+ MatrixSizeCheck<18, 1>::check(_res);
+
+ typedef typename D4::Scalar T;
+ typedef Map<Matrix<T, 3, 1>> Matrixt;
+
+ /* Will do the following:
+ *
+ * Compute estimated delta, using betweenStates()
+ * d_est = x2 (-) x1
+ *
+ * Compute correction step according to bias change, this is a linear operation
+ * d_step = J_delta_calib * (calib - bias_preint)
+ *
+ * Correct preintegrated delta with new bias, using plus()
+ * d_corr = d_preint (+) d_step
+ *
+ * Compute error, using diff()
+ * d_err = d_corr (-) d_est
+ *
+ * Compute residual
+ * res = W * d_err , with W the sqrt of the info matrix.
+ */
+
+ /* WARNING -- order of state blocks might be confusing!!
+ * Frame blocks are ordered as follows: "POVL" -- pos, ori, vel, ang_momentum
+ * Factor blocks are ordered as follows: "POVL-B-POVL" -- B is IMU bias
+ * Delta blocks are ordered as follows: "PVpvLO" -- where "P,V,O" are from IMU, and "p,v,L" from FT
+ */
+
+ Matrix<double, 3, 1> f;
+ Matrix<T, 19, 1> delta_est; // delta_est = x2 (-) x1, computed with betweenStates(x1,x2,dt)
+ Matrixt dpi(&delta_est(0)); // 'P'
+ Matrixt dvi(&delta_est(3)); // 'V'
+ Matrixt dpd(&delta_est(6)); // 'p'
+ Matrixt dvd(&delta_est(9)); // 'v'
+ Matrixt dL(&delta_est(12)); // 'L'
+ Map<Quaternion<T>> dq(&delta_est(15)); // 'O'
+ fti::betweenStates(_p1, _v1, _L1, _q1, _p2, _v2, _L2, _q2, dt_, dpi, dvi, dpd, dvd, dL, dq);
+
+ Matrix<T, 19, 1> delta_preint = delta_preint_.cast<T>();
+ Matrix<T, 13, 1> calib;
+ calib << _b1, _C, _i, _m;
+ Matrix<T, 18, 1> delta_step = J_delta_calib_ * (calib - calib_preint_);
+ Matrix<T, 19, 1> delta_corr = fti::plus(delta_preint, delta_step);
+ Matrix<T, 18, 1> delta_err = fti::diff(delta_est, delta_corr);
+ _res = sqrt_info_upper_ * delta_err;
+
+ return true;
+}
+
+inline VectorXd FactorForceTorqueInertialDynamics::residual() const
+{
+ VectorXd res(18);
+ Vector3d p0 = getFrameOther()->getStateBlock('P')->getState(); // previous frame P
+ Quaterniond q0 = Quaterniond(getFrameOther()->getStateBlock('O')->getState().data());
+ Vector3d v0 = getFrameOther()->getStateBlock('V')->getState();
+ Vector3d L0 = getFrameOther()->getStateBlock('L')->getState();
+ Vector6d bias = getCaptureOther()->getSensorIntrinsic()->getState();
+ Vector3d p1 = getFrame()->getStateBlock('P')->getState(); // previous frame P
+ Quaterniond q1 = Quaterniond(getFrame()->getStateBlock('O')->getState().data());
+ Vector3d v1 = getFrame()->getStateBlock('V')->getState();
+ Vector3d L1 = getFrame()->getStateBlock('L')->getState();
+ Vector3d C = getCapture()->getSensor()->getStateBlock('C')->getState();
+ Vector3d i = getCapture()->getSensor()->getStateBlock('i')->getState();
+ double m = getCapture()->getSensor()->getStateBlock('m')->getState()(0);
+
+ residual(p0, q0, v0, L0, bias, p1, q1, v1, L1, C, i, m, res);
+ return res;
+}
+
+template <typename T>
+inline bool FactorForceTorqueInertialDynamics::operator()(const T* const _p1,
+ const T* const _q1,
+ const T* const _v1,
+ const T* const _L1,
+ const T* const _b1,
+ const T* const _p2,
+ const T* const _q2,
+ const T* const _v2,
+ const T* const _L2,
+ const T* const _C,
+ const T* const _i,
+ const T* const _m,
+ T* _res) const
+{
+ Map<const Matrix<T, 3, 1>> p1(_p1);
+ Map<const Quaternion<T>> q1(_q1);
+ Map<const Matrix<T, 3, 1>> v1(_v1);
+ Map<const Matrix<T, 3, 1>> L1(_L1);
+ Map<const Matrix<T, 6, 1>> b1(_b1);
+ Map<const Matrix<T, 3, 1>> p2(_p2);
+ Map<const Quaternion<T>> q2(_q2);
+ Map<const Matrix<T, 3, 1>> v2(_v2);
+ Map<const Matrix<T, 3, 1>> L2(_L2);
+ Map<const Matrix<T, 3, 1>> C(_C);
+ Map<const Matrix<T, 3, 1>> i(_i);
+ const T& m = *_m;
+ Map<Matrix<T, 18, 1>> res(_res);
+
+ residual(p1, q1, v1, L1, b1, p2, q2, v2, L2, C, i, m, res);
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif // FACTOR_FORCE_TORQUE_INERTIAL_H
\ No newline at end of file
diff --git a/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h b/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
new file mode 100644
index 0000000000000000000000000000000000000000..6bc51007d7936cd4be9e8d37538132e2591d5735
--- /dev/null
+++ b/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
@@ -0,0 +1,252 @@
+//--------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--------
+/*
+ * processor_preintegrator_force_torque_inertial.h
+ *
+ * Created on: Aug 19, 2021
+ * Author: jsola
+ */
+
+#ifndef PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_DYNAMICS_H_
+#define PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_DYNAMICS_H_
+
+#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
+#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
+
+#include <core/processor/processor_motion.h>
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorqueInertialPreintDynamics);
+
+struct ParamsProcessorForceTorqueInertialPreintDynamics : public ParamsProcessorMotion
+{
+ ParamsProcessorForceTorqueInertialPreintDynamics() = default;
+ ParamsProcessorForceTorqueInertialPreintDynamics(std::string _unique_name, const ParamsServer& _server)
+ : ParamsProcessorMotion(_unique_name, _server)
+ {
+ // n_propellers = _server.getParam<int>(prefix + _unique_name + "/n_propellers");
+ }
+
+ std::string print() const override
+ {
+ return ParamsProcessorMotion::print() + "\n"; //
+ // + "n_propellers: " + std::to_string(n_propellers); //
+ }
+
+ // int n_propellers;
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorForceTorqueInertialPreintDynamics);
+class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion
+{
+ public:
+ ProcessorForceTorqueInertialPreintDynamics(
+ ParamsProcessorForceTorqueInertialPreintDynamicsPtr _params_force_torque_inertial_preint_dynamics_);
+ ~ProcessorForceTorqueInertialPreintDynamics() override;
+ void configure(SensorBasePtr _sensor) override;
+ WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertialPreintDynamics, ParamsProcessorForceTorqueInertialPreintDynamics);
+
+ protected:
+ ParamsProcessorForceTorqueInertialPreintDynamicsPtr params_force_torque_inertial_preint_dynamics_;
+ SensorForceTorqueInertialPtr sensor_fti_;
+
+ public:
+ /** \brief convert raw CaptureMotion data to the delta-state format.
+ *
+ * This function accepts raw data and time step dt,
+ * and computes the value of the delta-state and its covariance. Note that these values are
+ * held by the members delta_ and delta_cov_.
+ *
+ * @param _data measured motion data
+ * @param _data_cov covariance
+ * @param _dt time step
+ * @param _delta computed delta
+ * @param _delta_cov covariance
+ * @param _calib current state of the calibrated parameters
+ * @param _jacobian_calib Jacobian of the delta wrt calib
+ *
+ * Rationale:
+ *
+ * The delta-state format must be compatible for integration using
+ * the composition functions doing the math in this class: statePlusDelta() and deltaPlusDelta().
+ * See the class documentation for some Eigen::VectorXd suggestions.
+ *
+ * The data format is generally not the same as the delta format,
+ * because it is the format of the raw data provided by the Capture,
+ * which is unaware of the needs of this processor.
+ *
+ * Additionally, sometimes the data format is in the form of a
+ * velocity, or a higher derivative, while the delta is in the form of an increment.
+ * In such cases, converting from data to delta-state needs integrating
+ * the data over the period dt.
+ *
+ * Two trivial implementations would establish:
+ * - If `_data` is an increment:
+ *
+ * delta_ = _data;
+ * delta_cov_ = _data_cov
+ * - If `_data` is a velocity:
+ *
+ * delta_ = _data * _dt
+ * delta_cov_ = _data_cov * _dt.
+ *
+ * However, other more complicated relations are possible.
+ * In general, we'll have a nonlinear
+ * function relating `delta_` to `_data` and `_dt`, as follows:
+ *
+ * delta_ = f ( _data, _dt)
+ *
+ * The delta covariance is obtained by classical uncertainty propagation of the data covariance,
+ * that is, through the Jacobians of `f()`,
+ *
+ * delta_cov_ = F_data * _data_cov * F_data.transpose
+ *
+ * where `F_data = d_f/d_data` is the Jacobian of `f()`.
+ */
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+
+ /** \brief composes a delta-state on top of another delta-state
+ * \param _delta1 the first delta-state
+ * \param _delta2 the second delta-state
+ * \param _dt2 the second delta-state's time delta
+ * \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state.
+ *
+ * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2.
+ */
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const override;
+
+ /** \brief composes a delta-state on top of another delta-state, and computes the Jacobians
+ * \param _delta1 the first delta-state
+ * \param _delta2 the second delta-state
+ * \param _dt2 the second delta-state's time delta
+ * \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state.
+ * \param _jacobian1 the jacobian of the composition w.r.t. _delta1.
+ * \param _jacobian2 the jacobian of the composition w.r.t. _delta2.
+ *
+ * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2 and its
+ * jacobians.
+ */
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const override;
+
+ /** \brief composes a delta-state on top of a state
+ * \param _x the initial state
+ * \param _delta the delta-state
+ * \param _x_plus_delta the updated state. It has the same format as the initial state.
+ * \param _dt the time interval spanned by the Delta
+ *
+ * This function implements the composition (+) so that _x2 = _x1 (+) _delta.
+ */
+ void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const override;
+
+ /** \brief Delta zero
+ * \return a delta state equivalent to the null motion.
+ *
+ * Examples (see documentation of the the class for info on Eigen::VectorXd):
+ * - 2d odometry: a 3-vector with all zeros, e.g. VectorXd::Zero(3)
+ * - 3d odometry: delta type is a PQ vector: 7-vector with [0,0,0, 0,0,0,1]
+ * - IMU: PQVBB 10-vector with [0,0,0, 0,0,0,1, 0,0,0] // No biases in the delta !!
+ */
+ Eigen::VectorXd deltaZero() const override;
+
+ /** \brief correct the preintegrated delta
+ * This produces a delta correction according to the appropriate manifold.
+ * @param delta_preint : the preintegrated delta to correct
+ * @param delta_step : an increment in the tangent space of the manifold
+ *
+ * We want to do
+ *
+ * delta_corr = delta_preint (+) d_step
+ *
+ * where the operator (+) is the right-plus operator on the delta's manifold.
+ *
+ * Note: usually in motion pre-integration we have
+ *
+ * delta_step = Jac_delta_calib * (calib - calib_pre)
+ *
+ */
+ Eigen::VectorXd correctDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta_step) const override;
+
+ protected:
+ /** \brief emplace a CaptureMotion
+ * \param _frame_own frame owning the Capture to create
+ * \param _sensor Sensor that produced the Capture
+ * \param _ts time stamp
+ * \param _data a vector of motion data
+ * \param _data_cov covariances matrix of the motion data data
+ * \param _calib calibration vector
+ * \param _calib_preint calibration vector used during pre-integration
+ * \param _capture_origin_ptr capture acting as the origin of motions for the MorionBuffer of the created MotionCapture
+ */
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin_ptr) override;
+
+ /** \brief emplace a feature corresponding to given capture and add the feature to this capture
+ * \param _capture_motion: the parent capture
+ */
+ FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
+
+ /** \brief emplace a factor and link it in the wolf tree
+ * \param _feature_motion: the parent feature
+ * \param _frame_origin: the frame constrained by this motion factor
+ */
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) override;
+
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+
+ public:
+ virtual VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
+};
+
+inline Eigen::VectorXd ProcessorForceTorqueInertialPreintDynamics::deltaZero() const
+{
+ return bodydynamics::fti::identity();
+}
+
+} /* namespace wolf */
+
+#endif /* PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_DYNAMICS_H_ */
diff --git a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..015839adb79a10fc5dd6d03af1698052769441c5
--- /dev/null
+++ b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
@@ -0,0 +1,195 @@
+//--------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--------
+/*
+ * processor_preintegrator_force_torque_inertial_dynamics.cpp
+ *
+ * Created on: Aug 19, 2021
+ * Author: jsola
+ */
+
+// bodydynamics
+#include "bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h"
+#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
+#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
+
+// wolf
+#include <core/state_block/state_composite.h>
+#include <core/capture/capture_motion.h>
+
+namespace wolf
+{
+
+ProcessorForceTorqueInertialPreintDynamics::ProcessorForceTorqueInertialPreintDynamics(
+ ParamsProcessorForceTorqueInertialPreintDynamicsPtr _params_force_torque_inertial_preint_dynamics)
+ : ProcessorMotion("ProcessorForceTorqueInertialPreintDynamics",
+ "POVL",
+ 3, // dim
+ 13, // state size [p, q, v, L]
+ 19, // delta size [pi, vi, pd, vd, L, q]
+ 18, // delta cov size
+ 12, // data size [a, w, f, t]
+ 13, // calib size [ab, wb, C, i, m]
+ _params_force_torque_inertial_preint_dynamics),
+ params_force_torque_inertial_preint_dynamics_(_params_force_torque_inertial_preint_dynamics)
+{
+ // TODO Auto-generated constructor stub
+}
+
+ProcessorForceTorqueInertialPreintDynamics::~ProcessorForceTorqueInertialPreintDynamics()
+{
+ // TODO Auto-generated destructor stub
+}
+
+CaptureMotionPtr ProcessorForceTorqueInertialPreintDynamics::emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin_ptr)
+{
+ CaptureMotionPtr capture = CaptureBase::emplace<CaptureMotion> (_frame_own,
+ "CaptureMotion", _ts,
+ _sensor,
+ _data,
+ _data_cov,
+ _capture_origin_ptr);
+ setCalibration(capture, _calib);
+ capture->setCalibrationPreint(_calib_preint);
+ return capture;
+}
+
+FeatureBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFeature(CaptureMotionPtr _capture_own)
+{
+ auto motion = _capture_own->getBuffer().back();
+ FeatureBasePtr ftr = FeatureBase::emplace<FeatureMotion>(
+ _capture_own,
+ "FeatureMotion",
+ motion.delta_integr_,
+ motion.delta_integr_cov_,
+ _capture_own->getCalibrationPreint(),
+ motion.jacobian_calib_);
+ return ftr;
+}
+
+FactorBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFactor(FeatureBasePtr _feature_motion,
+ CaptureBasePtr _capture_origin)
+{
+
+ FeatureMotionPtr ftr_ftipd = std::static_pointer_cast<FeatureMotion>(_feature_motion);
+
+ auto fac_ftipd = FactorBase::emplace<FactorForceTorqueInertialDynamics>(_feature_motion, ftr_ftipd, _capture_origin, shared_from_this(), params_->apply_loss_function);
+ return fac_ftipd;
+
+ //FactorBasePtr returnValue;
+ //return returnValue;
+}
+
+void ProcessorForceTorqueInertialPreintDynamics::setCalibration(const CaptureBasePtr _capture,
+ const VectorXd& _calibration)
+{
+ _capture->getStateBlock('I')->setState(_calibration); // Set IMU bias
+}
+
+void ProcessorForceTorqueInertialPreintDynamics::configure(SensorBasePtr _sensor)
+{
+ sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor);
+}
+
+void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const
+{
+ // compute tangent by removing IMU bias
+ 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();
+}
+
+void ProcessorForceTorqueInertialPreintDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const
+{
+ bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2);
+}
+
+void ProcessorForceTorqueInertialPreintDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const
+{
+ bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
+}
+
+void ProcessorForceTorqueInertialPreintDynamics::statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const
+{
+ auto x = _x.vector("PVLO");
+ VectorXd x_plus_delta = bodydynamics::fti::composeOverState(x, _delta, _dt);
+ _x_plus_delta = VectorComposite(x_plus_delta, "PVLO", {3, 3, 3, 4});
+}
+
+Eigen::VectorXd ProcessorForceTorqueInertialPreintDynamics::correctDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta_step) const
+{
+ return fti::plus(_delta_preint, _delta_step);
+}
+
+VectorXd ProcessorForceTorqueInertialPreintDynamics::getCalibration(const CaptureBaseConstPtr _capture) const
+{
+ if (_capture)
+ return _capture->getStateBlock('I')->getState(); // IMU bias
+ else
+ return getSensor()->getStateBlock('I')->getState(); // IMU bias
+}
+} /* namespace wolf */
+
+#include <core/processor/factory_processor.h>
+namespace wolf
+{
+WOLF_REGISTER_PROCESSOR(ProcessorForceTorqueInertialPreintDynamics);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorForceTorqueInertialPreintDynamics);
+} // namespace wolf
\ No newline at end of file
diff --git a/test/gtest_factor_force_torque_inertial_dynamics.cpp b/test/gtest_factor_force_torque_inertial_dynamics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fd3587e25fc89551c564d59d77b45b7960cc6c5c
--- /dev/null
+++ b/test/gtest_factor_force_torque_inertial_dynamics.cpp
@@ -0,0 +1,252 @@
+//--------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/processor/processor_force_torque_inertial_preint_dynamics.h"
+#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
+#include "bodydynamics/internal/config.h"
+
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+#include <core/yaml/parser_yaml.h>
+#include <core/state_block/state_block_derived.h>
+#include <core/state_block/factory_state_block.h>
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+using namespace wolf;
+using namespace bodydynamics::fti;
+
+WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
+
+class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
+{
+ public:
+ ProblemPtr P;
+ SensorForceTorqueInertialPtr S;
+ ProcessorForceTorqueInertialPreintDynamicsPtr p;
+ FrameBasePtr F0, F1, F;
+ CaptureMotionPtr C0, C1, C;
+ FeatureMotionPtr f1;
+ FactorForceTorqueInertialDynamicsPtr c1;
+
+ VectorXd data;
+ MatrixXd data_cov;
+ VectorXd delta;
+ MatrixXd delta_cov;
+ Matrix<double, 13, 1> calib;
+ MatrixXd J_delta_calib;
+
+ protected:
+ void SetUp() override
+ {
+ data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data_cov = MatrixXd::Identity(12, 12);
+
+ std::string wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+ ParserYaml parser = ParserYaml("problem_force_torque_inertial_dynamics.yaml", wolf_root + "/test/yaml/");
+ ParamsServer server = ParamsServer(parser.getParams());
+ P = Problem::autoSetup(server);
+
+ S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
+ p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front());
+
+ //P->print(4, 1, 1, 1);
+
+ C = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
+ C->process();
+
+ P->print(4, 1, 1, 1);
+
+ C0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+ F0 = C0->getFrame();
+
+ ASSERT_EQ(C0->getTimeStamp(), F0->getTimeStamp());
+ ASSERT_EQ(C0->getTimeStamp(), F0->getCaptureOf(S)->getTimeStamp());
+
+ C = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+ C->process();
+
+ C1 = std::static_pointer_cast<CaptureMotion>(p->getLast());
+ F1 = C1->getFrame();
+ }
+
+};
+
+
+class Test_FactorForceTorqueInertialDynamics : public testing::Test
+{
+ public:
+ ProblemPtr P;
+ SensorForceTorqueInertialPtr S;
+ ProcessorForceTorqueInertialPreintDynamicsPtr p;
+ FrameBasePtr F0, F1;
+ CaptureMotionPtr C0, C1;
+ FeatureMotionPtr f1;
+ FactorForceTorqueInertialDynamicsPtr c1;
+
+ VectorXd data;
+ MatrixXd data_cov;
+ VectorXd delta;
+ MatrixXd delta_cov;
+ Matrix<double, 13, 1> calib;
+ MatrixXd J_delta_calib;
+
+ protected:
+ void SetUp() override
+ {
+ data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data_cov = MatrixXd::Identity(12, 12);
+
+ // crear un problem
+ P = Problem::create("POVL", 3);
+
+ // crear un sensor
+ auto params_sensor = std::make_shared<ParamsSensorForceTorqueInertial>();
+ Vector7d extrinsics;
+ extrinsics << 0, 0, 0, 0, 0, 0, 1;
+ S = SensorBase::emplace<SensorForceTorqueInertial>(P->getHardware(), extrinsics, params_sensor);
+ S->setStateBlockDynamic('I', true);
+
+ // crear processador
+ auto params_processor = std::make_shared<ParamsProcessorForceTorqueInertialPreintDynamics>();
+ p = ProcessorBase::emplace<ProcessorForceTorqueInertialPreintDynamics>(S, params_processor);
+
+ // crear dos frame
+ VectorXd state(13);
+ state << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0;
+ VectorComposite state_c(state, "POVL", {3, 4, 3, 3});
+ F0 = FrameBase::emplace<FrameBase>(P->getTrajectory(), 0.0, "POVL", state_c);
+ F1 = FrameBase::emplace<FrameBase>(P->getTrajectory(), 1.0, "POVL", state_c);
+
+ // crear dues capture
+ VectorXd data(VectorXd::Zero(12));
+ MatrixXd data_cov(MatrixXd::Identity(12, 12));
+ Vector6d bias(Vector6d::Zero());
+ auto sb_bias = std::make_shared<StateParams6>(bias);
+ C0 = CaptureBase::emplace<CaptureMotion>(F0, "CaptureMotion", 0.0, S, data, data_cov, nullptr, nullptr,
+ nullptr, sb_bias);
+ C1 = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1.0, S, data, data_cov, C0, nullptr, nullptr,
+ sb_bias);
+
+ // crear un feature
+ VectorXd delta_preint(VectorXd::Zero(19));
+ delta_preint.head<3>() = 0.5*gravity();
+ delta_preint.segment<3>(3) = gravity();
+ delta_preint(15) = 1;
+ delta_preint.segment<3>(6) = 0.5*gravity();
+ delta_preint.segment<3>(9) = gravity();
+ MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
+ VectorXd calib_preint(VectorXd::Zero(13));
+ MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
+ f1 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint,
+ jacobian_calib);
+
+ // crear un factor
+ c1 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f1, f1, C0, p, false);
+ }
+};
+
+TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, constructor)
+{
+ ASSERT_EQ(c1->getCapture(), C1);
+ ASSERT_EQ(c1->getCalibPreint().size(), 13);
+}
+
+//Test en el que no hi ha moviment
+// TEST_F(Test_FactorForceTorqueInertialDynamics, residual)
+// {
+// VectorXd res_exp = VectorXd::Zero(18);
+// Matrix<double, 18, 1> res;
+// VectorXd bias = VectorXd::Zero(6);
+
+// c1->residual(F0->getStateBlock('P')->getState(), // p0
+// Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
+// F0->getStateBlock('V')->getState(), // v0
+// F0->getStateBlock('L')->getState(), // L0
+// bias, // I
+// F1->getStateBlock('P')->getState(), // p1
+// Quaterniond(F1->getStateBlock('O')->getState().data()), // q1
+// F1->getStateBlock('V')->getState(), // v1
+// F1->getStateBlock('L')->getState(), // L1
+// S->getCom(), // C
+// S->getInertia(), // i
+// S->getMass(), // m
+// res);
+
+// ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
+// }
+
+//Test en el que s'avança 1m en direcció x
+
+// TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, residual)
+// {
+// VectorXd res_exp = VectorXd::Zero(18);
+// Matrix<double, 18, 1> res;
+// VectorXd bias = VectorXd::Zero(6);
+
+// //provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual
+// F1->getStateBlock('P')->setState(Vector3d (1,0,0));
+
+// //Hem de crear un nou feature i un nou factor ja que la delta preint canvia.
+// VectorXd delta_preint(VectorXd::Zero(19));
+// delta_preint.head<3>() = 0.5*gravity();
+// delta_preint(0) = 1;
+// delta_preint.segment<3>(3) = gravity();
+// delta_preint(15) = 1;
+// delta_preint.segment<3>(6) = 0.5*gravity();
+// delta_preint(6) = 1;
+// delta_preint.segment<3>(9) = gravity();
+// MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
+// VectorXd calib_preint(VectorXd::Zero(13));
+// MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
+// FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint,jacobian_calib);
+
+// FactorForceTorqueInertialDynamicsPtr c2 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f2, f2, C0, p, false);
+
+
+// c2->residual(F0->getStateBlock('P')->getState(), // p0
+// Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
+// F0->getStateBlock('V')->getState(), // v0
+// F0->getStateBlock('L')->getState(), // L0
+// bias, // I
+// F1->getStateBlock('P')->getState(), // p1
+// Quaterniond(F1->getStateBlock('O')->getState().data()), // q1
+// F1->getStateBlock('V')->getState(), // v1
+// F1->getStateBlock('L')->getState(), // L1
+// S->getCom(), // C
+// S->getInertia(), // i
+// S->getMass(), // m
+// res);
+
+// ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
+// }
+
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/test/yaml/problem_force_torque_inertial_dynamics.yaml b/test/yaml/problem_force_torque_inertial_dynamics.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2d9b19cc19d0519737336367906b1ee1e2621f88
--- /dev/null
+++ b/test/yaml/problem_force_torque_inertial_dynamics.yaml
@@ -0,0 +1,39 @@
+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: [1,1,1]
+ L: [1,1,1]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "None"
+ map:
+ type: "MapBase"
+ plugin: "core"
+ sensors:
+ -
+ name: "sensor FTI"
+ type: "SensorForceTorqueInertial"
+ plugin: "bodydynamics"
+ extrinsic:
+ pose: [0,0,0, 0,0,0,1]
+ follow: "./sensor_force_torque_inertial.yaml"
+
+ processors:
+ -
+ name: "proc FTID"
+ type: "ProcessorForceTorqueInertialPreintDynamics"
+ sensor_name: "sensor FTI"
+ plugin: "bodydynamics"
+ follow: "processor_force_torque_inertial_preint_dynamics.yaml"
+
\ No newline at end of file
diff --git a/test/yaml/processor_force_torque_inertial_preint_dynamics.yaml b/test/yaml/processor_force_torque_inertial_preint_dynamics.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..beafed9ea44a627f2c5b2ea30490bfd11db7868b
--- /dev/null
+++ b/test/yaml/processor_force_torque_inertial_preint_dynamics.yaml
@@ -0,0 +1,13 @@
+ time_tolerance: 0.1
+ apply_loss_function: false
+ unmeasured_perturbation_std: 0.00000001
+ state_getter: true
+ state_priority: 1
+ n_propellers: 3
+ keyframe_vote:
+ voting_active: false
+ max_time_span: 1
+ max_buff_length: 5 # motion deltas
+ dist_traveled: 1 # meters
+ angle_turned: 1 # radians (1 rad approx 57 deg, approx 60 deg)
+