Commit ef66244f authored by asantamaria's avatar asantamaria

Removed virtual sensor and added POSE and LINEAR VELOCITY sensors. still working.

parent ceaeb590
......@@ -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);
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);
/**
* \brief Print initial parameters
......@@ -209,6 +209,28 @@ class EskfOdomAlgorithm
*/
void set_flow2d_reading(const float& t, const Eigen::Vector2f& val);
/**
* \brief Set Pose Readings
*
* Store new Pose readings
*
* Input:
* t: Time stamp.
* val: Pose reading flow = [p_x,p_y,p_z,r_x,r_y,r_z].
*/
void set_pose_reading(const float& t, const Eigen::VectorXf& val);
/**
* \brief Set Linear Velocity Readings
*
* Store new Linear Velocity readings
*
* Input:
* t: Time stamp.
* val: Linear velocity reading flow = [v_x,v_y,v_z].
*/
void set_linvel_reading(const float& t, const Eigen::Vector3f& val);
/**
* \brief Run next step
*
......
......@@ -5,5 +5,5 @@
<remap from="~px4_of" to="/px4flow/opt_flow" />
<rosparam file="$(find eskf_odom)/launch/params.yaml"/>
</node>
z
</launch>
\ No newline at end of file
......@@ -29,9 +29,17 @@ range_std_insidebounds: 0.01
# Flow2d STD values
flow2d_std_outsidebounds: [10.0,10.0]
flow2d_std_insidebounds: [0.15,0.15]
# Focal length in pixels
# 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)
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)
{
this->filter_.set_init_params(f_params,x0,dx0,imu,px4,range,flow2d);
this->filter_.set_init_params(f_params,x0,dx0,imu,px4,range,flow2d,pose,linvel);
}
void EskfOdomAlgorithm::print_ini_params(void)
......@@ -55,6 +55,16 @@ void EskfOdomAlgorithm::set_flow2d_reading(const float& t, const Eigen::Vector2f
this->filter_.set_flow2d_reading(t,val);
}
void EskfOdomAlgorithm::set_pose_reading(const float& t, const Eigen::VectorXf& val)
{
this->filter_.set_pose_reading(t,val);
}
void EskfOdomAlgorithm::set_linvel_reading(const float& t, const Eigen::Vector3f& val)
{
this->filter_.set_linvel_reading(t,val);
}
float EskfOdomAlgorithm::get_proc_time(void)
{
return this->filter_.get_proc_time();
......
......@@ -116,6 +116,18 @@ 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);
......@@ -125,7 +137,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);
this->alg_.set_init_params(f_params, x_state, dx_state, imu_params, px4_params, range_params, flow2d_params, pose_params, linvel_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