added regularization for fti sensor params

include/bodydynamics/sensor/sensor_force_torque_inertial.h

@@ -48,6 +48,11 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
     Vector3d inertia;
     double   mass;
     bool     imu_bias_fix, com_fix, inertia_fix, mass_fix;
+    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;
+
 
     ParamsSensorForceTorqueInertial() = default;
     ParamsSensorForceTorqueInertial(std::string _unique_name, const ParamsServer& _server)
@@ -80,6 +85,33 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
         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");
+
+        // regularization
+        if (_server.hasParam(prefix + _unique_name + "/set_mass_prior"))
+        {
+            set_mass_prior     = _server.getParam<bool>(prefix + _unique_name + "/set_mass_prior");
+            prior_mass_std     = _server.getParam<double>(prefix + _unique_name + "/prior_mass_std");
+        } else
+            set_mass_prior = false;
+        if (_server.hasParam(prefix + _unique_name + "/set_com_prior"))
+        {
+            set_com_prior      = _server.getParam<bool>(prefix + _unique_name + "/set_com_prior");
+            prior_com_std      = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/prior_com_std");
+        } else
+            set_com_prior = false;
+        if (_server.hasParam(prefix + _unique_name + "/set_inertia_prior"))
+        {
+            set_inertia_prior  = _server.getParam<bool>(prefix + _unique_name + "/set_inertia_prior");
+            prior_inertia_std  = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/prior_inertia_std");
+        } else
+            set_inertia_prior = false;
+        if (_server.hasParam(prefix + _unique_name + "/set_bias_prior"))
+        {
+            set_bias_prior  = _server.getParam<bool>(prefix + _unique_name + "/set_bias_prior");
+            prior_bias_std  = _server.getParam<Eigen::Vector6d>(prefix + _unique_name + "/prior_bias_std");
+        } else
+            set_bias_prior = false;
+
     }
     ~ParamsSensorForceTorqueInertial() override = default;
     std::string print() const override

src/sensor/sensor_force_torque_inertial.cpp

@@ -53,6 +53,49 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd&
     setStateBlockDynamic('i', false);
     setStateBlockDynamic('m', false);
 
+    // set priors for regularization
+    if(params_fti_->set_mass_prior)
+    {
+        std::cout<<"Setting prior for mass: "<<params_fti_->mass;
+        std::cout<<"\n  With std: "<<params_fti_->prior_mass_std;
+        Vector1d prior_mass;
+        prior_mass<<params_fti_->mass;
+        Matrix1d prior_mass_var;
+        prior_mass_var<<params_fti_->prior_mass_std*params_fti_->prior_mass_std;
+        std::cout<<"\n  With cov:\n"<<prior_mass_var<<"\n";
+        addPriorParameter('m',prior_mass,prior_mass_var);
+        std::cout<<"Done\n";
+    }
+    if(params_fti_->set_com_prior)
+    {
+        std::cout<<"Setting prior for center of mass: "<<params_fti_->com.transpose();
+        std::cout<<"\n  With std: "<<params_fti_->prior_com_std.transpose();
+        Matrix3d prior_com_cov = params_fti_->prior_com_std.array().pow(2).matrix().asDiagonal();
+        std::cout<<"\n  With cov:\n"<<prior_com_cov<<"\n";
+        addPriorParameter('C',params_fti_->com, prior_com_cov);
+        std::cout<<"Done\n";
+    }
+    if(params_fti_->set_inertia_prior)
+    {
+        std::cout<<"Setting prior for inertia: "<<params_fti_->inertia.transpose();
+        Matrix3d prior_inertia_cov = params_fti_->prior_inertia_std.array().pow(2).matrix().asDiagonal();
+        std::cout<<"\n  With std: "<<params_fti_->prior_inertia_std.transpose();
+        std::cout<<"\n  With cov: \n"<<prior_inertia_cov<<"\n";
+        addPriorParameter('i',params_fti_->inertia, prior_inertia_cov);
+        std::cout<<"Done\n";
+    }
+    if(params_fti_->set_bias_prior)
+    {
+        std::cout<<"Setting prior for bias: "<<Vector6d::Zero().transpose();
+        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);
+        std::cout<<"Done\n";
+    }
+ 
+
+
     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;