Commit 5fa20026 authored by adrianamor's avatar adrianamor

debugging. previous to matlab comparison

parent 46fb2716
# Initial STD of Error-State values (filter)
dp0_std: [0.0, 0.0, 0.0]
dp0_std: [0.0, 0.0, 0.05]
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]
dtheta0_std: [0.05, 0.05, 0.05]
dab0_std: [0.0196, 0.0196, 0.0196]
dwb0_std: [0.0037, 0.0040, 0.0]
dg0_std: [0.0, 0.0, 0.0]
frame: global # Frame r.t. orientation derivation is computed (global or local)
#IMU STD values
imu_a_std: [0.006,0.006,0.006]
imu_w_std: [0.004,0.004,0.004]
imu_a_std: [0.2,0.2,0.2]
imu_w_std: [0.005,0.005,0.005]
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).
imu_method: discrete #Sets the data time nature (continous or discrete).
# PX4 STD values
px4_std_landed: [0.1,0.001,0.001]
......@@ -20,7 +20,7 @@ px4_std_below30cm: [10.0,10.0,10.0]
px4_std_30cmto5m: [0.01,0.15,0.15]
# 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]
xstate0: [0.0,0.0,0.1,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]
# 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
......@@ -153,9 +153,6 @@ void EskfOdomAlgNode::mainNodeThread(void)
// get state
VectorXd state = this->alg_.get_x_state();
cout << "TODO: Debug. Why the states get so big?" << endl;
cout << state(0) << "," << state(1) << "," << state(2) << endl;
this->alg_.unlock();
this->ll_status_mutex_exit();
this->px4_of_mutex_exit();
......@@ -195,9 +192,10 @@ void EskfOdomAlgNode::px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg)
// Get sensor values
this->px4_of_mutex_enter();
this->gnd_dist_ = msg->ground_distance;
double vx = msg->velocity_x;
double vy = msg->velocity_y;
double vy = -msg->velocity_y; // axis change according to simulation
ros::Time stamp = msg->header.stamp;
this->px4_of_mutex_exit();
......@@ -213,7 +211,7 @@ void EskfOdomAlgNode::px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg)
// Error observation and Nominal-State Update
this->alg_.obs_error_and_up_nominal();
this->alg_.unlock();
this->alg_.unlock();
}
void EskfOdomAlgNode::px4_of_mutex_enter(void)
......@@ -232,11 +230,11 @@ void EskfOdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
// Get sensor values
this->imu_mutex_enter();
double ax = msg->linear_acceleration.x;
double ay = msg->linear_acceleration.y;
double az = msg->linear_acceleration.z;
double ay = -msg->linear_acceleration.y; // axis change according to simulation
double az = -msg->linear_acceleration.z; // axis change according to simulation
double wx = msg->angular_velocity.x;
double wy = msg->angular_velocity.y;
double wz = msg->angular_velocity.z;
double wy = -msg->angular_velocity.y; // axis change according to simulation
double wz = -msg->angular_velocity.z;// axis change according to simulation
ros::Time stamp = msg->header.stamp;
this->imu_mutex_exit();
......@@ -250,7 +248,7 @@ void EskfOdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
// Set values into filter object
this->alg_.set_imu_reading(t,a,w);
// Propagate Nominal-State
this->alg_.prop_nominal();
......
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