Commit 6fbbc988 authored by asantamaria's avatar asantamaria

Added sensor extrinsics to state vectors

parent 2ea7bea9
......@@ -9,6 +9,10 @@ dtheta0_std: [0.05, 0.05, 0.0]
dab0_std: [0.0196, 0.0196, 0.0196]
dwb0_std: [0.0037, 0.0040, 0.0]
dg0_std: [0.0, 0.0, 0.0]
dpo0_std: [0.0, 0.0, 0.0]
dthetao0_std: [0.0, 0.0, 0.0]
dpr0_std: [0.0, 0.0, 0.0]
dthetar0_std: [0.0, 0.0, 0.0]
frame: global # Frame r.t. orientation derivation is computed (global or local)
#IMU pose`
......@@ -50,7 +54,7 @@ flow2d_std_insidebounds: [0.15,0.15]
flow2d_focal_length: 73
# 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]
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,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0]
# 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]
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,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
......@@ -83,7 +83,11 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
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);
f_params.dg0_std = read_vec("dg0_std", 3);
f_params.dpo0_std = read_vec("dpo0_std", 3);
f_params.dthetao0_std = read_vec("dthetao0_std", 3);
f_params.dpr0_std = read_vec("dpr0_std", 3);
f_params.dthetar0_std = read_vec("dthetar0_std", 3);
std::string frame; // Frame r.t. orientation derivation is computed (g:global, l:local)
this->public_node_handle_.param<std::string>("frame", frame, "global");
......@@ -159,10 +163,10 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
flow2d_params.focal_length = static_cast<float>(focal_length);
// Initialize Nominal-State vector
Eigen::VectorXf x_state = read_vec("xstate0", 19);
Eigen::VectorXf x_state = read_vec("xstate0", 33);
// Initialize Error-State vector
Eigen::VectorXf dx_state = read_vec("dxstate0", 18);
Eigen::VectorXf dx_state = read_vec("dxstate0", 30);
this->alg_.lock(); // protect algorithm
......@@ -222,7 +226,7 @@ void EskfOdomAlgNode::mainNodeThread(void)
bool is_flying = this->flying_;
this->flying_mutex_exit();
Eigen::VectorXf state(19,1);
Eigen::VectorXf state(33,1);
Eigen::Vector3f ang_vel;
bool step_done = false;
......
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