Added possibility to set sensor parameters online (covariances)

include/eskf_odom_alg.h

@@ -161,7 +161,22 @@ class EskfOdomAlgorithm
     *   There are no specific inputs due to all are class variables.
     */
     void set_obs_phase(const bool& flying, const float& gnd_dist);
-    // void set_obs_phase(const bool& flying);
+
+    /**
+    * \brief Get imu parameters
+    *
+    * Get imu parameters
+    *
+    */
+    Sensor::imu_params get_imu_params(void);
+
+    /**
+    * \brief Set imu parameters
+    *
+    * Set imu parameters
+    *
+    */
+    void set_imu_params(Sensor::imu_params& imu);
 
     /**
     * \brief Set IMU Readings
@@ -175,6 +190,22 @@ class EskfOdomAlgorithm
     */
     void set_imu_reading(const float& t, const Eigen::Vector3f& a, const Eigen::Vector3f& w);
 
+    /**
+    * \brief Get pose parameters
+    *
+    * Get pose parameters
+    *
+    */
+    Sensor::pose_params get_pose_params(void);
+
+    /**
+    * \brief Set pose parameters
+    *
+    * Set pose parameters
+    *
+    */
+    void set_pose_params(Sensor::pose_params& pose);
+
     /**
     * \brief Set Pose Readings
     *
@@ -186,6 +217,22 @@ class EskfOdomAlgorithm
     */
     void set_pose_reading(const float& t, const Eigen::VectorXf& val);
 
+    /**
+    * \brief Get position parameters
+    *
+    * Get position parameters
+    *
+    */
+    Sensor::position_params get_position_params(void);
+
+    /**
+    * \brief Set position parameters
+    *
+    * Set position parameters
+    *
+    */
+    void set_position_params(Sensor::position_params& position);
+
     /**
     * \brief Set Position Readings
     *
@@ -197,6 +244,22 @@ class EskfOdomAlgorithm
     */
     void set_position_reading(const float& t, const Eigen::Vector3f& val);
 
+    /**
+    * \brief Get orientation parameters
+    *
+    * Get orientation parameters
+    *
+    */
+    Sensor::orientation_params get_orientation_params(void);
+
+    /**
+    * \brief Set orientation parameters
+    *
+    * Set orientation parameters
+    *
+    */
+    void set_orientation_params(Sensor::orientation_params& orientation);
+
     /**
     * \brief Set Pose Readings
     *
@@ -208,6 +271,22 @@ class EskfOdomAlgorithm
     */
     void set_orientation_reading(const float& t, const Eigen::Vector3f& val);
 
+    /**
+    * \brief Get linear velocity parameters
+    *
+    * Get linear velocity parameters
+    *
+    */
+    Sensor::linvel_params get_linvel_params(void);
+
+    /**
+    * \brief Set linear velocity parameters
+    *
+    * Set linear velocity parameters
+    *
+    */
+    void set_linvel_params(Sensor::linvel_params& linvel);
+
     /**
     * \brief Set Linear Velocity Readings
     *
@@ -219,6 +298,22 @@ class EskfOdomAlgorithm
     */
     void set_linvel_reading(const float& t, const Eigen::Vector3f& val);
 
+    /**
+    * \brief Get range parameters
+    *
+    * Get range parameters
+    *
+    */
+    Sensor::range_params get_range_params(void);
+
+    /**
+    * \brief Set range parameters
+    *
+    * Set range parameters
+    *
+    */
+    void set_range_params(Sensor::range_params& range);
+
     /**
     * \brief Set Range Readings
     *
@@ -230,6 +325,22 @@ class EskfOdomAlgorithm
     */
     void set_range_reading(const float& t, const float& val);
 
+    /**
+    * \brief Get px4 parameters
+    *
+    * Get px4 parameters
+    *
+    */
+    Sensor::px4_params get_px4_params(void);
+
+    /**
+    * \brief Set px4 parameters
+    *
+    * Set px4 parameters
+    *
+    */
+    void set_px4_params(Sensor::px4_params& px4);
+
     /**
     * \brief Set PX4 Optical Flow Readings
     *
@@ -242,6 +353,22 @@ class EskfOdomAlgorithm
     */
     void set_px4_reading(const float& t, const Eigen::Vector3f& val, const Eigen::Vector2f& flow);
 
