From ccae799c75a88becb500444ce32936759c86bb20 Mon Sep 17 00:00:00 2001
From: Arnau Marzabal Gatell <amarzabal@iri.upc.edu>
Date: Wed, 18 Oct 2023 16:17:29 +0200
Subject: [PATCH] add bias prior option
---
include/bodydynamics/sensor/sensor_force_torque_inertial.h | 3 ++-
src/sensor/sensor_force_torque_inertial.cpp | 2 +-
2 files changed, 3 insertions(+), 2 deletions(-)
diff --git a/include/bodydynamics/sensor/sensor_force_torque_inertial.h b/include/bodydynamics/sensor/sensor_force_torque_inertial.h
index 7b9234c..35e9195 100644
--- a/include/bodydynamics/sensor/sensor_force_torque_inertial.h
+++ b/include/bodydynamics/sensor/sensor_force_torque_inertial.h
@@ -51,7 +51,7 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
bool set_mass_prior,set_com_prior,set_inertia_prior,set_bias_prior;
double prior_mass_std;
Vector3d prior_com_std, prior_inertia_std;
- Vector6d prior_bias_std;
+ Vector6d bias,prior_bias_std;
ParamsSensorForceTorqueInertial() = default;
@@ -80,6 +80,7 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
com = _server.getParam<Vector3d>(prefix + _unique_name + "/com");
inertia = _server.getParam<Vector3d>(prefix + _unique_name + "/inertia");
mass = _server.getParam<double>(prefix + _unique_name + "/mass");
+ bias = _server.getParam<Vector6d>(prefix + _unique_name + "/bias");
imu_bias_fix = _server.getParam<bool>(prefix + _unique_name + "/imu_bias_fix");
com_fix = _server.getParam<bool>(prefix + _unique_name + "/com_fix");
diff --git a/src/sensor/sensor_force_torque_inertial.cpp b/src/sensor/sensor_force_torque_inertial.cpp
index ed627f8..3d56446 100644
--- a/src/sensor/sensor_force_torque_inertial.cpp
+++ b/src/sensor/sensor_force_torque_inertial.cpp
@@ -90,7 +90,7 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd&
Matrix6d prior_bias_cov = params_fti_->prior_bias_std.array().pow(2).matrix().asDiagonal();
std::cout<<"\n With std: "<<params_fti_->prior_bias_std.transpose();
std::cout<<"\n With cov: \n"<<prior_bias_cov<<"\n";
- addPriorParameter('I',Vector6d::Zero(), prior_bias_cov);
+ addPriorParameter('I',params_fti_->bias, prior_bias_cov);
std::cout<<"Done\n";
}
--
GitLab