Commit 2c9c7cf7 authored by asantamaria's avatar asantamaria

Added POSITION, ORIENTATION and LINEAR VELOCITY sensors (together with POSE...

Added POSITION, ORIENTATION and LINEAR VELOCITY sensors (together with POSE sensor). Working. TODO: Fix simulation and add ROS pub/sub
parent ef66244f
......@@ -128,7 +128,7 @@ class EskfOdomAlgorithm
* nominal-state vector and error-state vector. Wrapper of the low-level library
*
*/
void set_init_params(const params& f_params, const Eigen::VectorXf& x0, const Eigen::VectorXf& dx0, const Sensor::imu_params& imu,const Sensor::px4_params& px4,const Sensor::range_params& range,const Sensor::flow2d_params& flow2d,const Sensor::pose_params& pose,const Sensor::linvel_params& linvel);
void set_init_params(const params& f_params, const Eigen::VectorXf& x0, const Eigen::VectorXf& dx0, const Sensor::imu_params& imu,const Sensor::position_params& position,const Sensor::orientation_params& orientation,const Sensor::linvel_params& linvel,const Sensor::range_params& range,const Sensor::px4_params& px4,const Sensor::flow2d_params& flow2d);
/**
* \brief Print initial parameters
......@@ -176,60 +176,82 @@ class EskfOdomAlgorithm
void set_imu_reading(const float& t, const Eigen::Vector3f& a, const Eigen::Vector3f& w);
/**
* \brief Set PX4 Optical Flow Readings
* \brief Set Pose Readings
*
* Store new PX4 Optical Flow readings
* Store new Pose readings
*
* Input:
* t: Time stamp.
* val: Sensor readings: val = [pz;vx;vy].
* flow: Optical flow raw reading flow = [flow_x,flow_y].
* val: Pose reading = [p_x,p_y,p_z,r_x,r_y,r_z].
*/
void set_px4_reading(const float& t, const Eigen::Vector3f& val, const Eigen::Vector2f& flow);
void set_pose_reading(const float& t, const Eigen::VectorXf& val);
/**
* \brief Set Range Readings
* \brief Set Position Readings
*
* Store new Range readings
* Store new Position readings
*
* Input:
* t: Time stamp.
* val: Range reading.
* val: Position reading = [p_x,p_y,p_z].
*/
void set_range_reading(const float& t, const float& val);
void set_position_reading(const float& t, const Eigen::Vector3f& val);
/**
* \brief Set Flow2d Readings
* \brief Set Pose Readings
*
* Store new Flow2d readings
* Store new Pose readings
*
* Input:
* t: Time stamp.
* val: Optical flow raw reading flow = [flow_x,flow_y].
* val: Orientation reading = [r_x,r_y,r_z].
*/
void set_flow2d_reading(const float& t, const Eigen::Vector2f& val);
void set_orientation_reading(const float& t, const Eigen::Vector3f& val);
/**
* \brief Set Pose Readings
* \brief Set Linear Velocity Readings
*
* Store new Pose readings
* Store new Linear Velocity readings
*
* Input:
* t: Time stamp.
* val: Pose reading flow = [p_x,p_y,p_z,r_x,r_y,r_z].
* val: Linear velocity reading = [v_x,v_y,v_z].
*/
void set_pose_reading(const float& t, const Eigen::VectorXf& val);
void set_linvel_reading(const float& t, const Eigen::Vector3f& val);
/**
* \brief Set Linear Velocity Readings
* \brief Set Range Readings
*
* Store new Linear Velocity readings
* Store new Range readings
*
* Input:
* t: Time stamp.
* val: Linear velocity reading flow = [v_x,v_y,v_z].
* val: Range reading.
*/
void set_linvel_reading(const float& t, const Eigen::Vector3f& val);
void set_range_reading(const float& t, const float& val);
/**
* \brief Set PX4 Optical Flow Readings
*
* Store new PX4 Optical Flow readings
*
* Input:
* t: Time stamp.
* val: Sensor readings: val = [pz;vx;vy].
* flow: Optical flow raw reading flow = [flow_x,flow_y].
*/
void set_px4_reading(const float& t, const Eigen::Vector3f& val, const Eigen::Vector2f& flow);
/**
* \brief Set Flow2d Readings
*
* Store new Flow2d readings
*
* Input:
* t: Time stamp.
* val: Optical flow raw reading flow = [flow_x,flow_y].
*/
void set_flow2d_reading(const float& t, const Eigen::Vector2f& val);
/**
* \brief Run next step
......
......@@ -18,28 +18,32 @@ imu_ab_std: [0.0,0.0,0.0]
imu_wb_std: [0.0,0.0,0.0]
imu_method: discrete #Sets the data time nature (continous: simulated IMU or discrete: IMU sensor).
# PX4 STD values
px4_std_outsidebounds: [10.0,10.0,10.0]
px4_std_insidebounds: [0.01,0.15,0.15]
# Position STD values
position_std_outsidebounds: [10.0,10.0,10.0]
position_std_insidebounds: [0.01,0.01,0.01]
# Orientation STD values
orientation_std_outsidebounds: [10.0,10.0,10.0]
orientation_std_insidebounds: [0.01,0.01,0.01]
# Linear velocity STD values
linvel_std_outsidebounds: [10.0,10.0,10.0]
linvel_std_insidebounds: [0.01,0.01,0.01]
# Range STD values
range_std_outsidebounds: 10.0
range_std_insidebounds: 0.01
# PX4 STD values
px4_std_outsidebounds: [10.0,10.0,10.0]
px4_std_insidebounds: [0.01,0.15,0.15]
# Flow2d STD values
flow2d_std_outsidebounds: [10.0,10.0]
flow2d_std_insidebounds: [0.15,0.15]
# focal_length in pixels
flow2d_focal_length: 73
# Pose STD values
pose_std_outsidebounds: [10.0,10.0,10.0,10.0,10.0,10.0]
pose_std_insidebounds: [0.01,0.01,0.01,0.01,0.01,0.01]
# Linear velocity STD values
linvel_std_outsidebounds: [10.0,10.0,10.0]
linvel_std_insidebounds: [0.01,0.01,0.01]
# Initial Nominal-State vector (p,v,q,ab,wb,g)
xstate0: [0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,-0.0005,-0.0026,-0.0004,0.0,0.0,-9.803057]
......
......@@ -20,9 +20,9 @@ void EskfOdomAlgorithm::config_update(Config& new_cfg, uint32_t level)
this->unlock();
}
void EskfOdomAlgorithm::set_init_params(const params& f_params, const Eigen::VectorXf& x0, const Eigen::VectorXf& dx0, const Sensor::imu_params& imu,const Sensor::px4_params& px4,const Sensor::range_params& range,const Sensor::flow2d_params& flow2d,const Sensor::pose_params& pose,const Sensor::linvel_params& linvel)
void EskfOdomAlgorithm::set_init_params(const params& f_params, const Eigen::VectorXf& x0, const Eigen::VectorXf& dx0, const Sensor::imu_params& imu,const Sensor::position_params& position,const Sensor::orientation_params& orientation,const Sensor::linvel_params& linvel,const Sensor::range_params& range,const Sensor::px4_params& px4,const Sensor::flow2d_params& flow2d)
{
this->filter_.set_init_params(f_params,x0,dx0,imu,px4,range,flow2d,pose,linvel);
this->filter_.set_init_params(f_params,x0,dx0,imu,position,orientation,linvel,range,px4,flow2d);
}
void EskfOdomAlgorithm::print_ini_params(void)
......@@ -40,29 +40,39 @@ void EskfOdomAlgorithm::set_imu_reading(const float& t, const Eigen::Vector3f& a
this->filter_.set_imu_reading(t,a,w);
}
void EskfOdomAlgorithm::set_px4_reading(const float& t, const Eigen::Vector3f& val, const Eigen::Vector2f& flow)
void EskfOdomAlgorithm::set_pose_reading(const float& t, const Eigen::VectorXf& val)
{
this->filter_.set_px4_reading(t,val,flow);
this->filter_.set_pose_reading(t,val);
}
void EskfOdomAlgorithm::set_range_reading(const float& t, const float& val)
void EskfOdomAlgorithm::set_position_reading(const float& t, const Eigen::Vector3f& val)
{
this->filter_.set_range_reading(t,val);
this->filter_.set_position_reading(t,val);
}
void EskfOdomAlgorithm::set_flow2d_reading(const float& t, const Eigen::Vector2f& val)
void EskfOdomAlgorithm::set_orientation_reading(const float& t, const Eigen::Vector3f& val)
{
this->filter_.set_flow2d_reading(t,val);
this->filter_.set_orientation_reading(t,val);
}
void EskfOdomAlgorithm::set_pose_reading(const float& t, const Eigen::VectorXf& val)
void EskfOdomAlgorithm::set_linvel_reading(const float& t, const Eigen::Vector3f& val)
{
this->filter_.set_pose_reading(t,val);
this->filter_.set_linvel_reading(t,val);
}
void EskfOdomAlgorithm::set_linvel_reading(const float& t, const Eigen::Vector3f& val)
void EskfOdomAlgorithm::set_range_reading(const float& t, const float& val)
{
this->filter_.set_linvel_reading(t,val);
this->filter_.set_range_reading(t,val);
}
void EskfOdomAlgorithm::set_px4_reading(const float& t, const Eigen::Vector3f& val, const Eigen::Vector2f& flow)
{
this->filter_.set_px4_reading(t,val,flow);
}
void EskfOdomAlgorithm::set_flow2d_reading(const float& t, const Eigen::Vector2f& val)
{
this->filter_.set_flow2d_reading(t,val);
}
float EskfOdomAlgorithm::get_proc_time(void)
......
......@@ -92,11 +92,23 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
else
std::cout << "ERROR loading frame: Wrong IMU method name (should be continuous or discrete)" << std::endl;
// PX4 STD values
Sensor::px4_params px4_params;
px4_params.std_outsidebounds = read_vec("px4_std_outsidebounds", 3);
px4_params.std_insidebounds = read_vec("px4_std_insidebounds", 3);
px4_params.std = px4_params.std_outsidebounds;
// Position STD values
Sensor::position_params position_params;
position_params.std_outsidebounds = read_vec("position_std_outsidebounds", 3);
position_params.std_insidebounds = read_vec("position_std_insidebounds", 3);
position_params.std = position_params.std_outsidebounds; // Considering initially landed
// Orientation STD values
Sensor::orientation_params orientation_params;
orientation_params.std_outsidebounds = read_vec("orientation_std_outsidebounds", 3);
orientation_params.std_insidebounds = read_vec("orientation_std_insidebounds", 3);
orientation_params.std = orientation_params.std_outsidebounds; // Considering initially landed
// Linear Velocity STD values
Sensor::linvel_params linvel_params;
linvel_params.std_outsidebounds = read_vec("linvel_std_outsidebounds", 3);
linvel_params.std_insidebounds = read_vec("linvel_std_insidebounds", 3);
linvel_params.std = linvel_params.std_outsidebounds; // Considering initially landed
// Range STD values
Sensor::range_params range_params;
......@@ -107,6 +119,12 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
range_params.std_insidebounds = static_cast<float>(std_insidebounds);
range_params.std = range_params.std_outsidebounds; // Considering initially landed
// PX4 STD values
Sensor::px4_params px4_params;
px4_params.std_outsidebounds = read_vec("px4_std_outsidebounds", 3);
px4_params.std_insidebounds = read_vec("px4_std_insidebounds", 3);
px4_params.std = px4_params.std_outsidebounds;
// Flow2d STD values
Sensor::flow2d_params flow2d_params;
flow2d_params.std_outsidebounds = read_vec("flow2d_std_outsidebounds", 2);
......@@ -116,18 +134,6 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
this->public_node_handle_.param<double>("flow2d_focal_length", focal_length, 0.016);
flow2d_params.focal_length = static_cast<float>(focal_length);
// Pose STD values
Sensor::pose_params pose_params;
pose_params.std_outsidebounds = read_vec("pose_std_outsidebounds", 6);
pose_params.std_insidebounds = read_vec("pose_std_insidebounds", 6);
pose_params.std = pose_params.std_outsidebounds; // Considering initially landed
// Linear Velocity STD values
Sensor::linvel_params linvel_params;
linvel_params.std_outsidebounds = read_vec("linvel_std_outsidebounds", 3);
linvel_params.std_insidebounds = read_vec("linvel_std_insidebounds", 3);
linvel_params.std = linvel_params.std_outsidebounds; // Considering initially landed
// Initialize Nominal-State vector
Eigen::VectorXf x_state = read_vec("xstate0", 19);
......@@ -137,7 +143,7 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
this->alg_.lock(); // protect algorithm
// Set filter and sensors initial parameters
this->alg_.set_init_params(f_params, x_state, dx_state, imu_params, px4_params, range_params, flow2d_params, pose_params, linvel_params);
this->alg_.set_init_params(f_params, x_state, dx_state, imu_params, position_params, orientation_params, linvel_params, range_params, px4_params, flow2d_params);
// Print already set values of filter and sensors initial parameters
//this->alg_.print_ini_params();
......
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