From d11370781ae13bc31fed495a81edb7e5522fb7fa Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sun, 7 Aug 2022 22:01:56 +0200
Subject: [PATCH] Add IMU noise and drift cov to PrcFTI
---
.../processor_force_torque_inertial_preint_dynamics.h | 2 ++
...rocessor_force_torque_inertial_preint_dynamics.cpp | 11 +++++++++++
2 files changed, 13 insertions(+)
diff --git a/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h b/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
index db76c49..11f2484 100644
--- a/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
+++ b/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
@@ -71,6 +71,8 @@ class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion
protected:
ParamsProcessorForceTorqueInertialPreintDynamicsPtr params_force_torque_inertial_preint_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/src/processor/processor_force_torque_inertial_preint_dynamics.cpp b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
index a0d8065..8ae3141 100644
--- a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
@@ -111,6 +111,17 @@ void ProcessorForceTorqueInertialPreintDynamics::setCalibration(const CaptureBas
void ProcessorForceTorqueInertialPreintDynamics::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;
+
+ gyro_noise_cov_ = Eigen::Array3d(gyro_noise_std, gyro_noise_std, gyro_noise_std).square().matrix().asDiagonal();
+ imu_drift_cov_ =
+ Eigen::Array<double,6,1>(acc_drift_std, acc_drift_std, acc_drift_std, gyro_drift_std, gyro_drift_std, gyro_drift_std)
+ .square()
+ .matrix()
+ .asDiagonal();
}
void ProcessorForceTorqueInertialPreintDynamics::data2tangent(const Eigen::VectorXd& _data,
--
GitLab