+    /**
+    * \brief Get flow2d parameters
+    *
+    * Get flow2d parameters
+    *
+    */
+    Sensor::flow2d_params get_flow2d_params(void);
+
+    /**
+    * \brief Set flow2d parameters
+    *
+    * Set flow2d parameters
+    *
+    */
+    void set_flow2d_params(Sensor::flow2d_params& flow2d);
+    
     /**
     * \brief Set Flow2d Readings
     *

src/eskf_odom_alg.cpp

@@ -35,41 +35,121 @@ void EskfOdomAlgorithm::set_obs_phase(const bool& flying, const float& gnd_dist)
 	this->filter_.set_obs_phase(flying,gnd_dist);
 }
 
+Sensor::imu_params EskfOdomAlgorithm::get_imu_params(void)
+{
+  return this->filter_.get_imu_params();
+}
+
+void EskfOdomAlgorithm::set_imu_params(Sensor::imu_params& imu)
+{
+  this->filter_.set_imu_params(imu);
+}
+
 void EskfOdomAlgorithm::set_imu_reading(const float& t, const Eigen::Vector3f& a, const Eigen::Vector3f& w)
 {
 	this->filter_.set_imu_reading(t,a,w);
 }
 
+Sensor::pose_params EskfOdomAlgorithm::get_pose_params(void)
+{
+  return this->filter_.get_pose_params();
+}
+
+void EskfOdomAlgorithm::set_pose_params(Sensor::pose_params& pose)
+{
+  this->filter_.set_pose_params(pose);
+}
+
 void EskfOdomAlgorithm::set_pose_reading(const float& t, const Eigen::VectorXf& val)
 {
   this->filter_.set_pose_reading(t,val);
 }
 
+Sensor::position_params EskfOdomAlgorithm::get_position_params(void)
+{
+  return this->filter_.get_position_params();
+}
+
+void EskfOdomAlgorithm::set_position_params(Sensor::position_params& position)
+{
+  this->filter_.set_position_params(position);
+}
+
 void EskfOdomAlgorithm::set_position_reading(const float& t, const Eigen::Vector3f& val)
 {
   this->filter_.set_position_reading(t,val);
 }
 
+Sensor::orientation_params EskfOdomAlgorithm::get_orientation_params(void)
+{
+  return this->filter_.get_orientation_params();
+}
+
+void EskfOdomAlgorithm::set_orientation_params(Sensor::orientation_params& orientation)
+{
+  this->filter_.set_orientation_params(orientation);
+}
+
 void EskfOdomAlgorithm::set_orientation_reading(const float& t, const Eigen::Vector3f& val)
 {
   this->filter_.set_orientation_reading(t,val);
 }
 
+Sensor::linvel_params EskfOdomAlgorithm::get_linvel_params(void)
+{
+  return this->filter_.get_linvel_params();
+}
+
+void EskfOdomAlgorithm::set_linvel_params(Sensor::linvel_params& linvel)
+{
+  this->filter_.set_linvel_params(linvel);
+}
+
 void EskfOdomAlgorithm::set_linvel_reading(const float& t, const Eigen::Vector3f& val)
 {
   this->filter_.set_linvel_reading(t,val);
 }
 
+Sensor::range_params EskfOdomAlgorithm::get_range_params(void)
+{
+  return this->filter_.get_range_params();
+}
+
+void EskfOdomAlgorithm::set_range_params(Sensor::range_params& range)
+{
+  this->filter_.set_range_params(range);
+}
+
 void EskfOdomAlgorithm::set_range_reading(const float& t, const float& val)
 {
   this->filter_.set_range_reading(t,val);
 }
 
+Sensor::px4_params EskfOdomAlgorithm::get_px4_params(void)
+{
+  return this->filter_.get_px4_params();
+}
+
+void EskfOdomAlgorithm::set_px4_params(Sensor::px4_params& px4)
+{
+  this->filter_.set_px4_params(px4);
+}
+
 void EskfOdomAlgorithm::set_px4_reading(const float& t, const Eigen::Vector3f& val, const Eigen::Vector2f& flow)
 {
   this->filter_.set_px4_reading(t,val,flow);
 }
 
+Sensor::flow2d_params EskfOdomAlgorithm::get_flow2d_params(void)
+{
+  return this->filter_.get_flow2d_params();
+}
+
+void EskfOdomAlgorithm::set_flow2d_params(Sensor::flow2d_params& flow2d)
+{
+  this->filter_.set_flow2d_params(flow2d);
+}
+
 void EskfOdomAlgorithm::set_flow2d_reading(const float& t, const Eigen::Vector2f& val)
 {
   this->filter_.set_flow2d_reading(t,val);

src/eskf_odom_alg_node.cpp

@@ -303,7 +303,18 @@ void EskfOdomAlgNode::set_imu_reading(const sensor_msgs::Imu::ConstPtr& msg,cons
 	w << wx, wy, wz;
 
 	this->alg_.lock();
-	this->alg_.set_imu_reading(msg_time, a, w); // Set values into filter object
+
+  // OPTIONAL: USE Actual IMU covariance. If first element is zero, then no covariance set.
+  // if (msg->angular_velocity_covariance[0]>0.0 && msg->linear_acceleration_covariance[0]>0.0)
+  // {
+  //   Sensor::imu_params imu = this->alg_.get_imu_params();
+  //   imu.w_std << msg->angular_velocity_covariance[0],msg->angular_velocity_covariance[4],msg->angular_velocity_covariance[8];
+  //   imu.a_std << msg->linear_acceleration_covariance[0],msg->linear_acceleration_covariance[4],msg->linear_acceleration_covariance[8];
+  //   this->alg_.set_imu_params(imu);
+  // }
+
+  this->alg_.set_imu_reading(msg_time, a, w); // Set values into filter object
+
 	this->alg_.unlock();
 }
 
@@ -326,9 +337,49 @@ void EskfOdomAlgNode::set_px4_reading(const px_comm::OpticalFlow::ConstPtr& msg,
 	flow << flowx, flowy;
 
 	this->alg_.lock();
+
+  //*****************************
+  // USING COMPENSATED VELOCITIES
+  //*****************************
+
+  // // // OPTIONAL: USE Actual PX4 Quality in the covariance.
+  // // Sensor::px4_params px4_params = this->alg_.get_px4_params();
+  // // if (msg->quality > 0 && px4_params.std(0)==px4_params.std_insidebounds(0))
+  // // {
+  // //   float qualnorm = std::pow(255,3)/std::pow(msg->quality,3);
+  // //   float gd_std = qualnorm*px4_params.std_insidebounds(0);
+  // //   float vxy_std = qualnorm*px4_params.std_insidebounds(1);
+  // //   px4_params.std << gd_std,vxy_std,vxy_std;
+  // //   this->alg_.set_px4_params(px4_params);
+  // // }
+
   // this->alg_.set_px4_reading(msg_time, val, flow); // Set values into filter object
+
+  //*****************************
+  // USING RANGE AND RAW FLOW
+  //*****************************
+  
+  // // OPTIONAL: USE Actual PX4 Quality in the covariance.
+  // Sensor::range_params range_params = this->alg_.get_range_params();
+  // if (msg->quality > 0 && range_params.std==range_params.std_insidebounds)
+  // {
+  //   float qualnorm = std::pow(255,3)/std::pow(msg->quality,3);
+  //   range_params.std = qualnorm*range_params.std_insidebounds;
+  //   this->alg_.set_range_params(range_params);
+  // }
+
+  // Sensor::flow2d_params flow2d_params = this->alg_.get_flow2d_params();
+  // if (msg->quality > 0 && flow2d_params.std==flow2d_params.std_insidebounds)
+  // {
+  //   float qualnorm = std::pow(255,3)/std::pow(msg->quality,3);
+  //   float flow2d_std = qualnorm*flow2d_params.std_insidebounds(0);
+  //   flow2d_params.std << flow2d_std,flow2d_std;
+  //   this->alg_.set_flow2d_params(flow2d_params);
+  // }
+
   this->alg_.set_range_reading(msg_time, this->gnd_dist_); // Set values into filter object
   this->alg_.set_flow2d_reading(msg_time, flow); // Set values into filter object
+
   this->alg_.unlock();
 }