add bias prior option

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");

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";
     }