Commit 3b32a01e authored by adrianamor's avatar adrianamor

call to low-level library needs to be solved

parent 01d1b395
......@@ -48,6 +48,7 @@ class EskfOdomAlgorithm
pthread_mutex_t access_;
// private attributes and methods
// CEskf filter_;
public:
/**
......
......@@ -141,7 +141,7 @@ class EskfOdomAlgNode : public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorith
*
* This function reads a 3-Vector parameter specified by param_name
*/
Vector3d read_vec3(const string& param_name);
VectorXd read_vec(const string& param_name, const int& exp_long);
};
#endif
# Initial Error-State values (filter)
# Initial STD of Error-State values (filter)
dp0_std: [0.0, 0.0, 0.0]
dv0_std: [0.0, 0.0, 0.0]
dtheta0_std: [0.0, 0.0, 0.0]
dab0_std: [0.0, 0.0, 0.0]
dwb0_std: [0.0, 0.0, 0.0]
dg0_std: [0.0, 0.0, 0.0]
frame: global # Frame r.t. orientation derivation is computed (global or local)
# Frame r.t. orientation derivation is computed (g:global, l:local)
frame: global
\ No newline at end of file
#IMU STD values
imu_a_std: [0.006,0.006,0.006]
imu_w_std: [0.004,0.004,0.004]
imu_ab_std: [0.0,0.0,0.0]
imu_wb_std: [0.0,0.0,0.0]
imu_method: continuous #Sets the data time nature (continous or discrete).
# PX4 STD values
px4_std: [0.01,0.1,0.1]
# 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.0,0.0,0.0,0.0,0.0,-9.803057]
# Initial Error-State vector (dp,dv,dtheta,dab,dwb,dg)
dxstate0: [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
\ No newline at end of file
......@@ -38,22 +38,16 @@ EskfOdomAlgNode::~EskfOdomAlgNode(void)
void EskfOdomAlgNode::read_params(void)
{
// double kk;
// this->public_node_handle_.param<double>("kk", kk, 0.0);
// Filter parameters
// Initial STD Error-State values (filter parameters)
params f_params;
// Initial Error-State values (filter)
f_params.dp0_std = read_vec3("dp0_std");
f_params.dv0_std = read_vec3("dv0_std");
f_params.dtheta0_std = read_vec3("dtheta0_std");
f_params.dab0_std = read_vec3("dab0_std");
f_params.dwb0_std = read_vec3("dwb0_std");
f_params.dg0_std = read_vec3("dg0_std");
// Frame r.t. orientation derivation is computed (g:global, l:local)
string frame;
f_params.dp0_std = read_vec("dp0_std",3);
f_params.dv0_std = read_vec("dv0_std",3);
f_params.dtheta0_std = read_vec("dtheta0_std",3);
f_params.dab0_std = read_vec("dab0_std",3);
f_params.dwb0_std = read_vec("dwb0_std",3);
f_params.dg0_std = read_vec("dg0_std",3);
string frame; // Frame r.t. orientation derivation is computed (g:global, l:local)
this->public_node_handle_.param<string>("frame", frame, "global");
if (frame == "global")
f_params.frame = 'g';
......@@ -61,6 +55,32 @@ void EskfOdomAlgNode::read_params(void)
f_params.frame = 'l';
else cout << "ERROR loading frame: Wrong frame name (should be local or global)" << endl;
// IMU STD values
Sensor::imu_params imu_params;
imu_params.a_std = read_vec("imu_a_std",3);
imu_params.w_std = read_vec("imu_w_std",3);
imu_params.ab_std = read_vec("imu_ab_std",3);
imu_params.wb_std = read_vec("imu_wb_std",3);
string imu_met;
this->public_node_handle_.param<string>("imu_method", imu_met, "continuous");
if (imu_met == "continuous")
imu_params.met = 'c';
else if (imu_met == "discrete")
imu_params.met = 'd';
else cout << "ERROR loading frame: Wrong IMU method name (should be continuous or discrete)" << endl;
// PX4 STD values
Sensor::px4_params px4_params;
px4_params.std = read_vec("px4_std",3);
// Initialize Nominal-State vector
VectorXd x_state = read_vec("xstate0",19);
// Initialize Error-State vector
VectorXd dx_state = read_vec("dxstate0",18);
//TODO: CALL low-level library to initialize parameters
// Print read values
cout << ">> ESKF_odom, loaded parameters: " << endl;
cout << " ______Filter parameters" << endl;
......@@ -71,18 +91,40 @@ void EskfOdomAlgNode::read_params(void)
cout << " dwb0_std: " << f_params.dwb0_std(0) << "," << f_params.dwb0_std(1) << "," << f_params.dwb0_std(2)<< endl;
cout << " dg0_std: " << f_params.dg0_std(0) << "," << f_params.dg0_std(1) << "," << f_params.dg0_std(2)<< endl;
cout << " frame: " << f_params.frame << endl;
cout << " ______IMU parameters" << endl;
cout << " a_std: " << imu_params.a_std(0) << "," << imu_params.a_std(1) << "," << imu_params.a_std(2)<< endl;
cout << " w_std: " << imu_params.w_std(0) << "," << imu_params.w_std(1) << "," << imu_params.w_std(2)<< endl;
cout << " ab_std: " << imu_params.ab_std(0) << "," << imu_params.ab_std(1) << "," << imu_params.ab_std(2)<< endl;
cout << " wb_std: " << imu_params.wb_std(0) << "," << imu_params.wb_std(1) << "," << imu_params.wb_std(2)<< endl;
cout << " method: " << imu_params.met << endl;
cout << " ______Initial Nominal-State Vector" << endl;
cout << " p0: " << x_state(0) << "," << x_state(1) << "," << x_state(2) << endl;
cout << " v0: " << x_state(3) << "," << x_state(4) << "," << x_state(5) << endl;
cout << " q0: " << x_state(6) << "," << x_state(7) << "," << x_state(8) <<"," << x_state(9)<< endl;
cout << " ab0: " << x_state(10) << "," << x_state(11) << "," << x_state(12) << endl;
cout << " wb0: " << x_state(13) << "," << x_state(14) << "," << x_state(15) << endl;
cout << " g0: " << x_state(16) << "," << x_state(17) << "," << x_state(18) << endl;
cout << " ______Initial Error-State Vector" << endl;
cout << " dp0: " << dx_state(0) << "," << dx_state(1) << "," << dx_state(2) << endl;
cout << " dv0: " << dx_state(3) << "," << dx_state(4) << "," << dx_state(5) << endl;
cout << " dtheta0: " << dx_state(6) << "," << dx_state(7) << "," << dx_state(8) << endl;
cout << " dab0: " << dx_state(9) << "," << dx_state(10) << "," << dx_state(11) << endl;
cout << " dwb0: " << dx_state(12) << "," << dx_state(13) << "," << dx_state(14) << endl;
cout << " dg0: " << dx_state(15) << "," << dx_state(16) << "," << dx_state(17) << endl;
}
Vector3d EskfOdomAlgNode::read_vec3(const string& param_name)
VectorXd EskfOdomAlgNode::read_vec(const string& param_name, const int& exp_long)
{
Vector3d params = Vector3d::Zero();
VectorXd params = VectorXd::Zero(exp_long);
XmlRpc::XmlRpcValue my_list;
this->public_node_handle_.getParam(param_name, my_list);
if (my_list.getType() == XmlRpc::XmlRpcValue::TypeArray) {
int vec_size = int(my_list.size());
if (vec_size == 3){
if (vec_size == exp_long){
for (int ii=0; ii<vec_size; ii++)
params(ii) = my_list[ii];
}
......
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