Commit 49478460 authored by asantamaria's avatar asantamaria

Added possibility to set sensor parameters online (covariances)

parent 2c9c7cf7
......@@ -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
*
......
......@@ -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);
......
......@@ -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();
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment